Code: Select all
#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;
}