/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2020 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 <reactphysics3d/collision/narrowphase/SAT/SATAlgorithm.h>
#include <reactphysics3d/constraint/ContactPoint.h>
#include <reactphysics3d/collision/PolyhedronMesh.h>
#include <reactphysics3d/collision/shapes/CapsuleShape.h>
#include <reactphysics3d/collision/shapes/SphereShape.h>
#include <reactphysics3d/engine/OverlappingPairs.h>
#include <reactphysics3d/collision/narrowphase/NarrowPhaseInfoBatch.h>
#include <reactphysics3d/collision/shapes/TriangleShape.h>
#include <reactphysics3d/configuration.h>
#include <reactphysics3d/utils/Profiler.h>
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Static variables initialization
const decimal SATAlgorithm::SEPARATING_AXIS_RELATIVE_TOLERANCE = decimal(1.002);
const decimal SATAlgorithm::SEPARATING_AXIS_ABSOLUTE_TOLERANCE = decimal(0.0005);
// Constructor
SATAlgorithm::SATAlgorithm(bool clipWithPreviousAxisIfStillColliding, MemoryAllocator& memoryAllocator)
: mClipWithPreviousAxisIfStillColliding(clipWithPreviousAxisIfStillColliding), mMemoryAllocator(memoryAllocator) {
#ifdef IS_RP3D_PROFILING_ENABLED
mProfiler = nullptr;
#endif
}
// Test collision between a sphere and a convex mesh
bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint32 batchStartIndex, uint32 batchNbItems) const {
bool isCollisionFound = false;
RP3D_PROFILE("SATAlgorithm::testCollisionSphereVsConvexPolyhedron()", mProfiler);
for (uint32 batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
bool isSphereShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE;
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the capsule collision shapes
const SphereShape* sphere = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
const Transform& polyhedronToWorldTransform = isSphereShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
// Get the transform from sphere local-space to polyhedron local-space
const Transform worldToPolyhedronTransform = polyhedronToWorldTransform.getInverse();
const Transform sphereToPolyhedronSpaceTransform = worldToPolyhedronTransform * sphereToWorldTransform;
// Transform the center of the sphere into the local-space of the convex polyhedron
const Vector3 sphereCenter = sphereToPolyhedronSpaceTransform.getPosition();
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint32 minFaceIndex = 0;
bool noContact = false;
// For each face of the convex mesh
for (uint32 f = 0; f < polyhedron->getNbFaces(); f++) {
// Compute the penetration depth of the shapes along the face normal direction
decimal penetrationDepth = computePolyhedronFaceVsSpherePenetrationDepth(f, polyhedron, sphere, sphereCenter);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
noContact = true;
break;
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = f;
}
}
if (noContact) {
continue;
}
// If we need to report contacts
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal;
Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius());
Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal,
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
isSphereShape1 ? contactPointPolyhedronLocal : contactPointSphereLocal);
}
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
}
return isCollisionFound;
}
// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
decimal SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth(uint32 faceIndex, const ConvexPolyhedronShape* polyhedron,
const SphereShape* sphere, const Vector3& sphereCenter) const {
RP3D_PROFILE("SATAlgorithm::computePolyhedronFaceVsSpherePenetrationDepth)", mProfiler);
// Get the face
const HalfEdgeStructure::Face& face = polyhedron->getFace(faceIndex);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(faceIndex);
Vector3 sphereCenterToFacePoint = polyhedron->getVertexPosition(face.faceVertices[0]) - sphereCenter;
decimal penetrationDepth = sphereCenterToFacePoint.dot(faceNormal) + sphere->getRadius();
return penetrationDepth;
}
// Test collision between a capsule and a convex mesh
bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint32 batchIndex) const {
RP3D_PROFILE("SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron()", mProfiler);
bool isCapsuleShape1 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE;
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2 : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform;
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform : narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
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);
const Vector3 capsuleSegmentAxis = capsuleSegB - capsuleSegA;
// Minimum penetration depth
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint32 minFaceIndex = 0;
bool isMinPenetrationFaceNormal = false;
Vector3 separatingAxisCapsuleSpace;
Vector3 separatingPolyhedronEdgeVertex1;
Vector3 separatingPolyhedronEdgeVertex2;
// For each face of the convex mesh
for (uint32 f = 0; f < polyhedron->getNbFaces(); f++) {
Vector3 outFaceNormalCapsuleSpace;
// Compute the penetration depth
const decimal penetrationDepth = computePolyhedronFaceVsCapsulePenetrationDepth(f, polyhedron, capsuleShape,
polyhedronToCapsuleTransform,
outFaceNormalCapsuleSpace);
// 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 = outFaceNormalCapsuleSpace;
}
}
// For each direction that is the cross product of the capsule inner segment and an edge of the polyhedron
for (uint32 e = 0; e < polyhedron->getNbHalfEdges(); e += 2) {
// Get an edge from the polyhedron (convert it into the capsule local-space)
const 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);
const 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)) {
Vector3 outAxis;
// Compute the penetration depth
const decimal penetrationDepth = computeEdgeVsCapsuleInnerSegmentPenetrationDepth(polyhedron, capsuleShape,
capsuleSegmentAxis, edgeVertex1,
edgeDirectionCapsuleSpace,
polyhedronToCapsuleTransform,
outAxis);
// 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;
isMinPenetrationFaceNormal = false;
separatingAxisCapsuleSpace = outAxis;
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;
Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
if (isCapsuleShape1) {
normalWorld = -normalWorld;
}
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) {
// If we need to report contacts
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfoBatch, batchIndex, isCapsuleShape1);
}
}
else { // The separating axis is the cross product of a polyhedron edge and the inner capsule segment
// If we need to report contacts
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// 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
Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * closestPointCapsuleInnerSegment) - separatingAxisCapsuleSpace * capsuleRadius;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule,
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
isCapsuleShape1 ? contactPointCapsule : closestPointPolyhedronEdge,
isCapsuleShape1 ? closestPointPolyhedronEdge : contactPointCapsule);
}
}
return true;
}
// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
decimal SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
const Vector3& edgeDirectionCapsuleSpace,
const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const {
RP3D_PROFILE("SATAlgorithm::computeEdgeVsCapsuleInnerSegmentPenetrationDepth)", mProfiler);
decimal penetrationDepth = DECIMAL_LARGEST;
// Compute the axis to test (cross product between capsule inner segment and polyhedron edge)
outAxis = capsuleSegmentAxis.cross(edgeDirectionCapsuleSpace);
// Skip separating axis test if polyhedron edge is parallel to the capsule inner segment
if (outAxis.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 (outAxis.dot(pointOnPolyhedronEdge - polyhedronCentroid) < 0) {
outAxis = -outAxis;
}
outAxis.normalize();
// Compute the penetration depth
const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outAxis);
const Vector3 capsuleSupportPointToEdgePoint = pointOnPolyhedronEdge - capsuleSupportPoint;
penetrationDepth = capsuleSupportPointToEdgePoint.dot(outAxis);
}
return penetrationDepth;
}
// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint32 polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
Vector3& outFaceNormalCapsuleSpace) const {
RP3D_PROFILE("SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth", mProfiler);
// Get the face
const HalfEdgeStructure::Face& face = polyhedron->getFace(polyhedronFaceIndex);
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(polyhedronFaceIndex);
// Compute the penetration depth (using the capsule support in the direction opposite to the face normal)
outFaceNormalCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
const Vector3 capsuleSupportPoint = capsule->getLocalSupportPointWithMargin(-outFaceNormalCapsuleSpace);
const Vector3 pointOnPolyhedronFace = polyhedronToCapsuleTransform * polyhedron->getVertexPosition(face.faceVertices[0]);
const Vector3 capsuleSupportPointToFacePoint = pointOnPolyhedronFace - capsuleSupportPoint;
const decimal penetrationDepth = capsuleSupportPointToFacePoint.dot(outFaceNormalCapsuleSpace);
return penetrationDepth;
}
// Compute the two contact points between a polyhedron and a capsule when the separating
// axis is a face normal of the polyhedron
bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint32 referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint32 batchIndex, bool isCapsuleShape1) const {
RP3D_PROFILE("SATAlgorithm::computeCapsulePolyhedronFaceContactPoints", mProfiler);
const HalfEdgeStructure::Face& face = polyhedron->getFace(referenceFaceIndex);
// Get the face normal
Vector3 faceNormal = polyhedron->getFaceNormal(referenceFaceIndex);
uint32 firstEdgeIndex = face.edgeIndex;
uint32 edgeIndex = firstEdgeIndex;
Array<Vector3> planesPoints(mMemoryAllocator, 2);
Array<Vector3> planesNormals(mMemoryAllocator, 2);
// For each adjacent edge of the separating face of the polyhedron
do {
const HalfEdgeStructure::Edge& edge = polyhedron->getHalfEdge(edgeIndex);
const HalfEdgeStructure::Edge& twinEdge = polyhedron->getHalfEdge(edge.twinEdgeIndex);
// Compute the edge vertices and edge direction
Vector3 edgeV1 = polyhedron->getVertexPosition(edge.vertexIndex);
Vector3 edgeV2 = polyhedron->getVertexPosition(twinEdge.vertexIndex);
Vector3 edgeDirection = edgeV2 - edgeV1;
// Compute the normal of the clipping plane for this edge
// The clipping plane is perpendicular to the edge direction and the reference face normal
Vector3 clipPlaneNormal = faceNormal.cross(edgeDirection);
// Construct a clipping plane for each adjacent edge of the separating face of the polyhedron
planesPoints.add(polyhedron->getVertexPosition(edge.vertexIndex));
planesNormals.add(clipPlaneNormal);
edgeIndex = edge.nextEdgeIndex;
} while(edgeIndex != firstEdgeIndex);
// First we clip the inner segment of the capsule with the four planes of the adjacent faces
Array<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals, mMemoryAllocator);
// Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
bool contactFound = false;
// For each of the two clipped points
const uint32 nbClipSegments = clipSegment.size();
for (uint32 i = 0; i < nbClipSegments; i++) {
// Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth)
const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);
// If the clipped point is one that produce this penetration depth, we keep it
if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
contactFound = true;
Vector3 contactPointPolyhedron = clipSegment[i] + delta;
// Project the clipped point into the capsule bounds
Vector3 contactPointCapsule = (polyhedronToCapsuleTransform * clipSegment[i]) - separatingAxisCapsuleSpace * capsuleRadius;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule,
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
penetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
isCapsuleShape1 ? contactPointCapsule : contactPointPolyhedron,
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
}
}
return contactFound;
}
// 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 two convex polyhedrons
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint32 batchStartIndex, uint32 batchNbItems) const {
RP3D_PROFILE("SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron()", mProfiler);
bool isCollisionFound = false;
for (uint32 batchIndex = batchStartIndex; batchIndex < batchStartIndex + batchNbItems; batchIndex++) {
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].nbContactPoints == 0);
const ConvexPolyhedronShape* polyhedron1 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1);
const ConvexPolyhedronShape* polyhedron2 = static_cast<const ConvexPolyhedronShape*>(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2);
const Transform polyhedron1ToPolyhedron2 = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getInverse() * narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform;
const Transform polyhedron2ToPolyhedron1 = polyhedron1ToPolyhedron2.getInverse();
decimal minPenetrationDepth = DECIMAL_LARGEST;
uint32 minFaceIndex = 0;
bool isMinPenetrationFaceNormal = false;
bool isMinPenetrationFaceNormalPolyhedron1 = false;
uint32 minSeparatingEdge1Index = 0;
uint32 minSeparatingEdge2Index = 0;
Vector3 separatingEdge1A, separatingEdge1B;
Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
const bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE;
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].lastFrameCollisionInfo;
// 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.
// If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 1
if (lastFrameCollisionInfo->satIsAxisFacePolyhedron1) {
const decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
lastFrameCollisionInfo->satMinAxisFaceIndex);
// If the previous axis was a separating axis and is still a separating axis in this frame
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Return no collision without running the whole SAT algorithm
continue;
}
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = true;
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfoBatch, batchIndex)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// The shapes are still overlapping in the previous axis (the contact manifold is not empty).
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
else if (lastFrameCollisionInfo->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,
lastFrameCollisionInfo->satMinAxisFaceIndex);
// If the previous axis was a separating axis and is still a separating axis in this frame
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Return no collision without running the whole SAT algorithm
continue;
}
// The two shapes were overlapping in the previous frame and still seem to overlap in this one
if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = false;
// Compute the contact points between two faces of two convex polyhedra.
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfoBatch, batchIndex)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// The shapes are still overlapping in the previous axis (the contact manifold is not empty).
// Therefore, we can return without running the whole SAT algorithm
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
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;
// 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 along the previous axis
const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
// If the shapes were not overlapping in the previous frame and are still not
// overlapping in the current one
if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// We have found a separating axis without running the whole SAT algorithm
continue;
}
// If the shapes were overlapping on the previous axis and still seem to overlap in this frame
if (lastFrameCollisionInfo->wasColliding && mClipWithPreviousAxisIfStillColliding && penetrationDepth > decimal(0.0) &&
penetrationDepth < DECIMAL_LARGEST) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
computeClosestPointBetweenTwoSegments(edge1A, edge1B, edge2A, edge2B,
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
// Here we try to project the closest point on edge1 onto the segment of edge 2 to see if
// the projected point falls onto the segment. We also try to project the closest point
// on edge 2 to see if it falls onto the segment of edge 1. If one of the point does not
// fall onto the opposite segment, it means the edges are not colliding (the contact manifold
// is empty). Therefore, we need to run the whole SAT algorithm again.
const Vector3 vec1 = closestPointPolyhedron1Edge - edge2A;
const Vector3 vec2 = closestPointPolyhedron2Edge - edge1A;
const decimal edge1LengthSquare = edge1Direction.lengthSquare();
const decimal edge2LengthSquare = edge2Direction.lengthSquare();
decimal t1 = vec1.dot(edge2Direction) / edge2LengthSquare;
decimal t2 = vec2.dot(edge1Direction) / edge1LengthSquare;
if (t1 >= decimal(0.0) && t1 <= decimal(1) && t2 >= decimal(0.0) && t2 <= decimal(1.0)) {
// If we need to report contact points
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * separatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
penetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, penetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
// The shapes are overlapping on the previous axis (the contact manifold is not empty). Therefore
// we return without running the whole SAT algorithm
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
continue;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
}
}
}
minPenetrationDepth = DECIMAL_LARGEST;
isMinPenetrationFaceNormal = false;
// Test all the face normals of the polyhedron 1 for separating axis
uint32 faceIndex1;
decimal penetrationDepth1 = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex1);
if (penetrationDepth1 <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex1;
// We have found a separating axis
continue;
}
// Test all the face normals of the polyhedron 2 for separating axis
uint32 faceIndex2;
decimal penetrationDepth2 = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex2);
if (penetrationDepth2 <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex2;
// We have found a separating axis
continue;
}
// Here we know that we have found penetration along both axis of a face of polyhedron1 and a face of
// polyhedron2. If the two penetration depths are almost the same, we need to make sure we always prefer
// one axis to the other for consistency between frames. This is to prevent the contact manifolds to switch
// from one reference axis to the other for a face to face resting contact for instance. This is better for
// stability. To do this, we use a relative and absolute bias to move penetrationDepth2 a little bit to the right.
// Now if:
// penetrationDepth1 < penetrationDepth2: Nothing happens and we use axis of polygon 1
// penetrationDepth1 ~ penetrationDepth2: Until penetrationDepth1 becomes significantly less than penetrationDepth2 we still use axis of polygon 1
// penetrationDepth1 >> penetrationDepth2: penetrationDepth2 is now significantly less than penetrationDepth1 and we use polygon 2 axis
if (penetrationDepth1 < penetrationDepth2 * SEPARATING_AXIS_RELATIVE_TOLERANCE + SEPARATING_AXIS_ABSOLUTE_TOLERANCE) {
// We use penetration axis of polygon 1
isMinPenetrationFaceNormal = true;
minPenetrationDepth = std::min(penetrationDepth1, penetrationDepth2);
minFaceIndex = faceIndex1;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
else {
// We use penetration axis of polygon 2
isMinPenetrationFaceNormal = true;
minPenetrationDepth = std::min(penetrationDepth1, penetrationDepth2);
minFaceIndex = faceIndex2;
isMinPenetrationFaceNormalPolyhedron1 = false;
}
bool separatingAxisFound = false;
// Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
for (uint32 i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
// Get an edge of polyhedron 1
const 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 (uint32 j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
// Get an edge of polyhedron 2
const 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
const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
if (penetrationDepth <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = i;
lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis
separatingAxisFound = true;
break;
}
// If the current minimum penetration depth is along a face normal axis (isMinPenetrationFaceNormal=true) and we have found a new
// smaller pentration depth along an edge-edge cross-product axis we want to favor the face normal axis because contact manifolds between
// faces have more contact points and therefore more stable than the single contact point of an edge-edge collision. It means that if the new minimum
// penetration depth from the edge-edge contact is only a little bit smaller than the current minPenetrationDepth (from a face contact), we favor
// the face contact and do not generate an edge-edge contact. However, if the new penetration depth from the edge-edge contact is really smaller than
// the current one, we generate an edge-edge contact.
// To do this, we use a relative and absolute bias to increase a little bit the new penetration depth from the edge-edge contact during the comparison test
if ((isMinPenetrationFaceNormal && penetrationDepth1 * SEPARATING_AXIS_RELATIVE_TOLERANCE + SEPARATING_AXIS_ABSOLUTE_TOLERANCE < minPenetrationDepth) ||
(!isMinPenetrationFaceNormal && penetrationDepth < minPenetrationDepth)) {
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
minSeparatingEdge1Index = i;
minSeparatingEdge2Index = j;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
}
}
}
if (separatingAxisFound) {
break;
}
}
if (separatingAxisFound) {
continue;
}
// 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));
// If the minimum separating axis is a face normal
if (isMinPenetrationFaceNormal) {
// Compute the contact points between two faces of two convex polyhedra.
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
minFaceIndex, narrowPhaseInfoBatch, batchIndex);
// There should be clipping points here. If it is not the case, it might be
// because of a numerical issue
if (!contactsFound) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
// Return no collision
continue;
}
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
// If we need to report contacts
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
// Compute the closest points between the two edges (in the local-space of poylhedron 2)
Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
computeClosestPointBetweenTwoSegments(separatingEdge1A, separatingEdge1B, separatingEdge2A, separatingEdge2B,
closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
Vector3 normalWorld = narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = minSeparatingEdge1Index;
lastFrameCollisionInfo->satMinEdge2Index = minSeparatingEdge2Index;
}
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
}
return isCollisionFound;
}
// Compute the contact points between two faces of two convex polyhedra.
/// The method returns true if contact points have been found
bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1,
const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, const Transform& polyhedron2ToPolyhedron1,
uint32 minFaceIndex, NarrowPhaseInfoBatch& narrowPhaseInfoBatch, uint32 batchIndex) const {
RP3D_PROFILE("SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints", mProfiler);
const ConvexPolyhedronShape* referencePolyhedron;
const ConvexPolyhedronShape* incidentPolyhedron;
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
if (isMinPenetrationFaceNormalPolyhedron1) {
referencePolyhedron = polyhedron1;
incidentPolyhedron = polyhedron2;
}
else {
referencePolyhedron = polyhedron2;
incidentPolyhedron = polyhedron1;
}
const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
// Compute the world normal
const Vector3 normalWorld = isMinPenetrationFaceNormalPolyhedron1 ? narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform.getOrientation() * axisReferenceSpace :
-(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform.getOrientation() * axisReferenceSpace);
// Get the reference face
const HalfEdgeStructure::Face& referenceFace = referencePolyhedron->getFace(minFaceIndex);
// Find the incident face on the other polyhedron (most anti-parallel face)
uint32 incidentFaceIndex = incidentPolyhedron->findMostAntiParallelFace(axisIncidentSpace);
// Get the incident face
const HalfEdgeStructure::Face& incidentFace = incidentPolyhedron->getFace(incidentFaceIndex);
const uint32 nbIncidentFaceVertices = incidentFace.faceVertices.size();
const uint32 nbMaxElements = nbIncidentFaceVertices * 2 * referenceFace.faceVertices.size();
Array<Vector3> verticesTemp1(mMemoryAllocator, nbMaxElements);
Array<Vector3> verticesTemp2(mMemoryAllocator, nbMaxElements);
// Get all the vertices of the incident face (in the reference local-space)
for (uint32 i=0; i < nbIncidentFaceVertices; i++) {
const Vector3 faceVertexIncidentSpace = incidentPolyhedron->getVertexPosition(incidentFace.faceVertices[i]);
verticesTemp1.add(incidentToReferenceTransform * faceVertexIncidentSpace);
}
// For each edge of the reference we use it to clip the incident face polygon using Sutherland-Hodgman algorithm
uint32 firstEdgeIndex = referenceFace.edgeIndex;
bool areVertices1Input = false;
uint32 nbOutputVertices;
uint32 currentEdgeIndex;
// Get the adjacent edge
const HalfEdgeStructure::Edge* currentEdge = &(referencePolyhedron->getHalfEdge(firstEdgeIndex));
Vector3 edgeV1 = referencePolyhedron->getVertexPosition(currentEdge->vertexIndex);
do {
// Switch the input/output arrays of vertices
areVertices1Input = !areVertices1Input;
// Get the adjacent edge
const HalfEdgeStructure::Edge* nextEdge = &(referencePolyhedron->getHalfEdge(currentEdge->nextEdgeIndex));
// Compute the edge vertices and edge direction
const Vector3 edgeV2 = referencePolyhedron->getVertexPosition(nextEdge->vertexIndex);
const Vector3 edgeDirection = edgeV2 - edgeV1;
// Compute the normal of the clipping plane for this edge
// The clipping plane is perpendicular to the edge direction and the reference face normal
const Vector3 planeNormal = axisReferenceSpace.cross(edgeDirection);
assert((areVertices1Input && verticesTemp1.size() > 0) || !areVertices1Input);
assert((!areVertices1Input && verticesTemp2.size() > 0) || areVertices1Input);
// Clip the incident face with one adjacent plane (corresponding to one edge) of the reference face
clipPolygonWithPlane(areVertices1Input ? verticesTemp1 : verticesTemp2, edgeV1, planeNormal, areVertices1Input ? verticesTemp2 : verticesTemp1);
currentEdgeIndex = currentEdge->nextEdgeIndex;
// Go to the next adjacent edge of the reference face
currentEdge = nextEdge;
edgeV1 = edgeV2;
// Clear the input array of vertices before the next loop
if (areVertices1Input) {
verticesTemp1.clear();
nbOutputVertices = verticesTemp2.size();
}
else {
verticesTemp2.clear();
nbOutputVertices = verticesTemp1.size();
}
} while (currentEdgeIndex != firstEdgeIndex && nbOutputVertices > 0);
// Reference to the output clipped polygon vertices
Array<Vector3>& clippedPolygonVertices = areVertices1Input ? verticesTemp2 : verticesTemp1;
// We only keep the clipped points that are below the reference face
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
bool contactPointsFound = false;
const uint32 nbClipPolygonVertices = clippedPolygonVertices.size();
for (uint32 i=0; i < nbClipPolygonVertices; i++) {
// Compute the penetration depth of this contact point (can be different from the minPenetration depth which is
// the maximal penetration depth of any contact point for this separating axis
decimal penetrationDepth = (referenceFaceVertex - clippedPolygonVertices[i]).dot(axisReferenceSpace);
// If the clip point is below the reference face
if (penetrationDepth > decimal(0.0)) {
contactPointsFound = true;
// If we need to report contacts
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
Vector3 outWorldNormal = normalWorld;
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * clippedPolygonVertices[i];
// Project the contact point onto the reference face
Vector3 contactPointReferencePolyhedron = projectPointOntoPlane(clippedPolygonVertices[i], axisReferenceSpace, referenceFaceVertex);
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape1, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].collisionShape2,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron,
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape1ToWorldTransform, narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].shape2ToWorldTransform,
penetrationDepth, outWorldNormal);
// Create a new contact point
narrowPhaseInfoBatch.addContactPoint(batchIndex, outWorldNormal, penetrationDepth,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointReferencePolyhedron : contactPointIncidentPolyhedron,
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
}
}
return contactPointsFound;
}
// Compute and return the distance between the two edges in the direction of the candidate separating axis
decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A,
const Vector3& polyhedron1Centroid, const Vector3& polyhedron2Centroid,
const Vector3& edge1Direction, const Vector3& edge2Direction,
bool isShape1Triangle, Vector3& outSeparatingAxisPolyhedron2Space) const {
RP3D_PROFILE("SATAlgorithm::computeDistanceBetweenEdges", mProfiler);
// If the two edges are parallel
if (areParallelVectors(edge1Direction, edge2Direction)) {
// Return a large penetration depth to skip those edges
return DECIMAL_LARGEST;
}
// Compute the candidate separating axis (cross product between two polyhedrons edges)
Vector3 axis = edge1Direction.cross(edge2Direction).getUnit();
// Make sure the axis direction is going from first to second polyhedron
decimal dotProd;
if (isShape1Triangle) {
// The shape 1 is a triangle. It is safer to use a vector from
// centroid to edge of the shape 2 because for a triangle, we
// can have a vector that is orthogonal to the axis
dotProd = axis.dot(edge2A - polyhedron2Centroid);
}
else {
// The shape 2 might be a triangle. It is safer to use a vector from
// centroid to edge of the shape 2 because for a triangle, we
// can have a vector that is orthogonal to the axis
dotProd = axis.dot(polyhedron1Centroid - edge1A);
}
if (dotProd > decimal(0.0)) {
axis = -axis;
}
outSeparatingAxisPolyhedron2Space = axis;
// Compute and return the distance between the edges
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,
uint32 faceIndex) const {
RP3D_PROFILE("SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron", mProfiler);
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);
// 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::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint& minFaceIndex) const {
RP3D_PROFILE("SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron", mProfiler);
decimal minPenetrationDepth = DECIMAL_LARGEST;
// For each face of the first polyhedron
for (uint32 f = 0; f < polyhedron1->getNbFaces(); f++) {
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;
}
// Check if we have found a new minimum penetration axis
if (penetrationDepth < minPenetrationDepth) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = f;
}
}
return minPenetrationDepth;
}
// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
bool SATAlgorithm::testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1,
const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
const Transform& polyhedron1ToPolyhedron2) const {
RP3D_PROFILE("SATAlgorithm::testEdgesBuildMinkowskiFace", mProfiler);
const Vector3 a = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(edge1.faceIndex);
const Vector3 b = polyhedron1ToPolyhedron2.getOrientation() * polyhedron1->getFaceNormal(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).faceIndex);
const Vector3 c = polyhedron2->getFaceNormal(edge2.faceIndex);
const Vector3 d = polyhedron2->getFaceNormal(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).faceIndex);
// Compute b.cross(a) using the edge direction
const Vector3 edge1Vertex1 = polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1Vertex2 = polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.twinEdgeIndex).vertexIndex);
const Vector3 bCrossA = polyhedron1ToPolyhedron2.getOrientation() * (edge1Vertex1 - edge1Vertex2);
// Compute d.cross(c) using the edge direction
const Vector3 edge2Vertex1 = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2Vertex2 = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.twinEdgeIndex).vertexIndex);
const Vector3 dCrossC = edge2Vertex1 - edge2Vertex2;
// Test if the two arcs of the Gauss Map intersect (therefore forming a minkowski face)
// Note that we negate the normals of the second polyhedron because we are looking at the
// Gauss map of the minkowski difference of the polyhedrons
return testGaussMapArcsIntersect(a, b, -c, -d, bCrossA, dCrossC);
}
// Return true if the arcs AB and CD on the Gauss Map (unit sphere) intersect
/// This is used to know if the edge between faces with normal A and B on first polyhedron
/// and edge between faces with normal C and D on second polygon create a face on the Minkowski
/// sum of both polygons. If this is the case, it means that the cross product of both edges
/// might be a separating axis.
bool SATAlgorithm::testGaussMapArcsIntersect(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d,
const Vector3& bCrossA, const Vector3& dCrossC) const {
RP3D_PROFILE("SATAlgorithm::testGaussMapArcsIntersect", mProfiler);
const decimal cba = c.dot(bCrossA);
const decimal dba = d.dot(bCrossA);
const decimal adc = a.dot(dCrossC);
const decimal bdc = b.dot(dCrossC);
return cba * dba < decimal(0.0) && adc * bdc < decimal(0.0) && cba * bdc > decimal(0.0);
}