protected bool RecoverFromPenetration(CollisionWorld collisionWorld) { Vector3 minAabb, maxAabb; m_convexShape.GetAabb(m_ghostObject.WorldTransform, out minAabb, out maxAabb); collisionWorld.Broadphase.SetAabbRef(m_ghostObject.BroadphaseHandle, ref minAabb, ref maxAabb, collisionWorld.Dispatcher); bool penetration = false; collisionWorld.Dispatcher.DispatchAllCollisionPairs(m_ghostObject.OverlappingPairCache, collisionWorld.DispatchInfo, collisionWorld.Dispatcher); m_currentPosition = m_ghostObject.WorldTransform.Origin; float maxPen = 0f; for (int i = 0; i < m_ghostObject.OverlappingPairCache.NumOverlappingPairs; i++) { m_manifoldArray.Clear(); BroadphasePair collisionPair = m_ghostObject.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; } collisionPair.GetAllContactManifolds(m_manifoldArray); for (int j = 0; j < m_manifoldArray.Count; j++) { var manifoldId = AlignedManifoldArray.btAlignedManifoldArray_at(m_manifoldArray._native, j); var bodyId = PersistentManifold.btPersistentManifold_getBody0(manifoldId); var numContacts = PersistentManifold.btPersistentManifold_getNumContacts(manifoldId); float directionSign = bodyId == m_ghostObject._native ? -1f : 1f; for (int p = 0; p < numContacts; p++) { var manifoldPointId = PersistentManifold.btPersistentManifold_getContactPoint(manifoldId, p); float dist = ManifoldPoint.btManifoldPoint_getDistance(manifoldPointId); if (dist < 0.0f) { Vector3 normalWorldOnB; ManifoldPoint.btManifoldPoint_getNormalWorldOnB(manifoldPointId, out normalWorldOnB); if (dist < maxPen) { maxPen = dist; } var counterPenDir = normalWorldOnB * directionSign;; m_currentPosition += counterPenDir * dist; penetration = true; if (counterPenDir.Dot(Vector3.UnitY) > 0) { m_verticalVelocity = 0; } } } } } Matrix newTrans = m_ghostObject.WorldTransform; newTrans.Origin = m_currentPosition; m_ghostObject.WorldTransform = newTrans; return(penetration); }