mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Working on temporal coherence in SAT (polyhedron vs polyhedron)

Browse Source
This commit is contained in:
Daniel Chappuis 2017-05-29 08:32:10 +02:00
parent 9d55255c56
commit 730b687877
14 changed files with 502 additions and 115 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -88,6 +88,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"

13
src/collision/CollisionDetection.cpp
View File

@ -241,7 +241,7 @@ void CollisionDetection::computeNarrowPhase() {
// If there is no collision algorithm between those two kinds of shapes, skip it
if (narrowPhaseAlgorithm != nullptr) {
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
@ -268,7 +268,15 @@ void CollisionDetection::computeNarrowPhase() {
// Trigger a callback event for the new contact
if (mWorld->mEventListener != nullptr) mWorld->mEventListener->newContact(contactManifoldInfo);
currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = true;
}
else {
currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasColliding = false;
}
// The previous frame collision info is now valid
currentNarrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
}
currentNarrowPhaseInfo = currentNarrowPhaseInfo->next;
@ -834,6 +842,9 @@ void CollisionDetection::testCollision(CollisionCallback* callback) {
// Report the contact to the user
callback->notifyContact(collisionInfo);
}
// The previous frame collision info is now valid
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().isValid = true;
}
NarrowPhaseInfo* currentNarrowPhaseInfo = narrowPhaseInfo;

14
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp
View File

@ -44,6 +44,9 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
SATAlgorithm satAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@ -104,9 +107,11 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
break;
}
}
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
// Return true
return true;
}
@ -115,7 +120,12 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* na
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
return satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
return isColliding;
}
return false;

48
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,48 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
// Libraries
#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
return isColliding;
}

71
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,71 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
* *
* This software is provided 'as-is', without any express or implied warranty. *
* In no event will the authors be held liable for any damages arising from the *
* use of this software. *
* *
* Permission is granted to anyone to use this software for any purpose, *
* including commercial applications, and to alter it and redistribute it *
* freely, subject to the following restrictions: *
* *
* 1. The origin of this software must not be misrepresented; you must not claim *
* that you wrote the original software. If you use this software in a *
* product, an acknowledgment in the product documentation would be *
* appreciated but is not required. *
* *
* 2. Altered source versions must be plainly marked as such, and must not be *
* misrepresented as being the original software. *
* *
* 3. This notice may not be removed or altered from any source distribution. *
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two convex polyhedra.
*/
class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};
}
#endif

9
src/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -56,6 +56,15 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mSphereVsConvexPolyhedronAlgorithm;
}
// Capsule vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mCapsuleVsConvexPolyhedronAlgorithm;
}
// Convex Polyhedron vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON &&
shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mConvexPolyhedronVsConvexPolyhedronAlgorithm;
}
return nullptr;
}

16
src/collision/narrowphase/DefaultCollisionDispatch.h
View File

