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); }