TriangleTriangleIntersection-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_TRIANGLETRIANGLEINTERSECTION_INL_H
17 #define SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
18 
19 namespace
20 {
21 static const double EPSILOND = 1e-12;
22 }
23 
24 namespace SurgSim
25 {
26 
27 namespace Math
28 {
29 
30 
43 template<class T>
44 void edgeIntersection(T dStart, T dEnd, T pvStart, T pvEnd, T* parametricIntersection,
45  size_t* parametricIntersectionIndex)
46 {
47  // Epsilon used in this function.
48  static const T EPSILON = static_cast<T>(EPSILOND);
49 
50  bool edgeFromUnderToAbove = dStart < 0.0 && dEnd >= 0.0;
51  bool edgeFromAboveToUnder = dStart > 0.0 && dEnd <= 0.0;
52 
53  if (edgeFromUnderToAbove || edgeFromAboveToUnder)
54  {
55  if (std::abs(dStart - dEnd) < EPSILON)
56  {
57  // Start and End are really close. Pick start.
58  parametricIntersection[(*parametricIntersectionIndex)++] = pvStart;
59  }
60  else
61  {
62  // Clip to the point in the intersection of Start->End and plane of the colliding triangle.
63  parametricIntersection[(*parametricIntersectionIndex)++] =
64  pvStart + (pvEnd - pvStart) * (dStart / (dStart - dEnd));
65  }
66  }
67 }
68 
69 template <class T, int MOpt> inline
71  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
72  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
73  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
74  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
75  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
76  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
77  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
78  const Eigen::Matrix<T, 3, 1, MOpt>& t1n)
79 {
80  typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
81  using SurgSim::Math::Geometry::DistanceEpsilon;
82 
83  if (t0n.isZero() || t1n.isZero())
84  {
85  // Degenerate triangle(s) passed to checkTriangleTriangleIntersection.
86  return false;
87  }
88 
89  // Variable names mentioned here are the notations used in the paper:
90  // T1 - Triangle with vertices (t0v0, t0v1, t0v2).
91  // T2 - Triangle with vertices (t1v0, t1v1, t1v2).
92  // d1[3] - Signed distances of the vertices of T1 from the plane of T2.
93  // d2[3] - Signed distances of the vertices of T2 from the plane of T1.
94  // D - Separating axis used for the test. This is calculated as the cross products of the triangle normals.
95  // pv1[3] - Projection of the vertices of T1 onto the separating axis (D).
96  // pv2[3] - Projection of the vertices of T2 onto the separating axis (D).
97  // s1[2] - The intersection between T1 and D is a line segment.
98  // s1[0] and s1[1] are the parametric representation of the ends of this line segment.
99  // s2[2] - The intersection between T2 and D is a line segment.
100  // s2[0] and s2[1] are the parametric representation of the ends of this line segment.
101 
102  // Early Rejection test:
103  // If all the vertices of one triangle are on one side of the plane of the other triangle,
104  // there is no intersection.
105 
106  // Check if all the vertices of T2 are on one side of p1.
107  // Plane eqn of T1: DotProduct(t0n, X) + distanceFromOrigin = 0
108  // where distanceFromOrigin = -DotProduct(t0n, t0v0)
109  // So, plane eqn of T1: DotProduct(t0n, X - t0v0) = 0
110  // Distance of first vertex of T2 from the plane of T1 is: DotProduct(t0n, t1v0 - t0v0)
111  Vector3 d2(t0n.dot(t1v0 - t0v0), t0n.dot(t1v1 - t0v0), t0n.dot(t1v2 - t0v0));
112 
113  if ((d2.array() < DistanceEpsilon).all() || (d2.array() > -DistanceEpsilon).all())
114  {
115  return false;
116  }
117 
118  // Check if all the vertices of T1 are on one side of p2.
119  Vector3 d1(t1n.dot(t0v0 - t1v0), t1n.dot(t0v1 - t1v0), t1n.dot(t0v2 - t1v0));
120 
121  if ((d1.array() < DistanceEpsilon).all() || (d1.array() > -DistanceEpsilon).all())
122  {
123  return false;
124  }
125 
126  // The separating axis.
127  Vector3 D = t0n.cross(t1n).normalized();
128 
129  // Projection of the triangle vertices on the separating axis.
130  Vector3 pv1(D.dot(t0v0), D.dot(t0v1), D.dot(t0v2));
131  Vector3 pv2(D.dot(t1v0), D.dot(t1v1), D.dot(t1v2));
132 
133  // The intersection of the triangles with the separating axis (D).
134  T s1[3];
135  T s2[3];
136  size_t s1Index = 0;
137  size_t s2Index = 0;
138 
139  // Loop through the edges of each triangle and find the intersection of these edges onto
140  // the plane of the other triangle.
141  for (int i = 0; i < 3; ++i)
142  {
143  int j = (i + 1) % 3;
144 
145  edgeIntersection(d1[i], d1[j], pv1[i], pv1[j], s1, &s1Index);
146  edgeIntersection(d2[i], d2[j], pv2[i], pv2[j], s2, &s2Index);
147  }
148 
149  SURGSIM_ASSERT(s1Index == 2 && s2Index == 2)
150  << "The intersection between the triangle and the separating axis is not a line segment."
151  << " This scenario cannot happen, at this point in the algorithm.";
152 
153  // s1[0], s1[1] are the (unordered) extents of the projection of T1 on D.
154  // s2[0], s2[1] are the (unordered) extents of the projection of T2 on D.
155  // If both these are line segments (i.e. the distance between them is > epsilon),
156  // and if they overlap, then the two triangles intersect.
157 
158  return !(std::abs(s1[0] - s1[1]) <= DistanceEpsilon || std::abs(s2[0] - s2[1]) <= DistanceEpsilon) &&
159  !(s1[0] <= (s2[0] + DistanceEpsilon) && s1[0] <= (s2[1] + DistanceEpsilon) &&
160  s1[1] <= (s2[0] + DistanceEpsilon) && s1[1] <= (s2[1] + DistanceEpsilon)) &&
161  !(s1[0] >= (s2[0] - DistanceEpsilon) && s1[0] >= (s2[1] - DistanceEpsilon) &&
162  s1[1] >= (s2[0] - DistanceEpsilon) && s1[1] >= (s2[1] - DistanceEpsilon));
163 }
164 
165 
166 template <class T, int MOpt> inline
168  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
169  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
170  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
171  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
172  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
173  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2)
174 {
175  Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
176  Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
177  if (t0n.isZero() || t1n.isZero())
178  {
179  // Degenerate triangle(s) passed to checkTriangleTriangleIntersection.
180  return false;
181  }
182  t0n.normalize();
183  t1n.normalize();
184  return doesIntersectTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n);
185 }
186 
187 
188 } // namespace Math
189 
190 } // namespace SurgSim
191 
192 
193 #endif // SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::edgeIntersection
void edgeIntersection(T dStart, T dEnd, T pvStart, T pvEnd, T *parametricIntersection, size_t *parametricIntersectionIndex)
Two ends of the triangle edge are given in terms of the following vertex properties.
Definition: TriangleTriangleIntersection-inl.h:44
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
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