Demos/ConcaveConvexcastDemo/ConcaveConvexcastDemo.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 #include "btBulletDynamicsCommon.h"
  17 #include "LinearMath/btIDebugDraw.h"
  18 #include "BulletCollision/CollisionShapes/btBoxShape.h"
  19 #include "GLDebugDrawer.h"
  20 #include "ConcaveConvexcastDemo.h"
  21 #include "GL_ShapeDrawer.h"
  22 #include "GlutStuff.h"
  23
  24 #define NUM_DYNAMIC_BOXES_X 30
  25 #define NUM_DYNAMIC_BOXES_Y 30
  26
  27 static btVector3*       gVertices=0;
  28 static int*     gIndices=0;
  29 static btBvhTriangleMeshShape* trimeshShape =0;
  30 static btRigidBody* staticBody = 0;
  31 static float waveheight = 5.f;
  32
  33 const float TRIANGLE_SIZE=8.f;
  34
  35
  36 /* Scrolls back and forth over terrain */
  37 #define NUMRAYS_IN_BAR 100
  38 class btConvexcastBatch
  39 {
  40 public:
  41         btVector3 source[NUMRAYS_IN_BAR];
  42         btVector3 dest[NUMRAYS_IN_BAR];
  43         btVector3 direction[NUMRAYS_IN_BAR];
  44         btVector3 hit_com[NUMRAYS_IN_BAR];
  45         btVector3 hit_surface[NUMRAYS_IN_BAR];
  46         btScalar hit_fraction[NUMRAYS_IN_BAR];
  47         btVector3 normal[NUMRAYS_IN_BAR];
  48
  49         int frame_counter;
  50         int ms;
  51         int sum_ms;
  52         int sum_ms_samples;
  53         int min_ms;
  54         int max_ms;
  55
  56 #ifdef USE_BT_CLOCK
  57         btClock frame_timer;
  58 #endif //USE_BT_CLOCK
  59
  60         btScalar dx;
  61         btScalar min_x;
  62         btScalar max_x;
  63         btScalar min_y;
  64         btScalar max_y;
  65         btScalar sign;
  66
  67         btVector3  boxShapeHalfExtents;
  68         btBoxShape boxShape;
  69
  70         btConvexcastBatch () : boxShape(btVector3(0.0, 0.0, 0.0))
  71         {
  72                 ms = 0;
  73                 max_ms = 0;
  74                 min_ms = 9999.0;
  75                 sum_ms_samples = 0;
  76                 sum_ms = 0;
  77         }
  78
  79         btConvexcastBatch (bool unused, btScalar ray_length, btScalar min_z, btScalar max_z, btScalar min_y , btScalar max_y ) : boxShape(btVector3(0.0, 0.0, 0.0))
  80         {
  81                 boxShapeHalfExtents = btVector3(1.0, 1.0, 1.0);
  82                 boxShape = btBoxShape(boxShapeHalfExtents);
  83                 frame_counter = 0;
  84                 ms = 0;
  85                 max_ms = 0;
  86                 min_ms = 9999.0;
  87                 sum_ms_samples = 0;
  88                 sum_ms = 0;
  89                 dx = 10.0;
  90                 min_x = -40;
  91                 max_x = 20;
  92                 this->min_y = min_y;
  93                 this->max_y = max_y;
  94                 sign = 1.0;
  95         //      btScalar dalpha = 2*SIMD_2_PI/NUMRAYS_IN_BAR;
  96                 for (int i = 0; i < NUMRAYS_IN_BAR; i++)
  97                 {
  98                         btScalar z = (max_z-min_z)/NUMRAYS_IN_BAR * i + min_z;
  99                         source[i] = btVector3(min_x, max_y, z);
 100                         dest[i] = btVector3(min_x + ray_length, min_y, z);
 101                         normal[i] = btVector3(1.0, 0.0, 0.0);
 102                 }
 103         }
 104
 105         btConvexcastBatch (btScalar ray_length, btScalar z, btScalar min_y = -1000, btScalar max_y = 10) : boxShape(btVector3(0.0, 0.0, 0.0))
 106         {
 107                 boxShapeHalfExtents = btVector3(1.0, 1.0, 1.0);
 108                 boxShape = btBoxShape(boxShapeHalfExtents);
 109                 frame_counter = 0;
 110                 ms = 0;
 111                 max_ms = 0;
 112                 min_ms = 9999.0;
 113                 sum_ms_samples = 0;
 114                 sum_ms = 0;
 115                 dx = 10.0;
 116                 min_x = -40;
 117                 max_x = 20;
 118                 this->min_y = min_y;
 119                 this->max_y = max_y;
 120                 sign = 1.0;
 121                 btScalar dalpha = btScalar(2)*SIMD_2_PI/btScalar(NUMRAYS_IN_BAR);
 122                 for (int i = 0; i < NUMRAYS_IN_BAR; i++)
 123                 {
 124                         btScalar alpha = dalpha * btScalar(i);
 125                         // rotate around by alpha degrees y
 126                         btTransform tr(btQuaternion(btVector3(0.0, 1.0, 0.0), alpha));
 127                         direction[i] = btVector3(1.0, 0.0, 0.0);
 128                         direction[i] = tr * direction[i];
 129                         source[i] = btVector3(min_x, max_y, z);
 130                         dest[i] = source[i] + direction[i] * ray_length;
 131                         dest[i][1] = min_y;
 132                         normal[i] = btVector3(1.0, 0.0, 0.0);
 133                 }
 134         }
 135
 136         void move (btScalar dt)
 137         {
 138                 if (dt > (1.0/60.0))
 139                         dt = 1.0/60.0;
 140                 for (int i = 0; i < NUMRAYS_IN_BAR; i++)
 141                 {
 142                         source[i][0] += dx * dt * sign;
 143                         dest[i][0] += dx * dt * sign;
 144                 }
 145                 if (source[0][0] < min_x)
 146                         sign = 1.0;
 147                 else if (source[0][0] > max_x)
 148                         sign = -1.0;
 149         }
 150
 151         void cast (btCollisionWorld* cw)
 152         {
 153 #ifdef USE_BT_CLOCK
 154                 frame_timer.reset ();
 155 #endif //USE_BT_CLOCK
 156                 for (int i = 0; i < NUMRAYS_IN_BAR; i++)
 157                 {
 158                         btCollisionWorld::ClosestConvexResultCallback cb(source[i], dest[i]);
 159                         btQuaternion qFrom;
 160                         btQuaternion qTo;
 161                         qFrom.setRotation (btVector3(1.0, 0.0, 0.0), 0.0);
 162                         qTo.setRotation (btVector3(1.0, 0.0, 0.0), 0.7);
 163                         btTransform from(qFrom, source[i]);
 164                         btTransform to(qTo, dest[i]);
 165                         cw->convexSweepTest (&boxShape, from, to, cb);
 166                         if (cb.hasHit ())
 167                         {
 168                                 hit_surface[i] = cb.m_hitPointWorld;
 169                                 hit_com[i].setInterpolate3(source[i], dest[i], cb.m_closestHitFraction);
 170                                 hit_fraction[i] = cb.m_closestHitFraction;
 171                                 normal[i] = cb.m_hitNormalWorld;
 172                                 normal[i].normalize ();
 173                         } else {
 174                                 hit_com[i] = dest[i];
 175                                 hit_surface[i] = dest[i];
 176                                 hit_fraction[i] = 1.0f;
 177                                 normal[i] = btVector3(1.0, 0.0, 0.0);
 178                         }
 179
 180                 }
 181 #ifdef USE_BT_CLOCK
 182                 ms += frame_timer.getTimeMilliseconds ();
 183 #endif //USE_BT_CLOCK
 184                 frame_counter++;
 185                 if (frame_counter > 50)
 186                 {
 187                         min_ms = ms < min_ms ? ms : min_ms;
 188                         max_ms = ms > max_ms ? ms : max_ms;
 189                         sum_ms += ms;
 190                         sum_ms_samples++;
 191                         btScalar mean_ms = (btScalar)sum_ms/(btScalar)sum_ms_samples;
 192                         printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
 193                         ms = 0;
 194                         frame_counter = 0;
 195                 }
 196         }
 197
 198
 199         void drawCube (const btTransform& T)
 200         {
 201                 ATTRIBUTE_ALIGNED16(btScalar) m[16];
 202                 T.getOpenGLMatrix (&m[0]);
 203                 glPushMatrix ();
 204 #ifdef BT_USE_DOUBLE_PRECISION
 205                 glMultMatrixd (&m[0]);
 206                         glScaled (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]);
 207 #else
 208                         glMultMatrixf (&m[0]);
 209                         glScalef (2.0 * boxShapeHalfExtents[0], 2.0 * boxShapeHalfExtents[1], 2.0 * boxShapeHalfExtents[2]);
 210 #endif //BT_USE_DOUBLE_PRECISION
 211                         glutSolidCube (1.0);
 212                 glPopMatrix ();
 213         }
 214
 215         void draw ()
 216         {
 217                 glDisable (GL_LIGHTING);
 218                 glColor3f (0.0, 1.0, 0.0);
 219                 glBegin (GL_LINES);
 220                 int i;
 221                 for (i = 0; i < NUMRAYS_IN_BAR; i++)
 222                 {
 223                         glVertex3f (source[i][0], source[i][1], source[i][2]);
 224                         glVertex3f (hit_com[i][0], hit_com[i][1], hit_com[i][2]);
 225                 }
 226                 glColor3f (1.0, 1.0, 1.0);
 227                 glBegin (GL_LINES);
 228                 btScalar normal_scale = 10.0; // easier to see if this is big
 229                 for (i = 0; i < NUMRAYS_IN_BAR; i++)
 230                 {
 231                         glVertex3f (hit_surface[i][0], hit_surface[i][1], hit_surface[i][2]);
 232                         glVertex3f (hit_surface[i][0] + normal_scale * normal[i][0], hit_surface[i][1] + normal_scale * normal[i][1], hit_surface[i][2] + normal_scale * normal[i][2]);
 233                 }
 234                 glEnd ();
 235                 glColor3f (0.0, 1.0, 1.0);
 236                 btQuaternion qFrom;
 237                 btQuaternion qTo;
 238                 qFrom.setRotation (btVector3(1.0, 0.0, 0.0), 0.0);
 239                 qTo.setRotation (btVector3(1.0, 0.0, 0.0), 0.7);
 240                 for ( i = 0; i < NUMRAYS_IN_BAR; i++)
 241                 {
 242                         btTransform from(qFrom, source[i]);
 243                         btTransform to(qTo, dest[i]);
 244                         btVector3 linVel, angVel;
 245                         btTransformUtil::calculateVelocity (from, to, 1.0, linVel, angVel);
 246                         btTransform T;
 247                         btTransformUtil::integrateTransform (from, linVel, angVel, hit_fraction[i], T);
 248                         drawCube (T);
 249                 }
 250                 glEnable (GL_LIGHTING);
 251         }
 252 };
 253
 254
 255 static btConvexcastBatch convexcastBatch;
 256
 257
 258
 259
 260
 261 const int NUM_VERTS_X = 30;
 262 const int NUM_VERTS_Y = 30;
 263 const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
 264
 265 void    ConcaveConvexcastDemo::setVertexPositions(float waveheight, float offset)
 266 {
 267         int i;
 268         int j;
 269
 270         for ( i=0;i<NUM_VERTS_X;i++)
 271         {
 272                 for (j=0;j<NUM_VERTS_Y;j++)
 273                 {
 274                         gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
 275                                 //0.f,
 276                                 waveheight*sinf((float)i+offset)*cosf((float)j+offset),
 277                                 (j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
 278                 }
 279         }
 280 }
 281
 282 void ConcaveConvexcastDemo::keyboardCallback(unsigned char key, int x, int y)
 283 {
 284         if (key == 'g')
 285         {
 286                 m_animatedMesh = !m_animatedMesh;
 287                 if (m_animatedMesh)
 288                 {
 289                         staticBody->setCollisionFlags( staticBody->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
 290                         staticBody->setActivationState(DISABLE_DEACTIVATION);
 291                 } else
 292                 {
 293                         staticBody->setCollisionFlags( staticBody->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
 294                         staticBody->forceActivationState(ACTIVE_TAG);
 295                 }
 296         }
 297
 298         DemoApplication::keyboardCallback(key,x,y);
 299
 300 }
 301
 302 void    ConcaveConvexcastDemo::initPhysics()
 303 {
 304         #define TRISIZE 10.f
 305
 306         setCameraDistance(100.f);
 307
 308
 309         int vertStride = sizeof(btVector3);
 310         int indexStride = 3*sizeof(int);
 311
 312
 313         const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
 314
 315         gVertices = new btVector3[totalVerts];
 316         gIndices = new int[totalTriangles*3];
 317
 318         int i;
 319
 320
 321         setVertexPositions(waveheight,0.f);
 322
 323         int index=0;
 324         for ( i=0;i<NUM_VERTS_X-1;i++)
 325         {
 326                 for (int j=0;j<NUM_VERTS_Y-1;j++)
 327                 {
 328                         gIndices[index++] = j*NUM_VERTS_X+i;
 329                         gIndices[index++] = j*NUM_VERTS_X+i+1;
 330                         gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
 331
 332                         gIndices[index++] = j*NUM_VERTS_X+i;
 333                         gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
 334                         gIndices[index++] = (j+1)*NUM_VERTS_X+i;
 335                 }
 336         }
 337
 338         m_indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
 339                 gIndices,
 340                 indexStride,
 341                 totalVerts,(btScalar*) &gVertices[0].x(),vertStride);
 342
 343         bool useQuantizedAabbCompression = true;
 344
 345         trimeshShape  = new btBvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression);
 346
 347         m_collisionShapes.push_back(trimeshShape);
 348
 349         btCollisionShape* groundShape = trimeshShape;
 350
 351
 352         m_collisionConfiguration = new btDefaultCollisionConfiguration();
 353
 354         m_dispatcher = new      btCollisionDispatcher(m_collisionConfiguration);
 355
 356         btVector3 worldMin(-1000,-1000,-1000);
 357         btVector3 worldMax(1000,1000,1000);
 358         m_broadphase = new btAxisSweep3(worldMin,worldMax);
 359         m_solver = new btSequentialImpulseConstraintSolver();
 360         m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
 361
 362         float mass = 0.f;
 363         btTransform     startTransform;
 364         startTransform.setIdentity();
 365         startTransform.setOrigin(btVector3(0,-2,0));
 366
 367         btCollisionShape* colShape = new btBoxShape(btVector3(1,1,1));
 368         m_collisionShapes.push_back(colShape);
 369
 370         {
 371                 for (int j=0;j<NUM_DYNAMIC_BOXES_X;j++)
 372                 for (int i=0;i<NUM_DYNAMIC_BOXES_Y;i++)
 373                 {
 374                         //btCollisionShape* colShape = new btCapsuleShape(0.5,2.0);//boxShape = new btSphereShape(1.f);
 375                         startTransform.setOrigin(btVector3(5*(i-NUM_DYNAMIC_BOXES_X/2),10,5*(j-NUM_DYNAMIC_BOXES_Y/2)));
 376                         localCreateRigidBody(1, startTransform,colShape);
 377                 }
 378         }
 379
 380         startTransform.setIdentity();
 381         //startTransform = btTransform(btQuaternion (btVector3(1,1,1), 1.5));
 382         staticBody = localCreateRigidBody(mass, startTransform,groundShape);
 383
 384         staticBody->setCollisionFlags(staticBody->getCollisionFlags() | btCollisionObject::CF_STATIC_OBJECT);
 385
 386         //enable custom material callback
 387         staticBody->setCollisionFlags(staticBody->getCollisionFlags()  | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
 388
 389         convexcastBatch = btConvexcastBatch (40.0, 0.0, -10.0,80.0);
 390         //convexcastBatch = btConvexcastBatch (true, 40.0, -50.0, 50.0);
 391 }
 392
 393 void ConcaveConvexcastDemo::clientMoveAndDisplay()
 394 {
 395          glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 396
 397         float dt = getDeltaTimeMicroseconds() * 0.000001f;
 398
 399         if (m_animatedMesh)
 400         {
 401                 static float offset=0.f;
 402                 offset+=0.01f;
 403
 404
 405
 406                 int i;
 407                 int j;
 408                 btVector3 aabbMin(BT_LARGE_FLOAT,BT_LARGE_FLOAT,BT_LARGE_FLOAT);
 409                 btVector3 aabbMax(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);
 410
 411                 for ( i=NUM_VERTS_X/2-3;i<NUM_VERTS_X/2+2;i++)
 412                 {
 413                         for (j=NUM_VERTS_X/2-3;j<NUM_VERTS_Y/2+2;j++)
 414                         {
 415
 416                         aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
 417                         aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
 418
 419                                 gVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
 420                                         //0.f,
 421                                         waveheight*sinf((float)i+offset)*cosf((float)j+offset),
 422                                         (j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
 423
 424                         aabbMin.setMin(gVertices[i+j*NUM_VERTS_X]);
 425                         aabbMax.setMax(gVertices[i+j*NUM_VERTS_X]);
 426
 427                         }
 428                 }
 429
 430                 trimeshShape->partialRefitTree(aabbMin,aabbMax);
 431
 432
 433                 //clear all contact points involving mesh proxy. Note: this is a slow/unoptimized operation.
 434                 m_dynamicsWorld->getBroadphase()->getOverlappingPairCache()->cleanProxyFromPairs(staticBody->getBroadphaseHandle(),getDynamicsWorld()->getDispatcher());
 435         }
 436
 437         m_dynamicsWorld->stepSimulation(dt);
 438
 439         //optional but useful: debug drawing
 440         m_dynamicsWorld->debugDrawWorld();
 441
 442         convexcastBatch.move (dt);
 443         convexcastBatch.cast (m_dynamicsWorld);
 444         renderme();
 445         convexcastBatch.draw ();
 446     glFlush();
 447     glutSwapBuffers();
 448
 449 }
 450
 451
 452
 453
 454 void ConcaveConvexcastDemo::displayCallback(void) {
 455
 456     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 457
 458         renderme();
 459         convexcastBatch.draw ();
 460     glFlush();
 461     glutSwapBuffers();
 462 }
 463
 464
 465
 466 void    ConcaveConvexcastDemo::exitPhysics()
 467 {
 468
 469
 470
 471         //cleanup in the reverse order of creation/initialization
 472
 473         //remove the rigidbodies from the dynamics world and delete them
 474         int i;
 475         for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
 476         {
 477                 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
 478                 btRigidBody* body = btRigidBody::upcast(obj);
 479                 if (body && body->getMotionState())
 480                 {
 481                         delete body->getMotionState();
 482                 }
 483                 m_dynamicsWorld->removeCollisionObject( obj );
 484                 delete obj;
 485         }
 486
 487         //delete collision shapes
 488         for (int j=0;j<m_collisionShapes.size();j++)
 489         {
 490                 btCollisionShape* shape = m_collisionShapes[j];
 491                 delete shape;
 492         }
 493
 494         //delete dynamics world
 495         delete m_dynamicsWorld;
 496
 497         if (m_indexVertexArrays)
 498                 delete m_indexVertexArrays;
 499
 500         //delete solver
 501         delete m_solver;
 502
 503         //delete broadphase
 504         delete m_broadphase;
 505
 506         //delete dispatcher
 507         delete m_dispatcher;
 508
 509         delete m_collisionConfiguration;
 510
 511
 512 }
 513
 514
 515
 516