16 #ifndef SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
20 #include <Eigen/Geometry>
37 template <
class T,
int MOpt>
40 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& P,
41 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
42 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
43 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
46 Eigen::Matrix<T, 3, 1, MOpt> Pt =
interpolate(P.first, P.second, time);
47 Eigen::Matrix<T, 3, 1, MOpt> At =
interpolate(A.first, A.second, time);
48 Eigen::Matrix<T, 3, 1, MOpt> Bt =
interpolate(B.first, B.second, time);
49 Eigen::Matrix<T, 3, 1, MOpt> Ct =
interpolate(C.first, C.second, time);
53 for (
int i = 0; i < 3; i++)
57 (*barycentricCoordinates)[i] = 0.0;
61 (*barycentricCoordinates)[i] = 1.0;
83 template <
class T,
int MOpt>
inline
85 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& P,
86 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
87 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
88 const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
89 T* timeOfImpact, T* tv01Param, T* tv02Param)
91 std::array<T, 3> roots;
95 for (
int rootId = 0; rootId < numberOfRoots; ++rootId)
97 Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
101 *timeOfImpact = roots[rootId];
102 *tv01Param = baryCoords[1];
103 *tv02Param = baryCoords[2];
109 SURGSIM_ASSERT(*tv01Param + *tv02Param <= 1.0 + Geometry::ScalarEpsilon);
121 #endif // SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_INL_H