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

Fix issue with ConvexMeshShape and BoxShape falling through ConcaveMeshShape and HeightFieldShape

Browse Source
This commit is contained in:
Daniel Chappuis 2018-07-01 15:47:50 +02:00
parent 5a2f84295e
commit 45f86ea543
3 changed files with 204 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -479,10 +479,7 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 separatingEdge1A, separatingEdge1B; Vector3 separatingEdge1A, separatingEdge1B;
Vector3 separatingEdge2A, separatingEdge2B; Vector3 separatingEdge2A, separatingEdge2B;
Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space; Vector3 minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
bool isShape1Triangle = polyhedron1->getName() == CollisionShapeName::TRIANGLE;
// True if the shapes were overlapping in the previous frame and are
// still overlapping on the same axis in this frame
bool isTemporalCoherenceValid = false;
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo(); LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
@ -499,18 +496,16 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2,
lastFrameCollisionInfo->satMinAxisFaceIndex); lastFrameCollisionInfo->satMinAxisFaceIndex);
// If the previous axis is a separating axis
if (penetrationDepth <= decimal(0.0)) {
// Return no collision // 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
return false; return false;
} }
// The two shapes are overlapping as in the previous frame and on the same axis, therefore // The two shapes were overlapping in the previous frame and still seem to overlap in this one
// we will skip the entire SAT algorithm because the minimum separating axis did not change if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth; minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@ -518,7 +513,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
isMinPenetrationFaceNormalPolyhedron1 = true; isMinPenetrationFaceNormalPolyhedron1 = true;
// Compute the contact points between two faces of two convex polyhedra. // 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, if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) { narrowPhaseInfo, minPenetrationDepth)) {
@ -527,13 +521,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; 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
return true; 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 // The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
isTemporalCoherenceValid = false;
}
} }
} }
else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth) else if (lastFrameCollisionInfo->satIsAxisFacePolyhedron2) { // If the previous separating axis (or axis with minimum penetration depth)
@ -542,18 +535,15 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, decimal penetrationDepth = testSingleFaceDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1,
lastFrameCollisionInfo->satMinAxisFaceIndex); lastFrameCollisionInfo->satMinAxisFaceIndex);
// If the previous axis is a separating axis // If the previous axis was a separating axis and is still a separating axis in this frame
if (penetrationDepth <= decimal(0.0)) { if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Return no collision // Return no collision without running the whole SAT algorithm
return false; return false;
} }
// The two shapes are overlapping as in the previous frame and on the same axis, therefore // The two shapes were overlapping in the previous frame and still seem to overlap in this one
// we will skip the entire SAT algorithm because the minimum separating axis did not change if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding;
if (isTemporalCoherenceValid) {
minPenetrationDepth = penetrationDepth; minPenetrationDepth = penetrationDepth;
minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex; minFaceIndex = lastFrameCollisionInfo->satMinAxisFaceIndex;
@ -561,7 +551,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
isMinPenetrationFaceNormalPolyhedron1 = false; isMinPenetrationFaceNormalPolyhedron1 = false;
// Compute the contact points between two faces of two convex polyhedra. // 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, if(computePolyhedronVsPolyhedronFaceContactPoints(isMinPenetrationFaceNormalPolyhedron1, polyhedron1, polyhedron2,
polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex, polyhedron1ToPolyhedron2, polyhedron2ToPolyhedron1, minFaceIndex,
narrowPhaseInfo, minPenetrationDepth)) { narrowPhaseInfo, minPenetrationDepth)) {
@ -570,13 +559,12 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1; lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = !isMinPenetrationFaceNormalPolyhedron1;
lastFrameCollisionInfo->satMinAxisFaceIndex = minFaceIndex; 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
return true; 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 // The contact manifold is empty. Therefore, we have 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 else { // If the previous separating axis (or axis with minimum penetration depth) was the cross product of two edges
@ -584,8 +572,6 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index); const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(lastFrameCollisionInfo->satMinEdge1Index);
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index); const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(lastFrameCollisionInfo->satMinEdge2Index);
Vector3 separatingAxisPolyhedron2Space;
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex); const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A; const Vector3 edge1Direction = edge1B - edge1A;
@ -593,138 +579,174 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A; const Vector3 edge2Direction = edge2B - edge2A;
// Compute the penetration depth // If the two edges build a minkowski face (and the cross product is
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(), // therefore a candidate for separating axis
edge1Direction, edge2Direction, separatingAxisPolyhedron2Space); if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
// If the previous axis is a separating axis Vector3 separatingAxisPolyhedron2Space;
if (penetrationDepth <= decimal(0.0)) {
// Return no collision // Compute the penetration depth along the previous axis
return false; const Vector3 polyhedron1Centroid = polyhedron1ToPolyhedron2 * polyhedron1->getCentroid();
} decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron1Centroid, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, isShape1Triangle, separatingAxisPolyhedron2Space);
// The two shapes are overlapping as in the previous frame and on the same axis, therefore // If the shapes were not overlapping in the previous frame and are still not
// we will skip the entire SAT algorithm because the minimum separating axis did not change // overlapping in the current one
isTemporalCoherenceValid = lastFrameCollisionInfo->wasColliding; if (!lastFrameCollisionInfo->wasColliding && penetrationDepth <= decimal(0.0)) {
// Temporal coherence is valid only if the two edges build a minkowski // We have found a separating axis without running the whole SAT algorithm
// face (and the cross product is therefore a candidate for separating axis return false;
if (isTemporalCoherenceValid && !testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) { }
isTemporalCoherenceValid = false;
}
if (isTemporalCoherenceValid) { // If the shapes were overlapping on the previous axis and still seem to overlap in this frame
if (lastFrameCollisionInfo->wasColliding && penetrationDepth > decimal(0.0)) {
minPenetrationDepth = penetrationDepth; // Compute the closest points between the two edges (in the local-space of poylhedron 2)
isMinPenetrationFaceNormal = false; Vector3 closestPointPolyhedron1Edge, closestPointPolyhedron2Edge;
isMinPenetrationFaceNormalPolyhedron1 = false; computeClosestPointBetweenTwoSegments(edge1A, edge1B, edge2A, edge2B,
minSeparatingEdge1Index = lastFrameCollisionInfo->satMinEdge1Index; closestPointPolyhedron1Edge, closestPointPolyhedron2Edge);
minSeparatingEdge2Index = lastFrameCollisionInfo->satMinEdge2Index;
separatingEdge1A = edge1A; // Here we try to project the closest point on edge1 onto the segment of edge 2 to see if
separatingEdge1B = edge1B; // the projected point falls onto the segment. We also try to project the closest point
separatingEdge2A = edge2A; // on edge 2 to see if it falls onto the segment of edge 1. If one of the point does not
separatingEdge2B = edge2B; // fall onto the opposite segment, it means the edges are not colliding (the contact manifold
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space; // 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)) {
// Compute the contact point on polyhedron 1 edge in the local-space of polyhedron 1
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal
// We use the direction from the centroid to the edge of the shape that is not a triangle
// to avoid possible degeneracies when axis direction is orthogonal to triangle normal
Vector3 normal;
if (isShape1Triangle) {
normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
}
else {
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normal.getUnit();
// 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,
penetrationDepth, normalWorld);
// Create the contact point
narrowPhaseInfo->addContactPoint(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
return true;
}
// The contact manifold is empty. Therefore, we have to run the whole SAT algorithm again
}
} }
} }
} }
// We the shapes are still overlapping in the same axis as in // Test all the face normals of the polyhedron 1 for separating axis
// the previous frame, we skip the whole SAT algorithm uint faceIndex;
if (!isTemporalCoherenceValid) { decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex);
if (penetrationDepth <= decimal(0.0)) {
// Test all the face normals of the polyhedron 1 for separating axis lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true;
uint faceIndex; lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
decimal penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron1, polyhedron2, polyhedron1ToPolyhedron2, faceIndex); lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
if (penetrationDepth <= decimal(0.0)) {
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = true; // We have found a separating axis
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; return false;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; }
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = true;
}
// We have found a separating axis // Test all the face normals of the polyhedron 2 for separating axis
return false; penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex);
} if (penetrationDepth <= decimal(0.0)) {
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 lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
penetrationDepth = testFacesDirectionPolyhedronVsPolyhedron(polyhedron2, polyhedron1, polyhedron2ToPolyhedron1, faceIndex); lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true;
if (penetrationDepth <= decimal(0.0)) { lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex;
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; // We have found a separating axis
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = true; return false;
lastFrameCollisionInfo->satMinAxisFaceIndex = faceIndex; }
if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
isMinPenetrationFaceNormal = true;
minPenetrationDepth = penetrationDepth;
minFaceIndex = faceIndex;
isMinPenetrationFaceNormalPolyhedron1 = false;
}
// We have found a separating axis // Test the cross products of edges of polyhedron 1 with edges of polyhedron 2 for separating axis
return false; for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) {
}
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 // Get an edge of polyhedron 1
for (uint i=0; i < polyhedron1->getNbHalfEdges(); i += 2) { const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i);
// Get an edge of polyhedron 1 const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex);
const HalfEdgeStructure::Edge& edge1 = polyhedron1->getHalfEdge(i); const Vector3 edge1B = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(polyhedron1->getHalfEdge(edge1.nextEdgeIndex).vertexIndex);
const Vector3 edge1Direction = edge1B - edge1A;
const Vector3 edge1A = polyhedron1ToPolyhedron2 * polyhedron1->getVertexPosition(edge1.vertexIndex); for (uint j=0; j < polyhedron2->getNbHalfEdges(); j += 2) {
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
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j);
// Get an edge of polyhedron 2 const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex);
const HalfEdgeStructure::Edge& edge2 = polyhedron2->getHalfEdge(j); const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex);
const Vector3 edge2Direction = edge2B - edge2A;
const Vector3 edge2A = polyhedron2->getVertexPosition(edge2.vertexIndex); // If the two edges build a minkowski face (and the cross product is
const Vector3 edge2B = polyhedron2->getVertexPosition(polyhedron2->getHalfEdge(edge2.nextEdgeIndex).vertexIndex); // therefore a candidate for separating axis
const Vector3 edge2Direction = edge2B - edge2A; if (testEdgesBuildMinkowskiFace(polyhedron1, edge1, polyhedron2, edge2, polyhedron1ToPolyhedron2)) {
// If the two edges build a minkowski face (and the cross product is Vector3 separatingAxisPolyhedron2Space;
// 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);
// Compute the penetration depth if (penetrationDepth <= decimal(0.0)) {
decimal penetrationDepth = computeDistanceBetweenEdges(edge1A, edge2A, polyhedron2->getCentroid(),
edge1Direction, edge2Direction, separatingAxisPolyhedron2Space);
if (penetrationDepth <= decimal(0.0)) { lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false;
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false;
lastFrameCollisionInfo->satMinEdge1Index = i;
lastFrameCollisionInfo->satMinEdge2Index = j;
lastFrameCollisionInfo->satIsAxisFacePolyhedron1 = false; // We have found a separating axis
lastFrameCollisionInfo->satIsAxisFacePolyhedron2 = false; return false;
lastFrameCollisionInfo->satMinEdge1Index = i; }
lastFrameCollisionInfo->satMinEdge2Index = j;
// We have found a separating axis if (penetrationDepth < minPenetrationDepth - SAME_SEPARATING_AXIS_BIAS) {
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;
}
minPenetrationDepth = penetrationDepth;
isMinPenetrationFaceNormalPolyhedron1 = false;
isMinPenetrationFaceNormal = false;
minSeparatingEdge1Index = i;
minSeparatingEdge2Index = j;
separatingEdge1A = edge1A;
separatingEdge1B = edge1B;
separatingEdge2A = edge2A;
separatingEdge2B = edge2B;
minEdgeVsEdgeSeparatingAxisPolyhedron2Space = separatingAxisPolyhedron2Space;
} }
} }
} }
@ -776,7 +798,17 @@ bool SATAlgorithm::testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseIn
Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge; Vector3 closestPointPolyhedron1EdgeLocalSpace = polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge;
// Compute the world normal // Compute the world normal
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space; // We use the direction from the centroid to the edge of the shape that is not a triangle
// to avoid possible degeneracies when axis direction is orthogonal to triangle normal
Vector3 normal;
if (isShape1Triangle) {
normal = polyhedron2->getCentroid() - closestPointPolyhedron2Edge;
}
else {
normal = polyhedron1ToPolyhedron2.getOrientation() * ((polyhedron2ToPolyhedron1 * closestPointPolyhedron1Edge) - polyhedron1->getCentroid());
}
//Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * minEdgeVsEdgeSeparatingAxisPolyhedron2Space;
Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normal.getUnit();
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle // Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2, TriangleShape::computeSmoothTriangleMeshContact(narrowPhaseInfo->collisionShape1, narrowPhaseInfo->collisionShape2,
@ -916,9 +948,10 @@ bool SATAlgorithm::computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPene
// Compute and return the distance between the two edges in the direction of the candidate separating axis // 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& polyhedron2Centroid, decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A,
const Vector3& polyhedron1Centroid, const Vector3& polyhedron2Centroid,
const Vector3& edge1Direction, const Vector3& edge2Direction, const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxisPolyhedron2Space) const { bool isShape1Triangle, Vector3& outSeparatingAxisPolyhedron2Space) const {
RP3D_PROFILE("SATAlgorithm::computeDistanceBetweenEdges", mProfiler); RP3D_PROFILE("SATAlgorithm::computeDistanceBetweenEdges", mProfiler);
@ -933,7 +966,24 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V
Vector3 axis = edge1Direction.cross(edge2Direction).getUnit(); Vector3 axis = edge1Direction.cross(edge2Direction).getUnit();
// Make sure the axis direction is going from first to second polyhedron // Make sure the axis direction is going from first to second polyhedron
if (axis.dot(edge2A - polyhedron2Centroid) > decimal(0.0)) { 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; axis = -axis;
} }
@ -943,6 +993,7 @@ decimal SATAlgorithm::computeDistanceBetweenEdges(const Vector3& edge1A, const V
return -axis.dot(edge2A - edge1A); return -axis.dot(edge2A - edge1A);
} }
// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron // Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, decimal SATAlgorithm::testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2, const ConvexPolyhedronShape* polyhedron2,

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

