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