Bullet Physics btMultiBody energy explosion demo

MultiBodyVehicle.cpp

//test addJointTorque
#include "MultiBodyVehicle.h"

#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletCollision/CollisionShapes/btBoxShape.h"

/*
 * Play with these constants until you find a setup that exhibits an explosion
 * of energy.
 */
const btScalar FIXED_STEP = 1.0f / 120;
const int NUM_MULTIBODIES = 7;

btVector3 flipYandZ(const btVector3 & v)
{
  return btVector3(v.getX(), v.getZ(), v.getY());
}

bool Comparator::operator()(const btVector3 & u, const btVector3 & v) const
{
  if (u.getX() != v.getX())
    return u.getX() < v.getX();
  else if (u.getY() != v.getY())
    return u.getY() < v.getY();
  else
    return u.getZ() < v.getZ();
}

btBoxShape * MultiBodyVehicleSetup::getBoxShape(const btVector3 & halfExtents)
{
  if (this->boxShapes.find(halfExtents) == this->boxShapes.cend())
  {
    this->boxShapes[halfExtents] = new btBoxShape(halfExtents);
  }
  return this->boxShapes[halfExtents];
}

MultiBodyVehicleSetup::MultiBodyVehicleSetup()
{
}

MultiBodyVehicleSetup::~MultiBodyVehicleSetup()
{
  for (std::map<btVector3, btBoxShape *, Comparator>::iterator it = this->boxShapes.begin();
      it != this->boxShapes.end();
      it++)
  {
    delete it->second;
  }
  for (unsigned i = 0; i < this->colliders.size(); i++)
  {
    delete this->colliders[i];
  }
}



class btMultiBody* MultiBodyVehicleSetup::createMultiBodyVehicle(const ModelConstructionInfo & info)
{
  class btMultiBodyDynamicsWorld* world = m_dynamicsWorld;

  const int NUM_LINKS = static_cast<int>(info.childLinks.size());

  btCollisionShape * baseCollisionShape = getBoxShape(info.baseHalfExtents);
  btVector3 inertia(0, 0, 0);
  baseCollisionShape->calculateLocalInertia(static_cast<btScalar>(info.baseMass), inertia);

  btMultiBody * multiBody = new btMultiBody(NUM_LINKS,
      static_cast<btScalar>(info.baseMass),
      inertia,
      false,
      true,
      true);

  multiBody->setBasePos(info.basePosition);
  this->colliders.push_back(new btMultiBodyLinkCollider(multiBody,
        -1));
  btMultiBodyLinkCollider * baseCollider = this->colliders.back();
  baseCollider->setCollisionShape(baseCollisionShape);
  btTransform transform;
  transform.setIdentity();
  transform.setOrigin(multiBody->getBasePos());
  baseCollider->setWorldTransform(transform);
  world->addCollisionObject(baseCollider, 1, 1+2); //needed to collide with the ground plane
  multiBody->setBaseCollider(baseCollider);

  for (int i = 0; i < NUM_LINKS; i++)
  {
    const BodyConstructionInfo & LINK = info.childLinks.at(i);
    btCollisionShape * collisionShape = getBoxShape(LINK.halfExtents);

    this->colliders.push_back(new btMultiBodyLinkCollider(multiBody,
          i));
    btMultiBodyLinkCollider * collider = this->colliders.back();
    collider->setCollisionShape(collisionShape);
    btTransform tr;
    tr.setIdentity();
    tr.setOrigin(LINK.position);
    collider->setWorldTransform(tr);
    world->addCollisionObject(collider, 1, 1+2);
    multiBody->getLink(i).m_collider = collider;

    collisionShape->calculateLocalInertia(static_cast<btScalar>(LINK.mass), inertia);
  }

