From 8b9fdc15a71501ef1990d614ec7a847d809cf98b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sat, 27 Jun 2020 23:48:19 +0200 Subject: [PATCH 1/2] =?UTF-8?q?Fix=20issue=20in=20SAT=20with=20edge-edge?= =?UTF-8?q?=20contact=20(wrong=20contact=20normal)=20and=20favor=20face=20?= =?UTF-8?q?contacts=20over=20edge-edge=20contacts=20in=20polyhedron=20vs?= =?UTF-8?q?=20polyhedron=20collision=20in=20SAT=C2=A0algorithm=20for=20bet?= =?UTF-8?q?ter=20stability?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../narrowphase/SAT/SATAlgorithm.cpp | 35 ++++++------------- 1 file changed, 11 insertions(+), 24 deletions(-) diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 54f8e90a..4271b900 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -657,18 +657,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - // We use the direction from the centroid to the edge of the shape that is not a triangle - // to avoid possible degeneracies when axis direction is orthogonal to triangle normal - Vector3 normal; - if (isShape1Triangle) { - normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge; - } - else { - normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid()); - } - - //Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; - Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normal.getUnit(); + Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * separatingAxisPolyhedron2Space; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], @@ -794,7 +783,15 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn break; } - if (penetrationDepth < minPenetrationDepth) { + // If the current minimum penetration depth is along a face normal axis (isMinPenetrationFaceNormal=true) and we have found a new + // smaller pentration depth along an edge-edge cross-product axis we want to favor the face normal axis because contact manifolds between + // faces have more contact points and therefore more stable than the single contact point of an edge-edge collision. It means that if the new minimum + // penetration depth from the edge-edge contact is only a little bit smaller than the current minPenetrationDepth (from a face contact), we favor + // the face contact and do not generate an edge-edge contact. However, if the new penetration depth from the edge-edge contact is really smaller than + // the current one, we generate an edge-edge contact. + // To do this, we use a relative and absolute bias to increase a little bit the new penetration depth from the edge-edge contact during the comparison test + if ((isMinPenetrationFaceNormal && penetrationDepth1 * SEPARATING_AXIS_RELATIVE_TOLERANCE + SEPARATING_AXIS_ABSOLUTE_TOLERANCE < minPenetrationDepth) || + (!isMinPenetrationFaceNormal && penetrationDepth < minPenetrationDepth)) { minPenetrationDepth = penetrationDepth; isMinPenetrationFaceNormalPolyhedron1 = false; @@ -862,17 +859,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; // Compute the world normal - // We use the direction from the centroid to the edge of the shape that is not a triangle - // to avoid possible degeneracies when axis direction is orthogonal to triangle normal - Vector3 normal; - if (isShape1Triangle) { - normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge; - } - else { - normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid()); - } - //Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; - Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * normal.getUnit(); + Vector3 normalWorld = narrowPhaseInfoBatch.shape2ToWorldTransforms[batchIndex].getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.collisionShapes1[batchIndex], narrowPhaseInfoBatch.collisionShapes2[batchIndex], From fa05e1561eea366a47e601663ea0af07f031aee1 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 7 Jul 2020 18:27:01 +0200 Subject: [PATCH 2/2] Use const parameter in PhysicsWorld::setGravity() method --- include/reactphysics3d/engine/PhysicsWorld.h | 2 +- src/engine/PhysicsWorld.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/reactphysics3d/engine/PhysicsWorld.h b/include/reactphysics3d/engine/PhysicsWorld.h index 9aa3da2c..8ebf4ab1 100644 --- a/include/reactphysics3d/engine/PhysicsWorld.h +++ b/include/reactphysics3d/engine/PhysicsWorld.h @@ -399,7 +399,7 @@ class PhysicsWorld { Vector3 getGravity() const; /// Set the gravity vector of the world - void setGravity(Vector3& gravity); + void setGravity(const Vector3& gravity); /// Return if the gravity is on bool isGravityEnabled() const; diff --git a/src/engine/PhysicsWorld.cpp b/src/engine/PhysicsWorld.cpp index b188f635..19e55eab 100644 --- a/src/engine/PhysicsWorld.cpp +++ b/src/engine/PhysicsWorld.cpp @@ -975,7 +975,7 @@ void PhysicsWorld::setNbIterationsPositionSolver(uint nbIterations) { /** * @param gravity The gravity vector (in meter per seconds squared) */ -void PhysicsWorld::setGravity(Vector3& gravity) { +void PhysicsWorld::setGravity(const Vector3& gravity) { mConfig.gravity = gravity;