Question about rigid body update/collision detection order?

Post Reply
AngryPixelShader
Posts: 2
Joined: Sat Jan 04, 2020 9:32 pm

Question about rigid body update/collision detection order?

Post by AngryPixelShader »

Hey!

I'm having a situation that is leaving me stumped. I'm not very experienced with Physics engines and have an elementary grasp about physics in general.

I'm trying to simulate a projectile(B) being fired from a moving target (A). I'm creating a new rigid body for B at the rigid body position of A + an offset. I add B to the world and then set its linear velocity to be the linear velocity of A + the intended velocity for B. The linear velocity of B is the same as the maximum linear velocity of A.

E.g: State after adding and setting linear velocity (P = Position, V = Velocity)

Code: Select all

P:vec3{ x:-917.5590209960937, y:-325.8153991699219, z:-153.44479370117187 } 
V:vec3{ x:-1228.0089111328125, y:432.7921142578125, z:3228.3515625 }
However, when checking the collisions after the next tick I get a collision between A & B with the following states:

Code: Select all

// States recorded after stepSimulation(), not the values of the contact points in the manifolds
B = P: vec3{ x:-938.0258178710937, y:-318.6022033691406, z:-99.63893127441406 } 
       V:vec3{ x:-1228.0089111328125, y:432.7921142578125, z:3228.3515625 }
A = P:vec3{ x:-920.6774291992187, y:-324.79351806640625, z:-144.39683532714844 } 
      V:vec3{ x:-509.7142028808594, y:176.45169067382812, z:1375.142822265625 }
When I inspect these values I notice that the position of A is extremely close to the spawn position of B. I would assume that since the velocity of B is >= double A this collision would never take place in the first place.

This leaves me wondering whether the collision detection is tied to the order the rigid bodies that are added to world, the update to the linear velocity was not taken into account for the collision calculation during this step or I'm doing something that isn't correct with my setup.

I'm currently using btDbvtBoradPhase, btSequentialImpuseConstraintSolver() and btDiscreteDynamicsWorld. I check for collisions by registering a custom tick callback and iterating over the manifolds after stepSimulation(). Simulation timestep is 1/60.

Any insights/pointers would be helpful to try to understand what's going on.

Thanks.
AngryPixelShader
Posts: 2
Joined: Sat Jan 04, 2020 9:32 pm

Re: Question about rigid body update/collision detection order?

Post by AngryPixelShader »

I've updated Hello World examples to illustrate my issue.

Output:

Code: Select all

// 0xbda -> object A, 0xbdb -> Projectile B, [x] simulation tick number
[0] world pos object (0xbda) = 0.000000,0.000000,0.000000 
[1] world pos object (0xbda) = 0.000000,0.000000,25.000002 // Position updated 2 ticks after inserted into world
Collisiton between 0xbda:{0.000000,0.000000,25.000002} 0xbdb:{0.000000, 0.000000, 40.000000} #contacts:0
[2] world pos object (0xbdb) = 0.000000,0.000000,39.999996
[2] world pos object (0xbda) = 0.000000,0.000000,50.000008
Collisiton between 0xbda:{0.000000,0.000000,50.000008} 0xbdb:{0.000000, 0.000000, 39.999996} #contacts:0
[3] world pos object (0xbdb) = 0.000000,0.000000,90.000000
[3] world pos object (0xbda) = 0.000000,0.000000,75.000008
As you can see the linear velocity is only updated 2 sim ticks after being added to world. Is this normal.

Full Example code

Code: Select all

