widdled down BullPhysSim

#include <sstream>
#include <iostream>
#include <string>
#include <math.h>
#include <time.h>
#include <fstream>
#include <cstdio>

#define CONSTRAINT_DEBUG_SIZE 0.2f

#include "btBulletDynamicsCommon.h"
#include "GlutStuff.h"
#include "GL_ShapeDrawer.h"
#include "LinearMath/btIDebugDraw.h"
#include "GLDebugDrawer.h"

#include "GlutDemoApplication.h"
#include "btBulletCollisionCommon.h"
#include "btBulletDynamicsCommon.h"

using namespace std;

class BullPhysSim : public GlutDemoApplication
{
    //keep the collision shapes, for deletion/cleanup
    btAlignedObjectArray<btCollisionShape*> m_collisionShapes;

    btBroadphaseInterface * m_broadphase;
    btCollisionDispatcher * m_dispatcher;
    btConstraintSolver    * m_solver;
    btDefaultCollisionConfiguration * m_collisionConfiguration;

    btCollisionShape  * geom[6];
    btCollisionObject * fixedGround;
    unsigned long int simtick;

public:
    btRigidBody       * body[6];
    unsigned long int timeStep;
    int threadID;

public:
    void initPhysics();
    void exitPhysics();

    virtual ~BullPhysSim()
    {
        exitPhysics();
    }

    virtual void clientMoveAndDisplay();
    virtual void displayCallback();
    virtual void keyboardCallback(unsigned char key, int x, int y);

    static DemoApplication* Create()
    {
        BullPhysSim * demo = new BullPhysSim();
        demo->myinit();
        demo->initPhysics();
        return demo;
    }

     virtual void renderme() {
        extern GLDebugDrawer gDebugDrawer;
          // Call the parent method.
        GlutDemoApplication::renderme();
    }

    void CreateBox(
        int index,
        double x,       double y,       double z,
        double length,  double width,   double height );

    void CreateSphere(
        int index,
        double x,       double y,       double z,
        double radius );


    void DeleteObject( int index );
    btVector3 PointWorldToLocal(int index, btVector3 &p);
    btVector3 AxisWorldToLocal( int index, btVector3 &a);
};





void BullPhysSim::initPhysics()
{
    simtick = 0;
    timeStep = 0;

    // Setup the basic world
    setTexturing(true);
    setShadows(true);
    setCameraDistance(btScalar(5.));

    m_collisionConfiguration = new btDefaultCollisionConfiguration();
    m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);

    btVector3 worldAabbMin(-10000,-10000,-10000);
    btVector3 worldAabbMax(10000,10000,10000);
    m_broadphase = new btAxisSweep3 (worldAabbMin, worldAabbMax);

    m_solver = new btSequentialImpulseConstraintSolver;
    m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);

    // Setup a big ground box
    {
        btCollisionShape * groundShape = new btBoxShape(btVector3(btScalar(300.),btScalar(10.),btScalar(300.)));
        m_collisionShapes.push_back(groundShape);
        btTransform groundTransform;
        groundTransform.setIdentity();
        groundTransform.setOrigin(btVector3(0,-10,0));

        fixedGround = new btCollisionObject();
        fixedGround->setCollisionShape(groundShape);
        fixedGround->setWorldTransform(groundTransform);
        m_dynamicsWorld->addCollisionObject(fixedGround);
    }

    CreateBox(0,
              0., 2., 0.,
              1., 1., 0.2 );

    CreateSphere(1,
                -3.00 , 2. , 0.0 ,
                0.27 );    // East kneeball

    CreateSphere(2,
                3.00 , 2. , 0.0 ,
                0.27 );    // West kneeball

    CreateSphere(3,
                0.0 , 2. , 3.00 ,
                0.27 );    // North kneeball

    CreateSphere(4,
                0.0 , 2. , -3.00 ,
                0.27 );    // South kneeball

    clientResetScene();
}



void BullPhysSim::clientMoveAndDisplay()
{
    double deltaT;
    //.

    float ms = getDeltaTimeMicroseconds();
    float minFPS = 90000.f/60.f;
    if (ms > minFPS)
        ms = minFPS;
    deltaT = ms / 90000.f;


    if (m_dynamicsWorld)
    {
        m_dynamicsWorld->stepSimulation( btScalar(deltaT) );
        timeStep++;
    }
}


void BullPhysSim
::exitPhysics()
{
    int i, nocans;

    nocans=5;
    i=0;
    while(i<nocans) {
        DeleteObject(i);
        i++;
    }


    //cleanup in the reverse order of creation/initialization
    //remove the rigidbodies from the dynamics world and delete them

    for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
    {
        btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
        btRigidBody* body = btRigidBody::upcast(obj);
        if (body && body->getMotionState())
        {
            delete body->getMotionState();
        }
        m_dynamicsWorld->removeCollisionObject( obj );
        delete obj;
    }


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


    delete m_dynamicsWorld;
    delete m_solver;
    delete m_broadphase;
    delete m_dispatcher;
    delete m_collisionConfiguration;
}



