Demos/Benchmarks/BenchmarkDemo.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
  17
  18 // Collision Radius
  19 #define COLLISION_RADIUS 0.0f
  20
  21 #include "BenchmarkDemo.h"
  22 #ifdef USE_GRAPHICAL_BENCHMARK
  23 #include "GlutStuff.h"
  24 #endif //USE_GRAPHICAL_BENCHMARK
  25
  26 ///btBulletDynamicsCommon.h is the main Bullet include file, contains most common include files.
  27 #include "btBulletDynamicsCommon.h"
  28 #include <stdio.h> //printf debugging
  29 #include "Taru.mdl"
  30 #include "landscape.mdl"
  31 #include "BulletCollision/BroadphaseCollision/btDbvtBroadphase.h"
  32 #ifdef USE_PARALLEL_DISPATCHER_BENCHMARK
  33 #include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
  34 #include "BulletMultiThreaded/SequentialThreadSupport.h"
  35 #include "BulletMultiThreaded/SpuNarrowPhaseCollisionTask/SpuGatheringCollisionTask.h"
  36 #endif
  37
  38 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
  39
  40
  41 #ifdef USE_PARALLEL_DISPATCHER_BENCHMARK
  42 #ifdef _WIN32
  43 #include "BulletMultiThreaded/Win32ThreadSupport.h"
  44 #elif defined (USE_PTHREADS)
  45 #include "BulletMultiThreaded/PosixThreadSupport.h"
  46 #endif
  47 #include "BulletMultiThreaded/SpuGatheringCollisionDispatcher.h"
  48 #include "BulletMultiThreaded/btParallelConstraintSolver.h"
  49
  50
  51
  52
  53 btThreadSupportInterface* createSolverThreadSupport(int maxNumThreads)
  54 {
  55 //#define SEQUENTIAL
  56 #ifdef SEQUENTIAL
  57         SequentialThreadSupport::SequentialThreadConstructionInfo tci("solverThreads",SolverThreadFunc,SolverlsMemoryFunc);
  58         SequentialThreadSupport* threadSupport = new SequentialThreadSupport(tci);
  59         threadSupport->startSPU();
  60 #else
  61
  62 #ifdef _WIN32
  63         Win32ThreadSupport::Win32ThreadConstructionInfo threadConstructionInfo("solverThreads",SolverThreadFunc,SolverlsMemoryFunc,maxNumThreads);
  64         Win32ThreadSupport* threadSupport = new Win32ThreadSupport(threadConstructionInfo);
  65         threadSupport->startSPU();
  66 #elif defined (USE_PTHREADS)
  67         PosixThreadSupport::ThreadConstructionInfo solverConstructionInfo("solver", SolverThreadFunc,
  68                                                                                                                                           SolverlsMemoryFunc, maxNumThreads);
  69
  70         PosixThreadSupport* threadSupport = new PosixThreadSupport(solverConstructionInfo);
  71
  72 #else
  73         SequentialThreadSupport::SequentialThreadConstructionInfo tci("solverThreads",SolverThreadFunc,SolverlsMemoryFunc);
  74         SequentialThreadSupport* threadSupport = new SequentialThreadSupport(tci);
  75         threadSupport->startSPU();
  76 #endif
  77
  78 #endif
  79
  80         return threadSupport;
  81 }
  82 #endif
  83
  84 class btRaycastBar2
  85 {
  86 public:
  87         btVector3 source[NUMRAYS];
  88         btVector3 dest[NUMRAYS];
  89         btVector3 direction[NUMRAYS];
  90         btVector3 hit[NUMRAYS];
  91         btVector3 normal[NUMRAYS];
  92
  93         int frame_counter;
  94         int ms;
  95         int sum_ms;
  96         int sum_ms_samples;
  97         int min_ms;
  98         int max_ms;
  99
 100 #ifdef USE_BT_CLOCK
 101         btClock frame_timer;
 102 #endif //USE_BT_CLOCK
 103
 104         btScalar dx;
 105         btScalar min_x;
 106         btScalar max_x;
 107         btScalar max_y;
 108         btScalar sign;
 109
 110         btRaycastBar2 ()
 111         {
 112                 ms = 0;
 113                 max_ms = 0;
 114                 min_ms = 9999;
 115                 sum_ms_samples = 0;
 116                 sum_ms = 0;
 117         }
 118
 119
 120
 121         btRaycastBar2 (btScalar ray_length, btScalar z,btScalar max_y)
 122         {
 123                 frame_counter = 0;
 124                 ms = 0;
 125                 max_ms = 0;
 126                 min_ms = 9999;
 127                 sum_ms_samples = 0;
 128                 sum_ms = 0;
 129                 dx = 10.0;
 130                 min_x = 0;
 131                 max_x = 0;
 132                 this->max_y = max_y;
 133                 sign = 1.0;
 134                 btScalar dalpha = 2*SIMD_2_PI/NUMRAYS;
 135                 for (int i = 0; i < NUMRAYS; i++)
 136                 {
 137                         btScalar alpha = dalpha * i;
 138                         // rotate around by alpha degrees y
 139                         btQuaternion q(btVector3(0.0, 1.0, 0.0), alpha);
 140                         direction[i] = btVector3(1.0, 0.0, 0.0);
 141                         direction[i] = quatRotate(q , direction[i]);
 142                         direction[i] = direction[i] * ray_length;
 143
 144
 145                         source[i] = btVector3(min_x, max_y, z);
 146                         dest[i] = source[i] + direction[i];
 147                         dest[i][1]=-1000;
 148                         normal[i] = btVector3(1.0, 0.0, 0.0);
 149                 }
 150         }
 151
 152         void move (btScalar dt)
 153         {
 154                 if (dt > btScalar(1.0/60.0))
 155                         dt = btScalar(1.0/60.0);
 156                 for (int i = 0; i < NUMRAYS; i++)
 157                 {
 158                         source[i][0] += dx * dt * sign;
 159                         dest[i][0] += dx * dt * sign;
 160                 }
 161                 if (source[0][0] < min_x)
 162                         sign = 1.0;
 163                 else if (source[0][0] > max_x)
 164                         sign = -1.0;
 165         }
 166
 167         void cast (btCollisionWorld* cw)
 168         {
 169 #ifdef USE_BT_CLOCK
 170                 frame_timer.reset ();
 171 #endif //USE_BT_CLOCK
 172
 173 #ifdef BATCH_RAYCASTER
 174                 if (!gBatchRaycaster)
 175                         return;
 176
 177                 gBatchRaycaster->clearRays ();
 178                 for (int i = 0; i < NUMRAYS; i++)
 179                 {
 180                         gBatchRaycaster->addRay (source[i], dest[i]);
 181                 }
 182                 gBatchRaycaster->performBatchRaycast ();
 183                 for (int i = 0; i < gBatchRaycaster->getNumRays (); i++)
 184                 {
 185                                 const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
 186                                 hit[i].setInterpolate3(source[i],dest[i],out.hitFraction);
 187                                 normal[i] = out.hitNormal;
 188                                 normal[i].normalize ();
 189                 }
 190 #else
 191                 for (int i = 0; i < NUMRAYS; i++)
 192                 {
 193                         btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
 194
 195                         cw->rayTest (source[i], dest[i], cb);
 196                         if (cb.hasHit ())
 197                         {
 198                                 hit[i] = cb.m_hitPointWorld;
 199                                 normal[i] = cb.m_hitNormalWorld;
 200                                 normal[i].normalize ();
 201                         } else {
 202                                 hit[i] = dest[i];
 203                                 normal[i] = btVector3(1.0, 0.0, 0.0);
 204                         }
 205
 206                 }
 207 #ifdef USE_BT_CLOCK
 208                 ms += frame_timer.getTimeMilliseconds ();
 209 #endif //USE_BT_CLOCK
 210                 frame_counter++;
 211                 if (frame_counter > 50)
 212                 {
 213                         min_ms = ms < min_ms ? ms : min_ms;
 214                         max_ms = ms > max_ms ? ms : max_ms;
 215                         sum_ms += ms;
 216                         sum_ms_samples++;
 217                         btScalar mean_ms = (btScalar)sum_ms/(btScalar)sum_ms_samples;
 218                         printf("%d rays in %d ms %d %d %f\n", NUMRAYS * frame_counter, ms, min_ms, max_ms, mean_ms);
 219                         ms = 0;
 220                         frame_counter = 0;
 221                 }
 222 #endif
 223         }
 224
 225         void draw ()
 226         {
 227 #ifdef USE_GRAPHICAL_BENCHMARK
 228                 glDisable (GL_LIGHTING);
 229                 glColor3f (0.0, 1.0, 0.0);
 230                 glBegin (GL_LINES);
 231                 int i;
 232
 233                 for (i = 0; i < NUMRAYS; i++)
 234                 {
 235                         glVertex3f (source[i][0], source[i][1], source[i][2]);
 236                         glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
 237                 }
 238                 glEnd ();
 239                 glColor3f (1.0, 1.0, 1.0);
 240                 glBegin (GL_LINES);
 241                 for (i = 0; i < NUMRAYS; i++)
 242                 {
 243                         glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
 244                         glVertex3f (hit[i][0] + normal[i][0], hit[i][1] + normal[i][1], hit[i][2] + normal[i][2]);
 245                 }
 246                 glEnd ();
 247                 glColor3f (0.0, 1.0, 1.0);
 248                 glBegin (GL_POINTS);
 249                 for ( i = 0; i < NUMRAYS; i++)
 250                 {
 251                         glVertex3f (hit[i][0], hit[i][1], hit[i][2]);
 252                 }
 253                 glEnd ();
 254                 glEnable (GL_LIGHTING);
 255 #endif //USE_GRAPHICAL_BENCHMARK
 256
 257         }
 258 };
 259
 260
 261 static btRaycastBar2 raycastBar;
 262
 263
 264 void BenchmarkDemo::clientMoveAndDisplay()
 265 {
 266 #ifdef USE_GRAPHICAL_BENCHMARK
 267         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 268 #endif //USE_GRAPHICAL_BENCHMARK
 269
 270         //simple dynamics world doesn't handle fixed-time-stepping
 271         //float ms = getDeltaTimeMicroseconds();
 272
 273         ///step the simulation
 274         if (m_dynamicsWorld)
 275         {
 276                 m_dynamicsWorld->stepSimulation(btScalar(1./60.));
 277                 //optional but useful: debug drawing
 278                 m_dynamicsWorld->debugDrawWorld();
 279         }
 280
 281         if (m_benchmark==7)
 282         {
 283                 castRays();
 284
 285                 raycastBar.draw();
 286
 287         }
 288
 289         renderme();
 290
 291 #ifdef USE_GRAPHICAL_BENCHMARK
 292         glFlush();
 293
 294         swapBuffers();
 295 #endif //USE_GRAPHICAL_BENCHMARK
 296
 297 }
 298
 299
 300
 301 void BenchmarkDemo::displayCallback(void)
 302 {
 303
 304 #ifdef USE_GRAPHICAL_BENCHMARK
 305         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 306
 307         renderme();
 308
 309         //optional but useful: debug drawing to detect problems
 310         if (m_dynamicsWorld)
 311                 m_dynamicsWorld->debugDrawWorld();
 312
 313         glFlush();
 314         swapBuffers();
 315 #endif //USE_GRAPHICAL_BENCHMARK
 316 }
 317
 318
 319
 320
 321 void    BenchmarkDemo::initPhysics()
 322 {
 323
 324         setCameraDistance(btScalar(100.));
 325
 326         ///collision configuration contains default setup for memory, collision setup
 327         btDefaultCollisionConstructionInfo cci;
 328         cci.m_defaultMaxPersistentManifoldPoolSize = 32768;
 329         m_collisionConfiguration = new btDefaultCollisionConfiguration(cci);
 330
 331         ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
 332         m_dispatcher = new      btCollisionDispatcher(m_collisionConfiguration);
 333
 334         m_dispatcher->setDispatcherFlags(btCollisionDispatcher::CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
 335
 336 #if USE_PARALLEL_DISPATCHER_BENCHMARK
 337
 338         int maxNumOutstandingTasks = 4;
 339 #ifdef _WIN32
 340         Win32ThreadSupport* threadSupportCollision = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(    "collision",processCollisionTask,       createCollisionLocalStoreMemory,maxNumOutstandingTasks));
 341 #elif defined (USE_PTHREADS)
 342         PosixThreadSupport::ThreadConstructionInfo collisionConstructionInfo( "collision",processCollisionTask,       createCollisionLocalStoreMemory,maxNumOutstandingTasks);
 343         PosixThreadSupport* threadSupportCollision = new PosixThreadSupport(collisionConstructionInfo);
 344 #endif
 345         //SequentialThreadSupport::SequentialThreadConstructionInfo sci("spuCD",        processCollisionTask,   createCollisionLocalStoreMemory);
 346         //SequentialThreadSupport* seq = new SequentialThreadSupport(sci);
 347         m_dispatcher = new      SpuGatheringCollisionDispatcher(threadSupportCollision,1,m_collisionConfiguration);
 348 #endif
 349
 350
 351         ///the maximum size of the collision world. Make sure objects stay within these boundaries
 352         ///Don't make the world AABB size too large, it will harm simulation quality and performance
 353         btVector3 worldAabbMin(-1000,-1000,-1000);
 354         btVector3 worldAabbMax(1000,1000,1000);
 355
 356         btHashedOverlappingPairCache* pairCache = new btHashedOverlappingPairCache();
 357         m_overlappingPairCache = new btAxisSweep3(worldAabbMin,worldAabbMax,3500,pairCache);
 358 //      m_overlappingPairCache = new btSimpleBroadphase();
 359 //      m_overlappingPairCache = new btDbvtBroadphase();
 360
 361
 362         ///the default constraint solver. For parallel processing you can use a different solver (see Extras/BulletMultiThreaded)
 363 #ifdef USE_PARALLEL_DISPATCHER_BENCHMARK
 364
 365         btThreadSupportInterface* thread = createSolverThreadSupport(4);
 366         btConstraintSolver* sol = new btParallelConstraintSolver(thread);
 367 #else
 368         btSequentialImpulseConstraintSolver* sol = new btSequentialImpulseConstraintSolver;
 369 #endif //USE_PARALLEL_DISPATCHER_BENCHMARK
 370
 371
 372         m_solver = sol;
 373
 374         btDiscreteDynamicsWorld* dynamicsWorld;
 375         m_dynamicsWorld = dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_overlappingPairCache,m_solver,m_collisionConfiguration);
 376
 377 #ifdef USE_PARALLEL_DISPATCHER_BENCHMARK
 378         dynamicsWorld->getSimulationIslandManager()->setSplitIslands(false);
 379 #endif //USE_PARALLEL_DISPATCHER_BENCHMARK
 380
 381         ///the following 3 lines increase the performance dramatically, with a little bit of loss of quality
 382         m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_ENABLE_FRICTION_DIRECTION_CACHING; //don't recalculate friction values each frame
 383         dynamicsWorld->getSolverInfo().m_numIterations = 5; //few solver iterations
 384         m_defaultContactProcessingThreshold = 0.f;//used when creating bodies: body->setContactProcessingThreshold(...);
 385
 386
 387         m_dynamicsWorld->setGravity(btVector3(0,-10,0));
 388
 389         if (m_benchmark<5)
 390         {
 391                 ///create a few basic rigid bodies
 392                 btCollisionShape* groundShape = new btBoxShape(btVector3(btScalar(250.),btScalar(50.),btScalar(250.)));
 393         //      btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),0);
 394
 395                 m_collisionShapes.push_back(groundShape);
 396
 397                 btTransform groundTransform;
 398                 groundTransform.setIdentity();
 399                 groundTransform.setOrigin(btVector3(0,-50,0));
 400
 401                 //We can also use DemoApplication::localCreateRigidBody, but for clarity it is provided here:
 402                 {
 403                         btScalar mass(0.);
 404
 405                         //rigidbody is dynamic if and only if mass is non zero, otherwise static
 406                         bool isDynamic = (mass != 0.f);
 407
 408                         btVector3 localInertia(0,0,0);
 409                         if (isDynamic)
 410                                 groundShape->calculateLocalInertia(mass,localInertia);
 411
 412                         //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
 413                         btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
 414                         btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,groundShape,localInertia);
 415                         btRigidBody* body = new btRigidBody(rbInfo);
 416
 417                         //add the body to the dynamics world
 418                         m_dynamicsWorld->addRigidBody(body);
 419                 }
 420         }
 421
 422         switch (m_benchmark)
 423         {
 424                 case 1:
 425                         {
 426                                 createTest1();
 427                                 break;
 428                         }
 429                 case 2:
 430                         {
 431                                 createTest2();
 432                                 break;
 433                         }
 434                 case 3:
 435                         {
 436                                 createTest3();
 437                                 break;
 438                         }
 439                 case 4:
 440                         {
 441                                 createTest4();
 442                                 break;
 443                         }
 444                 case 5:
 445                         {
 446                                 createTest5();
 447                                 break;
 448                         }
 449                 case 6:
 450                 {
 451                         createTest6();
 452                         break;
 453                 }
 454                 case 7:
 455                 {
 456                         createTest7();
 457                         break;
 458                 }
 459
 460
 461         default:
 462                 {
 463                 }
 464         }
 465
 466
 467         clientResetScene();
 468 }
 469
 470
 471 void    BenchmarkDemo::createTest1()
 472 {
 473         // 3000
 474         int size = 8;
 475         const float cubeSize = 1.0f;
 476         float spacing = cubeSize;
 477         btVector3 pos(0.0f, cubeSize * 2,0.f);
 478         float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
 479
 480         btBoxShape* blockShape = new btBoxShape(btVector3(cubeSize-COLLISION_RADIUS,cubeSize-COLLISION_RADIUS,cubeSize-COLLISION_RADIUS));
 481         btVector3 localInertia(0,0,0);
 482         float mass = 2.f;
 483         blockShape->calculateLocalInertia(mass,localInertia);
 484
 485         btTransform trans;
 486         trans.setIdentity();
 487
 488         for(int k=0;k<47;k++) {
 489                 for(int j=0;j<size;j++) {
 490                         pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
 491                         for(int i=0;i<size;i++) {
 492                                 pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
 493
 494                                 trans.setOrigin(pos);
 495                                 btRigidBody* cmbody;
 496                                 cmbody= localCreateRigidBody(mass,trans,blockShape);
 497                         }
 498                 }
 499                 offset -= 0.05f * spacing * (size-1);
 500 //              spacing *= 1.01f;
 501                 pos[1] += (cubeSize * 2.0f + spacing);
 502         }
 503 }
 504
 505
 506 ///////////////////////////////////////////////////////////////////////////////
 507 // Pyramid 3
 508
 509 void BenchmarkDemo::createWall(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize)
 510 {
 511
 512         btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS));
 513
 514         float mass = 1.f;
 515         btVector3 localInertia(0,0,0);
 516         blockShape->calculateLocalInertia(mass,localInertia);
 517
 518 //      btScalar  diffX = boxSize[0] * 1.0f;
 519         btScalar  diffY = boxSize[1] * 1.0f;
 520         btScalar  diffZ = boxSize[2] * 1.0f;
 521
 522         btScalar  offset = -stackSize * (diffZ * 2.0f) * 0.5f;
 523         btVector3 pos(0.0f, diffY, 0.0f);
 524
 525         btTransform trans;
 526         trans.setIdentity();
 527
 528         while(stackSize) {
 529                 for(int i=0;i<stackSize;i++) {
 530                         pos[2] = offset + (float)i * (diffZ * 2.0f);
 531
 532                 trans.setOrigin(offsetPosition + pos);
 533                 localCreateRigidBody(mass,trans,blockShape);
 534
 535                 }
 536                 offset += diffZ;
 537                 pos[1] += (diffY * 2.0f);
 538                 stackSize--;
 539         }
 540 }
 541
 542 void BenchmarkDemo::createPyramid(const btVector3& offsetPosition,int stackSize,const btVector3& boxSize)
 543 {
 544         btScalar space = 0.0001f;
 545
 546         btVector3 pos(0.0f, boxSize[1], 0.0f);
 547
 548         btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS));
 549         btTransform trans;
 550         trans.setIdentity();
 551
 552         btScalar mass = 1.f;
 553         btVector3 localInertia(0,0,0);
 554         blockShape->calculateLocalInertia(mass,localInertia);
 555
 556
 557         btScalar diffX = boxSize[0]*1.02f;
 558         btScalar diffY = boxSize[1]*1.02f;
 559         btScalar diffZ = boxSize[2]*1.02f;
 560
 561         btScalar offsetX = -stackSize * (diffX * 2.0f + space) * 0.5f;
 562         btScalar offsetZ = -stackSize * (diffZ * 2.0f + space) * 0.5f;
 563         while(stackSize) {
 564                 for(int j=0;j<stackSize;j++) {
 565                         pos[2] = offsetZ + (float)j * (diffZ * 2.0f + space);
 566                         for(int i=0;i<stackSize;i++) {
 567                                 pos[0] = offsetX + (float)i * (diffX * 2.0f + space);
 568                                 trans.setOrigin(offsetPosition + pos);
 569                                 this->localCreateRigidBody(mass,trans,blockShape);
 570
 571
 572                         }
 573                 }
 574                 offsetX += diffX;
 575                 offsetZ += diffZ;
 576                 pos[1] += (diffY * 2.0f + space);
 577                 stackSize--;
 578         }
 579
 580 }
 581
 582  const btVector3 rotate( const btQuaternion& quat, const btVector3 & vec )
 583 {
 584     float tmpX, tmpY, tmpZ, tmpW;
 585     tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
 586     tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
 587     tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
 588     tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
 589     return btVector3(
 590         ( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
 591         ( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
 592         ( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
 593     );
 594 }
 595
 596 void BenchmarkDemo::createTowerCircle(const btVector3& offsetPosition,int stackSize,int rotSize,const btVector3& boxSize)
 597 {
 598
 599         btBoxShape* blockShape = new btBoxShape(btVector3(boxSize[0]-COLLISION_RADIUS,boxSize[1]-COLLISION_RADIUS,boxSize[2]-COLLISION_RADIUS));
 600
 601         btTransform trans;
 602         trans.setIdentity();
 603
 604         float mass = 1.f;
 605         btVector3 localInertia(0,0,0);
 606         blockShape->calculateLocalInertia(mass,localInertia);
 607
 608
 609         float radius = 1.3f * rotSize * boxSize[0] / SIMD_PI;
 610
 611         // create active boxes
 612         btQuaternion rotY(0,1,0,0);
 613         float posY = boxSize[1];
 614
 615         for(int i=0;i<stackSize;i++) {
 616                 for(int j=0;j<rotSize;j++) {
 617
 618
 619                         trans.setOrigin(offsetPosition+  rotate(rotY,btVector3(0.0f , posY, radius)));
 620                         trans.setRotation(rotY);
 621                         localCreateRigidBody(mass,trans,blockShape);
 622
 623                         rotY *= btQuaternion(btVector3(0,1,0),SIMD_PI/(rotSize*btScalar(0.5)));
 624                 }
 625
 626                 posY += boxSize[1] * 2.0f;
 627                 rotY *= btQuaternion(btVector3(0,1,0),SIMD_PI/(float)rotSize);
 628         }
 629
 630 }
 631
 632 void    BenchmarkDemo::createTest2()
 633 {
 634         setCameraDistance(btScalar(50.));
 635         const float cubeSize = 1.0f;
 636
 637         createPyramid(btVector3(-20.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
 638         createWall(btVector3(-2.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
 639         createWall(btVector3(4.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
 640         createWall(btVector3(10.0f,0.0f,0.0f),12,btVector3(cubeSize,cubeSize,cubeSize));
 641         createTowerCircle(btVector3(25.0f,0.0f,0.0f),8,24,btVector3(cubeSize,cubeSize,cubeSize));
 642
 643 }
 644
 645
 646
 647
 648 // Enrico: Shouldn't these three variables be real constants and not defines?
 649
 650 #ifndef M_PI
 651 #define M_PI       btScalar(3.14159265358979323846)
 652 #endif
 653
 654 #ifndef M_PI_2
 655 #define M_PI_2     btScalar(1.57079632679489661923)
 656 #endif
 657
 658 #ifndef M_PI_4
 659 #define M_PI_4     btScalar(0.785398163397448309616)
 660 #endif
 661
 662 class RagDoll
 663 {
 664         enum
 665         {
 666                 BODYPART_PELVIS = 0,
 667                 BODYPART_SPINE,
 668                 BODYPART_HEAD,
 669
 670                 BODYPART_LEFT_UPPER_LEG,
 671                 BODYPART_LEFT_LOWER_LEG,
 672
 673                 BODYPART_RIGHT_UPPER_LEG,
 674                 BODYPART_RIGHT_LOWER_LEG,
 675
 676                 BODYPART_LEFT_UPPER_ARM,
 677                 BODYPART_LEFT_LOWER_ARM,
 678
 679                 BODYPART_RIGHT_UPPER_ARM,
 680                 BODYPART_RIGHT_LOWER_ARM,
 681
 682                 BODYPART_COUNT
 683         };
 684
 685         enum
 686         {
 687                 JOINT_PELVIS_SPINE = 0,
 688                 JOINT_SPINE_HEAD,
 689
 690                 JOINT_LEFT_HIP,
 691                 JOINT_LEFT_KNEE,
 692
 693                 JOINT_RIGHT_HIP,
 694                 JOINT_RIGHT_KNEE,
 695
 696                 JOINT_LEFT_SHOULDER,
 697                 JOINT_LEFT_ELBOW,
 698
 699                 JOINT_RIGHT_SHOULDER,
 700                 JOINT_RIGHT_ELBOW,
 701
 702                 JOINT_COUNT
 703         };
 704
 705         btDynamicsWorld* m_ownerWorld;
 706         btCollisionShape* m_shapes[BODYPART_COUNT];
 707         btRigidBody* m_bodies[BODYPART_COUNT];
 708         btTypedConstraint* m_joints[JOINT_COUNT];
 709
 710         btRigidBody* localCreateRigidBody (btScalar mass, const btTransform& startTransform, btCollisionShape* shape)
 711         {
 712                 bool isDynamic = (mass != 0.f);
 713
 714                 btVector3 localInertia(0,0,0);
 715                 if (isDynamic)
 716                         shape->calculateLocalInertia(mass,localInertia);
 717
 718                 btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
 719
 720                 btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
 721                 btRigidBody* body = new btRigidBody(rbInfo);
 722
 723                 m_ownerWorld->addRigidBody(body);
 724
 725                 return body;
 726         }
 727
 728 public:
 729         RagDoll (btDynamicsWorld* ownerWorld, const btVector3& positionOffset,btScalar scale)
 730                 : m_ownerWorld (ownerWorld)
 731         {
 732                 // Setup the geometry
 733                 m_shapes[BODYPART_PELVIS] = new btCapsuleShape(btScalar(0.15)*scale, btScalar(0.20)*scale);
 734                 m_shapes[BODYPART_SPINE] = new btCapsuleShape(btScalar(0.15)*scale, btScalar(0.28)*scale);
 735                 m_shapes[BODYPART_HEAD] = new btCapsuleShape(btScalar(0.10)*scale, btScalar(0.05)*scale);
 736                 m_shapes[BODYPART_LEFT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07)*scale, btScalar(0.45)*scale);
 737                 m_shapes[BODYPART_LEFT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.37)*scale);
 738                 m_shapes[BODYPART_RIGHT_UPPER_LEG] = new btCapsuleShape(btScalar(0.07)*scale, btScalar(0.45)*scale);
 739                 m_shapes[BODYPART_RIGHT_LOWER_LEG] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.37)*scale);
 740                 m_shapes[BODYPART_LEFT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.33)*scale);
 741                 m_shapes[BODYPART_LEFT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04)*scale, btScalar(0.25)*scale);
 742                 m_shapes[BODYPART_RIGHT_UPPER_ARM] = new btCapsuleShape(btScalar(0.05)*scale, btScalar(0.33)*scale);
 743                 m_shapes[BODYPART_RIGHT_LOWER_ARM] = new btCapsuleShape(btScalar(0.04)*scale, btScalar(0.25)*scale);
 744
 745                 // Setup all the rigid bodies
 746                 btTransform offset; offset.setIdentity();
 747                 offset.setOrigin(positionOffset);
 748
 749                 btTransform transform;
 750                 transform.setIdentity();
 751                 transform.setOrigin(scale*btVector3(btScalar(0.), btScalar(1.), btScalar(0.)));
 752                 m_bodies[BODYPART_PELVIS] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_PELVIS]);
 753
 754                 transform.setIdentity();
 755                 transform.setOrigin(scale*btVector3(btScalar(0.), btScalar(1.2), btScalar(0.)));
 756                 m_bodies[BODYPART_SPINE] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_SPINE]);
 757
 758                 transform.setIdentity();
 759                 transform.setOrigin(scale*btVector3(btScalar(0.), btScalar(1.6), btScalar(0.)));
 760                 m_bodies[BODYPART_HEAD] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_HEAD]);
 761
 762                 transform.setIdentity();
 763                 transform.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(0.65), btScalar(0.)));
 764                 m_bodies[BODYPART_LEFT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_LEG]);
 765
 766                 transform.setIdentity();
 767                 transform.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(0.2), btScalar(0.)));
 768                 m_bodies[BODYPART_LEFT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_LEG]);
 769
 770                 transform.setIdentity();
 771                 transform.setOrigin(scale*btVector3(btScalar(0.18), btScalar(0.65), btScalar(0.)));
 772                 m_bodies[BODYPART_RIGHT_UPPER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_LEG]);
 773
 774                 transform.setIdentity();
 775                 transform.setOrigin(scale*btVector3(btScalar(0.18), btScalar(0.2), btScalar(0.)));
 776                 m_bodies[BODYPART_RIGHT_LOWER_LEG] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_LEG]);
 777
 778                 transform.setIdentity();
 779                 transform.setOrigin(scale*btVector3(btScalar(-0.35), btScalar(1.45), btScalar(0.)));
 780                 transform.getBasis().setEulerZYX(0,0,M_PI_2);
 781                 m_bodies[BODYPART_LEFT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_UPPER_ARM]);
 782
 783                 transform.setIdentity();
 784                 transform.setOrigin(scale*btVector3(btScalar(-0.7), btScalar(1.45), btScalar(0.)));
 785                 transform.getBasis().setEulerZYX(0,0,M_PI_2);
 786                 m_bodies[BODYPART_LEFT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_LEFT_LOWER_ARM]);
 787
 788                 transform.setIdentity();
 789                 transform.setOrigin(scale*btVector3(btScalar(0.35), btScalar(1.45), btScalar(0.)));
 790                 transform.getBasis().setEulerZYX(0,0,-M_PI_2);
 791                 m_bodies[BODYPART_RIGHT_UPPER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_UPPER_ARM]);
 792
 793                 transform.setIdentity();
 794                 transform.setOrigin(scale*btVector3(btScalar(0.7), btScalar(1.45), btScalar(0.)));
 795                 transform.getBasis().setEulerZYX(0,0,-M_PI_2);
 796                 m_bodies[BODYPART_RIGHT_LOWER_ARM] = localCreateRigidBody(btScalar(1.), offset*transform, m_shapes[BODYPART_RIGHT_LOWER_ARM]);
 797
 798                 // Setup some damping on the m_bodies
 799                 for (int i = 0; i < BODYPART_COUNT; ++i)
 800                 {
 801                         m_bodies[i]->setDamping(btScalar(0.05), btScalar(0.85));
 802                         m_bodies[i]->setDeactivationTime(btScalar(0.8));
 803                         m_bodies[i]->setSleepingThresholds(btScalar(1.6), btScalar(2.5));
 804                 }
 805
 806                 // Now setup the constraints
 807                 btHingeConstraint* hingeC;
 808                 btConeTwistConstraint* coneC;
 809
 810                 btTransform localA, localB;
 811
 812                 localA.setIdentity(); localB.setIdentity();
 813                 localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.15), btScalar(0.)));
 814                 localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.15), btScalar(0.)));
 815                 hingeC =  new btHingeConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_SPINE], localA, localB);
 816                 hingeC->setLimit(btScalar(-M_PI_4), btScalar(M_PI_2));
 817                 m_joints[JOINT_PELVIS_SPINE] = hingeC;
 818                 m_ownerWorld->addConstraint(m_joints[JOINT_PELVIS_SPINE], true);
 819
 820
 821                 localA.setIdentity(); localB.setIdentity();
 822                 localA.getBasis().setEulerZYX(0,0,M_PI_2); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.30), btScalar(0.)));
 823                 localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
 824                 coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_HEAD], localA, localB);
 825                 coneC->setLimit(M_PI_4, M_PI_4, M_PI_2);
 826                 m_joints[JOINT_SPINE_HEAD] = coneC;
 827                 m_ownerWorld->addConstraint(m_joints[JOINT_SPINE_HEAD], true);
 828
 829
 830                 localA.setIdentity(); localB.setIdentity();
 831                 localA.getBasis().setEulerZYX(0,0,-M_PI_4*5); localA.setOrigin(scale*btVector3(btScalar(-0.18), btScalar(-0.10), btScalar(0.)));
 832                 localB.getBasis().setEulerZYX(0,0,-M_PI_4*5); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
 833                 coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_LEFT_UPPER_LEG], localA, localB);
 834                 coneC->setLimit(M_PI_4, M_PI_4, 0);
 835                 m_joints[JOINT_LEFT_HIP] = coneC;
 836                 m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_HIP], true);
 837
 838                 localA.setIdentity(); localB.setIdentity();
 839                 localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
 840                 localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
 841                 hingeC =  new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_LEG], *m_bodies[BODYPART_LEFT_LOWER_LEG], localA, localB);
 842                 hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
 843                 m_joints[JOINT_LEFT_KNEE] = hingeC;
 844                 m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_KNEE], true);
 845
 846
 847                 localA.setIdentity(); localB.setIdentity();
 848                 localA.getBasis().setEulerZYX(0,0,M_PI_4); localA.setOrigin(scale*btVector3(btScalar(0.18), btScalar(-0.10), btScalar(0.)));
 849                 localB.getBasis().setEulerZYX(0,0,M_PI_4); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.225), btScalar(0.)));
 850                 coneC = new btConeTwistConstraint(*m_bodies[BODYPART_PELVIS], *m_bodies[BODYPART_RIGHT_UPPER_LEG], localA, localB);
 851                 coneC->setLimit(M_PI_4, M_PI_4, 0);
 852                 m_joints[JOINT_RIGHT_HIP] = coneC;
 853                 m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_HIP], true);
 854
 855                 localA.setIdentity(); localB.setIdentity();
 856                 localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.225), btScalar(0.)));
 857                 localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.185), btScalar(0.)));
 858                 hingeC =  new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_LEG], *m_bodies[BODYPART_RIGHT_LOWER_LEG], localA, localB);
 859                 hingeC->setLimit(btScalar(0), btScalar(M_PI_2));
 860                 m_joints[JOINT_RIGHT_KNEE] = hingeC;
 861                 m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_KNEE], true);
 862
 863
 864                 localA.setIdentity(); localB.setIdentity();
 865                 localA.getBasis().setEulerZYX(0,0,M_PI); localA.setOrigin(scale*btVector3(btScalar(-0.2), btScalar(0.15), btScalar(0.)));
 866                 localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
 867                 coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_LEFT_UPPER_ARM], localA, localB);
 868                 coneC->setLimit(M_PI_2, M_PI_2, 0);
 869                 m_joints[JOINT_LEFT_SHOULDER] = coneC;
 870                 m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_SHOULDER], true);
 871
 872                 localA.setIdentity(); localB.setIdentity();
 873                 localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
 874                 localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
 875                 hingeC =  new btHingeConstraint(*m_bodies[BODYPART_LEFT_UPPER_ARM], *m_bodies[BODYPART_LEFT_LOWER_ARM], localA, localB);
 876                 hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
 877                 m_joints[JOINT_LEFT_ELBOW] = hingeC;
 878                 m_ownerWorld->addConstraint(m_joints[JOINT_LEFT_ELBOW], true);
 879
 880
 881
 882                 localA.setIdentity(); localB.setIdentity();
 883                 localA.getBasis().setEulerZYX(0,0,0); localA.setOrigin(scale*btVector3(btScalar(0.2), btScalar(0.15), btScalar(0.)));
 884                 localB.getBasis().setEulerZYX(0,0,M_PI_2); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.18), btScalar(0.)));
 885                 coneC = new btConeTwistConstraint(*m_bodies[BODYPART_SPINE], *m_bodies[BODYPART_RIGHT_UPPER_ARM], localA, localB);
 886                 coneC->setLimit(M_PI_2, M_PI_2, 0);
 887                 m_joints[JOINT_RIGHT_SHOULDER] = coneC;
 888                 m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_SHOULDER], true);
 889
 890                 localA.setIdentity(); localB.setIdentity();
 891                 localA.getBasis().setEulerZYX(0,M_PI_2,0); localA.setOrigin(scale*btVector3(btScalar(0.), btScalar(0.18), btScalar(0.)));
 892                 localB.getBasis().setEulerZYX(0,M_PI_2,0); localB.setOrigin(scale*btVector3(btScalar(0.), btScalar(-0.14), btScalar(0.)));
 893                 hingeC =  new btHingeConstraint(*m_bodies[BODYPART_RIGHT_UPPER_ARM], *m_bodies[BODYPART_RIGHT_LOWER_ARM], localA, localB);
 894                 hingeC->setLimit(btScalar(-M_PI_2), btScalar(0));
 895                 m_joints[JOINT_RIGHT_ELBOW] = hingeC;
 896                 m_ownerWorld->addConstraint(m_joints[JOINT_RIGHT_ELBOW], true);
 897         }
 898
 899         virtual ~RagDoll ()
 900         {
 901                 int i;
 902
 903                 // Remove all constraints
 904                 for ( i = 0; i < JOINT_COUNT; ++i)
 905                 {
 906                         m_ownerWorld->removeConstraint(m_joints[i]);
 907                         delete m_joints[i]; m_joints[i] = 0;
 908                 }
 909
 910                 // Remove all bodies and shapes
 911                 for ( i = 0; i < BODYPART_COUNT; ++i)
 912                 {
 913                         m_ownerWorld->removeRigidBody(m_bodies[i]);
 914
 915                         delete m_bodies[i]->getMotionState();
 916
 917                         delete m_bodies[i]; m_bodies[i] = 0;
 918                         delete m_shapes[i]; m_shapes[i] = 0;
 919                 }
 920         }
 921 };
 922
 923 void    BenchmarkDemo::createTest3()
 924 {
 925         setCameraDistance(btScalar(50.));
 926
 927         int size = 16;
 928
 929         float sizeX = 1.f;
 930         float sizeY = 1.f;
 931
 932         //int rc=0;
 933
 934         btScalar scale(3.5);
 935         btVector3 pos(0.0f, sizeY, 0.0f);
 936         while(size) {
 937                 float offset = -size * (sizeX * 6.0f) * 0.5f;
 938                 for(int i=0;i<size;i++) {
 939                         pos[0] = offset + (float)i * (sizeX * 6.0f);
 940
 941                                 RagDoll* ragDoll = new RagDoll (m_dynamicsWorld,pos,scale);
 942                                 m_ragdolls.push_back(ragDoll);
 943                 }
 944
 945                 offset += sizeX;
 946                 pos[1] += (sizeY * 7.0f);
 947                 pos[2] -= sizeX * 2.0f;
 948                 size--;
 949         }
 950
 951 }
 952 void    BenchmarkDemo::createTest4()
 953 {
 954         setCameraDistance(btScalar(50.));
 955
 956         int size = 8;
 957         const float cubeSize = 1.5f;
 958         float spacing = cubeSize;
 959         btVector3 pos(0.0f, cubeSize * 2, 0.0f);
 960         float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
 961
 962         btConvexHullShape* convexHullShape = new btConvexHullShape();
 963
 964         btScalar scaling(1);
 965
 966         convexHullShape->setLocalScaling(btVector3(scaling,scaling,scaling));
 967
 968         for (int i=0;i<TaruVtxCount;i++)
 969         {
 970                 btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
 971                 convexHullShape->addPoint(vtx*btScalar(1./scaling));
 972         }
 973
 974         //this will enable polyhedral contact clipping, better quality, slightly slower
 975         //convexHullShape->initializePolyhedralFeatures();
 976
 977         btTransform trans;
 978         trans.setIdentity();
 979
 980         float mass = 1.f;
 981         btVector3 localInertia(0,0,0);
 982         convexHullShape->calculateLocalInertia(mass,localInertia);
 983
 984         for(int k=0;k<15;k++) {
 985                 for(int j=0;j<size;j++) {
 986                         pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
 987                         for(int i=0;i<size;i++) {
 988                                 pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
 989                                 trans.setOrigin(pos);
 990                                 localCreateRigidBody(mass,trans,convexHullShape);
 991                         }
 992                 }
 993                 offset -= 0.05f * spacing * (size-1);
 994                 spacing *= 1.01f;
 995                 pos[1] += (cubeSize * 2.0f + spacing);
 996         }
 997 }
 998
 999
