public bool RecoverFromPenetration(CollisionWorld collisionWorld) { bool penetration = false; collisionWorld.GetDispatcher().DispatchAllCollisionPairs(m_ghostObject.GetOverlappingPairCache(), collisionWorld.GetDispatchInfo(), collisionWorld.GetDispatcher()); m_currentPosition = m_ghostObject.GetWorldTransform()._origin; float maxPen = 0f; for (int i = 0; i < m_ghostObject.GetOverlappingPairCache().GetNumOverlappingPairs(); i++) { m_manifoldArray.Clear(); BroadphasePair collisionPair = m_ghostObject.GetOverlappingPairCache().GetOverlappingPairArray()[i]; if (collisionPair.m_algorithm != null) { collisionPair.m_algorithm.GetAllContactManifolds(m_manifoldArray); } for (int j = 0; j < m_manifoldArray.Count; j++) { PersistentManifold manifold = m_manifoldArray[j]; float directionSign = manifold.GetBody0() == m_ghostObject ? -1f : 1f; for (int p = 0; p < manifold.GetNumContacts(); p++) { ManifoldPoint pt = manifold.GetContactPoint(p); float dist = pt.GetDistance(); if (dist < 0.0) { if (dist < maxPen) { maxPen = dist; m_touchingNormal = pt.m_normalWorldOnB * directionSign; //?? } m_currentPosition += pt.m_normalWorldOnB * directionSign * dist * 0.2f; penetration = true; } else { //printf("touching %f\n", dist); } } //manifold->clearManifold(); } } IndexedMatrix newTrans = m_ghostObject.GetWorldTransform(); newTrans._origin = m_currentPosition; m_ghostObject.SetWorldTransform(ref newTrans); // printf("m_touchingNormal = %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]); return(penetration); }