void BullPhysSim
::DeleteObject( int index )
{
    m_dynamicsWorld->removeRigidBody( body[index] );
    delete body[index];
    delete geom[index];
}


void BullPhysSim
::CreateBox(
        int index,
        double x ,      double y ,      double z ,
        double length , double width , double height)
{
    btCollisionShape * btCSptr;
    btDefaultMotionState * myMotionState;
    btTransform offset;
    btTransform transform;
    btTransform startTransform;
    btVector3 localInertia(0,0,0);
    double mass;
    //.

    mass = 1.000;

    // `offset` is the location of the entire robot in world.
    offset.setIdentity();
    offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );

    // `transform` is the  local body coordinates of appendages.
    transform.setIdentity();
    transform.setOrigin(
        btVector3(
            btScalar(x) ,
            btScalar(y) ,
            btScalar(z) )
    );

    geom[index] = new btBoxShape(
            btVector3(
                btScalar(length) ,
                btScalar(height) ,
                btScalar(width ) )
        );
    btCSptr = geom[index];

    btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );

    myMotionState = new btDefaultMotionState(offset*transform);

    btRigidBody::btRigidBodyConstructionInfo rbInfo(
        btScalar(mass),myMotionState,btCSptr,localInertia
        );

    body[index] = new btRigidBody(rbInfo);

    m_dynamicsWorld->addRigidBody( body[index] );
}





void BullPhysSim
::CreateSphere(
        int index,
        double x,       double y,       double z,
        double radius )
{
    btCollisionShape * btCSptr;
    btDefaultMotionState * myMotionState;
    btTransform offset;
    btTransform transform;
    btTransform startTransform;
    btVector3 localInertia(0,0,0);
    double mass;
    //.

    mass = 0.30;

    // `offset` is the location of the entire robot in world.
    offset.setIdentity();
    offset.setOrigin(  btVector3( btScalar(0.) , btScalar(0.) , btScalar(0.))    );

    // `transform` is the  local body coordinates of appendages.
    transform.setIdentity();
    transform.setOrigin(
        btVector3(
            btScalar(x) ,
            btScalar(y) ,
            btScalar(z) )
    );

    geom[index] = new btSphereShape(  btScalar(radius)  );
    btCSptr = geom[index];

    btCSptr->calculateLocalInertia( btScalar(mass) , localInertia );

    myMotionState = new btDefaultMotionState(offset*transform);

    btRigidBody::btRigidBodyConstructionInfo rbInfo(
        btScalar(mass) , myMotionState,btCSptr,localInertia);

    body[index] = new btRigidBody(rbInfo);

    m_dynamicsWorld->addRigidBody( body[index] );
}

btVector3 BullPhysSim
::PointWorldToLocal(int index, btVector3 &p)
{
  btVector3 btvloc;
  btTransform local1  = body[index]->getCenterOfMassTransform().inverse();
  //.
  btvloc = (local1)*(p);
  return (btvloc);
}

btVector3 BullPhysSim
::AxisWorldToLocal(int index, btVector3 &a) {
  btTransform local1 = body[index]->getCenterOfMassTransform().inverse();
  btVector3 zero(0,0,0);
  local1.setOrigin(zero);
  return (
      local1 * a
    );
}


void BullPhysSim::displayCallback()
{
    // .. //
}

void BullPhysSim::keyboardCallback(unsigned char key, int x, int y)
{
    switch (key)
    {
    case 'n':
        {

        }
        break;
    default:
        DemoApplication::keyboardCallback(key, x, y);
    }


}





long TaskDebug(int passedindex )
{
    BullPhysSim BPS;
    int T ;
    long esc_step;
    bool Life;
    //.
    T = passedindex;

    BPS.threadID = T;
    BPS.initPhysics();
    BPS.getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);

    Life = true;
    while( Life ) {
        if( BPS.timeStep <= 3000 ) {
            BPS.clientMoveAndDisplay();
        } else {
            esc_step = BPS.timeStep;
            Life = false;
        }
    }

    return esc_step;
}



// ////////////////////
// MAIN ENTRY POINT
// /////////////////////
int main(int argc,char* argv[])
{
    int mat = 0;
    long retTS;

    while( mat < 12 ) {
        cout << "calling TaskDebug( " ;
        cout << mat;
        cout << endl;
        retTS = TaskDebug(mat);
        cout << "returned from TaskDebug() with timeStep = ";
        cout << retTS;
        cout << endl << endl;
        mat++;
    }

    return 1;
}