public bool GetSphereDistance(CollisionObject boxObj, ref IndexedVector3 pointOnBox, ref IndexedVector3 normal, ref float penetrationDepth, IndexedVector3 sphereCenter, float fRadius, float maxContactDistance)
{
BoxShape boxShape = boxObj.GetCollisionShape() as BoxShape;
IndexedVector3 boxHalfExtent = boxShape.GetHalfExtentsWithoutMargin();
float boxMargin = boxShape.GetMargin();
penetrationDepth = 1.0f;
// convert the sphere position to the box's local space
IndexedMatrix m44T = boxObj.GetWorldTransform();
IndexedVector3 sphereRelPos = m44T.InvXform(sphereCenter);
// Determine the closest point to the sphere center in the box
IndexedVector3 closestPoint = sphereRelPos;
closestPoint.X = (Math.Min(boxHalfExtent.X, closestPoint.X));
closestPoint.X = (Math.Max(-boxHalfExtent.X, closestPoint.X));
closestPoint.Y = (Math.Min(boxHalfExtent.Y, closestPoint.Y));
closestPoint.Y = (Math.Max(-boxHalfExtent.Y, closestPoint.Y));
closestPoint.Z = (Math.Min(boxHalfExtent.Z, closestPoint.Z));
closestPoint.Z = (Math.Max(-boxHalfExtent.Z, closestPoint.Z));
float intersectionDist = fRadius + boxMargin;
float contactDist = intersectionDist + maxContactDistance;
normal = sphereRelPos - closestPoint;
//if there is no penetration, we are done
float dist2 = normal.LengthSquared();
if (dist2 > contactDist * contactDist)
{
return(false);
}
float distance;
//special case if the sphere center is inside the box
if (dist2 == 0.0f)
{
distance = -GetSpherePenetration(ref boxHalfExtent, ref sphereRelPos, ref closestPoint, ref normal);
}
else //compute the penetration details
{
distance = normal.Length();
normal /= distance;
}
pointOnBox = closestPoint + normal * boxMargin;
// v3PointOnSphere = sphereRelPos - (normal * fRadius);
penetrationDepth = distance - intersectionDist;
// transform back in world space
IndexedVector3 tmp = m44T * pointOnBox;
pointOnBox = tmp;
// tmp = m44T(v3PointOnSphere);
// v3PointOnSphere = tmp;
tmp = m44T._basis * normal;
normal = tmp;
return(true);
}