  for (int i = 0; i < static_cast<int>(info.joints.size()); i++)
  {
    const JointConstructionInfo & JOINT = info.joints.at(i);
    if (JOINT.jointType == JointConstructionInfo::REVOLUTE)
    {
      btVector3 parentPosition;
      if (JOINT.parentLinkIndex == -1)
      {
        parentPosition = info.basePosition;
      }
      else
      {
        parentPosition = info.childLinks.at(JOINT.parentLinkIndex).position;
      }

      const BodyConstructionInfo & LINK = info.childLinks.at(JOINT.linkIndex);

      btVector3 localInertia(0, 0, 0);
      multiBody->getLink(JOINT.linkIndex).m_collider->getCollisionShape()->calculateLocalInertia(static_cast<btScalar>(LINK.mass), localInertia);

      multiBody->setupRevolute(JOINT.linkIndex,
          LINK.mass,
          localInertia,
          JOINT.parentLinkIndex,
          btQuaternion(0,0,0,1).inverse(),
          JOINT.hingeAxis,
          JOINT.worldPosition - parentPosition,
          LINK.position - JOINT.worldPosition,
          false);
    }
    else if (JOINT.jointType == JointConstructionInfo::SPHERICAL)
    {
      btVector3 parentPosition;
      if (JOINT.parentLinkIndex == -1)
      {
        parentPosition = info.basePosition;
      }
      else
      {
        parentPosition = info.childLinks.at(JOINT.parentLinkIndex).position;
      }

      const BodyConstructionInfo & LINK = info.childLinks.at(JOINT.linkIndex);

      btVector3 localInertia(0, 0, 0);
      multiBody->getLink(JOINT.linkIndex).m_collider->getCollisionShape()->calculateLocalInertia(static_cast<btScalar>(LINK.mass), localInertia);

      multiBody->setupSpherical(JOINT.linkIndex,
          LINK.mass,
          localInertia,
          JOINT.parentLinkIndex,
          btQuaternion(0,0,0,1).inverse(),
          JOINT.worldPosition - parentPosition,
          LINK.position - JOINT.worldPosition,
          false);
    }
    else
    {
    }
  }

  multiBody->finalizeMultiDof();
  world->addMultiBody(multiBody);
  return multiBody;
}


void MultiBodyVehicleSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
{
  int upAxis = 1;

  btVector4 colors[4] =
  {
    btVector4(1,0,0,1),
    btVector4(0,1,0,1),
    btVector4(0,1,1,1),
    btVector4(1,1,0,1),
  };
  int curColor = 0;



    gfxBridge.setUpAxis(upAxis);

	this->createEmptyDynamicsWorld();
    gfxBridge.createPhysicsDebugDrawer(m_dynamicsWorld);
    m_dynamicsWorld->getDebugDrawer()->setDebugMode(
        //btIDebugDraw::DBG_DrawConstraints
        +btIDebugDraw::DBG_DrawWireframe
        +btIDebugDraw::DBG_DrawContactPoints
        +btIDebugDraw::DBG_DrawAabb
        );//+btIDebugDraw::DBG_DrawConstraintLimits);

    
  for (int i = 0; i < NUM_MULTIBODIES; i++)
  {
    const btVector3 OFFSET(flipYandZ(btVector3(1 * i, 1 * i, 0.05)));
    ModelConstructionInfo modelInfo;
    modelInfo.modelName = "M" + std::to_string(i);
    modelInfo.baseName = "hips";
    modelInfo.baseHalfExtents = flipYandZ(btVector3(0.05f, 0.15f, 0.05f));
    modelInfo.basePosition = flipYandZ(btVector3(0.0f, 0.0f, 1.05)) + OFFSET;
    modelInfo.baseMass = 1.0f;

    BodyConstructionInfo leftUpperLegInfo;
    leftUpperLegInfo.name = "leftUpperLeg";
    leftUpperLegInfo.halfExtents = flipYandZ(btVector3(0.05, 0.05, 0.2));
    leftUpperLegInfo.position = flipYandZ(btVector3(0.0f, 0.1, 0.75)) + OFFSET;
    leftUpperLegInfo.mass = 1.0f;
    leftUpperLegInfo.parentIndex = -1;

    BodyConstructionInfo leftLowerLegInfo;
    leftLowerLegInfo.name = "leftLowerLeg";
    leftLowerLegInfo.halfExtents = flipYandZ(btVector3(0.05, 0.05, 0.2));
    leftLowerLegInfo.position = flipYandZ(btVector3(0.0f, 0.1, 0.3)) + OFFSET;
    leftLowerLegInfo.mass = 1.0f;
    leftLowerLegInfo.parentIndex = 0;

    BodyConstructionInfo leftFootInfo;
    leftFootInfo.name = "leftFoot";
    leftFootInfo.halfExtents = flipYandZ(btVector3(0.105, 0.05, 0.025));
    leftFootInfo.position = flipYandZ(btVector3(0.055f, 0.1, 0.025)) + OFFSET;
    leftFootInfo.mass = 1.0f;
    leftFootInfo.parentIndex = 1;

    BodyConstructionInfo rightUpperLegInfo;
    rightUpperLegInfo.name = "rightUpperLeg";
    rightUpperLegInfo.halfExtents = flipYandZ(btVector3(0.05, 0.05, 0.2));
    rightUpperLegInfo.position = flipYandZ(btVector3(0.0f, -0.1, 0.75)) + OFFSET;
    rightUpperLegInfo.mass = 1.0f;
    rightUpperLegInfo.parentIndex = -1;

    BodyConstructionInfo rightLowerLegInfo;
    rightLowerLegInfo.name = "rightLowerLeg";
    rightLowerLegInfo.halfExtents = flipYandZ(btVector3(0.05, 0.05, 0.2));
    rightLowerLegInfo.position = flipYandZ(btVector3(0.0f, -0.1, 0.3)) + OFFSET;
    rightLowerLegInfo.mass = 1.0f;
    rightLowerLegInfo.parentIndex = 3;

    BodyConstructionInfo rightFootInfo;
    rightFootInfo.name = "rightFoot";
    rightFootInfo.halfExtents = flipYandZ(btVector3(0.105, 0.05, 0.025));
    rightFootInfo.position = flipYandZ(btVector3(0.055f, -0.1, 0.025)) + OFFSET;
    rightFootInfo.mass = 1.0f;
    rightFootInfo.parentIndex = 4;

    BodyConstructionInfo stomachInfo;
    stomachInfo.name = "stomach";
    stomachInfo.halfExtents = flipYandZ(btVector3(0.05f, 0.15f, 0.05f));
    stomachInfo.position = flipYandZ(btVector3(0.0f, 0.0f, 1.2)) + OFFSET;
    stomachInfo.mass = 1.0f;
    stomachInfo.parentIndex = -1;

    BodyConstructionInfo chestInfo;
    chestInfo.name = "chest";
    chestInfo.halfExtents = flipYandZ(btVector3(0.05f, 0.15f, 0.05f));
    chestInfo.position = flipYandZ(btVector3(0.0f, 0.0f, 1.35)) + OFFSET;
    chestInfo.mass = 1.0f;
    chestInfo.parentIndex = 6;

    JointConstructionInfo leftHipJointInfo;
    leftHipJointInfo.name = "leftHipJoint";
    leftHipJointInfo.linkIndex = 0;
    leftHipJointInfo.parentLinkIndex = -1;
    leftHipJointInfo.jointType = JointConstructionInfo::SPHERICAL;
    leftHipJointInfo.worldPosition = flipYandZ(btVector3(0.0f, 0.1, 0.975)) + OFFSET;

    JointConstructionInfo leftKneeJointInfo;
    leftKneeJointInfo.name = "leftKneeJoint";
    leftKneeJointInfo.linkIndex = 1;
    leftKneeJointInfo.parentLinkIndex = 0;
    leftKneeJointInfo.jointType = JointConstructionInfo::REVOLUTE;
    leftKneeJointInfo.worldPosition = flipYandZ(btVector3(0.0f, 0.1, 0.525)) + OFFSET;
    leftKneeJointInfo.hingeAxis = flipYandZ(btVector3(0, 1, 0));

    JointConstructionInfo leftAnkleJointInfo;
    leftAnkleJointInfo.name = "leftAnkleJoint";
    leftAnkleJointInfo.linkIndex = 2;
    leftAnkleJointInfo.parentLinkIndex = 1;
    leftAnkleJointInfo.jointType = JointConstructionInfo::SPHERICAL;
    leftAnkleJointInfo.worldPosition = flipYandZ(btVector3(0.0f, 0.1, 0.075)) + OFFSET;

    JointConstructionInfo rightHipJointInfo;
    rightHipJointInfo.name = "rightHipJoint";
    rightHipJointInfo.linkIndex = 3;
    rightHipJointInfo.parentLinkIndex = -1;
    rightHipJointInfo.jointType = JointConstructionInfo::SPHERICAL;
    rightHipJointInfo.worldPosition = flipYandZ(btVector3(0.0f, -0.1, 0.975)) + OFFSET;

    JointConstructionInfo rightKneeJointInfo;
    rightKneeJointInfo.name = "rightKneeJoint";
    rightKneeJointInfo.linkIndex = 4;
    rightKneeJointInfo.parentLinkIndex = 3;
    rightKneeJointInfo.jointType = JointConstructionInfo::REVOLUTE;
    rightKneeJointInfo.worldPosition = flipYandZ(btVector3(0.0f, -0.1, 0.525)) + OFFSET;
    rightKneeJointInfo.hingeAxis = flipYandZ(btVector3(0, 1, 0));

    JointConstructionInfo rightAnkleJointInfo;
    rightAnkleJointInfo.name = "rightAnkleJoint";
    rightAnkleJointInfo.linkIndex = 5;
    rightAnkleJointInfo.parentLinkIndex = 4;
    rightAnkleJointInfo.jointType = JointConstructionInfo::SPHERICAL;
    rightAnkleJointInfo.worldPosition = flipYandZ(btVector3(0.0f, -0.1, 0.075)) + OFFSET;

    JointConstructionInfo hipToStomachJointInfo;
    hipToStomachJointInfo.name = "hipToStomachJoint";
    hipToStomachJointInfo.linkIndex = 6;
    hipToStomachJointInfo.parentLinkIndex = -1;
    hipToStomachJointInfo.jointType = JointConstructionInfo::SPHERICAL;
    hipToStomachJointInfo.worldPosition = flipYandZ(btVector3(0.0f, 0.0f, 1.125)) + OFFSET;

    JointConstructionInfo stomachToChestJointInfo;
    stomachToChestJointInfo.name = "stomachToChestJoint";
    stomachToChestJointInfo.linkIndex = 7;
    stomachToChestJointInfo.parentLinkIndex = 6;
    stomachToChestJointInfo.jointType = JointConstructionInfo::SPHERICAL;
    stomachToChestJointInfo.worldPosition = flipYandZ(btVector3(0.0f, 0.0f, 1.275)) + OFFSET;

    modelInfo.childLinks.push_back(leftUpperLegInfo);
    modelInfo.childLinks.push_back(leftLowerLegInfo);
    modelInfo.childLinks.push_back(leftFootInfo);
    modelInfo.childLinks.push_back(rightUpperLegInfo);
    modelInfo.childLinks.push_back(rightLowerLegInfo);
    modelInfo.childLinks.push_back(rightFootInfo);
    modelInfo.childLinks.push_back(stomachInfo);
    modelInfo.childLinks.push_back(chestInfo);
    modelInfo.joints.push_back(leftHipJointInfo);
    modelInfo.joints.push_back(leftKneeJointInfo);
    modelInfo.joints.push_back(leftAnkleJointInfo);
    modelInfo.joints.push_back(rightHipJointInfo);
    modelInfo.joints.push_back(rightKneeJointInfo);
    modelInfo.joints.push_back(rightAnkleJointInfo);
    modelInfo.joints.push_back(hipToStomachJointInfo);
    modelInfo.joints.push_back(stomachToChestJointInfo);

    createMultiBodyVehicle(modelInfo);
  }    
  
    if (1)
    {
        btVector3 groundHalfExtents(20,20,20);
        groundHalfExtents[upAxis]=1.f;
        btBoxShape* box = new btBoxShape(groundHalfExtents);
        box->initializePolyhedralFeatures();
        
        gfxBridge.createCollisionShapeGraphicsObject(box);
        btTransform start; start.setIdentity();
        btVector3 groundOrigin(0,0,0);
        groundOrigin[upAxis]=-1.5;
        start.setOrigin(groundOrigin);
        btRigidBody* body =  createRigidBody(0,start,box);
        btVector4 color = colors[curColor];
        curColor++;
        curColor&=3;
        gfxBridge.createRigidBodyGraphicsObject(body,color);
    }

}

