Fix temporal coherence in SAT algorithm between two convex polyhedra
This commit is contained in:
parent
673e487f14
commit
6a22b3a81d
|
@ -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
|
||||
|
|
|
@ -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 -------------------- //
|
||||
|
|
Loading…
Reference in New Issue
Block a user