Physics Simulation Forum

#include <iostream>
#include <vector>
#include <time.h>
#include <glut.h>

#include "btBulletDynamicsCommon.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"
#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"

using namespace std;


   btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
   btCollisionDispatcher* dispatcher = new   btCollisionDispatcher(collisionConfiguration);
   btBroadphaseInterface* overlappingPairCache = new btDbvtBroadphase();
   btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
   btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,overlappingPairCache,solver,collisionConfiguration);


float zoom = 15.0f;
float rotx = 0;
float roty = 0.001f;
float tx = 0;
float ty = 0;
int lastx=0;
int lasty=0;
unsigned char Buttons[3] = {0};


void Init()
{
   glEnable(GL_DEPTH_TEST);
      glEnable (GL_BLEND);
      glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 glShadeModel (GL_FLAT);
   
}

//-------------------------------------------------------------------------------
/// \brief   Called when the screen gets resized
/// \param   w   -   the new width
/// \param   h   -   the new height
///
void reshape(int w, int h)
{
   // prevent divide by 0 error when minimised
   if(w==0)
      h = 1;

   glViewport(0,0,w,h);
   glMatrixMode(GL_PROJECTION);
   glLoadIdentity();
   gluPerspective(45,(float)w/h,0.1,100);
   glMatrixMode(GL_MODELVIEW);
   glLoadIdentity();
}


//-------------------------------------------------------------------------------
//
void Motion(int x,int y)
{
   int diffx=x-lastx;
   int diffy=y-lasty;
   lastx=x;
   lasty=y;

   if( Buttons[2] )
   {
      zoom -= (float) 0.05f * diffx*2;
   }
   else
      if( Buttons[0] )
      {
         rotx += (float) 0.5f * diffy;
         roty += (float) 0.5f * diffx;      
      }
      else
         if( Buttons[1] )
         {
            tx += (float) 0.05f * diffx;
            ty -= (float) 0.05f * diffy;
         }
         glutPostRedisplay();
}

//-------------------------------------------------------------------------------
//
void Mouse(int b,int s,int x,int y)
{
   lastx=x;
   lasty=y;
   switch(b)
   {
   case GLUT_LEFT_BUTTON:
      Buttons[0] = ((GLUT_DOWN==s)?1:0);
      break;
   case GLUT_MIDDLE_BUTTON:
      Buttons[1] = ((GLUT_DOWN==s)?1:0);
      break;
   case GLUT_RIGHT_BUTTON:
      Buttons[2] = ((GLUT_DOWN==s)?1:0);
      break;
   default:
      break;      
   }
   glutPostRedisplay();
}




//-------------------------------------------------------------------------------
///
void display()
{
   glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);

   glLoadIdentity();

   glTranslatef(0,0,-zoom);
   glTranslatef(tx,ty,0);
   glRotatef(rotx,1,0,0);
   glRotatef(roty,0,1,0);   

   glColor3f(1.0f,1.0f,1.0f);
   // draw grid
   glBegin(GL_LINES);
   for(int i=-10;i<=10;++i) {
      glVertex3f(i,0,-10);
      glVertex3f(i,0,10);

      glVertex3f(10,0,i);
      glVertex3f(-10,0,i);
   }
   glEnd();





   dynamicsWorld->stepSimulation(1.f/60.f,10);
   dynamicsWorld->applyGravity();
      
         //print positions of all objects
      for (int j=dynamicsWorld->getNumCollisionObjects()-1; j>=0 ;j--)
      {
         btCollisionObject* obj = dynamicsWorld->getCollisionObjectArray()[j];
         btRigidBody* body = btRigidBody::upcast(obj);
         if (body && body->getMotionState())
         {
            btTransform trans;
            body->getMotionState()->getWorldTransform(trans);
            printf("world pos = %f,%f,%f\n",float(trans.getOrigin().getX()),float(trans.getOrigin().getY()),float(trans.getOrigin().getZ()));
            
            btScalar m[16];
            trans.getOpenGLMatrix(m);

            glPushMatrix();
            glColor4f(1,1,1,1);
            
            glMultMatrixf((GLfloat*)m);


         //   glTranslatef(trans.getOrigin().getX(),trans.getOrigin().getY(), float(trans.getOrigin().getZ()));
            //glutSolidSphere(1.0f,20,20);


            glBegin(GL_TRIANGLES);
            glVertex3f(0,1,0);glVertex3f(1,0,0);glVertex3f(0,0,0);
            glVertex3f(0,1,0);glVertex3f(0,0,1);glVertex3f(1,0,0);
            glVertex3f(1,0,0);glVertex3f(0,0,1);glVertex3f(0,0,0);
            glVertex3f(0,1,0);glVertex3f(0,0,0);glVertex3f(0,0,1);
            glEnd();
            glPopMatrix();

         }


      }


      glutPostRedisplay();

   glutSwapBuffers();
}





