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