From 7fb6f49149b04f5303735394ab568af726c03cbf Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Fri, 28 Apr 2017 21:40:16 +0200 Subject: [PATCH] Working on capsule vs polyhedron narrow-phase algorithm --- src/collision/ContactManifoldInfo.h | 27 ++- src/collision/PolyhedronMesh.cpp | 15 ++ src/collision/PolyhedronMesh.h | 14 ++ src/collision/ProxyShape.h | 1 - .../narrowphase/CapsuleVsCapsuleAlgorithm.cpp | 7 +- .../narrowphase/CapsuleVsCapsuleAlgorithm.h | 2 +- .../CapsuleVsConvexPolyhedronAlgorithm.cpp | 71 +++++- .../CapsuleVsConvexPolyhedronAlgorithm.h | 2 +- .../narrowphase/SAT/SATAlgorithm.cpp | 218 +++++++++++++++++- src/collision/narrowphase/SAT/SATAlgorithm.h | 12 + .../narrowphase/SphereVsCapsuleAlgorithm.cpp | 3 + .../narrowphase/SphereVsCapsuleAlgorithm.h | 2 +- .../SphereVsConvexPolyhedronAlgorithm.cpp | 7 +- .../SphereVsConvexPolyhedronAlgorithm.h | 2 +- src/collision/shapes/BoxShape.h | 8 + src/collision/shapes/ConvexMeshShape.h | 8 + src/collision/shapes/ConvexPolyhedronShape.h | 3 + src/collision/shapes/ConvexShape.h | 2 +- src/mathematics/mathematics_functions.cpp | 5 + src/mathematics/mathematics_functions.h | 3 + 20 files changed, 382 insertions(+), 30 deletions(-) diff --git a/src/collision/ContactManifoldInfo.h b/src/collision/ContactManifoldInfo.h index d4afc590..f631b9fc 100644 --- a/src/collision/ContactManifoldInfo.h +++ b/src/collision/ContactManifoldInfo.h @@ -64,15 +64,8 @@ class ContactManifoldInfo { /// Destructor ~ContactManifoldInfo() { - // Delete all the contact points in the linked list - ContactPointInfo* element = mContactPointsList; - while(element != nullptr) { - ContactPointInfo* elementToDelete = element; - element = element->next; - - // Delete the current element - mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); - } + // Remove all the contact points + reset(); } /// Deleted copy-constructor @@ -96,6 +89,22 @@ class ContactManifoldInfo { mContactPointsList = contactPointInfo; } + /// Remove all the contact points + void reset() { + + // Delete all the contact points in the linked list + ContactPointInfo* element = mContactPointsList; + while(element != nullptr) { + ContactPointInfo* elementToDelete = element; + element = element->next; + + // Delete the current element + mAllocator.release(elementToDelete, sizeof(ContactPointInfo)); + } + + mContactPointsList = nullptr; + } + /// Get the first contact point info of the linked list of contact points ContactPointInfo* getFirstContactPointInfo() const { return mContactPointsList; diff --git a/src/collision/PolyhedronMesh.cpp b/src/collision/PolyhedronMesh.cpp index 60a3be9f..61b1e3c8 100644 --- a/src/collision/PolyhedronMesh.cpp +++ b/src/collision/PolyhedronMesh.cpp @@ -42,6 +42,9 @@ PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray) { // Compute the faces normals computeFacesNormals(); + + // Compute the centroid + computeCentroid(); } // Destructor @@ -126,3 +129,15 @@ void PolyhedronMesh::computeFacesNormals() { mFacesNormals[f].normalize(); } } + +// Compute the centroid of the polyhedron +void PolyhedronMesh::computeCentroid() { + + mCentroid.setToZero(); + + for (uint v=0; v < getNbVertices(); v++) { + mCentroid += getVertex(v); + } + + mCentroid /= getNbVertices(); +} diff --git a/src/collision/PolyhedronMesh.h b/src/collision/PolyhedronMesh.h index 940b2db0..a64db670 100644 --- a/src/collision/PolyhedronMesh.h +++ b/src/collision/PolyhedronMesh.h @@ -55,6 +55,9 @@ class PolyhedronMesh { /// Array with the face normals Vector3* mFacesNormals; + /// Centroid of the polyhedron + Vector3 mCentroid; + // -------------------- Methods -------------------- // /// Create the half-edge structure of the mesh @@ -63,6 +66,9 @@ class PolyhedronMesh { /// Compute the faces normals void computeFacesNormals(); + /// Compute the centroid of the polyhedron + void computeCentroid() ; + public: // -------------------- Methods -------------------- // @@ -84,6 +90,9 @@ class PolyhedronMesh { /// Return the half-edge structure of the mesh const HalfEdgeStructure& getHalfEdgeStructure() const; + + /// Return the centroid of the polyhedron + Vector3 getCentroid() const; }; // Return the number of vertices @@ -102,6 +111,11 @@ inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const { return mHalfEdgeStructure; } +// Return the centroid of the polyhedron +inline Vector3 PolyhedronMesh::getCentroid() const { + return mCentroid; +} + } #endif diff --git a/src/collision/ProxyShape.h b/src/collision/ProxyShape.h index 45c0add2..d9575b3d 100644 --- a/src/collision/ProxyShape.h +++ b/src/collision/ProxyShape.h @@ -175,7 +175,6 @@ class ProxyShape { friend class CollisionDetection; friend class CollisionWorld; friend class DynamicsWorld; - friend class EPAAlgorithm; friend class GJKAlgorithm; friend class ConvexMeshShape; diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp index 11160b8c..a6c7a020 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp @@ -30,13 +30,14 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection between two capsules +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); - const decimal parallelEpsilon = decimal(0.001); - // Get the capsule collision shapes const CapsuleShape* capsuleShape1 = static_cast(narrowPhaseInfo->collisionShape1); const CapsuleShape* capsuleShape2 = static_cast(narrowPhaseInfo->collisionShape2); @@ -62,7 +63,7 @@ bool CapsuleVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhase decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius(); // If the two capsules are parallel (we create two contact points) - if (seg1.cross(seg2).lengthSquare() < parallelEpsilon * parallelEpsilon) { + if (areParallelVectors(seg1, seg2)) { // If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping) const decimal segmentsDistance = computeDistancePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA); diff --git a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h index 898d6337..9d8393c9 100644 --- a/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h @@ -60,7 +60,7 @@ class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection between two capsules virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp index 8deb17e5..5e8dd5f9 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp @@ -27,24 +27,86 @@ #include "CapsuleVsConvexPolyhedronAlgorithm.h" #include "SAT/SATAlgorithm.h" #include "GJK/GJKAlgorithm.h" +#include "collision/shapes/CapsuleShape.h" +#include "collision/shapes/ConvexPolyhedronShape.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection between a capsule and a polyhedron +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; + SATAlgorithm satAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); // If we have found a contact point inside the margins (shallow penetration) if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) { + // GJK has found a shallow contact. If the contact normal is parallel to a face of the + // polyhedron mesh, we would like to create two contact points instead of a single one + // (as in the deep contact case with SAT algorithm) + + // Get the contact point created by GJK + ContactPointInfo* contactPoint = contactManifoldInfo.getFirstContactPointInfo(); + + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + // For each face of the polyhedron + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(f); + + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + const Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal; + + // If the polyhedron face normal is parallel to the computed GJK contact normal + if (areParallelVectors(faceNormalWorld, contactPoint->normal)) { + + // Remove the previous contact point computed by GJK + contactManifoldInfo.reset(); + + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + + // Compute and create two contact points + satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, + polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + contactManifoldInfo, isCapsuleShape1); + + break; + } + + } + // Return true return true; } @@ -53,7 +115,6 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) { // Run the SAT algorithm to find the separating axis and compute contact point - SATAlgorithm satAlgorithm; return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo); } diff --git a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h index e9905596..6b3f504f 100644 --- a/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h +++ b/src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h @@ -60,7 +60,7 @@ class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection between a capsule and a polyhedron virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.cpp b/src/collision/narrowphase/SAT/SATAlgorithm.cpp index 1b57e36f..3637b3dc 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.cpp +++ b/src/collision/narrowphase/SAT/SATAlgorithm.cpp @@ -27,7 +27,7 @@ #include "SATAlgorithm.h" #include "constraint/ContactPoint.h" #include "collision/PolyhedronMesh.h" -#include "collision/shapes/ConvexPolyhedronShape.h" +#include "collision/shapes/CapsuleShape.h" #include "collision/shapes/SphereShape.h" #include "configuration.h" #include "engine/Profiler.h" @@ -110,8 +110,220 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(const NarrowPhaseInfo* // Test collision between a capsule and a convex mesh bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const { - assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE); - assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE; + + assert(isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON); + assert(!isCapsuleShape1 || narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE); + + // Get the collision shapes + const CapsuleShape* capsuleShape = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2); + const ConvexPolyhedronShape* polyhedron = static_cast(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1); + + const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform; + const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform; + + const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld; + + // Minimum penetration depth + decimal minPenetrationDepth = DECIMAL_LARGEST; + uint minFaceIndex = 0; + uint minEdgeIndex = 0; + bool isMinPenetrationFaceNormal = false; + Vector3 separatingAxisCapsuleSpace; + Vector3 separatingPolyhedronEdgeVertex1; + Vector3 separatingPolyhedronEdgeVertex2; + + // For each face of the convex mesh + for (uint f = 0; f < polyhedron->getNbFaces(); f++) { + + // Get the face + HalfEdgeStructure::Face face = polyhedron->getFace(f); + + // Get the face normal + const Vector3 faceNormal = polyhedron->getFaceNormal(f); + + // Compute the penetration depth (using the capsule support in the direction opposite to the face normal) + const Vector3 faceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; + const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-faceNormalCapsuleSpace, nullptr); + const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]); + const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint; + const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(faceNormal); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minFaceIndex = f; + isMinPenetrationFaceNormal = true; + separatingAxisCapsuleSpace = faceNormalCapsuleSpace; + } + } + + // Compute the end-points of the inner segment of the capsule + const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0); + const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA; + + // For each direction that is the cross product of the capsule inner segment and + // an edge of the polyhedron + for (uint e = 0; e < polyhedron->getNbHalfEdges(); e += 2) { + + // Get an edge from the polyhedron (convert it into the capsule local-space) + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(e); + const Vector3 edgeVertex1 = polyhedron->getVertexPosition(edge.vertexIndex); + const Vector3 edgeVertex2 = polyhedron->getVertexPosition(polyhedron->getHalfEdge(edge.nextEdgeIndex).vertexIndex); + const Vector3 edgeDirectionCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * (edgeVertex2 - edgeVertex1); + + HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + const Vector3 adjacentFace1Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(edge.faceIndex); + const Vector3 adjacentFace2Normal = polyhedronToCapsuleTransform.getOrientation() * polyhedron->getFaceNormal(twinEdge.faceIndex); + + // Check using the Gauss Map if this edge cross product can be as separating axis + if (isMinkowskiFaceCapsuleVsEdge(capsuleSegmentAxis, adjacentFace1Normal, adjacentFace2Normal)) { + + // Compute the axis to test (cross product between capsule inner segment and polyhedron edge) + Vector3 axis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace); + + // Skip separating axis test if polyhedron edge is parallel to the capsule inner segment + if (axis.lengthSquare() >= decimal(0.00001)) { + + const Vector3 polyhedronCentroid = polyhedronToCapsuleTransform * polyhedron->getCentroid(); + const Vector3 pointOnPolyhedronEdge = polyhedronToCapsuleTransform * edgeVertex1; + + // Swap axis direction if necessary such that it points out of the polyhedron + if (axis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) { + axis = -axis; + } + + axis.normalize(); + + // Compute the penetration depth + const Vector3 capsuleSupportPoint = capsuleShape->getLocalSupportPointWithMargin(-axis, nullptr); + const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint; + const decimal penetrationDepth = capsuleSupportPointToEdgePoint.dot(axis); + + // If the penetration depth is negative, we have found a separating axis + if (penetrationDepth <= decimal(0.0)) { + return false; + } + + // Check if we have found a new minimum penetration axis + if (penetrationDepth < minPenetrationDepth) { + minPenetrationDepth = penetrationDepth; + minEdgeIndex = e; + isMinPenetrationFaceNormal = false; + separatingAxisCapsuleSpace = axis; + separatingPolyhedronEdgeVertex1 = edgeVertex1; + separatingPolyhedronEdgeVertex2 = edgeVertex2; + } + } + } + } + + // Convert the inner capsule segment points into the polyhedron local-space + const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse(); + const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA; + const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; + + const Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; + const decimal capsuleRadius = capsuleShape->getRadius(); + + // If the separating axis is a face normal + // We need to clip the inner capsule segment with the adjacent faces of the separating face + if (isMinPenetrationFaceNormal) { + + computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, + polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, + capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + contactManifoldInfo, isCapsuleShape1); + } + else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment + + // Compute the closest points between the inner capsule segment and the + // edge of the polyhedron in polyhedron local-space + Vector3 closestPointPolyhedronEdge, closestPointCapsuleInnerSegment; + computeClosestPointBetweenTwoSegments(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + separatingPolyhedronEdgeVertex1, separatingPolyhedronEdgeVertex2, + closestPointCapsuleInnerSegment, closestPointPolyhedronEdge); + + + // Project closest capsule inner segment point into the capsule bounds + const Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius; + + // Create the contact point + contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth, + isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge, + isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule); + } + + return true; +} + +// Compute the two contact points between a polyhedron and a capsule when the separating +// axis is a face normal of the polyhedron +void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, + const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, + ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const { + + HalfEdgeStructure::Face face = polyhedron->getFace(referenceFaceIndex); + uint firstEdgeIndex = face.edgeIndex; + uint edgeIndex = firstEdgeIndex; + + std::vector planesPoints; + std::vector planesNormals; + + // For each adjacent edge of the separating face of the polyhedron + do { + HalfEdgeStructure::Edge edge = polyhedron->getHalfEdge(edgeIndex); + HalfEdgeStructure::Edge twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex); + + // Construct a clippling plane for each adjacent edge of the separting face of the polyhedron + planesPoints.push_back(polyhedron->getVertexPosition(edge.vertexIndex)); + planesNormals.push_back(polyhedron->getFaceNormal(twinEdge.faceIndex)); + + edgeIndex = edge.nextEdgeIndex; + + } while(edgeIndex != firstEdgeIndex); + + // First we clip the inner segment of the capsule with the four planes of the adjacent faces + std::vector clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, + planesPoints, planesNormals); + + // Project the two clipped points into the polyhedron face + const Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex); + const Vector3 contactPoint1Polyhedron = clipSegment[0] + faceNormal * (penetrationDepth - capsuleRadius); + const Vector3 contactPoint2Polyhedron = clipSegment[1] + faceNormal * (penetrationDepth - capsuleRadius); + + // Project the two clipped points into the capsule bounds + const Vector3 contactPoint1Capsule = (polyhedronToCapsuleTransform * clipSegment[0]) - separatingAxisCapsuleSpace * capsuleRadius; + const Vector3 contactPoint2Capsule = (polyhedronToCapsuleTransform * clipSegment[1]) - separatingAxisCapsuleSpace * capsuleRadius; + + // Create the contact points + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPoint1Capsule : contactPoint1Polyhedron, + isCapsuleShape1 ? contactPoint1Polyhedron : contactPoint1Capsule); + contactManifoldInfo.addContactPoint(normalWorld, penetrationDepth, + isCapsuleShape1 ? contactPoint2Capsule : contactPoint2Polyhedron, + isCapsuleShape1 ? contactPoint2Polyhedron : contactPoint2Capsule); +} + +// This method returns true if an edge of a polyhedron and a capsule forms a +// face of the Minkowski Difference. This test is used to know if two edges +// (one edge of the polyhedron vs the inner segment of the capsule in this case) +// have to be test as a possible separating axis +bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, + const Vector3& edgeAdjacentFace2Normal) const { + + // Return true if the arc on the Gauss Map corresponding to the polyhedron edge + // intersect the unit circle plane corresponding to capsule Gauss Map + return capsuleSegment.dot(edgeAdjacentFace1Normal) * capsuleSegment.dot(edgeAdjacentFace2Normal) < decimal(0.0); } // Test collision between a triangle and a convex mesh diff --git a/src/collision/narrowphase/SAT/SATAlgorithm.h b/src/collision/narrowphase/SAT/SATAlgorithm.h index b779d711..42e380f2 100644 --- a/src/collision/narrowphase/SAT/SATAlgorithm.h +++ b/src/collision/narrowphase/SAT/SATAlgorithm.h @@ -29,6 +29,7 @@ // Libraries #include "collision/ContactManifoldInfo.h" #include "collision/NarrowPhaseInfo.h" +#include "collision/shapes/ConvexPolyhedronShape.h" /// ReactPhysics3D namespace namespace reactphysics3d { @@ -68,6 +69,17 @@ class SATAlgorithm { /// Test collision between a capsule and a convex mesh bool testCollisionCapsuleVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; + /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron + void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, + decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, + const Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, + const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, + ContactManifoldInfo& contactManifoldInfo, bool isCapsuleShape1) const; + + // This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference + bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal, + const Vector3& edgeAdjacentFace2Normal) const; + /// Test collision between a triangle and a convex mesh bool testCollisionTriangleVsConvexMesh(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp index a311b2bf..cd437d6e 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp @@ -31,6 +31,9 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection between a sphere and a capsule +// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation +// by Dirk Gregorius. bool SphereVsCapsuleAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) { bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE; diff --git a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h index 784df6f9..154f6e4c 100644 --- a/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h +++ b/src/collision/narrowphase/SphereVsCapsuleAlgorithm.h @@ -60,7 +60,7 @@ class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm { /// Deleted assignment operator SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete; - /// Compute a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection between a sphere and a capsule virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp index 44959971..708d9f8d 100644 --- a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp +++ b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp @@ -31,13 +31,12 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// Compute the narrow-phase collision detection 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, ContactManifoldInfo& contactManifoldInfo) { - // Get the local-space to world-space transforms - const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform; - const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform; - // First, we run the GJK algorithm GJKAlgorithm gjkAlgorithm; GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo); diff --git a/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h b/src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h index 591ebd44..c1fd3e55 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 a contact info if the two bounding volume collide + /// Compute the narrow-phase collision detection a sphere and a convex polyhedron virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) override; }; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index be79e3dd..d2b92dcc 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -128,6 +128,9 @@ class BoxShape : public ConvexPolyhedronShape { /// Return the normal vector of a given face of the polyhedron virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; }; // Return the extents of the box @@ -236,6 +239,11 @@ inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const { } } +// Return the centroid of the box +inline Vector3 BoxShape::getCentroid() const { + return Vector3::zero(); +} + // Return the number of half-edges of the polyhedron inline uint BoxShape::getNbHalfEdges() const { return 24; diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index ff11c232..28ba9c86 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -142,6 +142,9 @@ class ConvexMeshShape : public ConvexPolyhedronShape { /// Return the normal vector of a given face of the polyhedron virtual Vector3 getFaceNormal(uint faceIndex) const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const override; }; /// Set the scaling vector of the collision shape @@ -239,6 +242,11 @@ inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const { return mPolyhedronMesh->getFaceNormal(faceIndex); } +// Return the centroid of the polyhedron +inline Vector3 ConvexMeshShape::getCentroid() const { + return mPolyhedronMesh->getCentroid(); +} + } #endif diff --git a/src/collision/shapes/ConvexPolyhedronShape.h b/src/collision/shapes/ConvexPolyhedronShape.h index 059e5a3b..f6d2a34b 100644 --- a/src/collision/shapes/ConvexPolyhedronShape.h +++ b/src/collision/shapes/ConvexPolyhedronShape.h @@ -88,6 +88,9 @@ class ConvexPolyhedronShape : public ConvexShape { /// Return true if the collision shape is a polyhedron virtual bool isPolyhedron() const override; + + /// Return the centroid of the polyhedron + virtual Vector3 getCentroid() const=0; }; // Return true if the collision shape is a polyhedron diff --git a/src/collision/shapes/ConvexShape.h b/src/collision/shapes/ConvexShape.h index 9180fefe..4b98c9fa 100644 --- a/src/collision/shapes/ConvexShape.h +++ b/src/collision/shapes/ConvexShape.h @@ -81,7 +81,7 @@ class ConvexShape : public CollisionShape { // -------------------- Friendship -------------------- // friend class GJKAlgorithm; - friend class EPAAlgorithm; + friend class SATAlgorithm; }; // Return true if the collision shape is convex, false if it is concave diff --git a/src/mathematics/mathematics_functions.cpp b/src/mathematics/mathematics_functions.cpp index 07e3530f..36ba113a 100644 --- a/src/mathematics/mathematics_functions.cpp +++ b/src/mathematics/mathematics_functions.cpp @@ -60,6 +60,11 @@ Vector3 reactphysics3d::clamp(const Vector3& vector, decimal maxLength) { return vector; } +/// Return true if two vectors are parallel +bool reactphysics3d::areParallelVectors(const Vector3& vector1, const Vector3& vector2) { + return vector1.cross(vector2).lengthSquare() < decimal(0.00001); +} + /// Compute and return a point on segment from "segPointA" and "segPointB" that is closest to point "pointC" Vector3 reactphysics3d::computeClosestPointOnSegment(const Vector3& segPointA, const Vector3& segPointB, const Vector3& pointC) { diff --git a/src/mathematics/mathematics_functions.h b/src/mathematics/mathematics_functions.h index 639e8d3b..44ce9cf4 100644 --- a/src/mathematics/mathematics_functions.h +++ b/src/mathematics/mathematics_functions.h @@ -76,6 +76,9 @@ inline bool sameSign(decimal a, decimal b) { return a * b >= decimal(0.0); } +/// Return true if two vectors are parallel +bool areParallelVectors(const Vector3& vector1, const Vector3& vector2); + /// Clamp a vector such that it is no longer than a given maximum length Vector3 clamp(const Vector3& vector, decimal maxLength);