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

Fix robustness issue with SphereShape vs SphereShape collision detection

Browse Source
This commit is contained in:
Daniel Chappuis 2020-10-10 00:10:05 +02:00
parent 7071213617
commit 44e1b12aaf
1 changed files with 34 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -56,50 +56,56 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf
const decimal sphere2Radius = sphereShape2->getRadius(); const decimal sphere2Radius = sphereShape2->getRadius();
// Compute the sum of the radius // Compute the sum of the radius
decimal sumRadiuses = sphere1Radius + sphere2Radius; const decimal sumRadiuses = sphere1Radius + sphere2Radius;
// Compute the product of the sum of the radius // Compute the product of the sum of the radius
decimal sumRadiusesProducts = sumRadiuses * sumRadiuses; const decimal sumRadiusesProducts = sumRadiuses * sumRadiuses;
// If the sphere collision shapes intersect // If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters < sumRadiusesProducts) { if (squaredDistanceBetweenCenters < sumRadiusesProducts) {
// If we need to report contacts const decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters);
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
const Transform transform1Inverse = transform1.getInverse(); // Make sure the penetration depth is not zero (even if the previous condition test was true the penetration depth can still be
const Transform transform2Inverse = transform2.getInverse(); // zero because of precision issue of the computation at the previous line)
if (penetrationDepth > 0) {
decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters); // If we need to report contacts
Vector3 intersectionOnBody1; if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
Vector3 intersectionOnBody2;
Vector3 normal;
// If the two sphere centers are not at the same position const Transform transform1Inverse = transform1.getInverse();
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) { const Transform transform2Inverse = transform2.getInverse();
Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition(); Vector3 intersectionOnBody1;
Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition(); Vector3 intersectionOnBody2;
Vector3 normal;
intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit(); // If the two sphere centers are not at the same position
intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit(); if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// Take any contact normal direction const Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
normal.setAllValues(0, 1, 0); const Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal); intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal); intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
intersectionOnBody1 = sphere1Radius * (transform1Inverse.getOrientation() * normal);
intersectionOnBody2 = sphere2Radius * (transform2Inverse.getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
} }
// Create the contact info object narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
narrowPhaseInfoBatch.addContactPoint(batchIndex, normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2); isCollisionFound = true;
} }
narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].isColliding = true;
isCollisionFound = true;
} }
} }