From f403a6e8040832a222ff17ad648889b802aacef5 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 22 Nov 2017 22:43:27 +0100 Subject: [PATCH] Add temporal coherence for convex vs triangle collision detection --- src/collision/CollisionCallback.h | 2 + src/collision/CollisionDetection.cpp | 18 +- src/collision/ContactManifold.h | 1 + src/collision/ContactManifoldInfo.cpp | 3 + src/collision/MiddlePhaseTriangleCallback.cpp | 6 +- src/collision/MiddlePhaseTriangleCallback.h | 3 +- src/collision/NarrowPhaseInfo.cpp | 7 +- src/collision/NarrowPhaseInfo.h | 17 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 15 +- ...xPolyhedronVsConvexPolyhedronAlgorithm.cpp | 7 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 10 +- .../narrowphase/SAT/SATAlgorithm.cpp | 300 +++++++++--------- .../SphereVsConvexPolyhedronAlgorithm.cpp | 11 +- src/collision/shapes/CollisionShape.cpp | 2 +- src/collision/shapes/CollisionShape.h | 11 + src/collision/shapes/ConcaveMeshShape.cpp | 21 +- src/collision/shapes/ConcaveMeshShape.h | 5 +- src/collision/shapes/ConcaveShape.h | 3 +- src/collision/shapes/HeightFieldShape.cpp | 10 +- src/collision/shapes/HeightFieldShape.h | 12 +- src/collision/shapes/SphereShape.cpp | 3 +- src/collision/shapes/TriangleShape.cpp | 15 +- src/collision/shapes/TriangleShape.h | 28 +- src/constraint/ContactPoint.h | 3 +- src/engine/CollisionWorld.h | 2 +- src/engine/OverlappingPair.cpp | 73 ++++- src/engine/OverlappingPair.h | 50 ++- test/tests/collision/TestRaycast.h | 9 +- 28 files changed, 400 insertions(+), 247 deletions(-) diff --git a/src/collision/CollisionCallback.h b/src/collision/CollisionCallback.h index f860b42d..54208fe4 100644 --- a/src/collision/CollisionCallback.h +++ b/src/collision/CollisionCallback.h @@ -32,6 +32,8 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +class OverlappingPair; + // Class CollisionCallback /** * This class can be used to register a callback for collision test queries. diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index 7cb865f9..4808ea65 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -113,6 +113,9 @@ void CollisionDetection::computeMiddlePhase() { // Make all the contact manifolds and contact points of the pair obsolete pair->makeContactsObsolete(); + // Make all the last frame collision info obsolete + pair->makeLastFrameCollisionInfosObsolete(); + ProxyShape* shape1 = pair->getShape1(); ProxyShape* shape2 = pair->getShape2(); @@ -192,6 +195,9 @@ void CollisionDetection::computeMiddlePhase() { // Not handled continue; } + + // Remove the obsolete last frame collision infos + pair->clearObsoleteLastFrameCollisionInfos(); } } } @@ -263,6 +269,8 @@ void CollisionDetection::computeNarrowPhase() { // If there is no collision algorithm between those two kinds of shapes, skip it if (narrowPhaseAlgorithm != nullptr) { + LastFrameCollisionInfo* lastCollisionFrameInfo = currentNarrowPhaseInfo->getLastFrameCollisionInfo(); + // 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. @@ -271,14 +279,14 @@ void CollisionDetection::computeNarrowPhase() { // Add the contact points as a potential contact manifold into the pair currentNarrowPhaseInfo->addContactPointsAsPotentialContactManifold(); - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true; + lastCollisionFrameInfo->wasColliding = true; } else { - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false; + lastCollisionFrameInfo->wasColliding = false; } // The previous frame collision info is now valid - currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true; + lastCollisionFrameInfo->isValid = true; } currentNarrowPhaseInfo = currentNarrowPhaseInfo->next; @@ -471,6 +479,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin NarrowPhaseInfo* narrowPhaseInfo = nullptr; + pair->makeLastFrameCollisionInfosObsolete(); + // If both shapes are convex if ((isShape1Convex && isShape2Convex)) { @@ -490,6 +500,8 @@ NarrowPhaseInfo* CollisionDetection::computeMiddlePhaseForProxyShapes(Overlappin computeConvexVsConcaveMiddlePhase(pair, mPoolAllocator, &narrowPhaseInfo); } + pair->clearObsoleteLastFrameCollisionInfos(); + return narrowPhaseInfo; } diff --git a/src/collision/ContactManifold.h b/src/collision/ContactManifold.h index 80a9f456..8303f35c 100644 --- a/src/collision/ContactManifold.h +++ b/src/collision/ContactManifold.h @@ -410,5 +410,6 @@ inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoint } } + #endif diff --git a/src/collision/ContactManifoldInfo.cpp b/src/collision/ContactManifoldInfo.cpp index f22593ac..6740fe4f 100644 --- a/src/collision/ContactManifoldInfo.cpp +++ b/src/collision/ContactManifoldInfo.cpp @@ -62,6 +62,9 @@ void ContactManifoldInfo::reset() { ContactPointInfo* elementToDelete = element; element = element->next; + // Call the constructor + elementToDelete->~ContactPointInfo(); + // Delete the current element mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); } diff --git a/src/collision/MiddlePhaseTriangleCallback.cpp b/src/collision/MiddlePhaseTriangleCallback.cpp index 0bda49d4..58604087 100644 --- a/src/collision/MiddlePhaseTriangleCallback.cpp +++ b/src/collision/MiddlePhaseTriangleCallback.cpp @@ -29,14 +29,12 @@ using namespace reactphysics3d; // Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase) -void MiddlePhaseTriangleCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) { +void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the // destructor of the corresponding NarrowPhaseInfo. TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape))) - TriangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex); + TriangleShape(trianglePoints, verticesNormals, shapeId); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/MiddlePhaseTriangleCallback.h b/src/collision/MiddlePhaseTriangleCallback.h index 4927c6e0..e04d7a9c 100644 --- a/src/collision/MiddlePhaseTriangleCallback.h +++ b/src/collision/MiddlePhaseTriangleCallback.h @@ -82,8 +82,7 @@ class MiddlePhaseTriangleCallback : public TriangleCallback { } /// Test collision between a triangle and the convex mesh shape - virtual void testTriangle(uint meshSubpart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) override; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override; #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/NarrowPhaseInfo.cpp b/src/collision/NarrowPhaseInfo.cpp index b4a92a38..660f6704 100644 --- a/src/collision/NarrowPhaseInfo.cpp +++ b/src/collision/NarrowPhaseInfo.cpp @@ -35,12 +35,15 @@ using namespace reactphysics3d; // Constructor NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, - const Transform& shape2Transform, void** cachedData1, void** cachedData2, Allocator& shapeAllocator) + const Transform& shape2Transform, void** cachedData1, void** cachedData2, + Allocator& shapeAllocator) : overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2), shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform), contactPoints(nullptr), cachedCollisionData1(cachedData1), - cachedCollisionData2(cachedData2), collisionShapeAllocator(shapeAllocator), next(nullptr) { + cachedCollisionData2(cachedData2), next(nullptr), collisionShapeAllocator(shapeAllocator) { + // Add a collision info for the two collision shapes into the overlapping pair (if not present yet) + overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId()); } // Destructor diff --git a/src/collision/NarrowPhaseInfo.h b/src/collision/NarrowPhaseInfo.h index c9b18094..b100939f 100644 --- a/src/collision/NarrowPhaseInfo.h +++ b/src/collision/NarrowPhaseInfo.h @@ -29,11 +29,12 @@ // Libraries #include "shapes/CollisionShape.h" #include "collision/ContactManifoldInfo.h" +#include "engine/OverlappingPair.h" /// Namespace ReactPhysics3D namespace reactphysics3d { -class OverlappingPair; +struct LastFrameCollisionInfo; // Class NarrowPhaseInfo /** @@ -70,12 +71,12 @@ struct NarrowPhaseInfo { // TODO : Check if we can use separating axis in OverlappingPair instead of cachedCollisionData1 and cachedCollisionData2 void** cachedCollisionData2; - /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) - Allocator& collisionShapeAllocator; - /// Pointer to the next element in the linked list NarrowPhaseInfo* next; + /// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor) + Allocator& collisionShapeAllocator; + /// Constructor NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1, CollisionShape* shape2, const Transform& shape1Transform, @@ -93,8 +94,16 @@ struct NarrowPhaseInfo { /// Reset the remaining contact points void resetContactPoints(); + + /// Get the last collision frame info for temporal coherence + LastFrameCollisionInfo* getLastFrameCollisionInfo() const; }; +// Get the last collision frame info for temporal coherence +inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const { + return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId()); +} + } #endif diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 315dcf7a..300ba5f2 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -50,10 +50,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh #endif + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = true; + lastFrameCollisionInfo->wasUsingSAT = false; assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); @@ -140,8 +143,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh } } - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = false; // Return true return true; @@ -153,8 +156,8 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh // Run the SAT algorithm to find the separating axis and compute contact point bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; return isColliding; } diff --git a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp index 9f2bb47e..507f6043 100644 --- a/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp @@ -45,10 +45,13 @@ bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* #endif + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; return isColliding; } diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index c0f436a6..dcfff45e 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -88,11 +88,13 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase // Create a simplex set VoronoiSimplex simplex; + // Get the last collision frame info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + // Get the previous point V (last cached separating axis) Vector3 v; - LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) { - v = lastFrameInfo.gjkSeparatingAxis; + if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) { + v = lastFrameCollisionInfo->gjkSeparatingAxis; assert(v.lengthSquare() > decimal(0.000001)); } else { @@ -117,7 +119,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhase if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) { // Cache the current separating axis for frame coherence - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v; + lastFrameCollisionInfo->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 327f4465..761bd5ec 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -74,7 +74,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow decimal minPenetrationDepth = DECIMAL_LARGEST; uint minFaceIndex = 0; - // For each face of the convex mesh for (uint f = 0; f < polyhedron->getNbFaces(); f++) { @@ -110,7 +109,6 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform, minPenetrationDepth, normalWorld); - // Create the contact info object narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, @@ -468,156 +466,152 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn Vector3 separatingEdge2A, separatingEdge2B; Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; - LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo(); - // True if the shapes were overlapping in the previous frame and are // still overlapping on the same axis in this frame bool isTemporalCoherenceValid = false; - // 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->getName() != CollisionShapeName::TRIANGLE && polyhedron2->getName() != CollisionShapeName::TRIANGLE) { + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); - // If the last frame collision info is valid and was also using SAT algorithm - if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) { + // If the last frame collision info is valid and was also using SAT algorithm + if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingSAT) { - // 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. + // 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. - // If the previous separating axis (or axis with minimum penetration depth) - // was a face normal of polyhedron 1 - if (lastFrameInfo.satIsAxisFacePolyhedron1) { + // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 1 + if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) { - decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, - lastFrameInfo.satMinAxisFaceIndex); - // If the previous axis is a separating axis - if (penetrationDepth <= decimal(0.0)) { + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, + lastFrameCollisionInfo->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 = true; - - // Compute the contact points between two faces of two convex polyhedra. - // If contact points have been found, we report them without running the whole SAT algorithm - if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, - polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, - narrowPhaseInfo, minPenetrationDepth)) { - - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; - - return true; - } - else { // Contact points have not been found (the set of clipped points was empty) - - // Therefore, we need to run the whole SAT algorithm again - isTemporalCoherenceValid = false; - } - } + // Return no collision + return false; } - 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)) { + // 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 = lastFrameCollisionInfo->wasColliding; - // Return no collision - return false; + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = true; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + return true; } + else { // Contact points have not been found (the set of clipped points was empty) - // 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; - - // Compute the contact points between two faces of two convex polyhedra. - // If contact points have been found, we report them without running the whole SAT algorithm - if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, - polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, - narrowPhaseInfo, minPenetrationDepth)) { - - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; - - return true; - } - else { // Contact points have not been found (the set of clipped points was empty) - - // Therefore, we need to run the whole SAT algorithm again - isTemporalCoherenceValid = 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); - - 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; - - // Temporal coherence is valid only if the two edges build a minkowski - // face (and the cross product is therefore a candidate for separating axis - if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + // Therefore, we need to run the whole SAT algorithm again isTemporalCoherenceValid = false; } + } + } + else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) + // was a face normal of polyhedron 2 - if (isTemporalCoherenceValid) { + decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, + lastFrameCollisionInfo->satMinAxisFaceIndex); - minPenetrationDepth = penetrationDepth; - isMinPenetrationFaceNormal = false; - isMinPenetrationFaceNormalPolyhedron1 = false; - minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index; - minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index; - separatingEdge1A = edge1A; - separatingEdge1B = edge1B; - separatingEdge2A = edge2A; - separatingEdge2B = edge2B; - minEdgeVsEdgeSeparatingAxisPolyhedron2Space = 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 = lastFrameCollisionInfo->wasColliding; + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; + isMinPenetrationFaceNormal = true; + isMinPenetrationFaceNormalPolyhedron1 = false; + + // Compute the contact points between two faces of two convex polyhedra. + // If contact points have been found, we report them without running the whole SAT algorithm + if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2, + polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, + narrowPhaseInfo, minPenetrationDepth)) { + + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; + + return true; } + else { // Contact points have not been found (the set of clipped points was empty) + + // Therefore, we need to run the whole SAT algorithm again + isTemporalCoherenceValid = 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(lastFrameCollisionInfo->satMinEdge1Index); + HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); + + 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 = lastFrameCollisionInfo->wasColliding; + + // Temporal coherence is valid only if the two edges build a minkowski + // face (and the cross product is therefore a candidate for separating axis + if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { + isTemporalCoherenceValid = false; + } + + if (isTemporalCoherenceValid) { + + minPenetrationDepth = penetrationDepth; + isMinPenetrationFaceNormal = false; + isMinPenetrationFaceNormalPolyhedron1 = false; + minSeparatingEdge1Index = lastFrameCollisionInfo->satMinEdge1Index; + minSeparatingEdge2Index = lastFrameCollisionInfo->satMinEdge2Index; + separatingEdge1A = edge1A; + separatingEdge1B = edge1B; + separatingEdge2A = edge2A; + separatingEdge2B = edge2B; + minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; } } } @@ -631,9 +625,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = true; - lastFrameInfo.satIsAxisFacePolyhedron2 = false; - lastFrameInfo.satMinAxisFaceIndex = faceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; // We have found a separating axis return false; @@ -649,9 +643,9 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satIsAxisFacePolyhedron2 = true; - lastFrameInfo.satMinAxisFaceIndex = faceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true; + lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; // We have found a separating axis return false; @@ -694,10 +688,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn if (penetrationDepth <= decimal(0.0)) { - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satIsAxisFacePolyhedron2 = false; - lastFrameInfo.satMinEdge1Index = i; - lastFrameInfo.satMinEdge2Index = j; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinEdge1Index = i; + lastFrameCollisionInfo->satMinEdge2Index = j; // We have found a separating axis return false; @@ -742,18 +736,18 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn // because of a numerical issue if (!contactsFound) { - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; // Return no collision return false; } } - lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; - lastFrameInfo.satMinAxisFaceIndex = minFaceIndex; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; + lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; } else { // If we have an edge vs edge contact @@ -781,10 +775,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge); } - lastFrameInfo.satIsAxisFacePolyhedron1 = false; - lastFrameInfo.satIsAxisFacePolyhedron2 = false; - lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index; - lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index; + lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; + lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; + lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index; + lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index; } return true; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index ae3764ab..28a565c1 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE); + // Get the last frame collision info + LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); + // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; @@ -52,8 +55,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false; + lastFrameCollisionInfo->wasUsingGJK = true; + lastFrameCollisionInfo->wasUsingSAT = false; // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { @@ -76,8 +79,8 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPha bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts); - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false; - narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true; + lastFrameCollisionInfo->wasUsingGJK = false; + lastFrameCollisionInfo->wasUsingSAT = true; return isColliding; } diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp index a89747c9..0b64190e 100644 --- a/src/collision/shapes/CollisionShape.cpp +++ b/src/collision/shapes/CollisionShape.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; // Constructor CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) - : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) { + : mType(type), mName(name), mScaling(1.0, 1.0, 1.0), mId(0) { } diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index feb4c502..6792cc09 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -71,6 +71,9 @@ class CollisionShape { /// Scaling vector of the collision shape Vector3 mScaling; + /// Unique identifier of the shape inside an overlapping pair + uint mId; + #ifdef IS_PROFILING_ACTIVE /// Pointer to the profiler @@ -126,6 +129,9 @@ class CollisionShape { /// Set the local scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); + /// Return the id of the shape + uint getId() const; + /// Return the local inertia tensor of the collision shapes virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; @@ -171,6 +177,11 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) { mScaling = scaling; } +// Return the id of the shape +inline uint CollisionShape::getId() const { + return mId; +} + #ifdef IS_PROFILING_ACTIVE // Set the profiler diff --git a/src/collision/shapes/ConcaveMeshShape.cpp b/src/collision/shapes/ConcaveMeshShape.cpp index 646638dd..ee90ce46 100644 --- a/src/collision/shapes/ConcaveMeshShape.cpp +++ b/src/collision/shapes/ConcaveMeshShape.cpp @@ -135,6 +135,22 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh return raycastCallback.getIsHit(); } +// Compute the shape Id for a given triangle of the mesh +uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const { + + uint shapeId = 0; + + uint i=0; + while (i < subPart) { + + shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles(); + + i++; + } + + return shapeId + triangleIndex; +} + // Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) { @@ -162,9 +178,9 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { // Get the vertices normals of the triangle Vector3 verticesNormals[3]; mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); + // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, data[0], data[1]); + TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE @@ -194,5 +210,6 @@ void ConcaveMeshRaycastCallback::raycastTriangles() { smallestHitFraction = raycastInfo.hitFraction; mIsHit = true; } + } } diff --git a/src/collision/shapes/ConcaveMeshShape.h b/src/collision/shapes/ConcaveMeshShape.h index 678d5325..2d0a3e9c 100644 --- a/src/collision/shapes/ConcaveMeshShape.h +++ b/src/collision/shapes/ConcaveMeshShape.h @@ -155,6 +155,9 @@ class ConcaveMeshShape : public ConcaveShape { /// Return the three vertex normals (in the array outVerticesNormals) of a triangle void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const; + /// Compute the shape Id for a given triangle of the mesh + uint computeTriangleShapeId(uint subPart, uint triangleIndex) const; + public: /// Constructor @@ -259,7 +262,7 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals); // Call the callback to test narrow-phase collision with this triangle - mTriangleTestCallback.testTriangle(data[0], data[1], trianglePoints, verticesNormals); + mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1])); } #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index abc3759b..eb00d8f0 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -46,8 +46,7 @@ class TriangleCallback { virtual ~TriangleCallback() = default; /// Report a triangle - virtual void testTriangle(uint meshSubPart, uint triangleIndex, - const Vector3* trianglePoints, const Vector3* verticesNormals)=0; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId)=0; }; diff --git a/src/collision/shapes/HeightFieldShape.cpp b/src/collision/shapes/HeightFieldShape.cpp index 08ec19a4..74276a75 100644 --- a/src/collision/shapes/HeightFieldShape.cpp +++ b/src/collision/shapes/HeightFieldShape.cpp @@ -154,7 +154,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal}; // Test collision against the first triangle - callback.testTriangle(0, 0, trianglePoints, verticesNormals1); + callback.testTriangle(trianglePoints, verticesNormals1, computeTriangleShapeId(i, j, 0)); // Generate the second triangle for the current grid rectangle trianglePoints[0] = p3; @@ -174,7 +174,7 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal}; // Test collision against the second triangle - callback.testTriangle(0, 0, trianglePoints, verticesNormals2); + callback.testTriangle(trianglePoints, verticesNormals2, computeTriangleShapeId(i, j, 1)); } } } @@ -263,12 +263,10 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const { } // Raycast test between a ray and a triangle of the heightfield -void TriangleOverlapCallback::testTriangle(uint meshSubPart, uint triangleIndex, const Vector3* trianglePoints, - const Vector3* verticesNormals) { +void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) { // Create a triangle collision shape - TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], - verticesNormals, meshSubPart, triangleIndex); + TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId); triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType()); #ifdef IS_PROFILING_ACTIVE diff --git a/src/collision/shapes/HeightFieldShape.h b/src/collision/shapes/HeightFieldShape.h index 80c18d9c..f8b498d5 100644 --- a/src/collision/shapes/HeightFieldShape.h +++ b/src/collision/shapes/HeightFieldShape.h @@ -71,8 +71,7 @@ class TriangleOverlapCallback : public TriangleCallback { bool getIsHit() const {return mIsHit;} /// Raycast test between a ray and a triangle of the heightfield - virtual void testTriangle(uint meshSubPart, uint triangleIndex, - const Vector3* trianglePoints, const Vector3* verticesNormals) override; + virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override; #ifdef IS_PROFILING_ACTIVE @@ -169,6 +168,9 @@ class HeightFieldShape : public ConcaveShape { /// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const; + /// Compute the shape Id for a given triangle + uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const; + public: /// Constructor @@ -270,6 +272,12 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim 0, 0, mass); } +// Compute the shape Id for a given triangle +inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const { + + return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement; +} + } #endif diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp index 8583e80d..fa6b4f6a 100644 --- a/src/collision/shapes/SphereShape.cpp +++ b/src/collision/shapes/SphereShape.cpp @@ -35,7 +35,8 @@ using namespace reactphysics3d; /** * @param radius Radius of the sphere (in meters) */ -SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) { +SphereShape::SphereShape(decimal radius) + : ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) { assert(radius > decimal(0.0)); } diff --git a/src/collision/shapes/TriangleShape.cpp b/src/collision/shapes/TriangleShape.cpp index 020053b6..77d0929c 100644 --- a/src/collision/shapes/TriangleShape.cpp +++ b/src/collision/shapes/TriangleShape.cpp @@ -43,16 +43,15 @@ using namespace reactphysics3d; * @param verticesNormals The three vertices normals for smooth mesh collision * @param margin The collision margin (in meters) around the collision shape */ -TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex) - : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mMeshSubPart(meshSubPart), mTriangleIndex(triangleIndex) { +TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId) + : ConvexPolyhedronShape(CollisionShapeName::TRIANGLE) { - mPoints[0] = point1; - mPoints[1] = point2; - mPoints[2] = point3; + mPoints[0] = vertices[0]; + mPoints[1] = vertices[1]; + mPoints[2] = vertices[2]; // Compute the triangle normal - mNormal = (point2 - point1).cross(point3 - point1); + mNormal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]); mNormal.normalize(); mVerticesNormals[0] = verticesNormals[0]; @@ -60,6 +59,8 @@ TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const mVerticesNormals[2] = verticesNormals[2]; mRaycastTestType = TriangleRaycastSide::FRONT; + + mId = shapeId; } // This method compute the smooth mesh contact with a triangle in case one of the two collision diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 8360c32b..60ec216b 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -72,12 +72,6 @@ class TriangleShape : public ConvexPolyhedronShape { /// Raycast test type for the triangle (front, back, front-back) TriangleRaycastSide mRaycastTestType; - /// Index of the mesh sub part in the original mesh - uint mMeshSubPart; - - /// Triangle index of the triangle in the sub mesh - uint mTriangleIndex; - // -------------------- Methods -------------------- // /// Return a local support point in a given direction without the object margin @@ -95,6 +89,9 @@ class TriangleShape : public ConvexPolyhedronShape { /// Return the number of bytes used by the collision shape virtual size_t getSizeInBytes() const override; + /// Generate the id of the shape (used for temporal coherence) + void generateId(); + // -------------------- Methods -------------------- // /// This method implements the technique described in Game Physics Pearl book @@ -107,8 +104,7 @@ class TriangleShape : public ConvexPolyhedronShape { // -------------------- Methods -------------------- // /// Constructor - TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, - const Vector3* verticesNormals, uint meshSubPart, uint triangleIndex); + TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId); /// Destructor virtual ~TriangleShape() override = default; @@ -164,12 +160,6 @@ class TriangleShape : public ConvexPolyhedronShape { /// Return the centroid of the polyhedron virtual Vector3 getCentroid() const override; - /// Return the index of the sub part mesh of the original mesh - uint getMeshSubPart() const; - - /// Return the triangle index in the original mesh - uint getTriangleIndex() const; - /// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2, Vector3& localContactPointShape1, Vector3& localContactPointShape2, @@ -319,16 +309,6 @@ inline Vector3 TriangleShape::getCentroid() const { return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0); } -// Return the index of the sub part mesh of the original mesh -inline uint TriangleShape::getMeshSubPart() const { - return mMeshSubPart; -} - -// Return the triangle index in the original mesh -inline uint TriangleShape::getTriangleIndex() const { - return mTriangleIndex; -} - // Return the number of half-edges of the polyhedron inline uint TriangleShape::getNbHalfEdges() const { return 6; diff --git a/src/constraint/ContactPoint.h b/src/constraint/ContactPoint.h index e2a9a5e5..739a9efb 100644 --- a/src/constraint/ContactPoint.h +++ b/src/constraint/ContactPoint.h @@ -28,7 +28,6 @@ // Libraries #include "body/CollisionBody.h" -#include "collision/NarrowPhaseInfo.h" #include "collision/ContactPointInfo.h" #include "configuration.h" #include "mathematics/mathematics.h" @@ -36,6 +35,8 @@ /// ReactPhysics3D namespace namespace reactphysics3d { +struct NarrowPhaseInfo; + // Class ContactPoint /** * This class represents a collision contact point between two diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index f523d169..6c558d6d 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -259,4 +259,4 @@ inline void CollisionWorld::setProfilerName(std::string name) { } - #endif +#endif diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index 0055e683..eba2fb80 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -27,20 +27,31 @@ #include #include "OverlappingPair.h" #include "collision/ContactManifoldInfo.h" +#include "collision/NarrowPhaseInfo.h" using namespace reactphysics3d; // Constructor OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - Allocator& manifoldsAllocator, Allocator& temporaryMemoryAllocator) - : mContactManifoldSet(shape1, shape2, manifoldsAllocator), mPotentialContactManifolds(nullptr), - mTempMemoryAllocator(temporaryMemoryAllocator) { + Allocator& persistentMemoryAllocator, Allocator& temporaryMemoryAllocator) + : mContactManifoldSet(shape1, shape2, persistentMemoryAllocator), mPotentialContactManifolds(nullptr), + mPersistentAllocator(persistentMemoryAllocator), mTempMemoryAllocator(temporaryMemoryAllocator) { } // Destructor OverlappingPair::~OverlappingPair() { assert(mPotentialContactManifolds == nullptr); + + // Remove all the remaining last frame collision info + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { + + // Call the constructor + it->second->~LastFrameCollisionInfo(); + + // Release memory + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + } } // Create a new potential contact manifold using contact-points from narrow-phase @@ -137,3 +148,59 @@ void OverlappingPair::reducePotentialContactManifolds() { manifold = manifold->getNext(); } } + + +// Add a new last frame collision info if it does not exist for the given shapes already +void OverlappingPair::addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2) { + + // Try to get the corresponding last frame collision info + auto it = mLastFrameCollisionInfos.find(std::make_pair(shapeId1, shapeId2)); + + // If there is no collision info for those two shapes already + if (it == mLastFrameCollisionInfos.end()) { + + // Create a new collision info + LastFrameCollisionInfo* collisionInfo = new (mPersistentAllocator.allocate(sizeof(LastFrameCollisionInfo))) + LastFrameCollisionInfo(); + + // Add it into the map of collision infos + std::map, LastFrameCollisionInfo*>::iterator it; + auto ret = mLastFrameCollisionInfos.insert(std::make_pair(std::make_pair(shapeId1, shapeId2), collisionInfo)); + assert(ret.second); + } + else { + + // The existing collision info is not obsolete + it->second->isObsolete = false; + } +} + + +// Delete all the obsolete last frame collision info +void OverlappingPair::clearObsoleteLastFrameCollisionInfos() { + + // For each collision info + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ) { + + // If the collision info is obsolete + if (it->second->isObsolete) { + + // Delete it + it->second->~LastFrameCollisionInfo(); + mPersistentAllocator.release(it->second, sizeof(LastFrameCollisionInfo)); + + mLastFrameCollisionInfos.erase(it++); + } + else { + ++it; + } + } +} + +// Make all the last frame collision infos obsolete +void OverlappingPair::makeLastFrameCollisionInfosObsolete() { + + for (auto it = mLastFrameCollisionInfos.begin(); it != mLastFrameCollisionInfos.end(); ++it) { + it->second->isObsolete = true; + } +} diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h index ce22bb7e..8c65e633 100644 --- a/src/engine/OverlappingPair.h +++ b/src/engine/OverlappingPair.h @@ -30,6 +30,7 @@ #include "collision/ContactManifoldSet.h" #include "collision/ProxyShape.h" #include "collision/shapes/CollisionShape.h" +#include /// ReactPhysics3D namespace namespace reactphysics3d { @@ -47,6 +48,9 @@ struct LastFrameCollisionInfo { /// True if we have information about the previous frame bool isValid; + /// True if the frame info is obsolete (the collision shape are not overlapping in middle phase) + bool isObsolete; + /// True if the two shapes were colliding in the previous frame bool wasColliding; @@ -72,6 +76,7 @@ struct LastFrameCollisionInfo { LastFrameCollisionInfo() { isValid = false; + isObsolete = false; wasColliding = false; wasUsingSAT = false; wasUsingGJK = false; @@ -97,12 +102,19 @@ class OverlappingPair { /// Set of persistent contact manifolds ContactManifoldSet mContactManifoldSet; - /// Collision information about the last frame (for temporal coherence) - LastFrameCollisionInfo mLastFrameCollisionInfo; + /// Temporal coherence collision data for each overlapping collision shapes of this pair. + /// Temporal coherence data store collision information about the last frame. + /// If two convex shapes overlap, we have a single collision data but if one shape is concave, + /// we might have collision data for several overlapping triangles. The key in the map is the + /// shape Ids of the two collision shapes. + std::map, LastFrameCollisionInfo*> mLastFrameCollisionInfos; /// Linked-list of potential contact manifold ContactManifoldInfo* mPotentialContactManifolds; + /// Persistent memory allocator + Allocator& mPersistentAllocator; + /// Memory allocator used to allocated memory for the ContactManifoldInfo and ContactPointInfo Allocator& mTempMemoryAllocator; @@ -111,8 +123,8 @@ class OverlappingPair { // -------------------- Methods -------------------- // /// Constructor - OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, - Allocator& memoryAllocator, Allocator& temporaryMemoryAllocator); + OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, Allocator& persistentMemoryAllocator, + Allocator& temporaryMemoryAllocator); /// Destructor ~OverlappingPair(); @@ -130,7 +142,7 @@ class OverlappingPair { ProxyShape* getShape2() const; /// Return the last frame collision info - LastFrameCollisionInfo& getLastFrameCollisionInfo(); + LastFrameCollisionInfo* getLastFrameCollisionInfo(std::pair shapeIds); /// Return the a reference to the contact manifold set const ContactManifoldSet& getContactManifoldSet(); @@ -168,6 +180,18 @@ class OverlappingPair { /// Reduce the contact manifolds that have too many contact points void reduceContactManifolds(); + /// Add a new last frame collision info if it does not exist for the given shapes already + void addLastFrameInfoIfNecessary(uint shapeId1, uint shapeId2); + + /// Return the last frame collision info for a given pair of shape ids + LastFrameCollisionInfo* getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const; + + /// Delete all the obsolete last frame collision info + void clearObsoleteLastFrameCollisionInfos(); + + /// Make all the last frame collision infos obsolete + void makeLastFrameCollisionInfosObsolete(); + /// Return the pair of bodies index static overlappingpairid computeID(ProxyShape* shape1, ProxyShape* shape2); @@ -194,9 +218,14 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo* conta mContactManifoldSet.addContactManifold(contactManifoldInfo); } -// Return the last frame collision info -inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() { - return mLastFrameCollisionInfo; +// Return the last frame collision info for a given shape id or nullptr if none is found +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(std::pair shapeIds) { + std::map, LastFrameCollisionInfo*>::iterator it = mLastFrameCollisionInfos.find(shapeIds); + if (it != mLastFrameCollisionInfos.end()) { + return it->second; + } + + return nullptr; } // Return the contact manifold @@ -265,6 +294,11 @@ inline void OverlappingPair::reduceContactManifolds() { mContactManifoldSet.reduce(); } +// Return the last frame collision info for a given pair of shape ids +inline LastFrameCollisionInfo* OverlappingPair::getLastFrameCollisionInfo(uint shapeId1, uint shapeId2) const { + return mLastFrameCollisionInfos.at(std::make_pair(shapeId1, shapeId2)); +} + } #endif diff --git a/test/tests/collision/TestRaycast.h b/test/tests/collision/TestRaycast.h index f2ae3d6b..c62ca179 100644 --- a/test/tests/collision/TestRaycast.h +++ b/test/tests/collision/TestRaycast.h @@ -198,11 +198,12 @@ class TestRaycast : public Test { mSphereShape = new SphereShape(3); mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform); - const Vector3 triangleVertex1(100, 100, 0); - const Vector3 triangleVertex2(105, 100, 0); - const Vector3 triangleVertex3(100, 103, 0); + Vector3 triangleVertices[3]; + triangleVertices[0] = Vector3(100, 100, 0); + triangleVertices[1] = Vector3(105, 100, 0); + triangleVertices[2] = Vector3(100, 103, 0); Vector3 triangleVerticesNormals[3] = {Vector3(0, 0, 1), Vector3(0, 0, 1), Vector3(0, 0, 1)}; - mTriangleShape = new TriangleShape(triangleVertex1, triangleVertex2, triangleVertex3, triangleVerticesNormals, 0, 0); + mTriangleShape = new TriangleShape(triangleVertices, triangleVerticesNormals, 0); mTriangleProxyShape = mTriangleBody->addCollisionShape(mTriangleShape, mShapeTransform); mCapsuleShape = new CapsuleShape(2, 5);