mrq/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

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

@ -56,21 +56,26 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf
const decimal sphere2Radius = sphereShape2->getRadius();
// Compute the sum of the radius
decimal sumRadiuses = sphere1Radius + sphere2Radius;
const decimal sumRadiuses = sphere1Radius + sphere2Radius;
// 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 (squaredDistanceBetweenCenters < sumRadiusesProducts) {
const decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters);
// Make sure the penetration depth is not zero (even if the previous condition test was true the penetration depth can still be
// zero because of precision issue of the computation at the previous line)
if (penetrationDepth > 0) {
// If we need to report contacts
if (narrowPhaseInfoBatch.narrowPhaseInfos[batchIndex].reportContacts) {
const Transform transform1Inverse = transform1.getInverse();
const Transform transform2Inverse = transform2.getInverse();
decimal penetrationDepth = sumRadiuses - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
@ -78,8 +83,8 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf
// If the two sphere centers are not at the same position
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
const Vector3 centerSphere2InBody1LocalSpace = transform1Inverse * transform2.getPosition();
const Vector3 centerSphere1InBody2LocalSpace = transform2Inverse * transform1.getPosition();
intersectionOnBody1 = sphere1Radius * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphere2Radius * centerSphere1InBody2LocalSpace.getUnit();
@ -102,6 +107,7 @@ bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfoBatch& narrowPhaseInf
isCollisionFound = true;
}
}
}
return isCollisionFound;
}