@ -83,9 +83,10 @@ class SATAlgorithm {
const Vector3& bCrossA, const Vector3& dCrossC) const; const Vector3& bCrossA, const Vector3& dCrossC) const;
/// Compute and return the distance between the two edges in the direction of the candidate separating axis /// Compute and return the distance between the two edges in the direction of the candidate separating axis
decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid, decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A,
const Vector3& polyhedron1Centroid, const Vector3& polyhedron2Centroid,
const Vector3& edge1Direction, const Vector3& edge2Direction, const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxis) const; bool isShape1Triangle, Vector3& outSeparatingAxis) const;
/// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron /// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,

18
src/collision/shapes/TriangleShape.cpp
View File

@ -62,13 +62,17 @@ TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNor
mVerticesNormals[2] = verticesNormals[2]; mVerticesNormals[2] = verticesNormals[2];
// Faces // Faces
for (uint i=0; i<2; i++) { mFaces[0].faceVertices.reserve(3);
mFaces[i].faceVertices.reserve(3); mFaces[0].faceVertices.add(0);
mFaces[i].faceVertices.add(0); mFaces[0].faceVertices.add(1);
mFaces[i].faceVertices.add(1); mFaces[0].faceVertices.add(2);
mFaces[i].faceVertices.add(2); mFaces[0].edgeIndex = 0;
mFaces[i].edgeIndex = i;
} mFaces[1].faceVertices.reserve(3);
mFaces[1].faceVertices.add(0);
mFaces[1].faceVertices.add(2);
mFaces[1].faceVertices.add(1);
mFaces[1].edgeIndex = 1;
// Edges // Edges
for (uint i=0; i<6; i++) { for (uint i=0; i<6; i++) {