void MultiBodyVehicleSetup::stepSimulation(float deltaTime)
{
      m_dynamicsWorld->stepSimulation(deltaTime, 10, FIXED_STEP);
      //m_dynamicsWorld->stepSimulation(deltaTime);
}

MultiBodyVehicle.h

#ifndef TEST_MULTIBODY_VEHICLE_SETUP_H
#define TEST_MULTIBODY_VEHICLE_SETUP_H

#include "Bullet3AppSupport/CommonMultiBodySetup.h"

#include <map>
#include <string>
#include <vector>

struct Comparator
{
  bool operator()(const btVector3 & u, const btVector3 & v) const;
};

struct MultiBodyVehicleSetup : public CommonMultiBodySetup
{
  struct BodyConstructionInfo
  {
    std::string name;
    btVector3 halfExtents;
    btVector3 position;
    float mass;
    int parentIndex;
  };

  struct JointConstructionInfo
  {
    enum JointType
    {
      SPHERICAL,
      REVOLUTE
    };

    std::string name;

    /*
     * The link in the BodyConstructionInfo vector to join.
     */
    int linkIndex;

    /*
     * The link in the BodyConstructionInfo vector to join linkIndex to.
     */
    int parentLinkIndex;

    JointType jointType;
    btVector3 worldPosition;

