bool RecoverFromPenetration(CollisionWorld collisionWorld)
{
Vector3 minAabb, maxAabb;
collisionShape.GetAabb(collisionObject.WorldTransform, out minAabb, out maxAabb);
collisionWorld.Broadphase.SetAabbRef(collisionObject.BroadphaseHandle,
ref minAabb,
ref maxAabb,
collisionWorld.Dispatcher);
bool penetration = false;
collisionWorld.Dispatcher.DispatchAllCollisionPairs(collisionObjectCastToPairCache.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher);
currentPosition = collisionObject.WorldTransform.Origin;
float maxPen = 0f;
for (int i = 0; i < collisionObjectCastToPairCache.OverlappingPairCache.NumOverlappingPairs; i++)
{
manifoldArray.Clear();
BroadphasePair collisionPair = collisionObjectCastToPairCache.OverlappingPairCache.OverlappingPairArray[i];
CollisionObject obj0 = collisionPair.Proxy0.ClientObject as CollisionObject;
CollisionObject obj1 = collisionPair.Proxy1.ClientObject as CollisionObject;
if ((obj0 != null && !obj0.HasContactResponse) || (obj1 != null && !obj1.HasContactResponse))
{
continue;
}
if (collisionPair.Algorithm != null)
{
collisionPair.Algorithm.GetAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.Count; j++)
{
PersistentManifold manifold = manifoldArray[j];
float directionSign = manifold.Body0 == collisionObject ? -1f : 1f;
for (int p = 0; p < manifold.NumContacts; p++)
{
ManifoldPoint pt = manifold.GetContactPoint(p);
float dist = pt.Distance;
if (dist < 0.0f)
{
if (dist < maxPen)
{
maxPen = dist;
}
currentPosition += pt.NormalWorldOnB * directionSign * dist * 0.2f;
penetration = true;
}
}
}
}
Matrix newTrans = collisionObject.WorldTransform;
newTrans.Origin = currentPosition;
collisionObject.WorldTransform = newTrans;
return(penetration);
}
