All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
TriangleTriangleContactCalculation-inl.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 //
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_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
18 
19 namespace SurgSim
20 {
21 
22 namespace Math
23 {
24 
33 template <class T, int MOpt>
35 {
36  static const size_t CAPACITY = 10;
37  typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
38  typedef boost::container::static_vector<Vector3, CAPACITY> Vertices;
39 
40 public:
44  TriangleHelper(const Vector3& v0, const Vector3& v1, const Vector3& v2, const Vector3& n)
46  {
47  m_vertices[0] = &v0;
48  m_vertices[1] = &v1;
49  m_vertices[2] = &v2;
50  m_planeD = -m_vertices[0]->dot(m_normal);
51  }
52 
58  void findDeepestPenetrationWithTriangle(const TriangleHelper& triangle, T* penetrationDepth,
59  Vector3* penetrationPoint0, Vector3* penetrationPoint1)
60  {
61  m_clippedVerticesBuffer[0].push_back(*m_vertices[0]);
62  m_clippedVerticesBuffer[0].push_back(*m_vertices[1]);
63  m_clippedVerticesBuffer[0].push_back(*m_vertices[2]);
65 
66  Vector3 clipPlaneNormal;
67  T clipPlaneD;
68 
69  for (size_t i = 0; i < 3; ++i)
70  {
71  triangle.getPrismPlane(i, &clipPlaneNormal, &clipPlaneD);
72  clipAgainstPlane(clipPlaneNormal, clipPlaneD);
73  }
74 
75  findDeepestVertexUnderPlane(triangle.m_normal, triangle.m_planeD, penetrationDepth, penetrationPoint0);
76 
77  SURGSIM_ASSERT(*penetrationDepth <= T(0))
78  << "The distance from triangle is calculated as " << *penetrationDepth << ". At this point in the"
79  << " algorithm, the depth is expected to be negative.";
80 
81  *penetrationPoint1 = *penetrationPoint0 - (triangle.m_normal * (*penetrationDepth));
82  *penetrationDepth = -(*penetrationDepth);
83  }
84 
85 private:
91  void getPrismPlane(size_t index, Vector3* planeNormal, T* planeD) const
92  {
93  *planeNormal = *m_vertices[(index + 1) % 3] - *m_vertices[index];
94  *planeNormal = planeNormal->cross(m_normal);
95  planeNormal->normalize();
96  *planeD = -m_vertices[index]->dot(*planeNormal);
97  }
98 
103  void clipAgainstPlane(const Vector3& planeN, T planeD)
104  {
105  // Loop through the edges starting from (m_vertices[0]->m_vertices[1]) to
106  // (m_vertices[m_numVertices - 1]->m_vertices[0]).
107  // The start vertex and end vertex can be either under/on/over the clipping plane.
108  // start | end | action
109  // ----------------------------------------------
110  // under | under | add start to clipped vertices
111  // under | on | add start to clipped vertices
112  // under | over | add start to clipped vertices, clip the edge to the plane (creating a vertex)
113  // on | under | add start to clipped vertices
114  // on | on | add start to clipped vertices
115  // on | over | add start to clipped vertices
116  // over | under | clip the edge to the plane (creating a vertex)
117  // over | on | none
118  // over | over | none
119 
123  clippedVertices.clear();
124 
125  static const T EPSILON = T(Geometry::DistanceEpsilon);
126 
127  // Calculate the signed distance of the vertices from the clipping plane.
128  boost::container::static_vector<T, CAPACITY> signedDistanceFromPlane;
129  for (auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
130  {
131  signedDistanceFromPlane.push_back((*it).dot(planeN) + planeD);
132  }
133 
134  // Temp variable.
135  T ratio;
136 
137  // Iterators for the end vertices of an edge.
138  typename boost::container::static_vector<Vector3, CAPACITY>::iterator end;
139 
140  // Iterators for the signed distance from plane of the start and end vertices of an edge.
141  auto startSignedDistance = signedDistanceFromPlane.begin();
142  typename boost::container::static_vector<T, CAPACITY>::iterator endSignedDistance;
143 
144  // Iterate over the edges of the current polygon.
145  for (auto start = originalVertices.begin(); start != originalVertices.end(); ++start, ++startSignedDistance)
146  {
147  // If the end has reached the end of list, point it back to the front of list.
148  end = start + 1;
149  endSignedDistance = startSignedDistance + 1;
150  if (end == originalVertices.end())
151  {
152  end = originalVertices.begin();
153  endSignedDistance = signedDistanceFromPlane.begin();
154  }
155 
156  // If the vertex is under or on the plane, add to the clippedVertices.
157  if (*startSignedDistance <= EPSILON)
158  {
159  clippedVertices.push_back(*start);
160  }
161 
162  // If the edge runs from one side of the plane to another. Clip it.
163  if ((*startSignedDistance < -EPSILON && *endSignedDistance > EPSILON) ||
164  (*startSignedDistance > EPSILON && *endSignedDistance < -EPSILON))
165  {
166  ratio = *startSignedDistance / (*startSignedDistance - *endSignedDistance);
167  clippedVertices.push_back(*start + (*end - *start) * ratio);
168  }
169  }
170  }
171 
178  void findDeepestVertexUnderPlane(const Vector3& planeN, T planeD, T* depth, Vector3* point) const
179  {
180  const Vertices& originalVertices = m_clippedVerticesBuffer[(m_receiverBufferIndex + 1) % 2];
181 
182  SURGSIM_ASSERT(originalVertices.size() > 0)
183  << "There are no vertices under the plane. This scenario should not arise according to the"
184  << " Triangle-Triangle Contact Calculation algorithm, because of the circumstances under which"
185  << " this function is set to be called.";
186 
187  T signedDistanceFromPlane;
188  *depth = T(0);
189  for (auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
190  {
191  signedDistanceFromPlane = (*it).dot(planeN) + planeD;
192  if (signedDistanceFromPlane < *depth)
193  {
194  *depth = signedDistanceFromPlane;
195  *point = *it;
196  }
197  }
198  }
199 
201  const Vector3* m_vertices[3];
202 
205 
208 
212 };
213 
214 
215 template <class T, int MOpt> inline
217  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
218  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
219  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
220  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
221  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
222  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
223  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
224  const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
225  T* penetrationDepth,
226  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
227  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
228  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
229 {
230  typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
231 
232  // Check if the triangles intersect.
233  if (!doesIntersectTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n))
234  {
235  return false;
236  }
237 
238  // When control reaches here, the two triangles are definitely intersecting.
239  // Calculate the deepest penetration along each of the triangle normals.
240 
241  TriangleHelper<T, MOpt> triangle1(t0v0, t0v1, t0v2, t0n);
242  TriangleHelper<T, MOpt> triangle2(t1v0, t1v1, t1v2, t1n);
243 
244  // Penetration info to be calculated.
245  T penetrationDepths[2] = {T(0), T(0)};
246  Vector3 penetrationPoints[2][2];
247 
248  // Calculate deepest penetration for each of the triangle.
250  triangle2, &penetrationDepths[0], &penetrationPoints[0][0], &penetrationPoints[0][1]);
251 
253  triangle1, &penetrationDepths[1], &penetrationPoints[1][1], &penetrationPoints[1][0]);
254 
255  // Choose the lower penetration of the two as the contact.
256  if (penetrationDepths[0] < penetrationDepths[1])
257  {
258  *penetrationDepth = penetrationDepths[0];
259  *contactNormal = t1n;
260  *penetrationPoint0 = penetrationPoints[0][0];
261  *penetrationPoint1 = penetrationPoints[0][1];
262  }
263  else
264  {
265  *penetrationDepth = penetrationDepths[1];
266  *contactNormal = -t0n;
267  *penetrationPoint0 = penetrationPoints[1][0];
268  *penetrationPoint1 = penetrationPoints[1][1];
269  }
270 
271  return true;
272 }
273 
274 
275 template <class T, int MOpt> inline
277  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
278  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
279  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
280  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
281  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
282  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
283  T* penetrationDepth,
284  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
285  Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
286  Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
287 {
288  Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
289  Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
290  if (t0n.isZero() || t1n.isZero())
291  {
292  // Degenerate triangle(s) passed to calculateContactTriangleTriangle
293  return false;
294  }
295  t0n.normalize();
296  t1n.normalize();
297  return calculateContactTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n, penetrationDepth,
298  penetrationPoint0, penetrationPoint1, contactNormal);
299 }
300 
301 
302 } // namespace Math
303 
304 } // namespace SurgSim
305 
306 #endif // SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
T m_planeD
d from the plane equation (n.x + d = 0) for the plane of this triangle.
Definition: TriangleTriangleContactCalculation-inl.h:207
const Vector3 & m_normal
Normal of the triangle.
Definition: TriangleTriangleContactCalculation-inl.h:204
TriangleHelper(const Vector3 &v0, const Vector3 &v1, const Vector3 &v2, const Vector3 &n)
Constructor using the triangle data to initialize.
Definition: TriangleTriangleContactCalculation-inl.h:44
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
void findDeepestVertexUnderPlane(const Vector3 &planeN, T planeD, T *depth, Vector3 *point) const
Find the deepest vertex of this polygon under the plane.
Definition: TriangleTriangleContactCalculation-inl.h:178
boost::container::static_vector< Vector3, CAPACITY > Vertices
Definition: TriangleTriangleContactCalculation-inl.h:38
Vertices m_clippedVerticesBuffer[2]
The buffers for the clipped vertices of the triangle.
Definition: TriangleTriangleContactCalculation-inl.h:210
void findDeepestPenetrationWithTriangle(const TriangleHelper &triangle, T *penetrationDepth, Vector3 *penetrationPoint0, Vector3 *penetrationPoint1)
Given a triangle, find the deepest vertex in the swept volume of that triangle.
Definition: TriangleTriangleContactCalculation-inl.h:58
void clipAgainstPlane(const Vector3 &planeN, T planeD)
Clip the polygon given a plane.
Definition: TriangleTriangleContactCalculation-inl.h:103
size_t m_receiverBufferIndex
Definition: TriangleTriangleContactCalculation-inl.h:211
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
Eigen::Matrix< T, 3, 1, MOpt > Vector3
Definition: TriangleTriangleContactCalculation-inl.h:37
void getPrismPlane(size_t index, Vector3 *planeNormal, T *planeD) const
Get the bounding plane of the swept volume of this triangle.
Definition: TriangleTriangleContactCalculation-inl.h:91
static const size_t CAPACITY
Definition: TriangleTriangleContactCalculation-inl.h:36
A helper class for a triangle, used for the following two purposes:
Definition: TriangleTriangleContactCalculation-inl.h:34
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
const Vector3 * m_vertices[3]
Original vertices of the triangle.
Definition: TriangleTriangleContactCalculation-inl.h:201