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

Fix robustness issue in SAT Algorithm (convex polyhedron vs capsule)

Browse Source
This commit is contained in:
Daniel Chappuis 2017-11-01 23:09:02 +01:00
parent 6a69ef76c5
commit ad0f805f53
3 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -119,10 +119,13 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
} }
// Compute and create two contact points // Compute and create two contact points
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1); narrowPhaseInfo, isCapsuleShape1);
if (!contactsFound) {
return false;
}
break; break;
} }

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

@ -256,7 +256,7 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
if (reportContacts) { if (reportContacts) {
computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth, return computeCapsulePolyhedronFaceContactPoints(minFaceIndex, capsuleRadius, polyhedron, minPenetrationDepth,
polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace, polyhedronToCapsuleTransform, normalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1); narrowPhaseInfo, isCapsuleShape1);
@ -349,7 +349,7 @@ decimal SATAlgorithm::computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhe
// Compute the two contact points between a polyhedron and a capsule when the separating // Compute the two contact points between a polyhedron and a capsule when the separating
// axis is a face normal of the polyhedron // axis is a face normal of the polyhedron
void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, bool SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
@ -391,13 +391,14 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// First we clip the inner segment of the capsule with the four planes of the adjacent faces // First we clip the inner segment of the capsule with the four planes of the adjacent faces
std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals); std::vector<Vector3> clipSegment = clipSegmentWithPlanes(capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace, planesPoints, planesNormals);
assert(clipSegment.size() == 2);
// Project the two clipped points into the polyhedron face // Project the two clipped points into the polyhedron face
const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius); const Vector3 delta = faceNormal * (penetrationDepth - capsuleRadius);
bool contactFound = false;
// For each of the two clipped points // For each of the two clipped points
for (uint i = 0; i<2; i++) { for (uint i = 0; i<clipSegment.size(); i++) {
// Compute the penetration depth of the two clipped points (to filter out the points that does not correspond to the penetration depth) // 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); const decimal clipPointPenDepth = (planesPoints[0] - clipSegment[i]).dot(faceNormal);
@ -405,6 +406,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// If the clipped point is one that produce this penetration depth, we keep it // If the clipped point is one that produce this penetration depth, we keep it
if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) { if (clipPointPenDepth > penetrationDepth - capsuleRadius - decimal(0.001)) {
contactFound = true;
Vector3 contactPointPolyhedron = clipSegment[i] + delta; Vector3 contactPointPolyhedron = clipSegment[i] + delta;
// Project the clipped point into the capsule bounds // Project the clipped point into the capsule bounds
@ -426,6 +429,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule); isCapsuleShape1 ? contactPointPolyhedron : contactPointCapsule);
} }
} }
return contactFound;
} }
// This method returns true if an edge of a polyhedron and a capsule forms a // This method returns true if an edge of a polyhedron and a capsule forms a

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

@ -126,7 +126,7 @@ class SATAlgorithm {
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const; bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron /// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
void computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron, bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform, decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace, Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace, const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,