Geometry.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013, SimQuest Solutions Inc.
3 //
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
9 //
10 // Unless required by applicable law or agreed to in writing, software
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15
16 #ifndef SURGSIM_MATH_GEOMETRY_H
17 #define SURGSIM_MATH_GEOMETRY_H
18
19 #include <boost/container/static_vector.hpp>
20 #include <Eigen/Core>
21 #include <Eigen/Geometry>
22
23 #include "SurgSim/Framework/Log.h"
24 #include "SurgSim/Math/Vector.h"
25
37
38 namespace SurgSim
39 {
40 namespace Math
41 {
42
43 namespace Geometry
44 {
45
47 static const double DistanceEpsilon = 1e-10;
48
50 static const double SquaredDistanceEpsilon = 1e-10;
51
53 static const double AngularEpsilon = 1e-10;
54
56 static const double ScalarEpsilon = 1e-10;
57
58 }
59
70 template <class T,int MOpt> inline
71 bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
72  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
73  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
74  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
75  const Eigen::Matrix<T, 3, 1, MOpt>& tn,
76  Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
77 {
78  const T signedTriAreaX2 = ((tv1-tv0).cross(tv2-tv0)).dot(tn);
79  if (signedTriAreaX2 < Geometry::SquaredDistanceEpsilon)
80  {
81  // SQ_ASSERT_WARNING(false, "Cannot compute barycentric coords (degenetrate triangle), assigning center");
82  coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
83  return false;
84  }
85  (*coordinates)[0] = ((tv1-pt).cross(tv2-pt)).dot(tn) / signedTriAreaX2;
86  (*coordinates)[1] = ((tv2-pt).cross(tv0-pt)).dot(tn) / signedTriAreaX2;
87  (*coordinates)[2] = 1 - (*coordinates)[0] - (*coordinates)[1];
88  return true;
89 }
90
101 template <class T, int MOpt> inline
103  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
104  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
105  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
106  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
107  Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
108 {
109  const Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1-tv0).cross(tv2-tv0);
110  return barycentricCoordinates(pt, tv0, tv1, tv2, tn, coordinates);
111 }
112
123 template <class T, int MOpt> inline
125  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
126  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
127  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
128  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
129  const Eigen::Matrix<T, 3, 1, MOpt>& tn)
130 {
131  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
132  bool result = barycentricCoordinates(pt, tv0, tv1, tv2, tn, &baryCoords);
133  return (result &&
134  baryCoords[0] >= -Geometry::ScalarEpsilon &&
135  baryCoords[1] >= -Geometry::ScalarEpsilon &&
136  baryCoords[2] >= -Geometry::ScalarEpsilon);
137 }
138
148 template <class T, int MOpt> inline
150  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
151  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
152  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
153  const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
154 {
155  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
156  bool result = barycentricCoordinates(pt, tv0, tv1, tv2, &baryCoords);
157  return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
158  baryCoords[1] >= -Geometry::ScalarEpsilon &&
159  baryCoords[2] >= -Geometry::ScalarEpsilon);
160 }
161
167 template <class T, int MOpt> inline
169  const Eigen::Matrix<T, 3, 1, MOpt>& a,
170  const Eigen::Matrix<T, 3, 1, MOpt>& b,
171  const Eigen::Matrix<T, 3, 1, MOpt>& c,
172  const Eigen::Matrix<T, 3, 1, MOpt>& d)
173 {
174  return std::abs((c - a).dot((b - a).cross(d - c))) < Geometry::ScalarEpsilon;
175 }
176
182 template <class T, int MOpt> inline
184  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
185  const Eigen::Matrix<T, 3, 1, MOpt>& v0,
186  const Eigen::Matrix<T, 3, 1, MOpt>& v1,
187  Eigen::Matrix<T, 3, 1, MOpt>* result)
188 {
189  // The lines is parametrized by:
190  // q = v0 + lambda0 * (v1-v0)
191  // and we solve for pq.v01 = 0;
192  Eigen::Matrix<T, 3, 1, MOpt> v01 = v1-v0;
193  T v01_norm2 = v01.squaredNorm();
194  if (v01_norm2 <= Geometry::SquaredDistanceEpsilon)
195  {
196  *result = v0; // closest point is either
197  T pv_norm2 = (pt-v0).squaredNorm();
198  return sqrt(pv_norm2);
199  }
200  T lambda = (v01).dot(pt-v0);
201  *result = v0 + lambda*v01/v01_norm2;
202  return (*result-pt).norm();
203 }
204
213 template <class T, int MOpt> inline
215  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
216  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
217  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
218  Eigen::Matrix<T, 3, 1, MOpt>* result)
219 {
220  Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
221  T v01Norm2 = v01.squaredNorm();
222  if (v01Norm2 <= Geometry::SquaredDistanceEpsilon)
223  {
224  *result = sv0; // closest point is either
225  return (pt-sv0).norm();
226  }
227  T lambda = v01.dot(pt-sv0);
228  if (lambda <= 0)
229  {
230  *result = sv0;
231  }
232  else if (lambda >= v01Norm2)
233  {
234  *result = sv1;
235  }
236  else
237  {
238  *result = sv0 + lambda*v01/v01Norm2;
239  }
240  return (*result-pt).norm();
241 }
242
253 template <class T, int MOpt> inline
255  const Eigen::Matrix<T, 3, 1, MOpt>& l0v0,
256  const Eigen::Matrix<T, 3, 1, MOpt>& l0v1,
257  const Eigen::Matrix<T, 3, 1, MOpt>& l1v0,
258  const Eigen::Matrix<T, 3, 1, MOpt>& l1v1,
259  Eigen::Matrix<T, 3, 1, MOpt>* pt0,
260  Eigen::Matrix<T, 3, 1, MOpt>* pt1)
261 {
262  // Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
263  // http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
264  // The lines are parametrized by:
265  // p0 = l0v0 + lambda0 * (l0v1-l0v0)
266  // p1 = l1v0 + lambda1 * (l1v1-l1v0)
267  // and we solve for p0p1 perpendicular to both lines
268  T lambda0, lambda1;
269  Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1-l0v0;
270  T a = l0v01.squaredNorm();
271  if (a <= Geometry::SquaredDistanceEpsilon)
272  {
273  // Degenerate line 0
274  *pt0 = l0v0;
275  return distancePointSegment(l0v0, l1v0, l1v1, pt1);
276  }
277  Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1-l1v0;
278  T b = -l0v01.dot(l1v01);
279  T c = l1v01.squaredNorm();
280  if (c <= Geometry::SquaredDistanceEpsilon)
281  {
282  // Degenerate line 1
283  *pt1 = l1v0;
284  return distancePointSegment(l1v0, l0v0, l0v1, pt0);
285  }
286  Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0-l1v0;
287  T d = l0v01.dot(l0v0_l1v0);
288  T e = -l1v01.dot(l0v0_l1v0);
289  T ratio = a*c-b*b;
290  if (std::abs(ratio) <= Geometry::ScalarEpsilon)
291  {
292  // parallel case
293  lambda0 = 0;
294  lambda1 = e / c;
295  }
296  else
297  {
298  // non-parallel case
299  T inv_ratio = T(1) / ratio;
300  lambda0 = (b*e - c*d) * inv_ratio;
301  lambda1 = (b*d - a*e) * inv_ratio;
302  }
303  *pt0 = l0v0 + lambda0 * l0v01;
304  *pt1 = l1v0 + lambda1 * l1v01;
305  return ((*pt0)-(*pt1)).norm();
306 }
307
308
320 template <class T, int MOpt>
322  const Eigen::Matrix<T, 3, 1, MOpt>& s0v0,
323  const Eigen::Matrix<T, 3, 1, MOpt>& s0v1,
324  const Eigen::Matrix<T, 3, 1, MOpt>& s1v0,
325  const Eigen::Matrix<T, 3, 1, MOpt>& s1v1,
326  Eigen::Matrix<T, 3, 1, MOpt>* pt0,
327  Eigen::Matrix<T, 3, 1, MOpt>* pt1,
328  T *s0t = nullptr,
329  T *s1t = nullptr)
330 {
331  // Based on the outline of http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf, also refer to
332  // http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
333  // The segments are parametrized by:
334  // p0 = l0v0 + s * (l0v1-l0v0), with s between 0 and 1
335  // p1 = l1v0 + t * (l1v1-l1v0), with t between 0 and 1
336  // We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
337  Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1-s0v0;
338  T a = s0v01.squaredNorm();
339  if (a <= Geometry::SquaredDistanceEpsilon)
340  {
341  // Degenerate segment 0
342  *pt0 = s0v0;
343  return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
344  }
345  Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1-s1v0;
346  T b = -s0v01.dot(s1v01);
347  T c = s1v01.squaredNorm();
348  if (c <= Geometry::SquaredDistanceEpsilon)
349  {
350  // Degenerate segment 1
351  *pt1 = s1v1;
352  return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
353  }
354  Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0-s1v0;
355  T d = s0v01.dot(tempLine);
356  T e = -s1v01.dot(tempLine);
357  T ratio = a*c-b*b;
358  T s,t; // parametrization variables (do not initialize)
359  int region = -1;
360  T tmp;
361  // Non-parallel case
362  if (1.0 - std::abs(s0v01.normalized().dot(s1v01.normalized())) >= Geometry::SquaredDistanceEpsilon)
363  {
364  // Get the region of the global minimum in the s-t space based on the line-line solution
365  // s=0 s=1
366  // ^
367  // | |
368  // 4 | 3 | 2
369  // ----|-------|------- t=1
370  // | |
371  // 5 | 0 | 1
372  // | |
373  // ----|-------|-------> t=0
374  // | |
375  // 6 | 7 | 8
376  // | |
377  //
378  s = b*e-c*d;
379  t = b*d-a*e;
380  if (s >= 0)
381  {
382  if (s <= ratio)
383  {
384  if (t >= 0)
385  {
386  if (t <= ratio)
387  {
388  region = 0;
389  }
390  else
391  {
392  region = 3;
393  }
394  }
395  else
396  {
397  region = 7;
398  }
399  }
400  else
401  {
402  if (t >= 0)
403  {
404  if (t <= ratio)
405  {
406  region = 1;
407  }
408  else
409  {
410  region = 2;
411  }
412  }
413  else
414  {
415  region = 8;
416  }
417  }
418  }
419  else
420  {
421  if (t >= 0)
422  {
423  if (t <= ratio)
424  {
425  region = 5;
426  }
427  else
428  {
429  region = 4;
430  }
431  }
432  else
433  {
434  region = 6;
435  }
436  }
437  enum edge_type { s0, s1, t0, t1, edge_skip, edge_invalid };
438  edge_type edge = edge_invalid;
439  switch (region)
440  {
441  case 0:
442  // Global minimum inside [0,1] [0,1]
443  s /= ratio;
444  t /= ratio;
445  edge = edge_skip;
446  break;
447  case 1:
448  edge = s1;
449  break;
450  case 2:
451  // Q_s(1,1)/2 = a+b+d
452  if (a+b+d > 0)
453  {
454  edge = t1;
455  }
456  else
457  {
458  edge = s1;
459  }
460  break;
461  case 3:
462  edge = t1;
463  break;
464  case 4:
465  // Q_s(0,1)/2 = b+d
466  if (b+d > 0)
467  {
468  edge = s0;
469  }
470  else
471  {
472  edge = t1;
473  }
474  break;
475  case 5:
476  edge = s0;
477  break;
478  case 6:
479  // Q_s(0,0)/2 = d
480  if (d > 0)
481  {
482  edge = s0;
483  }
484  else
485  {
486  edge = t0;
487  }
488  break;
489  case 7:
490  edge = t0;
491  break;
492  case 8:
493  // Q_s(1,0)/2 = a+d
494  if (a+d > 0)
495  {
496  edge = t0;
497  }
498  else
499  {
500  edge = s1;
501  }
502  break;
503  default:
504  break;
505  }
506  switch (edge)
507  {
508  case s0:
509  // F(t) = Q(0,t), F?(t) = 2*(e+c*t)
510  // F?(T) = 0 when T = -e/c, then clamp between 0 and 1 (c always >= 0)
511  s = 0;
512  tmp = e;
513  if (tmp > 0)
514  {
515  t = 0;
516  }
517  else if (-tmp > c)
518  {
519  t = 1;
520  }
521  else
522  {
523  t = -tmp/c;
524  }
525  break;
526  case s1:
527  // F(t) = Q(1,t), F?(t) = 2*((b+e)+c*t)
528  // F?(T) = 0 when T = -(b+e)/c, then clamp between 0 and 1 (c always >= 0)
529  s = 1;
530  tmp = b+e;
531  if (tmp > 0)
532  {
533  t = 0;
534  }
535  else if (-tmp > c)
536  {
537  t = 1;
538  }
539  else
540  {
541  t = -tmp/c;
542  }
543  break;
544  case t0:
545  // F(s) = Q(s,0), F?(s) = 2*(d+a*s) =>
546  // F?(S) = 0 when S = -d/a, then clamp between 0 and 1 (a always >= 0)
547  t = 0;
548  tmp = d;
549  if (tmp > 0)
550  {
551  s = 0;
552  }
553  else if (-tmp > a)
554  {
555  s = 1;
556  }
557  else
558  {
559  s = -tmp/a;
560  }
561  break;
562  case t1:
563  // F(s) = Q(s,1), F?(s) = 2*(b+d+a*s) =>
564  // F?(S) = 0 when S = -(b+d)/a, then clamp between 0 and 1 (a always >= 0)
565  t = 1;
566  tmp = b+d;
567  if (tmp > 0)
568  {
569  s = 0;
570  }
571  else if (-tmp > a)
572  {
573  s = 1;
574  }
575  else
576  {
577  s = -tmp/a;
578  }
579  break;
580  case edge_skip:
581  break;
582  default:
583  break;
584  }
585  }
586  else
587  // Parallel case
588  {
589  if (b > 0)
590  {
591  // Segments have different directions
592  if (d >= 0)
593  {
594  // 0-0 end points since s-segment 0 less than t-segment 0
595  s = 0;
596  t = 0;
597  }
598  else if (-d <= a)
599  {
600  // s-segment 0 end-point in the middle of the t 0-1 segment, get distance
601  s = -d/a;
602  t = 0;
603  }
604  else
605  {
606  // s-segment 1 is definitely closer
607  s = 1;
608  tmp = a+d;
609  if (-tmp >= b)
610  {
611  t = 1;
612  }
613  else
614  {
615  t = -tmp/b;
616  }
617  }
618  }
619  else
620  {
621  // Both segments have the same dir
622  if (-d >= a)
623  {
624  // 1-0
625  s = 1;
626  t = 0;
627  }
628  else if (d <= 0)
629  {
630  // mid-0
631  s = -d/a;
632  t = 0;
633  }
634  else
635  {
636  s = 0;
637  // 1-mid
638  if (d >= -b)
639  {
640  t = 1;
641  }
642  else
643  {
644  t = -d/b;
645  }
646  }
647  }
648  }
649  *pt0 = s0v0 + s * (s0v01);
650  *pt1 = s1v0 + t * (s1v01);
651  if (s0t != nullptr && s1t != nullptr)
652  {
653  *s0t = s;
654  *s1t = t;
655  }
656  return ((*pt1)-(*pt0)).norm();
657 }
658
668 template <class T, int MOpt> inline
670  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
671  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
672  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
673  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
674  Eigen::Matrix<T, 3, 1, MOpt>* result)
675 {
676  // Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
677  // http://softsurfer.com/Archive/algorithm_0106 for a geometric interpretation
678  // The triangle is parametrized by:
679  // t: tv0 + s * (tv1-tv0) + t * (tv2-tv0) , with s and t between 0 and 1
680  // We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
681  Eigen::Matrix<T, 3, 1, MOpt> tv01 = tv1-tv0;
682  Eigen::Matrix<T, 3, 1, MOpt> tv02 = tv2-tv0;
683  T a = tv01.squaredNorm();
684  if (a <= Geometry::SquaredDistanceEpsilon)
685  {
686  // Degenerate edge 1
687  return distancePointSegment<T>(pt, tv0, tv2, result);
688  }
689  T b = tv01.dot(tv02);
690  T tCross = tv01.cross(tv02).squaredNorm();
691  if (tCross <= Geometry::SquaredDistanceEpsilon)
692  {
693  // Degenerate edge 2
694  return distancePointSegment<T>(pt, tv0, tv1, result);
695  }
696  T c = tv02.squaredNorm();
697  if (c <= Geometry::SquaredDistanceEpsilon)
698  {
699  // Degenerate edge 3
700  return distancePointSegment<T>(pt, tv0, tv1, result);
701  }
702  Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0-pt;
703  T d = tv01.dot(tv0pv0);
704  T e = tv02.dot(tv0pv0);
705  T ratio = a*c-b*b;
706  T s = b*e-c*d;
707  T t = b*d-a*e;
708  // Determine region (inside-outside triangle)
709  int region = -1;
710  if (s+t <= ratio)
711  {
712  if (s < 0)
713  {
714  if (t < 0)
715  {
716  region = 4;
717  }
718  else
719  {
720  region = 3;
721  }
722  }
723  else if (t < 0)
724  {
725  region = 5;
726  }
727  else
728  {
729  region = 0;
730  }
731  }
732  else
733  {
734  if (s < 0)
735  {
736  region = 2;
737  }
738  else if (t < 0)
739  {
740  region = 6;
741  }
742  else
743  {
744  region = 1;
745  }
746  }
747  // Regions: /
748  // ^ t=0 /
749  // \ 2| /
750  // \ | /
751  // \| /
752  // \ /
753  // |\ /
754  // | \ 1 /
755  // 3 | \ /
756  // | 0 \ /
757  // ----|----\-------> s=0 /
758  // | \ /
759  // 4 | 5 \ 6 /
760  // | \ /
761  // /
762  T numer, denom, tmp0, tmp1;
763  enum edge_type { s0, t0, s1t1, edge_skip, edge_invalid };
764  edge_type edge = edge_invalid;
765  switch (region)
766  {
767  case 0:
768  // Global minimum inside [0,1] [0,1]
769  numer = T(1) / ratio;
770  s *= numer;
771  t *= numer;
772  edge = edge_skip;
773  break;
774  case 1:
775  edge = s1t1;
776  break;
777  case 2:
780  tmp0 = b+d;
781  tmp1 = c+e;
782  if (tmp1 > tmp0)
783  {
784  edge = s1t1;
785  }
786  else
787  {
788  edge = s0;
789  }
790  break;
791  case 3:
792  edge = s0;
793  break;
794  case 4:
797  if (e >= d)
798  {
799  edge = t0;
800  }
801  else
802  {
803  edge = s0;
804  }
805  break;
806  case 5:
807  edge = t0;
808  break;
809  case 6:
812  tmp0 = -a-d;
813  tmp1 = -a-d+b+e;
814  if (tmp1 > tmp0)
815  {
816  edge = t0;
817  }
818  else
819  {
820  edge = s1t1;
821  }
822  break;
823  default:
824  break;
825  }
826  switch (edge)
827  {
828  case s0:
829  // F(t) = Q(0, t), F'(t)=0 when -e/c = 0
830  s = 0;
831  if (e >= 0)
832  {
833  t = 0;
834  }
835  else
836  {
837  t = (-e >= c ? 1 : -e/c);
838  }
839  break;
840  case t0:
841  // F(s) = Q(s, 0), F'(s)=0 when -d/a = 0
842  t = 0;
843  if (d >= 0)
844  {
845  s = 0;
846  }
847  else
848  {
849  s = (-d >= a ? 1 : -d/a);
850  }
851  break;
852  case s1t1:
853  // F(s) = Q(s, 1-s), F'(s) = 0 when (c+e-b-d)/(a-2b+c) = 0 (denom = || tv01-tv02 ||^2 always > 0)
854  numer = c+e-b-d;
855  if (numer <= 0)
856  {
857  s = 0;
858  }
859  else
860  {
861  denom = a-2*b+c;
862  s = (numer >= denom ? 1 : numer / denom);
863  }
864  t = 1-s;
865  break;
866  case edge_skip:
867  break;
868  default:
869  break;
870  }
871  *result = tv0 + s * tv01 + t * tv02;
872  return ((*result)-pt).norm();
873 }
874
889 template <class T, int MOpt> inline
891  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
892  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
893  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
894  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
895  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
896  const Eigen::Matrix<T, 3, 1, MOpt>& tn,
897  Eigen::Matrix<T, 3, 1, MOpt>* result)
898 {
899  // Triangle edges vectors
900  Eigen::Matrix<T, 3, 1, MOpt> u = tv1-tv0;
901  Eigen::Matrix<T, 3, 1, MOpt> v = tv2-tv0;
902
903  // Ray direction vector
904  Eigen::Matrix<T, 3, 1, MOpt> dir = sv1-sv0;
905  Eigen::Matrix<T, 3, 1, MOpt> w0 = sv0-tv0;
906  T a = -tn.dot(w0);
907  T b = tn.dot(dir);
908
909  result->setConstant((std::numeric_limits<double>::quiet_NaN()));
910
911  // Ray is parallel to triangle plane
912  if (std::abs(b) <= Geometry::AngularEpsilon)
913  {
914  if (a == 0)
915  {
916  // Ray lies in triangle plane
917  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
918  for (int i=0; i<2; ++i)
919  {
920  barycentricCoordinates((i==0?sv0:sv1), tv0, tv1, tv2, tn, &baryCoords);
921  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
922  {
923  *result = (i==0)?sv0:sv1;
924  return true;
925  }
926  }
927  // All segment endpoints outside of triangle
928  return false;
929  }
930  else
931  {
932  // Segment parallel to triangle but not in same plane
933  return false;
934  }
935  }
936
937  // Get intersect point of ray with triangle plane
938  T r = a / b;
939  // Ray goes away from triangle
940  if (r < -Geometry::DistanceEpsilon)
941  {
942  return false;
943  }
944  //Ray comes toward triangle but isn't long enough to reach it
945  if (r > 1+Geometry::DistanceEpsilon)
946  {
947  return false;
948  }
949
950  // Intersect point of ray and plane
951  Eigen::Matrix<T, 3, 1, MOpt> presumedIntersection = sv0 + r * dir;
952  // Collision point inside T?
953  T uu = u.dot(u);
954  T uv = u.dot(v);
955  T vv = v.dot(v);
956  Eigen::Matrix<T, 3, 1, MOpt> w = presumedIntersection - tv0;
957  T wu = w.dot(u);
958  T wv = w.dot(v);
959  T D = uv * uv - uu * vv;
960  // Get and test parametric coords
961  T s = (uv * wv - vv * wu) / D;
962  // I is outside T
963  if (s < 0 || s > 1)
964  {
965  return false;
966  }
967  T t = (uv * wu - uu * wv) / D;
968  // I is outside T
969  if (t < 0 || (s + t) > 1)
970  {
971  return false;
972  }
973  // I is in T
974  *result = sv0 + r * dir;
975  return true;
976 }
977
978
988 template <class T, int MOpt> inline
990  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
991  const Eigen::Matrix<T, 3, 1, MOpt>& n,
992  T d,
993  Eigen::Matrix<T, 3, 1, MOpt>* result)
994 {
995  T dist = n.dot(pt) + d;
996  *result = pt - n*dist;
997  return dist;
998 }
999
1000
1015 template <class T, int MOpt> inline
1017  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1018  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1019  const Eigen::Matrix<T, 3, 1, MOpt>& n,
1020  T d,
1021  Eigen::Matrix<T, 3, 1, MOpt>* closestPointSegment,
1022  Eigen::Matrix<T, 3, 1, MOpt>* planeIntersectionPoint)
1023 {
1024  T dist0 = n.dot(sv0) + d;
1025  T dist1 = n.dot(sv1) + d;
1026  // Parallel case
1027  Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1028  if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
1029  {
1030  *closestPointSegment = (sv0 + sv1)*T(0.5);
1031  dist0 = n.dot(*closestPointSegment) + d;
1032  *planeIntersectionPoint = *closestPointSegment - dist0*n;
1033  return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
1034  }
1035  // Both on the same side
1036  if ((dist0 > 0 && dist1 > 0) || (dist0 < 0 && dist1 < 0))
1037  {
1038  if (std::abs(dist0) < std::abs(dist1))
1039  {
1040  *closestPointSegment = sv0;
1041  *planeIntersectionPoint = sv0 - dist0*n;
1042  return dist0;
1043  }
1044  else
1045  {
1046  *closestPointSegment = sv1;
1047  *planeIntersectionPoint = sv1 - dist1*n;
1048  return dist1;
1049  }
1050  }
1051  // Segment cutting through
1052  else
1053  {
1054  Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
1055  T lambda= (-d-sv0.dot(n)) / v01.dot(n);
1056  *planeIntersectionPoint = sv0 + lambda * v01;
1057  *closestPointSegment = *planeIntersectionPoint;
1058  return 0;
1059  }
1060 }
1061
1062
1076 template <class T, int MOpt> inline
1078  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1079  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1080  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1081  const Eigen::Matrix<T, 3, 1, MOpt>& n,
1082  T d,
1083  Eigen::Matrix<T, 3, 1, MOpt>* closestPointTriangle,
1084  Eigen::Matrix<T, 3, 1, MOpt>* planeProjectionPoint)
1085 {
1086  Eigen::Matrix<T, 3, 1, MOpt> distances(n.dot(tv0) + d, n.dot(tv1) + d, n.dot(tv2) + d);
1087  Eigen::Matrix<T, 3, 1, MOpt> t01 = tv1-tv0;
1088  Eigen::Matrix<T, 3, 1, MOpt> t02 = tv2-tv0;
1089  Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
1090
1091  closestPointTriangle->setConstant((std::numeric_limits<double>::quiet_NaN()));
1092  planeProjectionPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1093
1094  // HS-2013-may-09 Could there be a case where we fall into the wrong tree because of the checks against
1095  // the various epsilon values all going against us ???
1096  // Parallel case (including Coplanar)
1097  if (std::abs(n.dot(t01)) <= Geometry::AngularEpsilon && std::abs(n.dot(t02)) <= Geometry::AngularEpsilon)
1098  {
1099  *closestPointTriangle = (tv0 + tv1 + tv2) / T(3);
1100  *planeProjectionPoint = *closestPointTriangle - n * distances[0];
1101  return distances[0];
1102  }
1103
1104  // Is there an intersection
1105  if ((distances.array() < -Geometry::DistanceEpsilon).any() &&
1106  (distances.array() > Geometry::DistanceEpsilon).any())
1107  {
1108  if (distances[0] * distances[1] < 0)
1109  {
1110  *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t01) * t01;
1111  if (distances[0] * distances[2] < 0)
1112  {
1113  *planeProjectionPoint = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1114  }
1115  else
1116  {
1117  Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2-tv1;
1118  *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1119  }
1120  }
1121  else
1122  {
1123  *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1124  *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1125  }
1126
1127  // Find the midpoint, take this out to return the segment endpoints
1128  *closestPointTriangle = *planeProjectionPoint = (*closestPointTriangle + *planeProjectionPoint) * T(0.5);
1129  return 0;
1130  }
1131
1132  int index;
1133  distances.cwiseAbs().minCoeff(&index);
1134  switch (index)
1135  {
1136  case 0: //distances[0] is closest
1137  *closestPointTriangle = tv0;
1138  *planeProjectionPoint = tv0 - n * distances[0];
1139  return distances[0];
1140  case 1: //distances[1] is closest
1141  *closestPointTriangle = tv1;
1142  *planeProjectionPoint = tv1 - n * distances[1];
1143  return distances[1];
1144  case 2: //distances[2] is closest
1145  *closestPointTriangle = tv2;
1146  *planeProjectionPoint = tv2 - n * distances[2];
1147  return distances[2];
1148  }
1149
1150  return std::numeric_limits<T>::quiet_NaN();
1151 }
1152
1160 template <class T, int MOpt> inline
1162  const Eigen::Matrix<T, 3, 1, MOpt>& pn0, T pd0,
1163  const Eigen::Matrix<T, 3, 1, MOpt>& pn1, T pd1,
1164  Eigen::Matrix<T, 3, 1, MOpt>* pt0,
1165  Eigen::Matrix<T, 3, 1, MOpt>* pt1)
1166 {
1167  // Algorithm from real time collision detection - optimized version page 210 (with extra checks)
1168  const Eigen::Matrix<T, 3, 1, MOpt> lineDir = pn0.cross(pn1);
1169  const T lineDirNorm2 = lineDir.squaredNorm();
1170
1171  pt0->setConstant((std::numeric_limits<double>::quiet_NaN()));
1172  pt1->setConstant((std::numeric_limits<double>::quiet_NaN()));
1173
1174  // Test if the two planes are parallel
1175  if (lineDirNorm2 <= Geometry::SquaredDistanceEpsilon)
1176  {
1177  return false; // planes disjoint
1178  }
1179  // Compute common point
1180  *pt0 = (pd1*pn0-pd0*pn1).cross(lineDir) / lineDirNorm2;
1181  *pt1 = *pt0 + lineDir;
1182  return true;
1183 }
1184
1185
1196 template <class T, int MOpt> inline
1198  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1199  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1200  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1201  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1202  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1203  Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1204  Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1205 {
1206  Eigen::Matrix<T, 3, 1, MOpt> n = (tv1 - tv0).cross(tv2 - tv1);
1207  n.normalize();
1208  return distanceSegmentTriangle(sv0, sv1, tv0, tv1, tv2, n, segmentPoint, trianglePoint);
1209 }
1210
1221 template <class T, int MOpt> inline
1223  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1224  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1225  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1226  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1227  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1228  const Eigen::Matrix<T, 3, 1, MOpt>& normal,
1229  Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1230  Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1231 {
1232  segmentPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1233  trianglePoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1234
1235  // Setting up the plane that the triangle is in
1236  const Eigen::Matrix<T, 3, 1, MOpt>& n = normal;
1237  T d = -n.dot(tv0);
1238  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1239  // Degenerate case: Line and triangle plane parallel
1240  const Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1-sv0;
1241  const T v01DotTn = n.dot(v01);
1242  if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
1243  {
1244  // Check if any of the points project onto the tri
1245  // otherwise normal (non-parallel) processing will get the right result
1246  T dst = std::abs(distancePointPlane(sv0, n, d, trianglePoint));
1247  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1248  barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
1249  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1250  {
1251  *segmentPoint = sv0;
1252  return dst;
1253  }
1254  dst = std::abs(distancePointPlane(sv1, n, d, trianglePoint));
1255  barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
1256  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1257  {
1258  *segmentPoint = sv1;
1259  return dst;
1260  }
1261  }
1262  // Line and triangle plane *not* parallel: check cut through case only, the rest will be check later
1263  else
1264  {
1265  T lambda = -n.dot(sv0-tv0) / v01DotTn;
1266  if (lambda >= 0 && lambda <= 1)
1267  {
1268  *segmentPoint = *trianglePoint = sv0 + lambda * v01;
1269  barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
1270  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1271  {
1272  // Segment goes through the triangle
1273  return 0;
1274  }
1275  }
1276  }
1277  // At this point the segment is nearest point to one of the triangle edges or one of the end points
1278  Eigen::Matrix<T, 3, 1, MOpt> segColPt01, segColPt02, segColPt12, triColPt01, triColPt02, triColPt12;
1279  T dst01 = distanceSegmentSegment(sv0, sv1, tv0, tv1, &segColPt01, &triColPt01);
1280  T dst02 = distanceSegmentSegment(sv0, sv1, tv0, tv2, &segColPt02, &triColPt02);
1281  T dst12 = distanceSegmentSegment(sv0, sv1, tv1, tv2, &segColPt12, &triColPt12);
1282  Eigen::Matrix<T, 3, 1, MOpt> ptTriCol0, ptTriCol1;
1283  T dstPtTri0 = std::abs(distancePointPlane(sv0, n, d, &ptTriCol0));
1284  barycentricCoordinates(ptTriCol0, tv0, tv1, tv2, normal, &baryCoords);
1285  if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1286  {
1287  dstPtTri0 = std::numeric_limits<T>::max();
1288  }
1289  T dstPtTri1 = std::abs(distancePointPlane(sv1, n, d, &ptTriCol1));
1290  barycentricCoordinates(ptTriCol1, tv0, tv1, tv2, normal, &baryCoords);
1291  if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1292  {
1293  dstPtTri1 = std::numeric_limits<T>::max();
1294  }
1295
1296  int minIndex;
1297  Eigen::Matrix<double, 5, 1> distances;
1298  (distances << dst01, dst02, dst12, dstPtTri0, dstPtTri1).finished().minCoeff(&minIndex);
1299  switch (minIndex)
1300  {
1301  case 0:
1302  *segmentPoint = segColPt01;
1303  *trianglePoint = triColPt01;
1304  return dst01;
1305  case 1:
1306  *segmentPoint = segColPt02;
1307  *trianglePoint = triColPt02;
1308  return dst02;
1309  case 2:
1310  *segmentPoint = segColPt12;
1311  *trianglePoint = triColPt12;
1312  return dst12;
1313  case 3:
1314  *segmentPoint = sv0;
1315  *trianglePoint = ptTriCol0;
1316  return dstPtTri0;
1317  case 4:
1318  *segmentPoint = sv1;
1319  *trianglePoint = ptTriCol1;
1320  return dstPtTri1;
1321  }
1322
1323  // Invalid ...
1324  return std::numeric_limits<T>::quiet_NaN();
1325
1326 }
1327
1328
1339 template <class T, int MOpt> inline
1341  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1342  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1343  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1344  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1345  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1346  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1347  Eigen::Matrix<T, 3, 1, MOpt>* closestPoint0,
1348  Eigen::Matrix<T, 3, 1, MOpt>* closestPoint1)
1349 {
1350  // Check the segments of t0 against t1
1351  T minDst = std::numeric_limits<T>::max();
1352  T currDst = 0;
1353  Eigen::Matrix<T, 3, 1, MOpt> segPt, triPt;
1354  Eigen::Matrix<T, 3, 1, MOpt> n0 = (t0v1-t0v0).cross(t0v2-t0v0);
1355  n0.normalize();
1356  Eigen::Matrix<T, 3, 1, MOpt> n1 = (t1v1-t1v0).cross(t1v2-t1v0);
1357  n1.normalize();
1358  currDst = distanceSegmentTriangle(t0v0, t0v1, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
1359  if (currDst < minDst)
1360  {
1361  minDst = currDst;
1362  *closestPoint0 = segPt;
1363  *closestPoint1 = triPt;
1364  }
1365  currDst = distanceSegmentTriangle(t0v1, t0v2, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
1366  if (currDst < minDst)
1367  {
1368  minDst = currDst;
1369  *closestPoint0 = segPt;
1370  *closestPoint1 = triPt;
1371  }
1372  currDst = distanceSegmentTriangle(t0v2, t0v0, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
1373  if (currDst < minDst)
1374  {
1375  minDst = currDst;
1376  *closestPoint0 = segPt;
1377  *closestPoint1 = triPt;
1378  }
1379  // Check the segments of t1 against t0
1380  currDst = distanceSegmentTriangle(t1v0, t1v1, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
1381  if (currDst < minDst)
1382  {
1383  minDst = currDst;
1384  *closestPoint1 = segPt;
1385  *closestPoint0 = triPt;
1386  }
1387  currDst = distanceSegmentTriangle(t1v1, t1v2, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
1388  if (currDst < minDst)
1389  {
1390  minDst = currDst;
1391  *closestPoint1 = segPt;
1392  *closestPoint0 = triPt;
1393  }
1394  currDst = distanceSegmentTriangle(t1v2, t1v0, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
1395  if (currDst < minDst)
1396  {
1397  minDst = currDst;
1398  *closestPoint1 = segPt;
1399  *closestPoint0 = triPt;
1400  }
1401  return (minDst);
1402 }
1403
1410 template <class T, int MOpt>
1412  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1413  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1414  const Eigen::AlignedBox<T, 3>& box,
1415  std::vector<Eigen::Matrix<T, 3, 1, MOpt> >* intersections)
1416 {
1417  Eigen::Array<T, 3, 1, MOpt> v01 = sv1 - sv0;
1418  Eigen::Array<bool, 3, 1, MOpt> parallelToPlane = (v01.cwiseAbs().array() < Geometry::DistanceEpsilon);
1419  if (parallelToPlane.any())
1420  {
1421  Eigen::Array<bool, 3, 1, MOpt> beyondMinCorner = (sv0.array() < box.min().array());
1422  Eigen::Array<bool, 3, 1, MOpt> beyondMaxCorner = (sv0.array() > box.max().array());
1423  if ((parallelToPlane && (beyondMinCorner || beyondMaxCorner)).any())
1424  {
1425  return;
1426  }
1427  }
1428
1429  // Calculate the intersection of the segment with each of the 6 box planes.
1430  // The intersection is calculated as the distance along the segment (abscissa)
1431  // scaled from 0 to 1.
1432  Eigen::Array<T, 3, 2, MOpt> planeIntersectionAbscissas;
1433  planeIntersectionAbscissas.col(0) = (box.min().array() - sv0.array());
1434  planeIntersectionAbscissas.col(1) = (box.max().array() - sv0.array());
1435
1436  // While we could be dividing by zero here, INF values are
1437  // correctly handled by the rest of the function.
1438  planeIntersectionAbscissas.colwise() /= v01;
1439
1440  T entranceAbscissa = planeIntersectionAbscissas.rowwise().minCoeff().maxCoeff();
1441  T exitAbscissa = planeIntersectionAbscissas.rowwise().maxCoeff().minCoeff();
1442  if (entranceAbscissa < exitAbscissa && exitAbscissa > T(0.0))
1443  {
1444  if (entranceAbscissa >= T(0.0) && entranceAbscissa <= T(1.0))
1445  {
1446  intersections->push_back(sv0 + v01.matrix() * entranceAbscissa);
1447  }
1448
1449  if (exitAbscissa >= T(0.0) && exitAbscissa <= T(1.0))
1450  {
1451  intersections->push_back(sv0 + v01.matrix() * exitAbscissa);
1452  }
1453  }
1454 }
1455
1464 template <class T, int MOpt>
1466  const Eigen::Matrix<T, 3, 1, MOpt>& capsuleBottom,
1467  const Eigen::Matrix<T, 3, 1, MOpt>& capsuleTop,
1469  const Eigen::AlignedBox<T, 3>& box)
1470 {
1472  std::vector<Vector3d> candidates;
1473  intersectionsSegmentBox(capsuleBottom, capsuleTop, dilatedBox, &candidates);
1474  if (dilatedBox.contains(capsuleBottom))
1475  {
1476  candidates.push_back(capsuleBottom);
1477  }
1478  if (dilatedBox.contains(capsuleTop))
1479  {
1480  candidates.push_back(capsuleTop);
1481  }
1482
1483  bool doesIntersect = false;
1484  ptrdiff_t dimensionsOutsideBox;
1485  Vector3d clampedPosition, segmentPoint;
1486  for (auto candidate = candidates.cbegin(); candidate != candidates.cend(); ++candidate)
1487  {
1488  // Collisions between a capsule and a box are the same as a segment and a dilated
1489  // box with rounded corners. If the intersection occurs outside the original box
1490  // in two dimensions (collision with an edge of the dilated box) or three
1491  // dimensions (collision with the corner of the dilated box) dimensions, we need
1492  // to check if it is inside these rounded corners.
1493  dimensionsOutsideBox = (candidate->array() > box.max().array()).count();
1494  dimensionsOutsideBox += (candidate->array() < box.min().array()).count();
1495  if (dimensionsOutsideBox >= 2)
1496  {
1497  clampedPosition = (*candidate).array().min(box.max().array()).max(box.min().array());
1498  if (distancePointSegment(clampedPosition, capsuleBottom, capsuleTop, &segmentPoint) > capsuleRadius)
1499  {
1500  // Doesn't intersect, try the next candidate.
1501  continue;
1502  }
1503  }
1504  doesIntersect = true;
1505  break;
1506  }
1507  return doesIntersect;
1508 }
1509
1520 template <class T, int MOpt> inline
1522  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1523  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1524  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1525  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1526  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1527  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1528  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1529  const Eigen::Matrix<T, 3, 1, MOpt>& t1n);
1530
1537 template <class T, int MOpt> inline
1539  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1540  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1541  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1542  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1543  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1544  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2);
1545
1565 template <class T, int MOpt> inline
1567  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1568  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1569  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1570  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1571  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1572  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1573  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1574  const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
1575  T* penetrationDepth,
1576  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1577  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1578  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1579
1597 template <class T, int MOpt> inline
1599  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1600  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1601  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1602  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1603  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1604  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1605  T* penetrationDepth,
1606  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1607  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1608  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1609
1610
1611 }; // namespace Math
1612 }; // namespace SurgSim
1613
1614
1615
1616
1617
1618
1619 #endif
T distanceTrianglePlane(const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *closestPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *planeProjectionPoint)
Calculate the distance of a triangle to a plane.
Definition: Geometry.h:1077
T distancePointTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the normal distance of a point from a triangle, the resulting point will be on the edge of ...
Definition: Geometry.h:669
T distancePointSegment(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Point segment distance, if the projection of the closest point is not within the segments, the closest segment point is used for the distance calculation.
Definition: Geometry.h:214
T distanceSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *segmentPoint, Eigen::Matrix< T, 3, 1, MOpt > *trianglePoint)
Calculate the distance of a line segment to a triangle.
Definition: Geometry.h:1197
bool doesIntersectPlanePlane(const Eigen::Matrix< T, 3, 1, MOpt > &pn0, T pd0, const Eigen::Matrix< T, 3, 1, MOpt > &pn1, T pd1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1)
Test if two planes are intersecting, if yes also calculate the intersection line. ...
Definition: Geometry.h:1161
T distanceTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, Eigen::Matrix< T, 3, 1, MOpt > *closestPoint0, Eigen::Matrix< T, 3, 1, MOpt > *closestPoint1)
Calculate the distance between two triangles.
Definition: Geometry.h:1340
T distancePointPlane(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the distance of a point to a plane.
Definition: Geometry.h:989
The convenience header that provides the entirety of the logging API.
bool doesIntersectBoxCapsule(const Eigen::Matrix< T, 3, 1, MOpt > &capsuleBottom, const Eigen::Matrix< T, 3, 1, MOpt > &capsuleTop, const T capsuleRadius, const Eigen::AlignedBox< T, 3 > &box)
Test if an axis aligned box intersects with a capsule.
Definition: Geometry.h:1465
T distancePointLine(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &v0, const Eigen::Matrix< T, 3, 1, MOpt > &v1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the normal distance between a point and a line.
Definition: Geometry.h:183
void intersectionsSegmentBox(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::AlignedBox< T, 3 > &box, std::vector< Eigen::Matrix< T, 3, 1, MOpt > > *intersections)
Calculate the intersections between a line segment and an axis aligned box.
Definition: Geometry.h:1411
T distanceSegmentSegment(const Eigen::Matrix< T, 3, 1, MOpt > &s0v0, const Eigen::Matrix< T, 3, 1, MOpt > &s0v1, const Eigen::Matrix< T, 3, 1, MOpt > &s1v0, const Eigen::Matrix< T, 3, 1, MOpt > &s1v1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1, T *s0t=nullptr, T *s1t=nullptr)
Distance between two segments, if the project of the closest point is not on the opposing segment...
Definition: Geometry.h:321
bool isCoplanar(const Eigen::Matrix< T, 3, 1, MOpt > &a, const Eigen::Matrix< T, 3, 1, MOpt > &b, const Eigen::Matrix< T, 3, 1, MOpt > &c, const Eigen::Matrix< T, 3, 1, MOpt > &d)
Check whether the points are coplanar.
Definition: Geometry.h:168
T distanceSegmentPlane(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *closestPointSegment, Eigen::Matrix< T, 3, 1, MOpt > *planeIntersectionPoint)
Calculate the distance between a segment and a plane.
Definition: Geometry.h:1016
bool barycentricCoordinates(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, Eigen::Matrix< T, 3, 1, MOpt > *coordinates)
Calculate the barycentric coordinates of a point with respect to a triangle.
Definition: Geometry.h:71
T distanceLineLine(const Eigen::Matrix< T, 3, 1, MOpt > &l0v0, const Eigen::Matrix< T, 3, 1, MOpt > &l0v1, const Eigen::Matrix< T, 3, 1, MOpt > &l1v0, const Eigen::Matrix< T, 3, 1, MOpt > &l1v1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1)
Determine the distance between two lines.
Definition: Geometry.h:254
Definitions of small fixed-size vector types.
bool calculateContactTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, const Eigen::Matrix< T, 3, 1, MOpt > &t0n, const Eigen::Matrix< T, 3, 1, MOpt > &t1n, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint0, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint1, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal)
Calculate the contact between two triangles.
Definition: TriangleTriangleContactCalculation-inl.h:216
bool isPointInsideTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn)
Check if a point is inside a triangle.
Definition: Geometry.h:124
bool doesCollideSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the intersection of a line segment with a triangle See http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle for the algorithm.
Definition: Geometry.h:890
bool doesIntersectTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, const Eigen::Matrix< T, 3, 1, MOpt > &t0n, const Eigen::Matrix< T, 3, 1, MOpt > &t1n)
Check if the two triangles intersect using separating axis test.
Definition: TriangleTriangleIntersection-inl.h:71
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:56