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