///-----includes_start-----
#include "btBulletDynamicsCommon.h"
#include <stdio.h>
/// This is a Hello World program for running a bas
void tick_callback(btDynamicsWorld* const world,
                                const btScalar time_step) {
    // get all contacts to detect collisions which are represented as manifolds
    // in bullet
    btDispatcher* const dispatcher = world->getDispatcher();
    const int num_manifolds = dispatcher->getNumManifolds();
    for (int manifold_idx = 0; manifold_idx < num_manifolds; ++manifold_idx) {
        const btPersistentManifold* const manifold =
            dispatcher->getManifoldByIndexInternal(manifold_idx);

        const btRigidBody* const object1 = static_cast<const btRigidBody*>(manifold->getBody0());
        const btRigidBody* const object2 = static_cast<const btRigidBody*>(manifold->getBody1());

        btTransform t1,t2;
        object1->getMotionState()->getWorldTransform(t1);
        object2->getMotionState()->getWorldTransform(t2);

        printf("Collisiton between %p:{%f,%f,%f} %p:{%f, %f, %f} #contacts:%d\n",
               object1->getUserPointer(),
               t1.getOrigin().x(), t1.getOrigin().y(), t1.getOrigin().z(),
               object2->getUserPointer(),
               t2.getOrigin().x(), t2.getOrigin().y(), t2.getOrigin().z(),
               manifold->getNumContacts());

        for (int i = 0; i < manifold->getNumContacts(); ++i) {
            const btManifoldPoint& manifold_point = manifold->getContactPoint(i);
            const btVector3 on_b = manifold_point.getPositionWorldOnB();
            const btVector3 on_a = manifold_point.getPositionWorldOnA();

            printf("\t ManifoldPoint on_a:{%f,%f,%f} on_b:{%f, %f, %f}\n",
               on_a.x(), on_a.y(), on_a.z(),
                   on_b.x(), on_b.y(), on_b.z());

        }

    }

}
static unsigned int tick_num  =0;
int main(int argc, char** argv)
{
    ///-----includes_end-----

    int i;
    ///-----initialization_start-----

    ///collision configuration contains default setup for memory, collision setup. Advanced users can create their own configuration.
    btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();

    ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
    btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);

    ///btDbvtBroadphase is a good general purpose broadphase. You can also try out btAxis3Sweep.
    btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();

    ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
    btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;

    btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher, overlappingPairCache, solver, collisionConfiguration);

    dynamicsWorld->setGravity(btVector3(0, 0, 0));
    dynamicsWorld->setInternalTickCallback(tick_callback);

    ///-----initialization_end-----

    //keep track of the shapes, we release memory at exit.
    //make sure to re-use collision shapes among rigid bodies whenever possible!
    btAlignedObjectArray<btCollisionShape*> collisionShapes;

    ///create a few basic rigid bodies

    auto do_step = [dynamicsWorld]() {

        dynamicsWorld->stepSimulation(1.f / 60.f, 10);

        //print positions of all objects
        for (int j = dynamicsWorld->getNumCollisionObjects() - 1; j >= 0; j--)
        {
            btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[j];
            btRigidBody* body = btRigidBody::upcast(obj);
            btTransform trans;
            if (body && body->getMotionState())
            {
                body->getMotionState()->getWorldTransform(trans);
            }
            else
            {
                trans = obj->getWorldTransform();
            }
            printf("[%u] world pos object (%p) = %f,%f,%f\n", tick_num, body->getUserPointer(), float(trans.getOrigin().getX()), float(trans.getOrigin().getY()), float(trans.getOrigin().getZ()));
        }

        ++tick_num;
    };

    btRigidBody* B, *A;

    {
        // Spwan moving target (A)
        btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(1.), btScalar(1.), btScalar(1.)));

        collisionShapes.push_back(groundShape);

        btTransform groundTransform;
        groundTransform.setIdentity();
        groundTransform.setOrigin(btVector3(0, 0, 0));

        btScalar mass(100.0f);

        //rigidbody is dynamic if and only if mass is non zero, otherwise static
        bool isDynamic = (mass != 0.f);

        btVector3 localInertia(0, 0, 0);
        if (isDynamic)
            groundShape->calculateLocalInertia(mass, localInertia);

        //using motionstate is optional, it provides interpolation capabilities, and only synchronizes 'active' objects
        btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
        btRigidBody* body = new btRigidBody(rbInfo);
        //add the body to the dynamics world
        dynamicsWorld->addRigidBody(body);
        body->activate();
        body->setLinearVelocity(btVector3(0.f, 0.f, 1500.f));
        body->setUserPointer((void*)0xBDA);
        A = body;
    }

    // Do Two Sim Tick
    do_step();
    do_step();

    {
        // Spawn projectile (B)

        btCollisionShape* colShape = new btSphereShape(btScalar(1.));
        collisionShapes.push_back(colShape);

        /// Create Dynamic Objects
        btTransform startTransform;
        startTransform.setIdentity();

        btScalar mass(1.f);

        //rigidbody is dynamic if and only if mass is non zero, otherwise static
        bool isDynamic = (mass != 0.f);

        btTransform A_transform;
        A->getMotionState()->getWorldTransform(A_transform);

        btVector3 direction = A_transform.getBasis() * btVector3(0,0,1);
        btVector3 offset = direction * 15.0f;
        btVector3 spawn_point = A_transform.getOrigin() + offset;

        btVector3 localInertia(0, 0, 0);
        if (isDynamic)
            colShape->calculateLocalInertia(mass, localInertia);

        startTransform.setOrigin(spawn_point);

        //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
        btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
        btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, colShape, localInertia);
        btRigidBody* body = new btRigidBody(rbInfo);
        dynamicsWorld->addRigidBody(body);
        body->setUserPointer((void*)0xBDB);
        body->activate();
        body->setLinearVelocity(A->getLinearVelocity() + (direction * 1500.f));
        B = body;

    }

    do_step();
    do_step();

    //cleanup in the reverse order of creation/initialization

    ///-----cleanup_start-----

    //remove the rigidbodies from the dynamics world and delete them
    for (i = dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--)
    {
        btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[i];
        btRigidBody* body = btRigidBody::upcast(obj);
        if (body && body->getMotionState())
        {
            delete body->getMotionState();
        }
        dynamicsWorld->removeCollisionObject(obj);
        delete obj;
    }

    //delete collision shapes
    for (int j = 0; j < collisionShapes.size(); j++)
    {
        btCollisionShape* shape = collisionShapes[j];
        collisionShapes[j] = 0;
        delete shape;
    }

    //delete dynamics world
    delete dynamicsWorld;

    //delete solver
    delete solver;

    //delete broadphase
    delete overlappingPairCache;

    //delete dispatcher
    delete dispatcher;

    delete collisionConfiguration;

    //next line is optional: it will be cleared by the destructor when the array goes out of scope
    collisionShapes.clear();
}
I have also noticed that the number of contact points is always zero. Does this mean that if there are no contact points, no collision takes place? Is this just an intersection between bounding boxes that I'm recoding at the moment?
User avatar
drleviathan
Posts: 849
Joined: Tue Sep 30, 2014 6:03 pm
Location: San Francisco

Re: Question about rigid body update/collision detection order?

Post by drleviathan »

The Bullet source is open so you can dig into it and see the order of operations. When I examine btDiscreteDynamicsWorld::stepSimulation() I see something like this (in pseudo code):

Code: Select all

int btDiscreteDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep) {
    computeNumberOfSubsteps();
    if (numberOfSubsteps > 0) {
        for (i = 0; i < numberOfSubsteps; ++i) {
            internalSingleStepSimulation(fixedTimeStep);
            synchronizeMotionStates();
        }
    } else {
        synchronizeMotionStates();
    }
}
Inside btDiscreteDynamicsWorld::internalSingleStepSimulation() the order goes like this (again in pseudo code):

Code: Select all

void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep) {
    predictUnconstraintMotion(timeStep);
    createPredictiveContacts(timeStep);
    performDiscreteCollisionDetection();
    calculateSimulationIslands();
    solveConstraints(getSolverInfo());
    integrateTransforms(timeStep);
    updateActivationState(timeStep);
}
So it looks like the predicted positions of moving objects are computed before collisions. Then the collisions and constraints are solved, and the objects are finally integrated forward.

It is true: contact manifolds are often created between two non-touching objects, and then end up with zero contact points. Contact points are created optimistically and then the manifolds are pruned to a minimum set, so yes it is very likely the two objects are near enough to overlap in the broadphase and then collision fails in the narrow-phase checks... and an empty contact manifold is left in the list.

If you definitely don't want the projectile to hit the thing throwing it then you can use collision groups to obviate overlaps in the broadphase.
Post Reply