bool CCharacterController::RecoverFromPenetration(btCollisionWorld* pCollisionWorld)
{
if (!IsColliding())
return false;
bool bPenetration = false;
pCollisionWorld->getDispatcher()->dispatchAllCollisionPairs(m_pGhostObject->getOverlappingPairCache(), pCollisionWorld->getDispatchInfo(), pCollisionWorld->getDispatcher());
btVector3 vecCurrentPosition = m_pGhostObject->getWorldTransform().getOrigin();
btVector3 vecOriginalPosition = vecCurrentPosition;
btScalar maxPen = btScalar(0.0);
for (int i = 0; i < m_pGhostObject->getOverlappingPairCache()->getNumOverlappingPairs(); i++)
{
m_aManifolds.resize(0);
btBroadphasePair* pCollisionPair = &m_pGhostObject->getOverlappingPairCache()->getOverlappingPairArray()[i];
btCollisionObject* pObject0 = static_cast<btCollisionObject*>(pCollisionPair->m_pProxy0->m_clientObject);
btCollisionObject* pObject1 = static_cast<btCollisionObject*>(pCollisionPair->m_pProxy1->m_clientObject);
if (!pObject0->hasContactResponse() || !pObject1->hasContactResponse())
continue;
if (pObject0->getBroadphaseHandle()->m_collisionFilterGroup == CG_TRIGGER || pObject1->getBroadphaseHandle()->m_collisionFilterGroup == CG_TRIGGER)
continue;
if (pCollisionPair->m_algorithm)
pCollisionPair->m_algorithm->getAllContactManifolds(m_aManifolds);
for (int j = 0; j < m_aManifolds.size(); j++)
{
btPersistentManifold* pManifold = m_aManifolds[j];
const btCollisionObject* obA = static_cast<const btCollisionObject*>(pManifold->getBody0());
const btCollisionObject* obB = static_cast<const btCollisionObject*>(pManifold->getBody1());
btScalar directionSign;
CEntityHandle<CBaseEntity> hOther;
size_t iExtra;
if (obA == m_pGhostObject)
{
if (obB->getBroadphaseHandle()->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
continue;
directionSign = btScalar(-1.0);
hOther = CEntityHandle<CBaseEntity>((size_t)obB->getUserPointer());
iExtra = (size_t)obB->getUserPointer()-GameServer()->GetMaxEntities();
if (obB->getCollisionFlags()&btCollisionObject::CF_CHARACTER_OBJECT)
{
// If I'm heavier than he, don't let him push me around
if (hOther->GetMass() < m_hEntity->GetMass())
continue;
}
}
else
{
if (obA->getBroadphaseHandle()->m_collisionFilterGroup & btBroadphaseProxy::SensorTrigger)
continue;
directionSign = btScalar(1.0);
hOther = CEntityHandle<CBaseEntity>((size_t)obA->getUserPointer());
iExtra = (size_t)obB->getUserPointer()-GameServer()->GetMaxEntities();
if (obA->getCollisionFlags()&btCollisionObject::CF_CHARACTER_OBJECT)
{
// If I'm heavier than he, don't let him push me around
if (hOther->GetMass() < m_hEntity->GetMass())
continue;
}
}
for (int p = 0; p < pManifold->getNumContacts(); p++)
{
const btManifoldPoint& pt = pManifold->getContactPoint(p);
if (obA == m_pGhostObject)
{
if (hOther)
{
if (!m_hEntity->ShouldCollideWith(hOther, Vector(pt.getPositionWorldOnB())))
continue;
}
else
{
if (!m_hEntity->ShouldCollideWithExtra(iExtra, Vector(pt.getPositionWorldOnB())))
continue;
}
}
else
{
if (hOther)
{
if (!m_hEntity->ShouldCollideWith(hOther, Vector(pt.getPositionWorldOnA())))
continue;
}
else
{
if (!m_hEntity->ShouldCollideWithExtra(iExtra, Vector(pt.getPositionWorldOnA())))
continue;
}
}
btScalar flDistance = pt.getDistance();
btScalar flMargin = std::max(obA->getCollisionShape()->getMargin(), obB->getCollisionShape()->getMargin());
if (flDistance < -flMargin)
{
flDistance += flMargin;
if (flDistance < maxPen)
maxPen = flDistance;
btScalar flDot = pt.m_normalWorldOnB.dot(GetUpVector());
btVector3 vecAdjustment;
if (flDot > 0.707f)
vecAdjustment = GetUpVector() * (directionSign * flDistance * 1.001f);
else
vecAdjustment = pt.m_normalWorldOnB * (directionSign * flDistance * 1.001f);
if (vecAdjustment.length2() < 0.001*0.001)
continue;
vecCurrentPosition += vecAdjustment;
bPenetration = true;
} else {
//printf("touching %f\n", dist);
}
}
//pManifold->clearManifold();
}
}
btTransform mNew = m_pGhostObject->getWorldTransform();
mNew.setOrigin(mNew.getOrigin() + (vecCurrentPosition - vecOriginalPosition) * m_vecLinearFactor);
m_pGhostObject->setWorldTransform(mNew);
return bPenetration;
}
