From 643c781fa09b050bbe85e71f57b14353bc421470 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 21 Sep 2017 22:44:42 +0200 Subject: [PATCH] Fix issues with smooth triangle contact --- .../narrowphase/SAT/SATAlgorithm.cpp | 6 ++-- src/collision/shapes/TriangleShape.cpp | 34 +++++++++---------- src/collision/shapes/TriangleShape.h | 2 +- 3 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 2f345738..0d2ae20e 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -857,6 +857,8 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene contactPointsFound = true; + Vector3 outWorldNormal = normalWorld; + // Convert the clip incident polyhedron vertex into the incident polyhedron local-space Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints); @@ -868,10 +870,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron, narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, - minPenetrationDepth, normalWorld); + minPenetrationDepth, outWorldNormal); // Create a new contact point - narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, + narrowPhaseInfo->addContactPoint(outWorldNormal, minPenetrationDepth, isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron, isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 254bc5a1..263595b3 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -85,14 +85,9 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2, isShape1Triangle ? shape1ToWorld : shape2ToWorld, isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(), - penetrationDepth, + penetrationDepth, isShape1Triangle, isShape1Triangle ? localContactPointShape2 : localContactPointShape1, outSmoothVertexNormal); - - // Make sure the direction of the contact normal is from shape1 to shape2 - if (!isShape1Triangle) { - outSmoothVertexNormal = -outSmoothVertexNormal; - } } } @@ -104,25 +99,30 @@ void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape // stay aligned with the new contact normal. This method will return the new smooth world contact // normal of the triangle and the the local contact point on the other shape. void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, - const Transform& worldToOtherShapeTransform, decimal penetrationDepth, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1, Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const { // Get the smooth contact normal of the mesh at the contact point on the triangle - Vector3 localNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); + Vector3 triangleLocalNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle); // Convert the local contact normal into world-space - outSmoothWorldContactTriangleNormal = triangleShapeToWorldTransform.getOrientation() * localNormal; + Vector3 triangleWorldNormal = triangleShapeToWorldTransform.getOrientation() * triangleLocalNormal; - // Convert the contact normal into the local-space of the other shape - Vector3 normalOtherShape = worldToOtherShapeTransform.getOrientation() * outSmoothWorldContactTriangleNormal; + // Penetration axis with direction from triangle to other shape + Vector3 triangleToOtherShapePenAxis = isTriangleShape1 ? outSmoothWorldContactTriangleNormal : -outSmoothWorldContactTriangleNormal; - // Convert the local contact point of the triangle into the local-space of the other shape - Vector3 trianglePointOtherShape = worldToOtherShapeTransform * triangleShapeToWorldTransform * - localContactPointTriangle; + // The triangle normal should be the one in the direction out of the current colliding face of the triangle + if (triangleWorldNormal.dot(triangleToOtherShapePenAxis) < decimal(0.0)) { + triangleWorldNormal = -triangleWorldNormal; + triangleLocalNormal = -triangleLocalNormal; + } - // Re-align the local contact point on the other shape such that it is aligned along - // the new contact normal - Vector3 otherShapePoint = trianglePointOtherShape - normalOtherShape * penetrationDepth; + // Compute the final contact normal from shape 1 to shape 2 + outSmoothWorldContactTriangleNormal = isTriangleShape1 ? triangleWorldNormal : -triangleWorldNormal; + + // Re-align the local contact point on the other shape such that it is aligned along the new contact normal + Vector3 otherShapePointTriangleSpace = localContactPointTriangle - triangleLocalNormal * penetrationDepth; + Vector3 otherShapePoint = worldToOtherShapeTransform * triangleShapeToWorldTransform * otherShapePointTriangleSpace; outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z); } diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index c31a594c..8360c32b 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -99,7 +99,7 @@ class TriangleShape : public ConvexPolyhedronShape { /// This method implements the technique described in Game Physics Pearl book void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform, - const Transform& worldToOtherShapeTransform, decimal penetrationDepth, + const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1, Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const; public: