Computing Minimum De-Penetration Between Two Colliders

Post Reply
mbetters
Posts: 2
Joined: Tue Jun 29, 2021 5:18 pm

Computing Minimum De-Penetration Between Two Colliders

Post by mbetters »

Hello, I'd like to compute the minimum translation needed to de-penetrate two collision bodies. I essentially want to port Unity's Physics.ComputePenetration to a roughly equivalent static function using Bullet. Here's my implementation:

Code: Select all

static bool physicsComputePenetration( btCollisionWorld* collisionWorld, 
                                btBroadphasePair* collisionPair,
                                btCollisionObject* penetrator,
                                btVector3* minDepenetrationDirection,
                                btScalar* minDepenetrationDistance )
{
    btCollisionObject* obj0 = static_cast<btCollisionObject*>(collisionPair->m_pProxy0->m_clientObject);
    btCollisionObject* obj1 = static_cast<btCollisionObject*>(collisionPair->m_pProxy1->m_clientObject);
    if ((obj0 && !obj0->hasContactResponse()) || (obj1 && !obj1->hasContactResponse())) {
        Ogre::LogManager::getSingleton().logMessage("No contact");
        return false;
    }
    Ogre::LogManager::getSingleton().logMessage("Contact happened");
    if (!needsCollision(obj0, obj1)) {
        Ogre::LogManager::getSingleton().logMessage("No collision");
        return false;
    }
    Ogre::LogManager::getSingleton().logMessage("Collision happened");
    btManifoldArray manifoldArray;
    if (collisionPair->m_algorithm)
		collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
    Ogre::LogManager::getSingleton().logMessage("Num manifolds in collision = "
        + Ogre::StringConverter::toString(manifoldArray.size()));
    btScalar maxPenetration = btScalar(0.0); // TODO: Why is this 0 ???
    bool isPenetrating = false;
    for (int j = 0; j < manifoldArray.size(); j++)
    {
        btPersistentManifold* manifold = manifoldArray[j];
        btScalar directionSign = manifold->getBody0() == penetrator ? btScalar(-1.0) : btScalar(1.0);
        Ogre::LogManager::getSingleton().logMessage(
            "Num Contact Points in Manifold "
            + Ogre::StringConverter::toString(j)
            + " = "
            + Ogre::StringConverter::toString(manifold->getNumContacts())
        );
        for (int p = 0; p < manifold->getNumContacts(); p++)
		{
            const btManifoldPoint& pt = manifold->getContactPoint(p);
            btScalar dist = pt.getDistance();
            Ogre::LogManager::getSingleton().logMessage(
                "Contact Point "
                + Ogre::StringConverter::toString(p) + "'s "
                + " Distance = "
                + Ogre::StringConverter::toString(dist)
            );
            if (dist < maxPenetration)
            {
                maxPenetration = dist;
                *minDepenetrationDirection = pt.m_normalWorldOnB * directionSign * dist;
                *minDepenetrationDistance = minDepenetrationDirection->length();
                *minDepenetrationDirection = minDepenetrationDirection->normalized();
                isPenetrating = true;
            }
        }
    }
    return isPenetrating;
}
Is this the right way to do this in Bullet? I was looking at btComputeMprPenetration for example, but that seemed too low-level, and I was wondering if functions like that already get called as part of collision detection itself, or something... I'm basically an idiot when it comes to physics, so help would be greatly appreciated. I just want minimum de-penetration distance, and to my mind the function above calculates it by finding the maximum penetration distance... unless I'm totally misunderstanding something here.

I should clarify and say this would be used as a helper function for a kinematic character controller using a btPairCachingGhostObject. The function above is just a factored-out version of the inner-most block of the recoverFromPenetration function in Bullet's existing kinematic character controller example.
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Computing Minimum De-Penetration Between Two Colliders

Post by drleviathan »

I would expect your algorithm to work well enough when only two objects are involved but it will sometimes have problems (oscillations) when the character is trapped between two or more objects (e.g. when it steps out of penetration from ObjectA and thereby into penetration with ObjectB). For more robust behavior you probably want to "integrate" the harvested penetration vectors across multiple objects and probably also will need to make your algorithm more iterative such that it doesn't try to fully extract itself in a single step. Of course that would require more CPU cycles and also more time: there will be tradeoffs between: accuracy, cost, and lag. You'll have to decide how to balance them.

One optimization suggestion: you appear to be doing wasted work inside the loop. Specifically this code unnecessarily computes the length of the penetration twice.

Code: Select all

                *minDepenetrationDirection = pt.m_normalWorldOnB * directionSign * dist;
                *minDepenetrationDistance = minDepenetrationDirection->length();
                *minDepenetrationDirection = minDepenetrationDirection->normalized();
Assuming pt.m_normalWorldOnB is already normalized I believe you could achieve the same results like so:

Code: Select all

                *minDepenetrationDistance = - dist;
                *minDepenetrationDirection = pt.m_normalWorldOnB * directionSign;
Alternatively you could store direction and distance in one btVector3 and extract its length as necessary outside the function call:

Code: Select all

                *minDepenetration = (-dist * directionSign) * pt.m_normalWorldOnB;
mbetters
Posts: 2
Joined: Tue Jun 29, 2021 5:18 pm

Re: Computing Minimum De-Penetration Between Two Colliders

Post by mbetters »

Thanks for the reply and the code fix!
Post Reply