Geometry.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013-2015, SimQuest Solutions Inc.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
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"
25 #include "SurgSim/Math/Vector.h"
26 
38 
39 namespace SurgSim
40 {
41 namespace Math
42 {
43 
44 namespace Geometry
45 {
46 
48 static const double DistanceEpsilon = 1e-10;
49 
51 static const double SquaredDistanceEpsilon = 1e-10;
52 
54 static const double AngularEpsilon = 1e-10;
55 
57 static const double ScalarEpsilon = 1e-10;
58 
59 }
60 
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>& sv0,
73  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
74  Eigen::Matrix<T, 2, 1, MOpt>* coordinates)
75 {
76  const Eigen::Matrix<T, 3, 1, MOpt> line = sv1 - sv0;
77  const T length2 = line.squaredNorm();
78  if (length2 < Geometry::SquaredDistanceEpsilon)
79  {
80  coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
81  return false;
82  }
83  (*coordinates)[1] = (pt - sv0).dot(line) / length2;
84  (*coordinates)[0] = static_cast<T>(1) - (*coordinates)[1];
85  return true;
86 }
87 
98 template <class T, int MOpt> inline
99 bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
100  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
101  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
102  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
103  const Eigen::Matrix<T, 3, 1, MOpt>& tn,
104  Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
105 {
106  const T signedTriAreaX2 = ((tv1 - tv0).cross(tv2 - tv0)).dot(tn);
107  if (signedTriAreaX2 < Geometry::SquaredDistanceEpsilon)
108  {
109  // SQ_ASSERT_WARNING(false, "Cannot compute barycentric coords (degenetrate triangle), assigning center");
110  coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
111  return false;
112  }
113  (*coordinates)[0] = ((tv1 - pt).cross(tv2 - pt)).dot(tn) / signedTriAreaX2;
114  (*coordinates)[1] = ((tv2 - pt).cross(tv0 - pt)).dot(tn) / signedTriAreaX2;
115  (*coordinates)[2] = 1 - (*coordinates)[0] - (*coordinates)[1];
116  return true;
117 }
118 
129 template <class T, int MOpt> inline
131  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
132  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
133  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
134  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
135  Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
136 {
137  Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
138  double norm = tn.norm();
139  if (norm < Geometry::DistanceEpsilon)
140  {
141  coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
142  return false;
143  }
144  tn /= norm;
145  return barycentricCoordinates(pt, tv0, tv1, tv2, tn, coordinates);
146 }
147 
158 template <class T, int MOpt> inline
160  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
161  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
162  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
163  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
164  const Eigen::Matrix<T, 3, 1, MOpt>& tn)
165 {
166  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
167  bool result = barycentricCoordinates(pt, tv0, tv1, tv2, tn, &baryCoords);
168  return (result &&
169  baryCoords[0] >= -Geometry::ScalarEpsilon &&
170  baryCoords[1] >= -Geometry::ScalarEpsilon &&
171  baryCoords[2] >= -Geometry::ScalarEpsilon);
172 }
173 
183 template <class T, int MOpt> inline
185  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
186  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
187  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
188  const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
189 {
190  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
191  bool result = barycentricCoordinates(pt, tv0, tv1, tv2, &baryCoords);
192  return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
193  baryCoords[1] >= -Geometry::ScalarEpsilon &&
194  baryCoords[2] >= -Geometry::ScalarEpsilon);
195 }
196 
205 template <class T, int MOpt> inline
207  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
208  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
209  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
210  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
211  const Eigen::Matrix<T, 3, 1, MOpt>& tn)
212 {
213  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
214  bool result = barycentricCoordinates(pt, tv0, tv1, tv2, tn, &baryCoords);
215  return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
216  baryCoords[1] >= -Geometry::ScalarEpsilon &&
217  baryCoords[2] >= -Geometry::ScalarEpsilon &&
218  baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
219 }
220 
230 template <class T, int MOpt> inline
232  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
233  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
234  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
235  const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
236 {
237  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
238  bool result = barycentricCoordinates(pt, tv0, tv1, tv2, &baryCoords);
239  return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
240  baryCoords[1] >= -Geometry::ScalarEpsilon &&
241  baryCoords[2] >= -Geometry::ScalarEpsilon &&
242  baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
243 }
244 
250 template <class T, int MOpt> inline
252  const Eigen::Matrix<T, 3, 1, MOpt>& a,
253  const Eigen::Matrix<T, 3, 1, MOpt>& b,
254  const Eigen::Matrix<T, 3, 1, MOpt>& c,
255  const Eigen::Matrix<T, 3, 1, MOpt>& d)
256 {
257  return std::abs((c - a).dot((b - a).cross(d - c))) < Geometry::ScalarEpsilon;
258 }
259 
265 template <class T, int MOpt> inline
267  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
268  const Eigen::Matrix<T, 3, 1, MOpt>& v0,
269  const Eigen::Matrix<T, 3, 1, MOpt>& v1,
270  Eigen::Matrix<T, 3, 1, MOpt>* result)
271 {
272  // The lines is parametrized by:
273  // q = v0 + lambda0 * (v1-v0)
274  // and we solve for pq.v01 = 0;
275  Eigen::Matrix<T, 3, 1, MOpt> v01 = v1 - v0;
276  T v01_norm2 = v01.squaredNorm();
277  if (v01_norm2 <= Geometry::SquaredDistanceEpsilon)
278  {
279  *result = v0; // closest point is either
280  T pv_norm2 = (pt - v0).squaredNorm();
281  return sqrt(pv_norm2);
282  }
283  T lambda = (v01).dot(pt - v0);
284  *result = v0 + lambda * v01 / v01_norm2;
285  return (*result - pt).norm();
286 }
287 
296 template <class T, int MOpt> inline
298  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
299  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
300  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
301  Eigen::Matrix<T, 3, 1, MOpt>* result)
302 {
303  Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
304  T v01Norm2 = v01.squaredNorm();
305  if (v01Norm2 <= Geometry::SquaredDistanceEpsilon)
306  {
307  *result = sv0; // closest point is either
308  return (pt - sv0).norm();
309  }
310  T lambda = v01.dot(pt - sv0);
311  if (lambda <= 0)
312  {
313  *result = sv0;
314  }
315  else if (lambda >= v01Norm2)
316  {
317  *result = sv1;
318  }
319  else
320  {
321  *result = sv0 + lambda * v01 / v01Norm2;
322  }
323  return (*result - pt).norm();
324 }
325 
336 template <class T, int MOpt> inline
338  const Eigen::Matrix<T, 3, 1, MOpt>& l0v0,
339  const Eigen::Matrix<T, 3, 1, MOpt>& l0v1,
340  const Eigen::Matrix<T, 3, 1, MOpt>& l1v0,
341  const Eigen::Matrix<T, 3, 1, MOpt>& l1v1,
342  Eigen::Matrix<T, 3, 1, MOpt>* pt0,
343  Eigen::Matrix<T, 3, 1, MOpt>* pt1)
344 {
345  // Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
346  // http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
347  // The lines are parametrized by:
348  // p0 = l0v0 + lambda0 * (l0v1-l0v0)
349  // p1 = l1v0 + lambda1 * (l1v1-l1v0)
350  // and we solve for p0p1 perpendicular to both lines
351  T lambda0, lambda1;
352  Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1 - l0v0;
353  T a = l0v01.squaredNorm();
354  if (a <= Geometry::SquaredDistanceEpsilon)
355  {
356  // Degenerate line 0
357  *pt0 = l0v0;
358  return distancePointSegment(l0v0, l1v0, l1v1, pt1);
359  }
360  Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1 - l1v0;
361  T b = -l0v01.dot(l1v01);
362  T c = l1v01.squaredNorm();
363  if (c <= Geometry::SquaredDistanceEpsilon)
364  {
365  // Degenerate line 1
366  *pt1 = l1v0;
367  return distancePointSegment(l1v0, l0v0, l0v1, pt0);
368  }
369  Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0 - l1v0;
370  T d = l0v01.dot(l0v0_l1v0);
371  T e = -l1v01.dot(l0v0_l1v0);
372  T ratio = a * c - b * b;
373  if (std::abs(ratio) <= Geometry::ScalarEpsilon)
374  {
375  // parallel case
376  lambda0 = 0;
377  lambda1 = e / c;
378  }
379  else
380  {
381  // non-parallel case
382  T inv_ratio = T(1) / ratio;
383  lambda0 = (b * e - c * d) * inv_ratio;
384  lambda1 = (b * d - a * e) * inv_ratio;
385  }
386  *pt0 = l0v0 + lambda0 * l0v01;
387  *pt1 = l1v0 + lambda1 * l1v01;
388  return ((*pt0) - (*pt1)).norm();
389 }
390 
391 
403 template <class T, int MOpt>
405  const Eigen::Matrix<T, 3, 1, MOpt>& s0v0,
406  const Eigen::Matrix<T, 3, 1, MOpt>& s0v1,
407  const Eigen::Matrix<T, 3, 1, MOpt>& s1v0,
408  const Eigen::Matrix<T, 3, 1, MOpt>& s1v1,
409  Eigen::Matrix<T, 3, 1, MOpt>* pt0,
410  Eigen::Matrix<T, 3, 1, MOpt>* pt1,
411  T* s0t = nullptr,
412  T* s1t = nullptr)
413 {
414  // Based on the outline of http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf, also refer to
415  // http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
416  // The segments are parametrized by:
417  // p0 = l0v0 + s * (l0v1-l0v0), with s between 0 and 1
418  // p1 = l1v0 + t * (l1v1-l1v0), with t between 0 and 1
419  // We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
420  Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1 - s0v0;
421  T a = s0v01.squaredNorm();
422  if (a <= Geometry::SquaredDistanceEpsilon)
423  {
424  // Degenerate segment 0
425  *pt0 = s0v0;
426  return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
427  }
428  Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1 - s1v0;
429  T b = -s0v01.dot(s1v01);
430  T c = s1v01.squaredNorm();
431  if (c <= Geometry::SquaredDistanceEpsilon)
432  {
433  // Degenerate segment 1
434  *pt1 = s1v1;
435  return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
436  }
437  Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0 - s1v0;
438  T d = s0v01.dot(tempLine);
439  T e = -s1v01.dot(tempLine);
440  T ratio = a * c - b * b;
441  T s, t; // parametrization variables (do not initialize)
442  int region = -1;
443  T tmp;
444  // Non-parallel case
445  if (1.0 - std::abs(s0v01.normalized().dot(s1v01.normalized())) >= Geometry::SquaredDistanceEpsilon)
446  {
447  // Get the region of the global minimum in the s-t space based on the line-line solution
448  // s=0 s=1
449  // ^
450  // | |
451  // 4 | 3 | 2
452  // ----|-------|------- t=1
453  // | |
454  // 5 | 0 | 1
455  // | |
456  // ----|-------|-------> t=0
457  // | |
458  // 6 | 7 | 8
459  // | |
460  //
461  s = b * e - c * d;
462  t = b * d - a * e;
463  if (s >= 0)
464  {
465  if (s <= ratio)
466  {
467  if (t >= 0)
468  {
469  if (t <= ratio)
470  {
471  region = 0;
472  }
473  else
474  {
475  region = 3;
476  }
477  }
478  else
479  {
480  region = 7;
481  }
482  }
483  else
484  {
485  if (t >= 0)
486  {
487  if (t <= ratio)
488  {
489  region = 1;
490  }
491  else
492  {
493  region = 2;
494  }
495  }
496  else
497  {
498  region = 8;
499  }
500  }
501  }
502  else
503  {
504  if (t >= 0)
505  {
506  if (t <= ratio)
507  {
508  region = 5;
509  }
510  else
511  {
512  region = 4;
513  }
514  }
515  else
516  {
517  region = 6;
518  }
519  }
520  enum edge_type { s0, s1, t0, t1, edge_skip, edge_invalid };
521  edge_type edge = edge_invalid;
522  switch (region)
523  {
524  case 0:
525  // Global minimum inside [0,1] [0,1]
526  s /= ratio;
527  t /= ratio;
528  edge = edge_skip;
529  break;
530  case 1:
531  edge = s1;
532  break;
533  case 2:
534  // Q_s(1,1)/2 = a+b+d
535  if (a + b + d > 0)
536  {
537  edge = t1;
538  }
539  else
540  {
541  edge = s1;
542  }
543  break;
544  case 3:
545  edge = t1;
546  break;
547  case 4:
548  // Q_s(0,1)/2 = b+d
549  if (b + d > 0)
550  {
551  edge = s0;
552  }
553  else
554  {
555  edge = t1;
556  }
557  break;
558  case 5:
559  edge = s0;
560  break;
561  case 6:
562  // Q_s(0,0)/2 = d
563  if (d > 0)
564  {
565  edge = s0;
566  }
567  else
568  {
569  edge = t0;
570  }
571  break;
572  case 7:
573  edge = t0;
574  break;
575  case 8:
576  // Q_s(1,0)/2 = a+d
577  if (a + d > 0)
578  {
579  edge = t0;
580  }
581  else
582  {
583  edge = s1;
584  }
585  break;
586  default:
587  break;
588  }
589  switch (edge)
590  {
591  case s0:
592  // F(t) = Q(0,t), F?(t) = 2*(e+c*t)
593  // F?(T) = 0 when T = -e/c, then clamp between 0 and 1 (c always >= 0)
594  s = 0;
595  tmp = e;
596  if (tmp > 0)
597  {
598  t = 0;
599  }
600  else if (-tmp > c)
601  {
602  t = 1;
603  }
604  else
605  {
606  t = -tmp / c;
607  }
608  break;
609  case s1:
610  // F(t) = Q(1,t), F?(t) = 2*((b+e)+c*t)
611  // F?(T) = 0 when T = -(b+e)/c, then clamp between 0 and 1 (c always >= 0)
612  s = 1;
613  tmp = b + e;
614  if (tmp > 0)
615  {
616  t = 0;
617  }
618  else if (-tmp > c)
619  {
620  t = 1;
621  }
622  else
623  {
624  t = -tmp / c;
625  }
626  break;
627  case t0:
628  // F(s) = Q(s,0), F?(s) = 2*(d+a*s) =>
629  // F?(S) = 0 when S = -d/a, then clamp between 0 and 1 (a always >= 0)
630  t = 0;
631  tmp = d;
632  if (tmp > 0)
633  {
634  s = 0;
635  }
636  else if (-tmp > a)
637  {
638  s = 1;
639  }
640  else
641  {
642  s = -tmp / a;
643  }
644  break;
645  case t1:
646  // F(s) = Q(s,1), F?(s) = 2*(b+d+a*s) =>
647  // F?(S) = 0 when S = -(b+d)/a, then clamp between 0 and 1 (a always >= 0)
648  t = 1;
649  tmp = b + d;
650  if (tmp > 0)
651  {
652  s = 0;
653  }
654  else if (-tmp > a)
655  {
656  s = 1;
657  }
658  else
659  {
660  s = -tmp / a;
661  }
662  break;
663  case edge_skip:
664  break;
665  default:
666  break;
667  }
668  }
669  else
670  // Parallel case
671  {
672  if (b > 0)
673  {
674  // Segments have different directions
675  if (d >= 0)
676  {
677  // 0-0 end points since s-segment 0 less than t-segment 0
678  s = 0;
679  t = 0;
680  }
681  else if (-d <= a)
682  {
683  // s-segment 0 end-point in the middle of the t 0-1 segment, get distance
684  s = -d / a;
685  t = 0;
686  }
687  else
688  {
689  // s-segment 1 is definitely closer
690  s = 1;
691  tmp = a + d;
692  if (-tmp >= b)
693  {
694  t = 1;
695  }
696  else
697  {
698  t = -tmp / b;
699  }
700  }
701  }
702  else
703  {
704  // Both segments have the same dir
705  if (-d >= a)
706  {
707  // 1-0
708  s = 1;
709  t = 0;
710  }
711  else if (d <= 0)
712  {
713  // mid-0
714  s = -d / a;
715  t = 0;
716  }
717  else
718  {
719  s = 0;
720  // 1-mid
721  if (d >= -b)
722  {
723  t = 1;
724  }
725  else
726  {
727  t = -d / b;
728  }
729  }
730  }
731  }
732  *pt0 = s0v0 + s * (s0v01);
733  *pt1 = s1v0 + t * (s1v01);
734  if (s0t != nullptr && s1t != nullptr)
735  {
736  *s0t = s;
737  *s1t = t;
738  }
739  return ((*pt1) - (*pt0)).norm();
740 }
741 
751 template <class T, int MOpt> inline
753  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
754  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
755  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
756  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
757  Eigen::Matrix<T, 3, 1, MOpt>* result)
758 {
759  // Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
760  // http://softsurfer.com/Archive/algorithm_0106 for a geometric interpretation
761  // The triangle is parametrized by:
762  // t: tv0 + s * (tv1-tv0) + t * (tv2-tv0) , with s and t between 0 and 1
763  // We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
764  Eigen::Matrix<T, 3, 1, MOpt> tv01 = tv1 - tv0;
765  Eigen::Matrix<T, 3, 1, MOpt> tv02 = tv2 - tv0;
766  T a = tv01.squaredNorm();
767  if (a <= Geometry::SquaredDistanceEpsilon)
768  {
769  // Degenerate edge 1
770  return distancePointSegment<T>(pt, tv0, tv2, result);
771  }
772  T b = tv01.dot(tv02);
773  T tCross = tv01.cross(tv02).squaredNorm();
774  if (tCross <= Geometry::SquaredDistanceEpsilon)
775  {
776  // Degenerate edge 2
777  return distancePointSegment<T>(pt, tv0, tv1, result);
778  }
779  T c = tv02.squaredNorm();
780  if (c <= Geometry::SquaredDistanceEpsilon)
781  {
782  // Degenerate edge 3
783  return distancePointSegment<T>(pt, tv0, tv1, result);
784  }
785  Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0 - pt;
786  T d = tv01.dot(tv0pv0);
787  T e = tv02.dot(tv0pv0);
788  T ratio = a * c - b * b;
789  T s = b * e - c * d;
790  T t = b * d - a * e;
791  // Determine region (inside-outside triangle)
792  int region = -1;
793  if (s + t <= ratio)
794  {
795  if (s < 0)
796  {
797  if (t < 0)
798  {
799  region = 4;
800  }
801  else
802  {
803  region = 3;
804  }
805  }
806  else if (t < 0)
807  {
808  region = 5;
809  }
810  else
811  {
812  region = 0;
813  }
814  }
815  else
816  {
817  if (s < 0)
818  {
819  region = 2;
820  }
821  else if (t < 0)
822  {
823  region = 6;
824  }
825  else
826  {
827  region = 1;
828  }
829  }
830  // Regions: /
831  // ^ t=0 /
832  // \ 2| /
833  // \ | /
834  // \| /
835  // \ /
836  // |\ /
837  // | \ 1 /
838  // 3 | \ /
839  // | 0 \ /
840  // ----|----\-------> s=0 /
841  // | \ /
842  // 4 | 5 \ 6 /
843  // | \ /
844  // /
845  T numer, denom, tmp0, tmp1;
846  enum edge_type { s0, t0, s1t1, edge_skip, edge_invalid };
847  edge_type edge = edge_invalid;
848  switch (region)
849  {
850  case 0:
851  // Global minimum inside [0,1] [0,1]
852  numer = T(1) / ratio;
853  s *= numer;
854  t *= numer;
855  edge = edge_skip;
856  break;
857  case 1:
858  edge = s1t1;
859  break;
860  case 2:
861  // Grad(Q(0,1)).(0,-1)/2 = -c-e
862  // Grad(Q(0,1)).(1,-1)/2 = b=d-c-e
863  tmp0 = b + d;
864  tmp1 = c + e;
865  if (tmp1 > tmp0)
866  {
867  edge = s1t1;
868  }
869  else
870  {
871  edge = s0;
872  }
873  break;
874  case 3:
875  edge = s0;
876  break;
877  case 4:
878  // Grad(Q(0,0)).(0,1)/2 = e
879  // Grad(Q(0,0)).(1,0)/2 = d
880  if (e >= d)
881  {
882  edge = t0;
883  }
884  else
885  {
886  edge = s0;
887  }
888  break;
889  case 5:
890  edge = t0;
891  break;
892  case 6:
893  // Grad(Q(1,0)).(-1,0)/2 = -a-d
894  // Grad(Q(1,0)).(-1,1)/2 = -a-d+b+e
895  tmp0 = -a - d;
896  tmp1 = -a - d + b + e;
897  if (tmp1 > tmp0)
898  {
899  edge = t0;
900  }
901  else
902  {
903  edge = s1t1;
904  }
905  break;
906  default:
907  break;
908  }
909  switch (edge)
910  {
911  case s0:
912  // F(t) = Q(0, t), F'(t)=0 when -e/c = 0
913  s = 0;
914  if (e >= 0)
915  {
916  t = 0;
917  }
918  else
919  {
920  t = (-e >= c ? 1 : -e / c);
921  }
922  break;
923  case t0:
924  // F(s) = Q(s, 0), F'(s)=0 when -d/a = 0
925  t = 0;
926  if (d >= 0)
927  {
928  s = 0;
929  }
930  else
931  {
932  s = (-d >= a ? 1 : -d / a);
933  }
934  break;
935  case s1t1:
936  // F(s) = Q(s, 1-s), F'(s) = 0 when (c+e-b-d)/(a-2b+c) = 0 (denom = || tv01-tv02 ||^2 always > 0)
937  numer = c + e - b - d;
938  if (numer <= 0)
939  {
940  s = 0;
941  }
942  else
943  {
944  denom = a - 2 * b + c;
945  s = (numer >= denom ? 1 : numer / denom);
946  }
947  t = 1 - s;
948  break;
949  case edge_skip:
950  break;
951  default:
952  break;
953  }
954  *result = tv0 + s * tv01 + t * tv02;
955  return ((*result) - pt).norm();
956 }
957 
972 template <class T, int MOpt> inline
974  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
975  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
976  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
977  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
978  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
979  const Eigen::Matrix<T, 3, 1, MOpt>& tn,
980  Eigen::Matrix<T, 3, 1, MOpt>* result)
981 {
982  // Triangle edges vectors
983  Eigen::Matrix<T, 3, 1, MOpt> u = tv1 - tv0;
984  Eigen::Matrix<T, 3, 1, MOpt> v = tv2 - tv0;
985 
986  // Ray direction vector
987  Eigen::Matrix<T, 3, 1, MOpt> dir = sv1 - sv0;
988  Eigen::Matrix<T, 3, 1, MOpt> w0 = sv0 - tv0;
989  T a = -tn.dot(w0);
990  T b = tn.dot(dir);
991 
992  result->setConstant((std::numeric_limits<double>::quiet_NaN()));
993 
994  // Ray is parallel to triangle plane
995  if (std::abs(b) <= Geometry::AngularEpsilon)
996  {
997  if (std::abs(a) <= Geometry::AngularEpsilon)
998  {
999  // Ray lies in triangle plane
1000  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1001  for (int i = 0; i < 2; ++i)
1002  {
1003  barycentricCoordinates((i == 0 ? sv0 : sv1), tv0, tv1, tv2, tn, &baryCoords);
1004  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1005  {
1006  *result = (i == 0) ? sv0 : sv1;
1007  return true;
1008  }
1009  }
1010  // All segment endpoints outside of triangle
1011  return false;
1012  }
1013  else
1014  {
1015  // Segment parallel to triangle but not in same plane
1016  return false;
1017  }
1018  }
1019 
1020  // Get intersect point of ray with triangle plane
1021  T r = a / b;
1022  // Ray goes away from triangle
1023  if (r < -Geometry::DistanceEpsilon)
1024  {
1025  return false;
1026  }
1027  //Ray comes toward triangle but isn't long enough to reach it
1028  if (r > 1 + Geometry::DistanceEpsilon)
1029  {
1030  return false;
1031  }
1032 
1033  // Intersect point of ray and plane
1034  Eigen::Matrix<T, 3, 1, MOpt> presumedIntersection = sv0 + r * dir;
1035  // Collision point inside T?
1036  T uu = u.dot(u);
1037  T uv = u.dot(v);
1038  T vv = v.dot(v);
1039  Eigen::Matrix<T, 3, 1, MOpt> w = presumedIntersection - tv0;
1040  T wu = w.dot(u);
1041  T wv = w.dot(v);
1042  T D = uv * uv - uu * vv;
1043  // Get and test parametric coords
1044  T s = (uv * wv - vv * wu) / D;
1045  // I is outside T
1046  if (s < -Geometry::DistanceEpsilon || s > 1 + Geometry::DistanceEpsilon)
1047  {
1048  return false;
1049  }
1050  T t = (uv * wu - uu * wv) / D;
1051  // I is outside T
1052  if (t < -Geometry::DistanceEpsilon || (s + t) > 1 + Geometry::DistanceEpsilon)
1053  {
1054  return false;
1055  }
1056  // I is in T
1057  *result = sv0 + r * dir;
1058  return true;
1059 }
1060 
1061 
1071 template <class T, int MOpt> inline
1073  const Eigen::Matrix<T, 3, 1, MOpt>& pt,
1074  const Eigen::Matrix<T, 3, 1, MOpt>& n,
1075  T d,
1076  Eigen::Matrix<T, 3, 1, MOpt>* result)
1077 {
1078  T dist = n.dot(pt) + d;
1079  *result = pt - n * dist;
1080  return dist;
1081 }
1082 
1083 
1098 template <class T, int MOpt> inline
1100  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1101  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1102  const Eigen::Matrix<T, 3, 1, MOpt>& n,
1103  T d,
1104  Eigen::Matrix<T, 3, 1, MOpt>* closestPointSegment,
1105  Eigen::Matrix<T, 3, 1, MOpt>* planeIntersectionPoint)
1106 {
1107  T dist0 = n.dot(sv0) + d;
1108  T dist1 = n.dot(sv1) + d;
1109  // Parallel case
1110  Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1111  if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
1112  {
1113  *closestPointSegment = (sv0 + sv1) * T(0.5);
1114  dist0 = n.dot(*closestPointSegment) + d;
1115  *planeIntersectionPoint = *closestPointSegment - dist0 * n;
1116  return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
1117  }
1118  // Both on the same side
1119  if ((dist0 > 0 && dist1 > 0) || (dist0 < 0 && dist1 < 0))
1120  {
1121  if (std::abs(dist0) < std::abs(dist1))
1122  {
1123  *closestPointSegment = sv0;
1124  *planeIntersectionPoint = sv0 - dist0 * n;
1125  return dist0;
1126  }
1127  else
1128  {
1129  *closestPointSegment = sv1;
1130  *planeIntersectionPoint = sv1 - dist1 * n;
1131  return dist1;
1132  }
1133  }
1134  // Segment cutting through
1135  else
1136  {
1137  Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1138  T lambda = (-d - sv0.dot(n)) / v01.dot(n);
1139  *planeIntersectionPoint = sv0 + lambda * v01;
1140  *closestPointSegment = *planeIntersectionPoint;
1141  return 0;
1142  }
1143 }
1144 
1145 
1159 template <class T, int MOpt> inline
1161  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1162  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1163  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1164  const Eigen::Matrix<T, 3, 1, MOpt>& n,
1165  T d,
1166  Eigen::Matrix<T, 3, 1, MOpt>* closestPointTriangle,
1167  Eigen::Matrix<T, 3, 1, MOpt>* planeProjectionPoint)
1168 {
1169  Eigen::Matrix<T, 3, 1, MOpt> distances(n.dot(tv0) + d, n.dot(tv1) + d, n.dot(tv2) + d);
1170  Eigen::Matrix<T, 3, 1, MOpt> t01 = tv1 - tv0;
1171  Eigen::Matrix<T, 3, 1, MOpt> t02 = tv2 - tv0;
1172  Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
1173 
1174  closestPointTriangle->setConstant((std::numeric_limits<double>::quiet_NaN()));
1175  planeProjectionPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1176 
1177  // HS-2013-may-09 Could there be a case where we fall into the wrong tree because of the checks against
1178  // the various epsilon values all going against us ???
1179  // Parallel case (including Coplanar)
1180  if (std::abs(n.dot(t01)) <= Geometry::AngularEpsilon && std::abs(n.dot(t02)) <= Geometry::AngularEpsilon)
1181  {
1182  *closestPointTriangle = (tv0 + tv1 + tv2) / T(3);
1183  *planeProjectionPoint = *closestPointTriangle - n * distances[0];
1184  return distances[0];
1185  }
1186 
1187  // Is there an intersection
1188  if ((distances.array() < -Geometry::DistanceEpsilon).any() &&
1189  (distances.array() > Geometry::DistanceEpsilon).any())
1190  {
1191  if (distances[0] * distances[1] < 0)
1192  {
1193  *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t01) * t01;
1194  if (distances[0] * distances[2] < 0)
1195  {
1196  *planeProjectionPoint = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1197  }
1198  else
1199  {
1200  Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
1201  *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1202  }
1203  }
1204  else
1205  {
1206  *closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
1207  *planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
1208  }
1209 
1210  // Find the midpoint, take this out to return the segment endpoints
1211  *closestPointTriangle = *planeProjectionPoint = (*closestPointTriangle + *planeProjectionPoint) * T(0.5);
1212  return 0;
1213  }
1214 
1215  int index;
1216  distances.cwiseAbs().minCoeff(&index);
1217  switch (index)
1218  {
1219  case 0: //distances[0] is closest
1220  *closestPointTriangle = tv0;
1221  *planeProjectionPoint = tv0 - n * distances[0];
1222  return distances[0];
1223  case 1: //distances[1] is closest
1224  *closestPointTriangle = tv1;
1225  *planeProjectionPoint = tv1 - n * distances[1];
1226  return distances[1];
1227  case 2: //distances[2] is closest
1228  *closestPointTriangle = tv2;
1229  *planeProjectionPoint = tv2 - n * distances[2];
1230  return distances[2];
1231  }
1232 
1233  return std::numeric_limits<T>::quiet_NaN();
1234 }
1235 
1243 template <class T, int MOpt> inline
1245  const Eigen::Matrix<T, 3, 1, MOpt>& pn0, T pd0,
1246  const Eigen::Matrix<T, 3, 1, MOpt>& pn1, T pd1,
1247  Eigen::Matrix<T, 3, 1, MOpt>* pt0,
1248  Eigen::Matrix<T, 3, 1, MOpt>* pt1)
1249 {
1250  // Algorithm from real time collision detection - optimized version page 210 (with extra checks)
1251  const Eigen::Matrix<T, 3, 1, MOpt> lineDir = pn0.cross(pn1);
1252  const T lineDirNorm2 = lineDir.squaredNorm();
1253 
1254  pt0->setConstant((std::numeric_limits<double>::quiet_NaN()));
1255  pt1->setConstant((std::numeric_limits<double>::quiet_NaN()));
1256 
1257  // Test if the two planes are parallel
1258  if (lineDirNorm2 <= Geometry::SquaredDistanceEpsilon)
1259  {
1260  return false; // planes disjoint
1261  }
1262  // Compute common point
1263  *pt0 = (pd1 * pn0 - pd0 * pn1).cross(lineDir) / lineDirNorm2;
1264  *pt1 = *pt0 + lineDir;
1265  return true;
1266 }
1267 
1268 
1279 template <class T, int MOpt> inline
1281  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1282  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1283  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1284  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1285  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1286  Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1287  Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1288 {
1289  Eigen::Matrix<T, 3, 1, MOpt> n = (tv1 - tv0).cross(tv2 - tv1);
1290  n.normalize();
1291  return distanceSegmentTriangle(sv0, sv1, tv0, tv1, tv2, n, segmentPoint, trianglePoint);
1292 }
1293 
1304 template <class T, int MOpt> inline
1306  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1307  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1308  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1309  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1310  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1311  const Eigen::Matrix<T, 3, 1, MOpt>& normal,
1312  Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
1313  Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
1314 {
1315  segmentPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1316  trianglePoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
1317 
1318  // Setting up the plane that the triangle is in
1319  const Eigen::Matrix<T, 3, 1, MOpt>& n = normal;
1320  T d = -n.dot(tv0);
1321  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1322  // Degenerate case: Line and triangle plane parallel
1323  const Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
1324  const T v01DotTn = n.dot(v01);
1325  if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
1326  {
1327  // Check if any of the points project onto the tri
1328  // otherwise normal (non-parallel) processing will get the right result
1329  T dst = std::abs(distancePointPlane(sv0, n, d, trianglePoint));
1330  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
1331  barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
1332  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1333  {
1334  *segmentPoint = sv0;
1335  return dst;
1336  }
1337  dst = std::abs(distancePointPlane(sv1, n, d, trianglePoint));
1338  barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
1339  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1340  {
1341  *segmentPoint = sv1;
1342  return dst;
1343  }
1344  }
1345  // Line and triangle plane *not* parallel: check cut through case only, the rest will be check later
1346  else
1347  {
1348  T lambda = -n.dot(sv0 - tv0) / v01DotTn;
1349  if (lambda >= 0 && lambda <= 1)
1350  {
1351  *segmentPoint = *trianglePoint = sv0 + lambda * v01;
1352  barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
1353  if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
1354  {
1355  // Segment goes through the triangle
1356  return 0;
1357  }
1358  }
1359  }
1360  // At this point the segment is nearest point to one of the triangle edges or one of the end points
1361  Eigen::Matrix<T, 3, 1, MOpt> segColPt01, segColPt02, segColPt12, triColPt01, triColPt02, triColPt12;
1362  T dst01 = distanceSegmentSegment(sv0, sv1, tv0, tv1, &segColPt01, &triColPt01);
1363  T dst02 = distanceSegmentSegment(sv0, sv1, tv0, tv2, &segColPt02, &triColPt02);
1364  T dst12 = distanceSegmentSegment(sv0, sv1, tv1, tv2, &segColPt12, &triColPt12);
1365  Eigen::Matrix<T, 3, 1, MOpt> ptTriCol0, ptTriCol1;
1366  T dstPtTri0 = std::abs(distancePointPlane(sv0, n, d, &ptTriCol0));
1367  barycentricCoordinates(ptTriCol0, tv0, tv1, tv2, normal, &baryCoords);
1368  if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1369  {
1370  dstPtTri0 = std::numeric_limits<T>::max();
1371  }
1372  T dstPtTri1 = std::abs(distancePointPlane(sv1, n, d, &ptTriCol1));
1373  barycentricCoordinates(ptTriCol1, tv0, tv1, tv2, normal, &baryCoords);
1374  if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
1375  {
1376  dstPtTri1 = std::numeric_limits<T>::max();
1377  }
1378 
1379  int minIndex;
1380  Eigen::Matrix<double, 5, 1> distances;
1381  (distances << dst01, dst02, dst12, dstPtTri0, dstPtTri1).finished().minCoeff(&minIndex);
1382  switch (minIndex)
1383  {
1384  case 0:
1385  *segmentPoint = segColPt01;
1386  *trianglePoint = triColPt01;
1387  return dst01;
1388  case 1:
1389  *segmentPoint = segColPt02;
1390  *trianglePoint = triColPt02;
1391  return dst02;
1392  case 2:
1393  *segmentPoint = segColPt12;
1394  *trianglePoint = triColPt12;
1395  return dst12;
1396  case 3:
1397  *segmentPoint = sv0;
1398  *trianglePoint = ptTriCol0;
1399  return dstPtTri0;
1400  case 4:
1401  *segmentPoint = sv1;
1402  *trianglePoint = ptTriCol1;
1403  return dstPtTri1;
1404  }
1405 
1406  // Invalid ...
1407  return std::numeric_limits<T>::quiet_NaN();
1408 
1409 }
1410 
1411 
1422 template <class T, int MOpt> inline
1424  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1425  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1426  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1427  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1428  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1429  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1430  Eigen::Matrix<T, 3, 1, MOpt>* closestPoint0,
1431  Eigen::Matrix<T, 3, 1, MOpt>* closestPoint1)
1432 {
1433  // Check the segments of t0 against t1
1434  T minDst = std::numeric_limits<T>::max();
1435  T currDst = 0;
1436  Eigen::Matrix<T, 3, 1, MOpt> segPt, triPt;
1437  Eigen::Matrix<T, 3, 1, MOpt> n0 = (t0v1 - t0v0).cross(t0v2 - t0v0);
1438  n0.normalize();
1439  Eigen::Matrix<T, 3, 1, MOpt> n1 = (t1v1 - t1v0).cross(t1v2 - t1v0);
1440  n1.normalize();
1441  currDst = distanceSegmentTriangle(t0v0, t0v1, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
1442  if (currDst < minDst)
1443  {
1444  minDst = currDst;
1445  *closestPoint0 = segPt;
1446  *closestPoint1 = triPt;
1447  }
1448  currDst = distanceSegmentTriangle(t0v1, t0v2, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
1449  if (currDst < minDst)
1450  {
1451  minDst = currDst;
1452  *closestPoint0 = segPt;
1453  *closestPoint1 = triPt;
1454  }
1455  currDst = distanceSegmentTriangle(t0v2, t0v0, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
1456  if (currDst < minDst)
1457  {
1458  minDst = currDst;
1459  *closestPoint0 = segPt;
1460  *closestPoint1 = triPt;
1461  }
1462  // Check the segments of t1 against t0
1463  currDst = distanceSegmentTriangle(t1v0, t1v1, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
1464  if (currDst < minDst)
1465  {
1466  minDst = currDst;
1467  *closestPoint1 = segPt;
1468  *closestPoint0 = triPt;
1469  }
1470  currDst = distanceSegmentTriangle(t1v1, t1v2, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
1471  if (currDst < minDst)
1472  {
1473  minDst = currDst;
1474  *closestPoint1 = segPt;
1475  *closestPoint0 = triPt;
1476  }
1477  currDst = distanceSegmentTriangle(t1v2, t1v0, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
1478  if (currDst < minDst)
1479  {
1480  minDst = currDst;
1481  *closestPoint1 = segPt;
1482  *closestPoint0 = triPt;
1483  }
1484  return (minDst);
1485 }
1486 
1493 template <class T, int MOpt>
1495  const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
1496  const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
1497  const Eigen::AlignedBox<T, 3>& box,
1498  std::vector<Eigen::Matrix<T, 3, 1, MOpt>>* intersections)
1499 {
1500  Eigen::Array<T, 3, 1, MOpt> v01 = sv1 - sv0;
1501  Eigen::Array<bool, 3, 1, MOpt> parallelToPlane = (v01.cwiseAbs().array() < Geometry::DistanceEpsilon);
1502  if (parallelToPlane.any())
1503  {
1504  Eigen::Array<bool, 3, 1, MOpt> beyondMinCorner = (sv0.array() < box.min().array());
1505  Eigen::Array<bool, 3, 1, MOpt> beyondMaxCorner = (sv0.array() > box.max().array());
1506  if ((parallelToPlane && (beyondMinCorner || beyondMaxCorner)).any())
1507  {
1508  return;
1509  }
1510  }
1511 
1512  // Calculate the intersection of the segment with each of the 6 box planes.
1513  // The intersection is calculated as the distance along the segment (abscissa)
1514  // scaled from 0 to 1.
1515  Eigen::Array<T, 3, 2, MOpt> planeIntersectionAbscissas;
1516  planeIntersectionAbscissas.col(0) = (box.min().array() - sv0.array());
1517  planeIntersectionAbscissas.col(1) = (box.max().array() - sv0.array());
1518 
1519  // While we could be dividing by zero here, INF values are
1520  // correctly handled by the rest of the function.
1521  planeIntersectionAbscissas.colwise() /= v01;
1522 
1523  T entranceAbscissa = planeIntersectionAbscissas.rowwise().minCoeff().maxCoeff();
1524  T exitAbscissa = planeIntersectionAbscissas.rowwise().maxCoeff().minCoeff();
1525  if (entranceAbscissa < exitAbscissa && exitAbscissa > T(0.0))
1526  {
1527  if (entranceAbscissa >= T(0.0) && entranceAbscissa <= T(1.0))
1528  {
1529  intersections->push_back(sv0 + v01.matrix() * entranceAbscissa);
1530  }
1531 
1532  if (exitAbscissa >= T(0.0) && exitAbscissa <= T(1.0))
1533  {
1534  intersections->push_back(sv0 + v01.matrix() * exitAbscissa);
1535  }
1536  }
1537 }
1538 
1547 template <class T, int MOpt>
1549  const Eigen::Matrix<T, 3, 1, MOpt>& capsuleBottom,
1550  const Eigen::Matrix<T, 3, 1, MOpt>& capsuleTop,
1551  const T capsuleRadius,
1552  const Eigen::AlignedBox<T, 3>& box)
1553 {
1554  Eigen::AlignedBox<double, 3> dilatedBox(box.min().array() - capsuleRadius, box.max().array() + capsuleRadius);
1555  std::vector<Vector3d> candidates;
1556  intersectionsSegmentBox(capsuleBottom, capsuleTop, dilatedBox, &candidates);
1557  if (dilatedBox.contains(capsuleBottom))
1558  {
1559  candidates.push_back(capsuleBottom);
1560  }
1561  if (dilatedBox.contains(capsuleTop))
1562  {
1563  candidates.push_back(capsuleTop);
1564  }
1565 
1566  bool doesIntersect = false;
1567  ptrdiff_t dimensionsOutsideBox;
1568  Vector3d clampedPosition, segmentPoint;
1569  for (auto candidate = candidates.cbegin(); candidate != candidates.cend(); ++candidate)
1570  {
1571  // Collisions between a capsule and a box are the same as a segment and a dilated
1572  // box with rounded corners. If the intersection occurs outside the original box
1573  // in two dimensions (collision with an edge of the dilated box) or three
1574  // dimensions (collision with the corner of the dilated box) dimensions, we need
1575  // to check if it is inside these rounded corners.
1576  dimensionsOutsideBox = (candidate->array() > box.max().array()).count();
1577  dimensionsOutsideBox += (candidate->array() < box.min().array()).count();
1578  if (dimensionsOutsideBox >= 2)
1579  {
1580  clampedPosition = (*candidate).array().min(box.max().array()).max(box.min().array());
1581  if (distancePointSegment(clampedPosition, capsuleBottom, capsuleTop, &segmentPoint) > capsuleRadius)
1582  {
1583  // Doesn't intersect, try the next candidate.
1584  continue;
1585  }
1586  }
1587  doesIntersect = true;
1588  break;
1589  }
1590  return doesIntersect;
1591 }
1592 
1600 template <class T, int VOpt>
1601 Eigen::Matrix<T, 3, 1, VOpt> nearestPointOnLine(const Eigen::Matrix<T, 3, 1, VOpt>& point,
1602  const Eigen::Matrix<T, 3, 1, VOpt>& segment0, const Eigen::Matrix<T, 3, 1, VOpt>& segment1)
1603 {
1604  auto pointToSegmentStart = segment0 - point;
1605  auto segmentDirection = segment1 - segment0;
1606  auto squaredNorm = segmentDirection.squaredNorm();
1607  SURGSIM_ASSERT(squaredNorm != 0.0) << "Line is defined by two collocated points.";
1608  auto distance = -pointToSegmentStart.dot(segmentDirection) / squaredNorm;
1609  auto p0Proj = segment0 + distance * segmentDirection;
1610  return p0Proj;
1611 }
1612 
1623 template <class T, int MOpt> inline
1625  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1626  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1627  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1628  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1629  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1630  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1631  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1632  const Eigen::Matrix<T, 3, 1, MOpt>& t1n);
1633 
1640 template <class T, int MOpt> inline
1642  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1643  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1644  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1645  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1646  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1647  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2);
1648 
1668 template <class T, int MOpt> inline
1670  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1671  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1672  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1673  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1674  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1675  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1676  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1677  const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
1678  T* penetrationDepth,
1679  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1680  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1681  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1682 
1700 template <class T, int MOpt> inline
1702  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1703  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1704  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1705  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1706  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1707  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1708  T* penetrationDepth,
1709  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1710  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1711  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1712 
1730 template <class T, int MOpt> inline
1732  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1733  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1734  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1735  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1736  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1737  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1738  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
1739  const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
1740  T* penetrationDepth,
1741  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1742  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1743  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1744 
1758 template <class T, int MOpt> inline
1760  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
1761  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
1762  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
1763  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
1764  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
1765  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
1766  T* penetrationDepth,
1767  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
1768  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
1769  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
1770 
1788 template <class T, int MOpt> inline
1790  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1791  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1792  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1793  const Eigen::Matrix<T, 3, 1, MOpt>& tn,
1794  const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
1795  const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
1796  double cr,
1797  T* penetrationDepth,
1798  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
1799  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
1800  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
1801  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
1802 
1819 template <class T, int MOpt> inline
1821  const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
1822  const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
1823  const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
1824  const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
1825  const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
1826  double cr,
1827  T* penetrationDepth,
1828  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
1829  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
1830  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
1831  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
1832 
1839 template <class T, int MOpt>
1841  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
1842  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
1843  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
1844  const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
1845  std::array<T, 3>* timesOfCoplanarity)
1846 {
1862  Eigen::Matrix<T, 3, 1, MOpt> AB0 = B.first - A.first;
1863  Eigen::Matrix<T, 3, 1, MOpt> AC0 = C.first - A.first;
1864  Eigen::Matrix<T, 3, 1, MOpt> CD0 = D.first - C.first;
1865  Eigen::Matrix<T, 3, 1, MOpt> VA = (A.second - A.first);
1866  Eigen::Matrix<T, 3, 1, MOpt> VC = (C.second - C.first);
1867  Eigen::Matrix<T, 3, 1, MOpt> VAB = (B.second - B.first) - VA;
1868  Eigen::Matrix<T, 3, 1, MOpt> VAC = VC - VA;
1869  Eigen::Matrix<T, 3, 1, MOpt> VCD = (D.second - D.first) - VC;
1870  T a0 = AB0.cross(CD0).dot(AC0);
1871  T a1 = AB0.cross(CD0).dot(VAC) + (AB0.cross(VCD) + VAB.cross(CD0)).dot(AC0);
1872  T a2 = (AB0.cross(VCD) + VAB.cross(CD0)).dot(VAC) + VAB.cross(VCD).dot(AC0);
1873  T a3 = VAB.cross(VCD).dot(VAC);
1874 
1875  return findRootsInRange01(Polynomial<T, 3>(a0, a1, a2, a3), timesOfCoplanarity);
1876 }
1877 
1878 }; // namespace Math
1879 }; // namespace SurgSim
1880 
1881 
1887 
1888 #endif
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::findRootsInRange01
int findRootsInRange01(const Polynomial< T, 3 > &p, std::array< T, 3 > *roots)
Find all roots in range of a cubic equation.
Definition: CubicSolver-inl.h:35
SegmentSegmentCcdContactCalculation-inl.h
SurgSim::Math::calculateContactTriangleTriangleSeparatingAxis
bool calculateContactTriangleTriangleSeparatingAxis(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:302
TriangleTriangleIntersection-inl.h
Vector.h
SurgSim::Math::calculateContactTriangleTriangle
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
SurgSim::Math::distancePointTriangle
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:752
SurgSim::Math::distanceTrianglePlane
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:1160
SurgSim::Math::distancePointSegment
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,...
Definition: Geometry.h:297
SurgSim::Math::doesCollideSegmentTriangle
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....
Definition: Geometry.h:973
TriangleTriangleContactCalculation-inl.h
SurgSim::Math::isPointInsideTriangle
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:159
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
SurgSim::Math::distanceSegmentTriangle
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:1280
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::isCoplanar
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:251
Log.h
SurgSim::Math::distanceSegmentPlane
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:1099
SurgSim::Math::distanceSegmentSegment
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:404
SurgSim::Math::timesOfCoplanarityInRange01
int timesOfCoplanarityInRange01(const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &A, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &B, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &C, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &D, std::array< T, 3 > *timesOfCoplanarity)
Test when 4 points are coplanar in the range [0..1] given their linear motion.
Definition: Geometry.h:1840
SurgSim::Math::doesIntersectTriangleTriangle
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:70
SurgSim::Math::isPointOnTriangleEdge
bool isPointOnTriangleEdge(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 on the edge of a triangle.
Definition: Geometry.h:206
SurgSim::Math::doesIntersectBoxCapsule
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:1548
SurgSim::Math::distanceTriangleTriangle
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:1423
Polynomial.h
PointTriangleCcdContactCalculation-inl.h
SurgSim::Math::distanceLineLine
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:337
SurgSim::Math::Polynomial< T, 3 >
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials)
Definition: Polynomial.h:255
SurgSim::Math::barycentricCoordinates
bool barycentricCoordinates(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, 2, 1, MOpt > *coordinates)
Calculate the barycentric coordinates of a point with respect to a line segment.
Definition: Geometry.h:71
SurgSim::Math::nearestPointOnLine
Eigen::Matrix< T, 3, 1, VOpt > nearestPointOnLine(const Eigen::Matrix< T, 3, 1, VOpt > &point, const Eigen::Matrix< T, 3, 1, VOpt > &segment0, const Eigen::Matrix< T, 3, 1, VOpt > &segment1)
Helper method to determine the nearest point between a point and a line.
Definition: Geometry.h:1601
SurgSim::Math::intersectionsSegmentBox
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:1494
SurgSim::Math::distancePointLine
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:266
SurgSim::Math::distancePointPlane
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:1072
SurgSim::Math::doesIntersectPlanePlane
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:1244
SurgSim::Math::calculateContactTriangleCapsule
bool calculateContactTriangleCapsule(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, const Eigen::Matrix< T, 3, 1, MOpt > &cv0, const Eigen::Matrix< T, 3, 1, MOpt > &cv1, double cr, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsule, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsuleAxis)
Calculate the contact between a capsule and a triangle.
Definition: TriangleCapsuleContactCalculation-inl.h:521
TriangleCapsuleContactCalculation-inl.h