PointTriangleCcdContactCalculation-inl.h
Go to the documentation of this file.
1 // This file is a part of the OpenSurgSim project.
2 // Copyright 2013-2016, 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_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_INL_H
18 
19 #include <Eigen/Core>
20 #include <Eigen/Geometry>
21 
23 
24 namespace SurgSim
25 {
26 namespace Math
27 {
28 
37 template <class T, int MOpt>
39  T time,
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,
44  Eigen::Matrix<T, 3, 1, MOpt>* barycentricCoordinates)
45 {
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);
50 
51  bool result = Math::barycentricCoordinates(Pt, At, Bt, Ct, barycentricCoordinates);
52 
53  for (int i = 0; i < 3; i++)
54  {
55  if (std::abs((*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
56  {
57  (*barycentricCoordinates)[i] = 0.0;
58  }
59  if (std::abs(1.0 - (*barycentricCoordinates)[i]) < Math::Geometry::ScalarEpsilon)
60  {
61  (*barycentricCoordinates)[i] = 1.0;
62  }
63  }
64 
65  return (result &&
66  (*barycentricCoordinates)[0] >= 0.0 &&
67  (*barycentricCoordinates)[1] >= 0.0 &&
68  (*barycentricCoordinates)[2] >= 0.0);
69 }
70 
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)
90 {
91  std::array<T, 3> roots;
92  int numberOfRoots = timesOfCoplanarityInRange01(P, A, B, C, &roots);
93 
94  // The roots are all in [0..1] and ordered ascendingly
95  for (int rootId = 0; rootId < numberOfRoots; ++rootId)
96  {
97  Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
98  if (isPointInsideTriangle(roots[rootId], P, A, B, C, &baryCoords))
99  {
100  // The point P is in the triangle plane at time t, and is inside the triangle
101  *timeOfImpact = roots[rootId];
102  *tv01Param = baryCoords[1];
103  *tv02Param = baryCoords[2];
104 
105  // None of these assertion should be necessary, but just to double check
106  SURGSIM_ASSERT(*timeOfImpact >= 0.0 && *timeOfImpact <= 1.0);
107  SURGSIM_ASSERT(*tv01Param >= 0.0);
108  SURGSIM_ASSERT(*tv02Param >= 0.0);
109  SURGSIM_ASSERT(*tv01Param + *tv02Param <= 1.0 + Geometry::ScalarEpsilon);
110 
111  return true;
112  }
113  }
114 
115  return false;
116 }
117 
118 }; // namespace Math
119 }; // namespace SurgSim
120 
121 #endif // SURGSIM_MATH_POINTTRIANGLECCDCONTACTCALCULATION_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
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 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
bool calculateCcdContactPointTriangle(const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &P, 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, T *timeOfImpact, T *tv01Param, T *tv02Param)
Continuous collision detection between a point P and a triangle ABC.
Definition: PointTriangleCcdContactCalculation-inl.h:84