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