Go to the documentation of this file.
16 #ifndef SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
17 #define SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
40 template <
class T,
int MOpt>
42 const Eigen::Matrix<T, 3, 1, MOpt>& majorAxis,
43 const Eigen::Matrix<T, 3, 1, MOpt>& minorAxis,
44 const double majorRadius,
const double minorRadius,
45 const Eigen::Matrix<T, 3, 1, MOpt>& tangent)
47 Eigen::Transform<T, 3, Eigen::Isometry> transform;
48 transform.translation() = center;
49 transform.linear().col(0) = majorAxis;
50 transform.linear().col(1) = minorAxis;
51 transform.linear().col(2) = majorAxis.cross(minorAxis);
54 Eigen::Matrix<T, 3, 1, MOpt> localTangent = transform.inverse().linear() * tangent;
57 T m = localTangent[1] / localTangent[0];
63 T x = std::sqrt((m * m * majorRadius * majorRadius * majorRadius * majorRadius) /
64 (minorRadius * minorRadius + m * m * majorRadius * majorRadius));
65 T y = (minorRadius / majorRadius) *
66 std::sqrt(majorRadius * majorRadius - x * x) * ((m > 0.0) ? -1.0 : 1.0);
69 return transform * Eigen::Matrix<T, 3, 1, MOpt>(x, y,
static_cast<T
>(0));
75 template <
class T,
int MOpt>
78 typedef Eigen::Matrix<T, 3, 1, MOpt>
Vector3;
79 typedef Eigen::Matrix<T, 2, 1, MOpt>
Vector2;
96 m_epsilon =
static_cast<T
>(Geometry::DistanceEpsilon);
133 SURGSIM_ASSERT(result) <<
"At this point, there has to be an intersection.";
229 double planeD[4] = {-
m_tv0.dot(planeN[0]), -
m_tv0.dot(planeN[1]), -
m_tv1.dot(planeN[2]), -
m_tv2.dot(planeN[3])};
245 Vector3 deepestPoint = ((segmentStart - segmentEnd).dot(planeN[0]) < 0.0) ? segmentStart : segmentEnd;
246 Vector3 edgeVertices[2] = {v[(j - 1) % 3], v[j % 3]};
247 Vector3 triangleEdge = (edgeVertices[1] - edgeVertices[0]).normalized();
257 if (std::abs(majorAxis.dot(triangleEdge)) >
m_epsilon)
262 auto minorAxis = planeN[j].cross(majorAxis);
276 Vector3 origin = edgeVertices[0], xAxis = triangleEdge, yAxis =
m_tn, zAxis = planeN[j];
278 double sphereCenterToXYPlane = (
m_cvBottom - origin).dot(zAxis);
280 <<
"The sphere center is too far from the triangle edge plane.";
282 double circleRadius = std::sqrt(
m_cr *
m_cr - sphereCenterToXYPlane * sphereCenterToXYPlane);
284 <<
"The radius of the circle of intersection between the sphere and the plane is invalid.";
286 double x = (circleCenter - origin).dot(xAxis);
287 double y = (circleCenter - origin).dot(yAxis) - circleRadius;
288 deepestPoint = xAxis * x + yAxis * y + origin;
293 auto edgeLength = (edgeVertices[1] - edgeVertices[0]).norm();
294 double deepestPointDotEdge = triangleEdge.dot(deepestPoint - edgeVertices[0]);
295 if (deepestPointDotEdge <= -m_epsilon || deepestPointDotEdge >= edgeLength +
m_epsilon)
305 Vector3 edgeVertex = (deepestPointDotEdge < 0.0) ? edgeVertices[0] : edgeVertices[1];
308 SURGSIM_ASSERT(
isValid(d)) <<
"There must be a part of the ellipse between the triangle edge at this point";
347 T a = D[1] * D[1] + D[2] * D[2];
348 T b =
static_cast<T
>(2) * (P[1] * D[1] + P[2] * D[2]);
349 T c = (P[1] * P[1] + P[2] * P[2] -
m_cr *
m_cr);
351 T discriminant = b * b -
static_cast<T
>(4) * a * c;
353 if (discriminant < 0.0)
356 if (discriminant >= -Geometry::ScalarEpsilon)
362 return std::numeric_limits<T>::quiet_NaN();
367 double d = (-b / (
static_cast<T
>(2) * a)) + std::abs(std::sqrt(discriminant) / (
static_cast<T
>(2) * a));
369 if (point !=
nullptr)
371 *point = lineStart + lineDir * d;
398 *point = lineStart + lineDir * d;
419 T a = D[0] * D[0] + D[1] * D[1] + D[2] * D[2];
420 T b =
static_cast<T
>(2) * (P[0] * D[0] + P[1] * D[1] + P[2] * D[2] - D[0] * x);
421 T c = (P[0] * P[0] + P[1] * P[1] + P[2] * P[2] + x * x -
static_cast<T
>(2) * P[0] * x -
m_cr *
m_cr);
425 T discriminant = b * b -
static_cast<T
>(4) * a * c;
427 if (discriminant < 0.0 && discriminant >= -Geometry::ScalarEpsilon)
433 d = (-b - std::abs(std::sqrt(discriminant))) / (
static_cast<T
>(2) * a);
435 *point = lineStart + lineDir * d;
451 double ratio, dStart, dEnd;
453 for (
size_t i = 0; i < 4; ++i)
455 dStart = segmentStart->dot(planeN[i]) + planeD[i];
456 dEnd = segmentEnd->dot(planeN[i]) + planeD[i];
458 if (dStart < -m_epsilon && dEnd >
m_epsilon)
460 ratio = std::abs(dStart) / (std::abs(dStart) + dEnd);
461 *segmentEnd = *segmentStart + (*segmentEnd - *segmentStart) * ratio;
466 ratio = dStart / (dStart + std::abs(dEnd));
467 *segmentStart = *segmentStart + (*segmentEnd - *segmentStart) * ratio;
470 else if (dStart < m_epsilon && dEnd >
m_epsilon)
472 *segmentEnd = *segmentStart;
477 *segmentStart = *segmentEnd;
482 SURGSIM_ASSERT(j < 4) <<
"The clipping should have happened at least with the triangle plane.";
520 template <
class T,
int MOpt>
inline
522 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
523 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
524 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
525 const Eigen::Matrix<T, 3, 1, MOpt>& tn,
526 const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
527 const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
530 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
531 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
532 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
533 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis)
536 calc(tv0, tv1, tv2, tn, cv0, cv1, cr);
540 calc.
calculateContact(penetrationDepth, penetrationPointTriangle, penetrationPointCapsule,
541 contactNormal, penetrationPointCapsuleAxis);
549 template <
class T,
int MOpt>
inline
551 const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
552 const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
553 const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
554 const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
555 const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
558 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
559 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
560 Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
561 Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis)
563 Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
564 Eigen::Matrix<T, 3, 1, MOpt> ca = cv1 - cv0;
565 if (tn.isZero() || ca.isZero())
572 penetrationPoint0, penetrationPoint1, contactNormal, penetrationPointCapsuleAxis);
579 #endif // SURGSIM_MATH_TRIANGLECAPSULECONTACTCALCULATION_INL_H
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
bool buildOrthonormalBasis(Eigen::Matrix< T, 3, 1, VOpt > *i, Eigen::Matrix< T, 3, 1, VOpt > *j, Eigen::Matrix< T, 3, 1, VOpt > *k)
Helper method to construct an orthonormal basis (i, j, k) given the 1st vector direction.
Definition: Vector.h:178
T distancePointSegment(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, 3, 1, MOpt > *result)
Point segment distance, if the projection of the closest point is not within the segments,...
Definition: Geometry.h:297
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
T distanceSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *segmentPoint, Eigen::Matrix< T, 3, 1, MOpt > *trianglePoint)
Calculate the distance of a line segment to a triangle.
Definition: Geometry.h:1280
Definition: CompoundShapeToGraphics.cpp:30
bool isValid(float value)
Check if a float value is valid.
Definition: Valid-inl.h:98
bool hasValue() const
Query if this object has been assigned a value.
Definition: OptionalValue.h:56
Declarations of isValid(), isSubnormal() and setSubnormalToZero().
bool isPointOnTriangleEdge(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 on the edge of a triangle.
Definition: Geometry.h:206
const T & getValue() const
Gets the value.
Definition: OptionalValue.h:78
Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
A dynamic size column vector.
Definition: Vector.h:68
bool calculateContactTriangleCapsule(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, const Eigen::Matrix< T, 3, 1, MOpt > &cv0, const Eigen::Matrix< T, 3, 1, MOpt > &cv1, double cr, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsule, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsuleAxis)
Calculate the contact between a capsule and a triangle.
Definition: TriangleCapsuleContactCalculation-inl.h:521