@ -33,6 +33,8 @@
#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SphereVsCapsuleAlgorithm.h"
#include "CapsuleVsCapsuleAlgorithm.h"
#include "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
namespace reactphysics3d {
@ -50,14 +52,20 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
/// Sphere vs Convex Mesh collision algorithm
SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
/// Capsule vs Capsule collision algorithm
CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
/// Sphere vs Capsule collision algorithm
SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
/// Capsule vs Capsule collision algorithm
CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
/// Sphere vs Convex Polyhedron collision algorithm
SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
/// Capsule vs Convex Polyhedron collision algorithm
CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm;
/// Convex Polyhedron vs Convex Polyhedron collision algorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm;
public:

11
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -93,7 +93,14 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
VoronoiSimplex simplex;
// Get the previous point V (last cached separating axis)
Vector3 v = narrowPhaseInfo->overlappingPair->getCachedSeparatingAxis();
Vector3 v;
LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
if (lastFrameInfo.isValid && lastFrameInfo.wasUsingGJK) {
v = lastFrameInfo.gjkSeparatingAxis;
}
else {
v.setAllValues(0, 1, 0);
}
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@ -113,7 +120,7 @@ GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(const NarrowPhaseInfo* narro
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
narrowPhaseInfo->overlappingPair->setCachedSeparatingAxis(v);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().gjkSeparatingAxis = v;
// No intersection, we return
return GJKResult::SEPARATED;

333
src/collision/narrowphase/SAT/SATAlgorithm.cpp
View File

@ -29,6 +29,7 @@
#include "collision/PolyhedronMesh.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/SphereShape.h"
#include "engine/OverlappingPair.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@ -329,7 +330,8 @@ bool SATAlgorithm::isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, c
}
// Test collision between two convex polyhedrons
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const {
bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) const {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
@ -344,94 +346,235 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
uint minFaceIndex = 0;
bool isMinPenetrationFaceNormal = false;
bool isMinPenetrationFaceNormalPolyhedron1 = false;
uint minSeparatingEdge1Index, minSeparatingEdge2Index;
Vector3 separatingEdge1A, separatingEdge1B;
Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Test all the face normals of the polyhedron 1 for separating axis
uint faceIndex;
decimal penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
LastFrameCollisionInfo& lastFrameInfo = narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo();
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
// True if the shapes were overlapping in the previous frame and are
// still overlapping on the same axis in this frame
bool isTemporalCoherenceValid = false;
// Test all the face normals of the polyhedron 2 for separating axis
penetrationDepth = testFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
// If the shapes are not triangles (no temporal coherence for triangle collision because we do not store previous
// frame collision data per triangle)
if (polyhedron1->getType() != CollisionShapeType::TRIANGLE && polyhedron2->getType() != CollisionShapeType::TRIANGLE) {
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = false;
}
// If the last frame collision info is valid and was also using SAT algorithm
if (lastFrameInfo.isValid && lastFrameInfo.wasUsingSAT) {
// Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
// We perform temporal coherence, we check if there is still an overlapping along the previous minimum separating
// axis. If it is the case, we directly report the collision without executing the whole SAT algorithm again. If
// the shapes are still separated along this axis, we directly exit with no collision.
// Get an edge of polyhedron 1
HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
// Get an edge of polyhedron 2
HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
// Compute the penetration depth
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
// If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 1
if (lastFrameInfo.satIsAxisFacePolyhedron1) {
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
lastFrameInfo.satMinAxisFaceIndex);
// If the previous axis is a separating axis
if (penetrationDepth <= decimal(0.0)) {
// We have found a separating axis
// Return no collision
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
// The two shapes are overlapping as in the previous frame and on the same axis, therefore
// we will skip the entire SAT algorithm because the minimum separating axis did not change
isTemporalCoherenceValid = lastFrameInfo.wasColliding;
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
}
else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
// was a face normal of polyhedron 2
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
lastFrameInfo.satMinAxisFaceIndex);
// If the previous axis is a separating axis
if (penetrationDepth <= decimal(0.0)) {
// Return no collision
return false;
}
// The two shapes are overlapping as in the previous frame and on the same axis, therefore
// we will skip the entire SAT algorithm because the minimum separating axis did not change
isTemporalCoherenceValid = lastFrameInfo.wasColliding;
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = false;
}
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(lastFrameInfo.satMinEdge1Index);
HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(lastFrameInfo.satMinEdge2Index);
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// Compute the penetration depth
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
// If the previous axis is a separating axis
if (penetrationDepth <= decimal(0.0)) {
// Return no collision
return false;
}
// The two shapes are overlapping as in the previous frame and on the same axis, therefore
// we will skip the entire SAT algorithm because the minimum separating axis did not change
isTemporalCoherenceValid = lastFrameInfo.wasColliding;
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormal = false;
isMinPenetrationFaceNormalPolyhedron1 = false;
minSeparatingEdge1Index = lastFrameInfo.satMinEdge1Index;
minSeparatingEdge2Index = lastFrameInfo.satMinEdge2Index;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
}
}
}
}
}
// We the shapes are still overlapping in the same axis as in
// the previous frame, we skip the whole SAT algorithm
if (isTemporalCoherenceValid) {
// Test all the face normals of the polyhedron 1 for separating axis
uint faceIndex;
decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
lastFrameInfo.satIsAxisFacePolyhedron1 = true;
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
lastFrameInfo.satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
// Test all the face normals of the polyhedron 2 for separating axis
penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
lastFrameInfo.satIsAxisFacePolyhedron2 = true;
lastFrameInfo.satMinAxisFaceIndex = faceIndex;
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = false;
}
// Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
// Get an edge of polyhedron 1
HalfEdgeStructure::Edge edge1 = polyhedron1->getHalfEdge(i);
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
// Get an edge of polyhedron 2
HalfEdgeStructure::Edge edge2 = polyhedron2->getHalfEdge(j);
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
// If the two edges build a minkowski face (and the cross product is
// therefore a candidate for separating axis
if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
Vector3 separatingAxisPolyhedron2Space;
// Compute the penetration depth
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
if (penetrationDepth <= decimal(0.0)) {
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
lastFrameInfo.satMinEdge1Index = i;
lastFrameInfo.satMinEdge2Index = j;
// We have found a separating axis
return false;
}
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
minSeparatingEdge1Index = i;
minSeparatingEdge2Index = j;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
}
}
}
}
}
// Here we know the shapes are overlapping on a given minimum separating axis.
// Now, we will clip the shapes along this axis to find the contact points
assert(minPenetrationDepth > decimal(0.0));
assert((isMinPenetrationFaceNormal && minFaceIndex >= 0) || !isMinPenetrationFaceNormal);
// If the separation axis is a face normal
// If the minimum separating axis is a face normal
if (isMinPenetrationFaceNormal) {
const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
@ -439,6 +582,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
const Transform& incidentToReferenceTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2ToPolyhedron1 : polyhedron1ToPolyhedron2;
assert(minPenetrationDepth > decimal(0.0));
const Vector3 axisReferenceSpace = referencePolyhedron->getFaceNormal(minFaceIndex);
const Vector3 axisIncidentSpace = referenceToIncidentTransform.getOrientation() * axisReferenceSpace;
@ -491,8 +636,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
} while (currentEdgeIndex != firstEdgeIndex);
assert(planesNormals.size() > 0);
assert(planesNormals.size() == planesPoints.size());
// Clip the reference faces with the adjacent planes of the reference face
std::vector<Vector3> clipPolygonVertices = clipPolygonWithPlanes(polygonVertices, planesPoints, planesNormals);
assert(clipPolygonVertices.size() > 0);
// We only keep the clipped points that are below the reference face
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(firstEdgeIndex);
@ -514,6 +663,10 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
}
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
@ -531,6 +684,11 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowP
// Create the contact point
contactManifoldInfo.addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
}
return true;
@ -583,34 +741,47 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V
return -axis.dot(edge2A - edge1A);
}
// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint faceIndex) const {
HalfEdgeStructure::Face face = polyhedron1->getFace(faceIndex);
// Get the face normal
const Vector3 faceNormal = polyhedron1->getFaceNormal(faceIndex);
// Convert the face normal into the local-space of polyhedron 2
const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
// Get the support point of polyhedron 2 in the inverse direction of face normal
const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr);
// Compute the penetration depth
const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space);
return penetrationDepth;
}
// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
decimal SATAlgorithm::testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint& minFaceIndex) const {
decimal SATAlgorithm::testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint& minFaceIndex) const {
decimal minPenetrationDepth = DECIMAL_LARGEST;
// For each face of the first polyhedron
for (uint f = 0; f < polyhedron1->getNbFaces(); f++) {
HalfEdgeStructure::Face face = polyhedron1->getFace(f);
// Get the face normal
const Vector3 faceNormal = polyhedron1->getFaceNormal(f);
// Convert the face normal into the local-space of polyhedron 2
const Vector3 faceNormalPolyhedron2Space = polyhedron1ToPolyhedron2.getOrientation() * faceNormal;
// Get the support point of polyhedron 2 in the inverse direction of face normal
const Vector3 supportPoint = polyhedron2->getLocalSupportPointWithoutMargin(-faceNormalPolyhedron2Space, nullptr);
// Compute the penetration depth
const Vector3 faceVertex = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(face.faceVertices[0]);
decimal penetrationDepth = (faceVertex - supportPoint).dot(faceNormalPolyhedron2Space);
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, f);
// If the penetration depth is negative, we have found a separating axis
if (penetrationDepth <= decimal(0.0)) {
minFaceIndex = f;
return penetrationDepth;
}

15
src/collision/narrowphase/SAT/SATAlgorithm.h
View File

@ -66,6 +66,17 @@ class SATAlgorithm {
const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxis) const;
/// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint faceIndex) const;
/// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
public :
// -------------------- Methods -------------------- //
@ -101,10 +112,6 @@ class SATAlgorithm {
/// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(const NarrowPhaseInfo* narrowPhaseInfo, ContactManifoldInfo& contactManifoldInfo) const;
/// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
decimal testFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
};
}

