From 5da57a96c80be764d77e38478c33b64de2d9bc34 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 24 Oct 2017 22:47:35 +0200 Subject: [PATCH] Fix issue with sphere and capsule SAT collision detection --- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 4 ++++ src/collision/narrowphase/SAT/SATAlgorithm.cpp | 15 ++++++++------- 2 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index d6de739a..89336c4e 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -114,6 +114,10 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + if (isCapsuleShape1) { + faceNormalWorld = -faceNormalWorld; + } + // Compute and create two contact points satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 4411187b..1ba5143b 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -97,10 +97,12 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow if (reportContacts) { const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); - Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); - Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); + Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal; + Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius()); Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); + Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld; + // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, @@ -243,6 +245,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; + if (isCapsuleShape1) { + normalWorld = -normalWorld; + } const decimal capsuleRadius = capsuleShape->getRadius(); // If the separating axis is a face normal @@ -391,12 +396,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI // Project the two clipped points into the polyhedron face const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); - if (isCapsuleShape1) { - normalWorld = -normalWorld; - } - // For each of the two clipped points - for (int i = 0; i<2; i++) { + for (uint i = 0; i<2; i++) { // Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth) const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);