16 #ifndef SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H 17 #define SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H 20 #include <Eigen/Geometry> 39 template <
class T,
int MOpt>
42 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
43 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
44 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
45 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
48 Eigen::Matrix<T, 3, 1, MOpt> At =
interpolate(A.first, A.second, time);
49 Eigen::Matrix<T, 3, 1, MOpt> Bt =
interpolate(B.first, B.second, time);
50 Eigen::Matrix<T, 3, 1, MOpt> Ct =
interpolate(C.first, C.second, time);
51 Eigen::Matrix<T, 3, 1, MOpt> Dt =
interpolate(D.first, D.second, time);
61 Eigen::Matrix<T, 3, 2> matrixA;
62 matrixA.col(0) = Bt - At;
63 matrixA.col(1) = -(Dt - Ct);
65 Eigen::Matrix<T, 3, 1, MOpt> b = Ct - At;
66 Eigen::Matrix<T, 2, 2> inv;
68 (matrixA.transpose() * matrixA).computeInverseWithCheck(inv, invertible);
74 *barycentricCoordinates = inv * matrixA.transpose() * b;
76 for (
int i = 0; i < 2; i++)
78 if (std::abs((*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
80 (*barycentricCoordinates)[i] = 0.0;
82 if (std::abs(1.0 - (*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
84 (*barycentricCoordinates)[i] = 1.0;
89 (*barycentricCoordinates)[0] >= 0.0 && (*barycentricCoordinates)[0] <= 1.0 &&
90 (*barycentricCoordinates)[1] >= 0.0 && (*barycentricCoordinates)[1] <= 1.0;
106 template <
class T,
int MOpt>
inline 108 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
109 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
110 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
111 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
112 T* timeOfImpact, T* s0p1Factor, T* s1p1Factor)
114 std::array<T, 3> roots;
118 for (
int rootId = 0; rootId < numberOfRoots; ++rootId)
124 *timeOfImpact = roots[rootId];
125 *s0p1Factor = barycentricCoordinates[0];
126 *s1p1Factor = barycentricCoordinates[1];
143 #endif // SURGSIM_MATH_SEGMENTSEGMENTCCDCONTACTCALCULATION_INL_H Definition: CompoundShapeToGraphics.cpp:29
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
Eigen::Quaternion< T, QOpt > interpolate(const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
Interpolate (slerp) between 2 quaternions.
Definition: Quaternion.h:149
bool calculateCcdContactSegmentSegment(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, T *timeOfImpact, T *s0p1Factor, T *s1p1Factor)
Continuous collision detection between two segments AB and CD.
Definition: SegmentSegmentCcdContactCalculation-inl.h:107
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
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
bool areSegmentsIntersecting(T time, 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, Eigen::Matrix< T, 2, 1, MOpt > *barycentricCoordinates)
Check if 2 segments intersect at a given time of their motion.
Definition: SegmentSegmentCcdContactCalculation-inl.h:40