int main(int argc,char** argv) {

   dynamicsWorld->setGravity(btVector3(0,-10,0));

   ///create a few basic rigid bodies
   btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(50.),btScalar(55.),btScalar(50.)));

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

   collisionShapes.push_back(groundShape);

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

   {
      btScalar mass(0.);

      //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 recommended, 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);

   }


   vector <int> indices;
   indices.push_back(2);indices.push_back(1);indices.push_back(0);
   indices.push_back(2);indices.push_back(3);indices.push_back(1);
   indices.push_back(1);indices.push_back(3);indices.push_back(0);
   indices.push_back(2);indices.push_back(0);indices.push_back(3);

   std::vector <float> positions;
   positions.push_back(0);positions.push_back(0);positions.push_back(0);
   positions.push_back(1);positions.push_back(0);positions.push_back(0);
   positions.push_back(0);positions.push_back(1);positions.push_back(0);
   positions.push_back(0);positions.push_back(0);positions.push_back(1);

   btCollisionDispatcher * dispatcher = static_cast<btCollisionDispatcher *>(dynamicsWorld ->getDispatcher());
   btGImpactCollisionAlgorithm::registerAlgorithm(dispatcher);

btTriangleIndexVertexArray * vert = new btTriangleIndexVertexArray
                                 ( 4,
                                   &indices[0],
                                   3*sizeof(int),
                                   4,
                                   &positions[0],
                                   sizeof(float)  );


   btGImpactMeshShape * impact_shape = new btGImpactMeshShape(vert);
   impact_shape->setLocalScaling(btVector3(1.,1.,1.));
   impact_shape->setMargin(0.04f);
   impact_shape->updateBound();
   collisionShapes.push_back(impact_shape);
   btVector3 localInertia2(0,0,0);
   btScalar mass(3.0f);
   btTransform startTransform2;
   startTransform2.setIdentity();
   startTransform2.setOrigin( btVector3(2,10,0) ); //center is myShape's center
   impact_shape->calculateLocalInertia(mass,localInertia2);
   btDefaultMotionState* motionState = new btDefaultMotionState(startTransform2);
   btRigidBody * rig = new btRigidBody(mass,motionState,impact_shape,localInertia2);
   dynamicsWorld->addRigidBody(rig);
   

   btGImpactMeshShape * impact_shape2 = new btGImpactMeshShape(vert);
   impact_shape2->setLocalScaling(btVector3(1.,1.,1.));
   impact_shape2->setMargin(0.04f);
   impact_shape2->updateBound();
   collisionShapes.push_back(impact_shape2);
   startTransform2.setIdentity();
   startTransform2.setOrigin( btVector3(3,14,0) ); //center is myShape's center
   impact_shape2->calculateLocalInertia(mass,localInertia2);
   btDefaultMotionState* motionState2 = new btDefaultMotionState(startTransform2);
   btRigidBody * rig2 = new btRigidBody(mass,motionState2,impact_shape2,localInertia2);
   dynamicsWorld->addRigidBody(rig2);


   glutInit(&argc,argv);
   glutInitDisplayMode(GLUT_DOUBLE|GLUT_RGBA|GLUT_DEPTH);
   glutInitWindowSize(900,900);
   glutInitWindowPosition(200,100);
   glutCreateWindow("TestBed Framework");
   glutDisplayFunc(display);
   glutReshapeFunc(reshape);
   glutMouseFunc(Mouse);
   glutMotionFunc(Motion);

   Init();

   glutMainLoop();

   return 0;
}