1000 ///////////////////////////////////////////////////////////////////////////////
1001 // LargeMesh
1002
1003 int LandscapeVtxCount[] = {
1004         Landscape01VtxCount,
1005         Landscape02VtxCount,
1006         Landscape03VtxCount,
1007         Landscape04VtxCount,
1008         Landscape05VtxCount,
1009         Landscape06VtxCount,
1010         Landscape07VtxCount,
1011         Landscape08VtxCount,
1012 };
1013
1014 int LandscapeIdxCount[] = {
1015         Landscape01IdxCount,
1016         Landscape02IdxCount,
1017         Landscape03IdxCount,
1018         Landscape04IdxCount,
1019         Landscape05IdxCount,
1020         Landscape06IdxCount,
1021         Landscape07IdxCount,
1022         Landscape08IdxCount,
1023 };
1024
1025 btScalar *LandscapeVtx[] = {
1026         Landscape01Vtx,
1027         Landscape02Vtx,
1028         Landscape03Vtx,
1029         Landscape04Vtx,
1030         Landscape05Vtx,
1031         Landscape06Vtx,
1032         Landscape07Vtx,
1033         Landscape08Vtx,
1034 };
1035
1036 btScalar *LandscapeNml[] = {
1037         Landscape01Nml,
1038         Landscape02Nml,
1039         Landscape03Nml,
1040         Landscape04Nml,
1041         Landscape05Nml,
1042         Landscape06Nml,
1043         Landscape07Nml,
1044         Landscape08Nml,
1045 };
1046
1047 btScalar* LandscapeTex[] = {
1048         Landscape01Tex,
1049         Landscape02Tex,
1050         Landscape03Tex,
1051         Landscape04Tex,
1052         Landscape05Tex,
1053         Landscape06Tex,
1054         Landscape07Tex,
1055         Landscape08Tex,
1056 };
1057
1058 unsigned short  *LandscapeIdx[] = {
1059         Landscape01Idx,
1060         Landscape02Idx,
1061         Landscape03Idx,
1062         Landscape04Idx,
1063         Landscape05Idx,
1064         Landscape06Idx,
1065         Landscape07Idx,
1066         Landscape08Idx,
1067 };
1068
1069 void BenchmarkDemo::createLargeMeshBody()
1070 {
1071         btTransform trans;
1072         trans.setIdentity();
1073
1074         for(int i=0;i<8;i++) {
1075
1076                 btTriangleIndexVertexArray* meshInterface = new btTriangleIndexVertexArray();
1077                 btIndexedMesh part;
1078
1079                 part.m_vertexBase = (const unsigned char*)LandscapeVtx[i];
1080                 part.m_vertexStride = sizeof(btScalar) * 3;
1081                 part.m_numVertices = LandscapeVtxCount[i];
1082                 part.m_triangleIndexBase = (const unsigned char*)LandscapeIdx[i];
1083                 part.m_triangleIndexStride = sizeof( short) * 3;
1084                 part.m_numTriangles = LandscapeIdxCount[i]/3;
1085                 part.m_indexType = PHY_SHORT;
1086
1087                 meshInterface->addIndexedMesh(part,PHY_SHORT);
1088
1089                 bool    useQuantizedAabbCompression = true;
1090                 btBvhTriangleMeshShape* trimeshShape = new btBvhTriangleMeshShape(meshInterface,useQuantizedAabbCompression);
1091                 btVector3 localInertia(0,0,0);
1092                 trans.setOrigin(btVector3(0,-25,0));
1093
1094                 btRigidBody* body = localCreateRigidBody(0,trans,trimeshShape);
1095                 body->setFriction (btScalar(0.9));
1096
1097         }
1098
1099 }
1100
1101
1102 void    BenchmarkDemo::createTest5()
1103 {
1104         setCameraDistance(btScalar(250.));
1105         btVector3 boxSize(1.5f,1.5f,1.5f);
1106         float boxMass = 1.0f;
1107         float sphereRadius = 1.5f;
1108         float sphereMass = 1.0f;
1109         float capsuleHalf = 2.0f;
1110         float capsuleRadius = 1.0f;
1111         float capsuleMass = 1.0f;
1112
1113         {
1114                 int size = 10;
1115                 int height = 10;
1116
1117                 const float cubeSize = boxSize[0];
1118                 float spacing = 2.0f;
1119                 btVector3 pos(0.0f, 20.0f, 0.0f);
1120                 float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
1121
1122                 int numBodies = 0;
1123
1124                 for(int k=0;k<height;k++) {
1125                         for(int j=0;j<size;j++) {
1126                                 pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
1127                                 for(int i=0;i<size;i++) {
1128                                         pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
1129                                         btVector3 bpos = btVector3(0,25,0) + btVector3(5.0f,1.0f,5.0f)*pos;
1130                                         int idx = rand() % 9;
1131                                         btTransform trans;
1132                                         trans.setIdentity();
1133                                         trans.setOrigin(bpos);
1134
1135                                         switch(idx) {
1136                                                 case 0:case 1:case 2:
1137                                                 {
1138                                                         float r = 0.5f * (idx+1);
1139                                                         btBoxShape* boxShape = new btBoxShape(boxSize*r);
1140                                                         localCreateRigidBody(boxMass*r,trans,boxShape);
1141                                                 }
1142                                                 break;
1143
1144                                                 case 3:case 4:case 5:
1145                                                 {
1146                                                         float r = 0.5f * (idx-3+1);
1147                                                         btSphereShape* sphereShape = new btSphereShape(sphereRadius*r);
1148                                                         localCreateRigidBody(sphereMass*r,trans,sphereShape);
1149                                                 }
1150                                                 break;
1151
1152                                                 case 6:case 7:case 8:
1153                                                 {
1154                                                         float r = 0.5f * (idx-6+1);
1155                                                         btCapsuleShape* capsuleShape = new btCapsuleShape(capsuleRadius*r,capsuleHalf*r);
1156                                                         localCreateRigidBody(capsuleMass*r,trans,capsuleShape);
1157                                                 }
1158                                                 break;
1159                                         }
1160
1161                                         numBodies++;
1162                                 }
1163                         }
1164                         offset -= 0.05f * spacing * (size-1);
1165                         spacing *= 1.1f;
1166                         pos[1] += (cubeSize * 2.0f + spacing);
1167                 }
1168         }
1169
1170         createLargeMeshBody();
1171 }
1172 void    BenchmarkDemo::createTest6()
1173 {
1174         setCameraDistance(btScalar(250.));
1175
1176         btVector3 boxSize(1.5f,1.5f,1.5f);
1177
1178         btConvexHullShape* convexHullShape = new btConvexHullShape();
1179
1180         for (int i=0;i<TaruVtxCount;i++)
1181         {
1182                 btVector3 vtx(TaruVtx[i*3],TaruVtx[i*3+1],TaruVtx[i*3+2]);
1183                 convexHullShape->addPoint(vtx);
1184         }
1185
1186         btTransform trans;
1187         trans.setIdentity();
1188
1189         float mass = 1.f;
1190         btVector3 localInertia(0,0,0);
1191         convexHullShape->calculateLocalInertia(mass,localInertia);
1192
1193
1194         {
1195                 int size = 10;
1196                 int height = 10;
1197
1198                 const float cubeSize = boxSize[0];
1199                 float spacing = 2.0f;
1200                 btVector3 pos(0.0f, 20.0f, 0.0f);
1201                 float offset = -size * (cubeSize * 2.0f + spacing) * 0.5f;
1202
1203
1204                 for(int k=0;k<height;k++) {
1205                         for(int j=0;j<size;j++) {
1206                                 pos[2] = offset + (float)j * (cubeSize * 2.0f + spacing);
1207                                 for(int i=0;i<size;i++) {
1208                                         pos[0] = offset + (float)i * (cubeSize * 2.0f + spacing);
1209                                         btVector3 bpos = btVector3(0,25,0) + btVector3(5.0f,1.0f,5.0f)*pos;
1210                                         trans.setOrigin(bpos);
1211
1212                                         localCreateRigidBody(mass,trans,convexHullShape);
1213                                 }
1214                         }
1215                         offset -= 0.05f * spacing * (size-1);
1216                         spacing *= 1.1f;
1217                         pos[1] += (cubeSize * 2.0f + spacing);
1218                 }
1219         }
1220
1221
1222         createLargeMeshBody();
1223 }
1224
1225
1226
1227
1228 void BenchmarkDemo::initRays()
1229 {
1230         raycastBar = btRaycastBar2 (2500.0, 0,50.0);
1231 }
1232
1233 void BenchmarkDemo::castRays()
1234 {
1235         raycastBar.cast (m_dynamicsWorld);
1236 }
1237
1238 void    BenchmarkDemo::createTest7()
1239 {
1240
1241         createTest6();
1242         setCameraDistance(btScalar(150.));
1243         initRays();
1244 }
1245
1246 void    BenchmarkDemo::exitPhysics()
1247 {
1248         int i;
1249
1250         for (i=0;i<m_ragdolls.size();i++)
1251         {
1252                 RagDoll* doll = m_ragdolls[i];
1253                 delete doll;
1254         }
1255     m_ragdolls.clear();
1256
1257         //cleanup in the reverse order of creation/initialization
1258     if (m_dynamicsWorld)
1259     {
1260         //remove the rigidbodies from the dynamics world and delete them
1261         for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
1262         {
1263             btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
1264             btRigidBody* body = btRigidBody::upcast(obj);
1265             if (body && body->getMotionState())
1266             {
1267                 delete body->getMotionState();
1268             }
1269             m_dynamicsWorld->removeCollisionObject( obj );
1270             delete obj;
1271         }
1272     }
1273
1274         //delete collision shapes
1275         for (int j=0;j<m_collisionShapes.size();j++)
1276         {
1277                 btCollisionShape* shape = m_collisionShapes[j];
1278                 delete shape;
1279         }
1280     m_collisionShapes.clear();
1281
1282         //delete dynamics world
1283         delete m_dynamicsWorld;
1284     m_dynamicsWorld=0;
1285
1286         //delete solver
1287         delete m_solver;
1288     m_solver=0;
1289
1290         //delete broadphase
1291         delete m_overlappingPairCache;
1292     m_overlappingPairCache=0;
1293
1294         //delete dispatcher
1295         delete m_dispatcher;
1296     m_dispatcher=0;
1297
1298         delete m_collisionConfiguration;
1299     m_collisionConfiguration=0;
1300
1301
1302 }
1303
1304
1305
1306 #ifndef USE_GRAPHICAL_BENCHMARK
1307
1308 btRigidBody*    DemoApplication::localCreateRigidBody(float mass, const btTransform& startTransform,btCollisionShape* shape)
1309 {
1310         btAssert((!shape || shape->getShapeType() != INVALID_SHAPE_PROXYTYPE));
1311
1312         //rigidbody is dynamic if and only if mass is non zero, otherwise static
1313         bool isDynamic = (mass != 0.f);
1314
1315         btVector3 localInertia(0,0,0);
1316         if (isDynamic)
1317                 shape->calculateLocalInertia(mass,localInertia);
1318
1319         //using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
1320
1321         btRigidBody* body = new btRigidBody(mass,0,shape,localInertia);
1322         body->setWorldTransform(startTransform);
1323         body->setContactProcessingThreshold(m_defaultContactProcessingThreshold);
1324         m_dynamicsWorld->addRigidBody(body);
1325
1326         return body;
1327 }
1328 #endif //USE_GRAPHICAL_BENCHMARK
1329