16 #ifndef SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
33 template <
class T,
int MOpt>
37 typedef Eigen::Matrix<T, 3, 1, MOpt>
Vector3;
38 typedef boost::container::static_vector<Vector3, CAPACITY>
Vertices;
44 TriangleHelper(
const Vector3& v0,
const Vector3& v1,
const Vector3& v2,
const Vector3& n)
59 Vector3* penetrationPoint0, Vector3* penetrationPoint1)
66 Vector3 clipPlaneNormal;
69 for (
size_t i = 0; i < 3; ++i)
78 <<
"The distance from triangle is calculated as " << *penetrationDepth <<
". At this point in the"
79 <<
" algorithm, the depth is expected to be negative.";
81 *penetrationPoint1 = *penetrationPoint0 - (triangle.
m_normal * (*penetrationDepth));
82 *penetrationDepth = -(*penetrationDepth);
94 *planeNormal = planeNormal->cross(
m_normal);
95 planeNormal->normalize();
96 *planeD = -
m_vertices[index]->dot(*planeNormal);
123 clippedVertices.clear();
125 static const T EPSILON = T(Geometry::DistanceEpsilon);
128 boost::container::static_vector<T, CAPACITY> signedDistanceFromPlane;
129 for (
auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
131 signedDistanceFromPlane.push_back((*it).dot(planeN) + planeD);
138 typename boost::container::static_vector<Vector3, CAPACITY>::iterator end;
141 auto startSignedDistance = signedDistanceFromPlane.begin();
142 typename boost::container::static_vector<T, CAPACITY>::iterator endSignedDistance;
145 for (
auto start = originalVertices.begin(); start != originalVertices.end(); ++start, ++startSignedDistance)
149 endSignedDistance = startSignedDistance + 1;
150 if (end == originalVertices.end())
152 end = originalVertices.begin();
153 endSignedDistance = signedDistanceFromPlane.begin();
157 if (*startSignedDistance <= EPSILON)
159 clippedVertices.push_back(*start);
163 if ((*startSignedDistance < -EPSILON && *endSignedDistance > EPSILON) ||
164 (*startSignedDistance > EPSILON && *endSignedDistance < -EPSILON))
166 ratio = *startSignedDistance / (*startSignedDistance - *endSignedDistance);
167 clippedVertices.push_back(*start + (*end - *start) * ratio);
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.";
187 T signedDistanceFromPlane;
189 for (
auto it = originalVertices.cbegin(); it != originalVertices.cend(); ++it)
191 signedDistanceFromPlane = (*it).dot(planeN) + planeD;
192 if (signedDistanceFromPlane < *depth)
194 *depth = signedDistanceFromPlane;
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,
226 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
227 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
228 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
230 typedef Eigen::Matrix<T, 3, 1, MOpt> Vector3;
245 T penetrationDepths[2] = {T(0), T(0)};
246 Vector3 penetrationPoints[2][2];
250 triangle2, &penetrationDepths[0], &penetrationPoints[0][0], &penetrationPoints[0][1]);
253 triangle1, &penetrationDepths[1], &penetrationPoints[1][1], &penetrationPoints[1][0]);
256 if (penetrationDepths[0] < penetrationDepths[1])
258 *penetrationDepth = penetrationDepths[0];
259 *contactNormal = t1n;
260 *penetrationPoint0 = penetrationPoints[0][0];
261 *penetrationPoint1 = penetrationPoints[0][1];
265 *penetrationDepth = penetrationDepths[1];
266 *contactNormal = -t0n;
267 *penetrationPoint0 = penetrationPoints[1][0];
268 *penetrationPoint1 = penetrationPoints[1][1];
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,
284 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
285 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
286 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal)
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())
298 penetrationPoint0, penetrationPoint1, contactNormal);
306 #endif // SURGSIM_MATH_TRIANGLETRIANGLECONTACTCALCULATION_INL_H
Definition: DriveElementFromInputBehavior.cpp:27
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:70
const Vector3 * m_vertices[3]
Original vertices of the triangle.
Definition: TriangleTriangleContactCalculation-inl.h:201