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

Fix issue with sphere and capsule SAT collision detection

Browse Source
This commit is contained in:
Daniel Chappuis 2017-10-24 22:47:35 +02:00
parent b1aad2b7c4
commit 5da57a96c8
2 changed files with 12 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -114,6 +114,10 @@ bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPh
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal; const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
if (isCapsuleShape1) {
faceNormalWorld = -faceNormalWorld;
}
// Compute and create two contact points // Compute and create two contact points
satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth, satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace, polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,

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

@ -97,10 +97,12 @@ bool SATAlgorithm::testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrow
if (reportContacts) { if (reportContacts) {
const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex); const Vector3 minFaceNormal = polyhedron->getFaceNormal(minFaceIndex);
Vector3 normalWorld = -(polyhedronToWorldTransform.getOrientation() * minFaceNormal); Vector3 minFaceNormalWorld = polyhedronToWorldTransform.getOrientation() * minFaceNormal;
Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * normalWorld * sphere->getRadius(); Vector3 contactPointSphereLocal = sphereToWorldTransform.getInverse().getOrientation() * (-minFaceNormalWorld * sphere->getRadius());
Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius()); Vector3 contactPointPolyhedronLocal = sphereCenter + minFaceNormal * (minPenetrationDepth - sphere->getRadius());
Vector3 normalWorld = isSphereShape1 ? -minFaceNormalWorld : minFaceNormalWorld;
// 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,
isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal, isSphereShape1 ? contactPointSphereLocal : contactPointPolyhedronLocal,
@ -243,6 +245,9 @@ bool SATAlgorithm::testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narro
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB; const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace; Vector3 normalWorld = capsuleToWorld.getOrientation() * separatingAxisCapsuleSpace;
if (isCapsuleShape1) {
normalWorld = -normalWorld;
}
const decimal capsuleRadius = capsuleShape->getRadius(); const decimal capsuleRadius = capsuleShape->getRadius();
// If the separating axis is a face normal // If the separating axis is a face normal
@ -391,12 +396,8 @@ void SATAlgorithm::computeCapsulePolyhedronFaceContactPoints(uint referenceFaceI
// 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);
if (isCapsuleShape1) {
normalWorld = -normalWorld;
}
// For each of the two clipped points // For each of the two clipped points
for (int i = 0; i<2; i++) { for (uint i = 0; i<2; 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);