From cdec7413c516ffe2671d0cbde45f5b60ef1d68e5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 18 Oct 2017 21:26:53 +0200 Subject: [PATCH] Improve capsule resting on another shape stability --- .../narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp | 5 +++-- src/mathematics/mathematics_functions.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 72ba2e06..d6de739a 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -86,14 +86,15 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); - const Vector3 capsuleInnerSegmentWorld = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA); + capsuleInnerSegmentDirection.normalize(); bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0); bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal); // If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal // is in direction of the contact normal (from the polyhedron point of view). - if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentWorld) + if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection) && areParallelVectors(faceNormalWorld, contactPoint->normal)) { // Remove the previous contact point computed by GJK diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index fd813cdc..37335abe 100755 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -80,7 +80,7 @@ bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& v // Return true if two vectors are orthogonal bool reactphysics3d::areOrthogonalVectors(const Vector3& vector1, const Vector3& vector2) { - return std::abs(vector1.dot(vector2)) < decimal(0.00001); + return std::abs(vector1.dot(vector2)) < decimal(0.001); } // Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC"