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

Fix temporal coherence in SAT algorithm between two convex polyhedra

Browse Source
This commit is contained in:
Daniel Chappuis 2017-09-03 10:48:39 +02:00
parent 673e487f14
commit 6a22b3a81d
2 changed files with 164 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -489,6 +489,24 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = true;
// Compute the contact points between two faces of two convex polyhedra.
// If contact points have been found, we report them without running the whole SAT algorithm
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) {
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
return true;
}
else { // Contact points have not been found (the set of clipped points was empty)
// Therefore, we need to run the whole SAT algorithm again
isTemporalCoherenceValid = false;
}
}
}
else if (lastFrameInfo.satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
@ -513,6 +531,24 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
minFaceIndex = lastFrameInfo.satMinAxisFaceIndex;
isMinPenetrationFaceNormal = true;
isMinPenetrationFaceNormalPolyhedron1 = false;
// Compute the contact points between two faces of two convex polyhedra.
// If contact points have been found, we report them without running the whole SAT algorithm
if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) {
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
return true;
}
else { // Contact points have not been found (the set of clipped points was empty)
// Therefore, we need to run the whole SAT algorithm again
isTemporalCoherenceValid = false;
}
}
}
else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
@ -544,6 +580,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// we will skip the entire SAT algorithm because the minimum separating axis did not change
isTemporalCoherenceValid = lastFrameInfo.wasColliding;
// Temporal coherence is valid only if the two edges build a minkowski
// face (and the cross product is therefore a candidate for separating axis
if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
isTemporalCoherenceValid = false;
}
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth;
@ -672,6 +714,59 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
if (reportContacts) {
// Compute the contact points between two faces of two convex polyhedra.
bool contactsFound = computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1,
polyhedron2, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1,
minFaceIndex, narrowPhaseInfo, minPenetrationDepth);
assert(contactsFound);
}
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
if (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 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
}
return true;
}
// 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,
uint minFaceIndex, NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const {
const ConvexPolyhedronShape* referencePolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1 : polyhedron2;
const ConvexPolyhedronShape* incidentPolyhedron = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron2 : polyhedron1;
const Transform& referenceToIncidentTransform = isMinPenetrationFaceNormalPolyhedron1 ? polyhedron1ToPolyhedron2 : polyhedron2ToPolyhedron1;
@ -744,11 +839,14 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
// We only keep the clipped points that are below the reference face
const Vector3 referenceFaceVertex = referencePolyhedron->getVertexPosition(referencePolyhedron->getHalfEdge(firstEdgeIndex).vertexIndex);
std::vector<Vector3>::const_iterator itPoints;
bool contactPointsFound = false;
for (itPoints = clipPolygonVertices.begin(); itPoints != clipPolygonVertices.end(); ++itPoints) {
// If the clip point is bellow the reference face
if (((*itPoints) - referenceFaceVertex).dot(axisReferenceSpace) < decimal(0.0)) {
contactPointsFound = true;
// Convert the clip incident polyhedron vertex into the incident polyhedron local-space
Vector3 contactPointIncidentPolyhedron = referenceToIncidentTransform * (*itPoints);
@ -768,45 +866,8 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
isMinPenetrationFaceNormalPolyhedron1 ? contactPointIncidentPolyhedron : contactPointReferencePolyhedron);
}
}
}
lastFrameInfo.satIsAxisFacePolyhedron1 = isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameInfo.satMinAxisFaceIndex = minFaceIndex;
}
else { // If we have an edge vs edge contact
if (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 = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge,
narrowPhaseInfo->shape1ToWorldTransform, narrowPhaseInfo->shape2ToWorldTransform,
minPenetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(normalWorld, minPenetrationDepth,
closestPointPolyhedron1EdgeLocalSpace, closestPointPolyhedron2Edge);
}
lastFrameInfo.satIsAxisFacePolyhedron1 = false;
lastFrameInfo.satIsAxisFacePolyhedron2 = false;
lastFrameInfo.satMinEdge1Index = minSeparatingEdge1Index;
lastFrameInfo.satMinEdge2Index = minSeparatingEdge2Index;
}
return true;
return contactPointsFound;
}
// Find and return the index of the polyhedron face with the most anti-parallel face normal given a direction vector

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

@ -96,6 +96,13 @@ class SATAlgorithm {
const Vector3& edgeDirectionCapsuleSpace,
const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const;
/// Compute the contact points between two faces of two convex polyhedra.
bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2,
const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex,
NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const;
public :
// -------------------- Methods -------------------- //