All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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 
20 namespace
21 {
22 static const double EPSILOND = 1e-12;
23 }
24 
25 namespace SurgSim
26 {
27 
28 namespace Math
29 {
30 
31 
44 template<class T>
45 void edgeIntersection(T dStart, T dEnd, T pvStart, T pvEnd, T* parametricIntersection,
46  size_t* parametricIntersectionIndex)
47 {
48  // Epsilon used in this function.
49  static const T EPSILON = static_cast<T>(EPSILOND);
50 
51  bool edgeFromUnderToAbove = dStart < 0.0 && dEnd >= 0.0;
52  bool edgeFromAboveToUnder = dStart > 0.0 && dEnd <= 0.0;
53 
54  if (edgeFromUnderToAbove || edgeFromAboveToUnder)
55  {
56  if (std::abs(dStart - dEnd) < EPSILON)
57  {
58  // Start and End are really close. Pick start.
59  parametricIntersection[(*parametricIntersectionIndex)++] = pvStart;
60  }
61  else
62  {
63  // Clip to the point in the intersection of Start->End and plane of the colliding triangle.
64  parametricIntersection[(*parametricIntersectionIndex)++] =
65  pvStart + (pvEnd - pvStart) * (dStart / (dStart - dEnd));
66  }
67  }
68 }
69 
70 template <class T, int MOpt> inline
72  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
73  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
74  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
75  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
76  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
77  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
78  const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
79  const Eigen::Matrix<T, 3, 1, MOpt>& t1n)
80 {
81  typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
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() <= 0.0).all() || (d2.array() >= 0.0).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() <= 0.0).all() || (d1.array() >= 0.0).all())
122  {
123  return false;
124  }
125 
126  // The separating axis.
127  Vector3 D = t0n.cross(t1n);
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 intersectio 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  return !(s1[0] <= s2[0] && s1[0] <= s2[1] && s1[1] <= s2[0] && s1[1] <= s2[1]) &&
154  !(s1[0] >= s2[0] && s1[0] >= s2[1] && s1[1] >= s2[0] && s1[1] >= s2[1]);
155 }
156 
157 
158 template <class T, int MOpt> inline
160  const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
161  const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
162  const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
163  const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
164  const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
165  const Eigen::Matrix<T, 3, 1, MOpt>& t1v2)
166 {
167  Eigen::Matrix<T, 3, 1, MOpt> t0n = (t0v1 - t0v0).cross(t0v2 - t0v0);
168  Eigen::Matrix<T, 3, 1, MOpt> t1n = (t1v1 - t1v0).cross(t1v2 - t1v0);
169  if (t0n.isZero() || t1n.isZero())
170  {
171  // Degenerate triangle(s) passed to checkTriangleTriangleIntersection.
172  return false;
173  }
174  t0n.normalize();
175  t1n.normalize();
176  return doesIntersectTriangleTriangle(t0v0, t0v1, t0v2, t1v0, t1v1, t1v2, t0n, t1n);
177 }
178 
179 
180 } // namespace Math
181 
182 } // namespace SurgSim
183 
184 
185 #endif // SURGSIM_MATH_TRIANGLETRIANGLEINTERSECTION_INL_H
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
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
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:45