12
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp
View File

@ -31,7 +31,7 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection a sphere and a convex polyhedron
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
@ -41,6 +41,9 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
GJKAlgorithm gjkAlgorithm;
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, contactManifoldInfo);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = true;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
@ -53,7 +56,12 @@ bool SphereVsConvexPolyhedronAlgorithm::testCollision(const NarrowPhaseInfo* nar
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm;
return satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, contactManifoldInfo);
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingGJK = false;
narrowPhaseInfo->overlappingPair->getLastFrameCollisionInfo().wasUsingSAT = true;
return isColliding;
}
return false;

2
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h
View File

@ -60,7 +60,7 @@ class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection a sphere and a convex polyhedron
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
virtual bool testCollision(const NarrowPhaseInfo* narrowPhaseInfo,
ContactManifoldInfo& contactManifoldInfo) override;
};

3
src/engine/OverlappingPair.cpp
View File

@ -32,7 +32,6 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, PoolAllocator& memoryAllocator)
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(0.0, 1.0, 0.0) {
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds) {
}

68
src/engine/OverlappingPair.h
View File

@ -37,6 +37,51 @@ namespace reactphysics3d {
// Type for the overlapping pair ID
using overlappingpairid = std::pair<uint, uint>;
// Structure LastFrameCollisionInfo
/**
* This structure contains collision info about the last frame.
* This is used for temporal coherence between frames.
*/
struct LastFrameCollisionInfo {
/// True if we have information about the previous frame
bool isValid;
/// True if the two shapes were colliding in the previous frame
bool wasColliding;
/// True if we were using GJK algorithm to check for collision in the previous frame
bool wasUsingGJK;
/// True if we were using SAT algorithm to check for collision in the previous frame
bool wasUsingSAT;
/// True if there was a narrow-phase collision
/// in the previous frame
bool wasCollidingLastFrame;
// ----- GJK Algorithm -----
/// Previous separating axis
Vector3 gjkSeparatingAxis;
// SAT Algorithm
bool satIsAxisFacePolyhedron1;
bool satIsAxisFacePolyhedron2;
uint satMinAxisFaceIndex;
uint satMinEdge1Index;
uint satMinEdge2Index;
/// Constructor
LastFrameCollisionInfo() {
isValid = false;
wasColliding = false;
wasUsingSAT = false;
wasUsingGJK = false;
}
};
// Class OverlappingPair
/**
* This class represents a pair of two proxy collision shapes that are overlapping
@ -54,8 +99,8 @@ class OverlappingPair {
/// Set of persistent contact manifolds
ContactManifoldSet mContactManifoldSet;
/// Cached previous separating axis
Vector3 mCachedSeparatingAxis;
/// Collision information about the last frame (for temporal coherence)
LastFrameCollisionInfo mLastFrameCollisionInfo;
public:
@ -83,11 +128,8 @@ class OverlappingPair {
/// Add a contact manifold
void addContactManifold(const ContactManifoldInfo& contactManifoldInfo);
/// Return the cached separating axis
Vector3 getCachedSeparatingAxis() const;
/// Set the cached separating axis
void setCachedSeparatingAxis(const Vector3& axis);
/// Return the last frame collision info
LastFrameCollisionInfo& getLastFrameCollisionInfo();
/// Return the number of contacts in the cache
uint getNbContactPoints() const;
@ -124,17 +166,11 @@ inline void OverlappingPair::addContactManifold(const ContactManifoldInfo& conta
mContactManifoldSet.addContactManifold(contactManifoldInfo);
}
// Return the cached separating axis
inline Vector3 OverlappingPair::getCachedSeparatingAxis() const {
return mCachedSeparatingAxis;
// Return the last frame collision info
inline LastFrameCollisionInfo& OverlappingPair::getLastFrameCollisionInfo() {
return mLastFrameCollisionInfo;
}
// Set the cached separating axis
inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
mCachedSeparatingAxis = axis;
}
// Return the number of contact points in the contact manifold
inline uint OverlappingPair::getNbContactPoints() const {
return mContactManifoldSet.getTotalNbContactPoints();