From 730b68787735cf246a23b18c1ed78332520b026c Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 29 May 2017 08:32:10 +0200 Subject: [PATCH] Working on temporal coherence in SAT (polyhedron vs polyhedron) --- CMakeLists.txt | 2 + src/collision/CollisionDetection.cpp | 13 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 14 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 48 +++ ...vexPolyhedronVsConvexPolyhedronAlgorithm.h | 71 ++++ .../narrowphase/DefaultCollisionDispatch.cpp | 9 + .../narrowphase/DefaultCollisionDispatch.h | 16 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 11 +- .../narrowphase/SAT/SATAlgorithm.cpp | 333 +++++++++++++----- src/collision/narrowphase/SAT/SATAlgorithm.h | 15 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 12 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- src/engine/OverlappingPair.cpp | 3 +- src/engine/OverlappingPair.h | 68 +++- 14 files changed, 502 insertions(+), 115 deletions(-) create mode 100644 src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp create mode 100644 src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 5cacac60..2e3be144 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -88,6 +88,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h" "src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp" + "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" + "src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp" "src/collision/shapes/AABB.h" "src/collision/shapes/AABB.cpp" "src/collision/shapes/ConvexShape.h" diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 908c5bf5..9ce02f9d 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -241,7 +241,7 @@ void CollisionDetection::computeNarrowPhase() { // If there is no collision algorithm between those two kinds of shapes, skip it if (narrowPhaseAlgorithm != nullptr) { - + // Use the narrow-phase collision detection algorithm to check // if there really is a collision. If a collision occurs, the // notifyContact() callback method will be called. @@ -268,7 +268,15 @@ void CollisionDetection::computeNarrowPhase() { // Trigger a callback event for the new contact if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo); + + currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; } + else { + currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false; + } + + // The previous frame collision info is now valid + currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; } currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; @@ -834,6 +842,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) { // Report the contact to the user callback->notifyContact(collisionInfo); } + + // The previous frame collision info is now valid + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; } NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 5e8dd5f9..142a7fc9 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -44,6 +44,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na SATAlgorithm satAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -104,9 +107,11 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na break; } - } + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + // Return true return true; } @@ -115,7 +120,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + + return isColliding; } return false; diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp new file mode 100644 index 00000000..16b9edf5 --- /dev/null +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -0,0 +1,48 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +// Libraries +#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" +#include "GJK/GJKAlgorithm.h" +#include "SAT/SATAlgorithm.h" + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +// Compute the narrow-phase collision detection between two convex polyhedra +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. +bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) { + + // Run the SAT algorithm to find the separating axis and compute contact point + SATAlgorithm satAlgorithm; + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + + return isColliding; +} diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h new file mode 100644 index 00000000..ed76d6fc --- /dev/null +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h @@ -0,0 +1,71 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 Daniel Chappuis * +********************************************************************************* +* * +* This software is provided 'as-is', without any express or implied warranty. * +* In no event will the authors be held liable for any damages arising from the * +* use of this software. * +* * +* Permission is granted to anyone to use this software for any purpose, * +* including commercial applications, and to alter it and redistribute it * +* freely, subject to the following restrictions: * +* * +* 1. The origin of this software must not be misrepresented; you must not claim * +* that you wrote the original software. If you use this software in a * +* product, an acknowledgment in the product documentation would be * +* appreciated but is not required. * +* * +* 2. Altered source versions must be plainly marked as such, and must not be * +* misrepresented as being the original software. * +* * +* 3. This notice may not be removed or altered from any source distribution. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H +#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H + +// Libraries +#include "body/Body.h" +#include "constraint/ContactPoint.h" +#include "NarrowPhaseAlgorithm.h" + + +/// Namespace ReactPhysics3D +namespace reactphysics3d { + +// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm +/** + * This class is used to compute the narrow-phase collision detection + * between two convex polyhedra. + */ +class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { + + protected : + + public : + + // -------------------- Methods -------------------- // + + /// Constructor + ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default; + + /// Destructor + virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default; + + /// Deleted copy-constructor + ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Deleted assignment operator + ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete; + + /// Compute the narrow-phase collision detection between two convex polyhedra + virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) override; +}; + +} + +#endif + diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.cpp b/src/collision/narrowphase/DefaultCollisionDispatch.cpp index 0a1e498c..df1828ad 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.cpp +++ b/src/collision/narrowphase/DefaultCollisionDispatch.cpp @@ -56,6 +56,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { return &mSphereVsConvexPolyhedronAlgorithm; } + // Capsule vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mCapsuleVsConvexPolyhedronAlgorithm; + } + // Convex Polyhedron vs Convex Polyhedron algorithm + if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON && + shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) { + return &mConvexPolyhedronVsConvexPolyhedronAlgorithm; + } return nullptr; } diff --git a/src/collision/narrowphase/DefaultCollisionDispatch.h b/src/collision/narrowphase/DefaultCollisionDispatch.h index 7c5d77ea..7b3945d3 100644 --- a/src/collision/narrowphase/DefaultCollisionDispatch.h +++ b/src/collision/narrowphase/DefaultCollisionDispatch.h @@ -33,6 +33,8 @@ #include "SphereVsConvexPolyhedronAlgorithm.h" #include "SphereVsCapsuleAlgorithm.h" #include "CapsuleVsCapsuleAlgorithm.h" +#include "CapsuleVsConvexPolyhedronAlgorithm.h" +#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h" #include "GJK/GJKAlgorithm.h" namespace reactphysics3d { @@ -50,14 +52,20 @@ class DefaultCollisionDispatch : public CollisionDispatch { /// Sphere vs Sphere collision algorithm SphereVsSphereAlgorithm mSphereVsSphereAlgorithm; - /// Sphere vs Convex Mesh collision algorithm - SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; + /// Capsule vs Capsule collision algorithm + CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; /// Sphere vs Capsule collision algorithm SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm; - /// Capsule vs Capsule collision algorithm - CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm; + /// Sphere vs Convex Polyhedron collision algorithm + SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm; + + /// Capsule vs Convex Polyhedron collision algorithm + CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm; + + /// Convex Polyhedron vs Convex Polyhedron collision algorithm + ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm; public: diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 6be90769..4f7e1177 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -93,7 +93,14 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro VoronoiSimplex simplex; // Get the previous point V (last cached separating axis) - Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis(); + Vector3 v; + LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) { + v = lastFrameInfo.gjkSeparatingAxis; + } + else { + v.setAllValues(0, 1, 0); + } // Initialize the upper bound for the square distance decimal distSquare = DECIMAL_LARGEST; @@ -113,7 +120,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v; // No intersection, we return return GJKResult::SEPARATED; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index c82256f3..6e85e303 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -29,6 +29,7 @@ #include "collision/PolyhedronMesh.h" #include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" +#include "engine/OverlappingPair.h" #include "configuration.h" #include "engine/Profiler.h" #include @@ -329,7 +330,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c } // Test collision between two convex polyhedrons -bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { +bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, + ContactManifoldInfo& contactManifoldInfo) const { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -344,94 +346,235 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP uint minFaceIndex = 0; bool isMinPenetrationFaceNormal = false; bool isMinPenetrationFaceNormalPolyhedron1 = false; + uint minSeparatingEdge1Index, minSeparatingEdge2Index; Vector3 separatingEdge1A, separatingEdge1B; Vector3 separatingEdge2A, separatingEdge2B; Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; - // Test all the face normals of the polyhedron 1 for separating axis - uint faceIndex; - decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); - if (penetrationDepth <= decimal(0.0)) { + LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // We have found a separating axis - return false; - } - if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { - isMinPenetrationFaceNormal = true; - minPenetrationDepth = penetrationDepth; - minFaceIndex = faceIndex; - isMinPenetrationFaceNormalPolyhedron1 = true; - } + // True if the shapes were overlapping in the previous frame and are + // still overlapping on the same axis in this frame + bool isTemporalCoherenceValid = false; - // Test all the face normals of the polyhedron 2 for separating axis - penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); - if (penetrationDepth <= decimal(0.0)) { + // If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous + // frame collision data per triangle) + if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) { - // We have found a separating axis - return false; - } - if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { - isMinPenetrationFaceNormal = true; - minPenetrationDepth = penetrationDepth; - minFaceIndex = faceIndex; - isMinPenetrationFaceNormalPolyhedron1 = false; - } + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { - // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis - for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + // We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating + // axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If + // the shapes are still separated along this axis, we directly exit with no collision. - // Get an edge of polyhedron 1 - HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); - - const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); - const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); - const Vector3 edge1Direction = edge1B - edge1A; - - for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { - - // Get an edge of polyhedron 2 - HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); - - const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); - const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); - const Vector3 edge2Direction = edge2B - edge2A; - - // If the two edges build a minkowski face (and the cross product is - // therefore a candidate for separating axis - if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { - - Vector3 separatingAxisPolyhedron2Space; - - // Compute the penetration depth - decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), - edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 1 + if (lastFrameInfo.satIsAxisFacePolyhedron1) { + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + lastFrameInfo.satMinAxisFaceIndex); + // If the previous axis is a separating axis if (penetrationDepth <= decimal(0.0)) { - // We have found a separating axis + // Return no collision return false; } - if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { minPenetrationDepth = penetrationDepth; - isMinPenetrationFaceNormalPolyhedron1 = false; - isMinPenetrationFaceNormal = false; - separatingEdge1A = edge1A; - separatingEdge1B = edge1B; - separatingEdge2A = edge2A; - separatingEdge2B = edge2B; - minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + } + else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 2 + + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, + lastFrameInfo.satMinAxisFaceIndex); + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; } + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameInfo.satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + } + else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges + + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index); + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index); + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + // If the previous axis is a separating axis + if (penetrationDepth <= decimal(0.0)) { + + // Return no collision + return false; + } + + // The two shapes are overlapping as in the previous frame and on the same axis, therefore + // we will skip the entire SAT algorithm because the minimum separating axis did not change + isTemporalCoherenceValid = lastFrameInfo.wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormal = false; + isMinPenetrationFaceNormalPolyhedron1 = false; + minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index; + minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + } } } } + // We the shapes are still overlapping in the same axis as in + // the previous frame, we skip the whole SAT algorithm + if (isTemporalCoherenceValid) { + + // Test all the face normals of the polyhedron 1 for separating axis + uint faceIndex; + decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = true; + lastFrameInfo.satIsAxisFacePolyhedron2 = false; + lastFrameInfo.satMinAxisFaceIndex = faceIndex; + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = true; + } + + // Test all the face normals of the polyhedron 2 for separating axis + penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satIsAxisFacePolyhedron2 = true; + lastFrameInfo.satMinAxisFaceIndex = faceIndex; + + // We have found a separating axis + return false; + } + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + isMinPenetrationFaceNormal = true; + minPenetrationDepth = penetrationDepth; + minFaceIndex = faceIndex; + isMinPenetrationFaceNormalPolyhedron1 = false; + } + + // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis + for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { + + // Get an edge of polyhedron 1 + HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i); + + const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); + const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); + const Vector3 edge1Direction = edge1B - edge1A; + + for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) { + + // Get an edge of polyhedron 2 + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j); + + const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); + const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); + const Vector3 edge2Direction = edge2B - edge2A; + + // If the two edges build a minkowski face (and the cross product is + // therefore a candidate for separating axis + if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + + Vector3 separatingAxisPolyhedron2Space; + + // Compute the penetration depth + decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), + edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); + + if (penetrationDepth <= decimal(0.0)) { + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satIsAxisFacePolyhedron2 = false; + lastFrameInfo.satMinEdge1Index = i; + lastFrameInfo.satMinEdge2Index = j; + + // We have found a separating axis + return false; + } + + if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormalPolyhedron1 = false; + isMinPenetrationFaceNormal = false; + minSeparatingEdge1Index = i; + minSeparatingEdge2Index = j; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; + } + + } + } + } + } + + // Here we know the shapes are overlapping on a given minimum separating axis. + // Now, we will clip the shapes along this axis to find the contact points + assert(minPenetrationDepth > decimal(0.0)); assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal); - // If the separation axis is a face normal + // If the minimum separating axis is a face normal if (isMinPenetrationFaceNormal) { const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2; @@ -439,6 +582,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1; const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2; + assert(minPenetrationDepth > decimal(0.0)); + const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex); const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace; @@ -491,8 +636,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP } while (currentEdgeIndex != firstEdgeIndex); + assert(planesNormals.size() > 0); + assert(planesNormals.size() == planesPoints.size()); + // Clip the reference faces with the adjacent planes of the reference face std::vector clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals); + assert(clipPolygonVertices.size() > 0); // We only keep the clipped points that are below the reference face const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex); @@ -514,6 +663,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron); } } + + lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; } else { // If we have an edge vs edge contact @@ -531,6 +684,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP // Create the contact point contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); + + lastFrameInfo.satIsAxisFacePolyhedron1 = false; + lastFrameInfo.satIsAxisFacePolyhedron2 = false; + lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index; + lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index; } return true; @@ -583,34 +741,47 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V return -axis.dot(edge2A - edge1A); } +// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron +decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint faceIndex) const { + + HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex); + + // Get the face normal + const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex); + + // Convert the face normal into the local-space of polyhedron 2 + const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; + + // Get the support point of polyhedron 2 in the inverse direction of face normal + const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); + + // Compute the penetration depth + const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); + decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + + return penetrationDepth; +} + // Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case -decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, - const ConvexPolyhedronShape* polyhedron2, - const Transform& polyhedron1ToPolyhedron2, - uint& minFaceIndex) const { +decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint& minFaceIndex) const { decimal minPenetrationDepth = DECIMAL_LARGEST; // For each face of the first polyhedron for (uint f = 0; f < polyhedron1->getNbFaces(); f++) { - HalfEdgeStructure::Face face = polyhedron1->getFace(f); - - // Get the face normal - const Vector3 faceNormal = polyhedron1->getFaceNormal(f); - - // Convert the face normal into the local-space of polyhedron 2 - const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal; - - // Get the support point of polyhedron 2 in the inverse direction of face normal - const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr); - - // Compute the penetration depth - const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]); - decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space); + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, f); // If the penetration depth is negative, we have found a separating axis if (penetrationDepth <= decimal(0.0)) { + minFaceIndex = f; return penetrationDepth; } diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index a95abe31..3512ef4d 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -66,6 +66,17 @@ class SATAlgorithm { const Vector3& edge1Direction, const Vector3& edge2Direction, Vector3& outSeparatingAxis) const; + /// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron + decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, + const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, + uint faceIndex) const; + + + /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case + decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, + const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; + public : // -------------------- Methods -------------------- // @@ -101,10 +112,6 @@ class SATAlgorithm { /// Test collision between two convex meshes bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; - - /// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case - decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2, - const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const; }; } diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 708d9f8d..bb43bd33 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -31,7 +31,7 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; -// Compute the narrow-phase collision detection a sphere and a convex polyhedron +// Compute the narrow-phase collision detection between a sphere and a convex polyhedron // This technique is based on the "Robust Contact Creation for Physics Simulations" presentation // by Dirk Gregorius. bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, @@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -53,7 +56,12 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar // Run the SAT algorithm to find the separating axis and compute contact point SATAlgorithm satAlgorithm; - return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); + + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + + return isColliding; } return false; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index c1fd3e55..f2f8d6e5 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h @@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete; - /// Compute the narrow-phase collision detection a sphere and a convex polyhedron + /// Compute the narrow-phase collision detection between a sphere and a convex polyhedron virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 1f5d894e..ce628b7d 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -32,7 +32,6 @@ using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int nbMaxContactManifolds, PoolAllocator& memoryAllocator) - : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), - mCachedSeparatingAxis(0.0, 1.0, 0.0) { + : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) { } diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index c34b91b7..ce028f9c 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -37,6 +37,51 @@ namespace reactphysics3d { // Type for the overlapping pair ID using overlappingpairid = std::pair; +// Structure LastFrameCollisionInfo +/** + * This structure contains collision info about the last frame. + * This is used for temporal coherence between frames. + */ +struct LastFrameCollisionInfo { + + /// True if we have information about the previous frame + bool isValid; + + /// True if the two shapes were colliding in the previous frame + bool wasColliding; + + /// True if we were using GJK algorithm to check for collision in the previous frame + bool wasUsingGJK; + + /// True if we were using SAT algorithm to check for collision in the previous frame + bool wasUsingSAT; + + /// True if there was a narrow-phase collision + /// in the previous frame + bool wasCollidingLastFrame; + + // ----- GJK Algorithm ----- + + /// Previous separating axis + Vector3 gjkSeparatingAxis; + + // SAT Algorithm + bool satIsAxisFacePolyhedron1; + bool satIsAxisFacePolyhedron2; + uint satMinAxisFaceIndex; + uint satMinEdge1Index; + uint satMinEdge2Index; + + /// Constructor + LastFrameCollisionInfo() { + + isValid = false; + wasColliding = false; + wasUsingSAT = false; + wasUsingGJK = false; + } +}; + // Class OverlappingPair /** * This class represents a pair of two proxy collision shapes that are overlapping @@ -54,8 +99,8 @@ class OverlappingPair { /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Cached previous separating axis - Vector3 mCachedSeparatingAxis; + /// Collision information about the last frame (for temporal coherence) + LastFrameCollisionInfo mLastFrameCollisionInfo; public: @@ -83,11 +128,8 @@ class OverlappingPair { /// Add a contact manifold void addContactManifold(const ContactManifoldInfo& contactManifoldInfo); - /// Return the cached separating axis - Vector3 getCachedSeparatingAxis() const; - - /// Set the cached separating axis - void setCachedSeparatingAxis(const Vector3& axis); + /// Return the last frame collision info + LastFrameCollisionInfo& getLastFrameCollisionInfo(); /// Return the number of contacts in the cache uint getNbContactPoints() const; @@ -124,17 +166,11 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& conta mContactManifoldSet.addContactManifold(contactManifoldInfo); } -// Return the cached separating axis -inline Vector3 OverlappingPair::getCachedSeparatingAxis() const { - return mCachedSeparatingAxis; +// Return the last frame collision info +inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() { + return mLastFrameCollisionInfo; } -// Set the cached separating axis -inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) { - mCachedSeparatingAxis = axis; -} - - // Return the number of contact points in the contact manifold inline uint OverlappingPair::getNbContactPoints() const { return mContactManifoldSet.getTotalNbContactPoints();