← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/Demos/Benchmarks/BenchmarkDemo.cpp

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/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