    /*
       * Axis of rotation if using hinge joint.
       */
      btVector3 hingeAxis;
    };

    struct ModelConstructionInfo
    {
      std::string modelName;

      /*
       * The base link.
       */
      std::string baseName;
      btVector3 baseHalfExtents;
      btVector3 basePosition;
      float baseMass;

      std::vector<BodyConstructionInfo> childLinks;
      std::vector<JointConstructionInfo> joints;
    };

    btMultiBody* m_multiBody;

public:

    MultiBodyVehicleSetup();
    virtual ~MultiBodyVehicleSetup();

    virtual void initPhysics(GraphicsPhysicsBridge& gfxBridge);

    virtual void stepSimulation(float deltaTime);
    
    class btMultiBody* createMultiBodyVehicle(const ModelConstructionInfo & info);

private:
    std::map<btVector3, btBoxShape *, Comparator> boxShapes;
    std::vector<btMultiBodyLinkCollider *> colliders;
    btBoxShape * getBoxShape(const btVector3 & halfExtents);
};
#endif //TEST_MULTIBODY_VEHICLE_SETUP_H

This a modification of the MultiBodyVehicle demo in Bullet revision dc731280b865317d0df66da64ab292bfbb341c32 (November 5, 2014).

The two files modified can be found in the Bullet repository at the path:
Demos3 / bullet2 / MultiBodyDemo /

In MultiBodyVehicle.cpp, the constants at the top of the file (FIXED_STEP, NUM_MULTIBODIES) can be modified to find a configuration that exhibits the exploding behavior, since I have observed the explosion appears in different scenarios on different computers (Windows 8.1, Ubuntu 14.04, OS X 10.9).

What you will see is when the bodies hit the floor, some will explode upward into the air.