← Back to branch summary

~ubuntu-branches/ubuntu/maverick/blender/maverick

« back to all changes in this revision

Viewing changes to extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Khashayar Naderehvandi, Khashayar Naderehvandi, Alessio Treglia
  • Date: 2009-01-22 16:53:59 UTC
  • mfrom: (14.1.1 experimental)
  • Revision ID: james.westby@ubuntu.com-20090122165359-v0996tn7fbit64ni
Tags: 2.48a+dfsg-1ubuntu1
https://launchpad.net/bugs/320045
[ Khashayar Naderehvandi ]
* Merge from debian experimental (LP: #320045), Ubuntu remaining changes:
  - Add patch correcting header file locations.
  - Add libvorbis-dev and libgsm1-dev to Build-Depends.
  - Use avcodec_decode_audio2() in source/blender/src/hddaudio.c

[ Alessio Treglia ]
* Add missing previous changelog entries.

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp
21
21
#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
22
22
#include "BulletCollision/CollisionShapes/btCollisionShape.h"
23
23
#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
24
 
#include <LinearMath/btTransformUtil.h>
 
24
#include "LinearMath/btTransformUtil.h"
 
25
#include "LinearMath/btQuickprof.h"
25
26
 
26
27
//rigidbody & constraints
27
28
#include "BulletDynamics/Dynamics/btRigidBody.h"
41
42
#include "BulletCollision/CollisionShapes/btSphereShape.h"
42
43
#include "BulletCollision/CollisionShapes/btTriangleCallback.h"
43
44
#include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
 
45
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
44
46
#include "LinearMath/btIDebugDraw.h"
45
47
 
46
48
 
57
59
 
58
60
 
59
61
 
60
 
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
61
 
:btDynamicsWorld(dispatcher,pairCache),
62
 
m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
63
 
m_debugDrawer(0),
 
62
btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
 
63
:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
 
64
m_constraintSolver(constraintSolver),
64
65
m_gravity(0,-10,0),
65
66
m_localTime(btScalar(1.)/btScalar(60.)),
66
67
m_profileTimings(0)
67
68
{
68
 
        m_islandManager = new btSimulationIslandManager();
 
69
        if (!m_constraintSolver)
 
70
        {
 
71
                void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
 
72
                m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
 
73
                m_ownsConstraintSolver = true;
 
74
        } else
 
75
        {
 
76
                m_ownsConstraintSolver = false;
 
77
        }
 
78
 
 
79
        {
 
80
                void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
 
81
                m_islandManager = new (mem) btSimulationIslandManager();
 
82
        }
 
83
 
69
84
        m_ownsIslandManager = true;
70
 
        m_ownsConstraintSolver = (constraintSolver==0);
71
85
}
72
86
 
73
87
 
75
89
{
76
90
        //only delete it when we created it
77
91
        if (m_ownsIslandManager)
78
 
                delete m_islandManager;
 
92
        {
 
93
                m_islandManager->~btSimulationIslandManager();
 
94
                btAlignedFree( m_islandManager);
 
95
        }
79
96
        if (m_ownsConstraintSolver)
80
 
                 delete m_constraintSolver;
 
97
        {
 
98
 
 
99
                m_constraintSolver->~btConstraintSolver();
 
100
                btAlignedFree(m_constraintSolver);
 
101
        }
81
102
}
82
103
 
83
104
void    btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
102
123
        }
103
124
}
104
125
 
105
 
void    btDiscreteDynamicsWorld::synchronizeMotionStates()
 
126
void    btDiscreteDynamicsWorld::debugDrawWorld()
106
127
{
107
 
        //debug vehicle wheels
108
 
        
109
 
        
110
 
        {
 
128
 
 
129
        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
 
130
        {
 
131
                int numManifolds = getDispatcher()->getNumManifolds();
 
132
                btVector3 color(0,0,0);
 
133
                for (int i=0;i<numManifolds;i++)
 
134
                {
 
135
                        btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
 
136
                        //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
 
137
                        //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
 
138
 
 
139
                        int numContacts = contactManifold->getNumContacts();
 
140
                        for (int j=0;j<numContacts;j++)
 
141
                        {
 
142
                                btManifoldPoint& cp = contactManifold->getContactPoint(j);
 
143
                                getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
 
144
                        }
 
145
                }
 
146
        }
 
147
 
 
148
 
 
149
        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
 
150
        {
 
151
                int i;
 
152
 
111
153
                //todo: iterate over awake simulation islands!
112
 
                for ( int i=0;i<m_collisionObjects.size();i++)
 
154
                for (  i=0;i<m_collisionObjects.size();i++)
113
155
                {
114
156
                        btCollisionObject* colObj = m_collisionObjects[i];
115
157
                        if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
135
177
 
136
178
                                debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
137
179
                        }
 
180
                        if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
 
181
                        {
 
182
                                btPoint3 minAabb,maxAabb;
 
183
                                btVector3 colorvec(1,0,0);
 
184
                                colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
 
185
                                m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
 
186
                        }
 
187
 
 
188
                }
 
189
        
 
190
                for (  i=0;i<this->m_vehicles.size();i++)
 
191
                {
 
192
                        for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
 
193
                        {
 
194
                                btVector3 wheelColor(0,255,255);
 
195
                                if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
 
196
                                {
 
197
                                        wheelColor.setValue(0,0,255);
 
198
                                } else
 
199
                                {
 
200
                                        wheelColor.setValue(255,0,255);
 
201
                                }
 
202
                
 
203
                                btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
 
204
 
 
205
                                btVector3 axle = btVector3(     
 
206
                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
 
207
                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
 
208
                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
 
209
 
 
210
 
 
211
                                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
 
212
                                //debug wheels (cylinders)
 
213
                                m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
 
214
                                m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
 
215
 
 
216
                        }
 
217
                }
 
218
        }
 
219
}
 
220
 
 
221
void    btDiscreteDynamicsWorld::clearForces()
 
222
{
 
223
        //todo: iterate over awake simulation islands!
 
224
        for ( int i=0;i<m_collisionObjects.size();i++)
 
225
        {
 
226
                btCollisionObject* colObj = m_collisionObjects[i];
 
227
                
 
228
                btRigidBody* body = btRigidBody::upcast(colObj);
 
229
                if (body)
 
230
                {
 
231
                        body->clearForces();
 
232
                }
 
233
        }
 
234
}       
 
235
 
 
236
///apply gravity, call this once per timestep
 
237
void    btDiscreteDynamicsWorld::applyGravity()
 
238
{
 
239
        //todo: iterate over awake simulation islands!
 
240
        for ( int i=0;i<m_collisionObjects.size();i++)
 
241
        {
 
242
                btCollisionObject* colObj = m_collisionObjects[i];
 
243
                
 
244
                btRigidBody* body = btRigidBody::upcast(colObj);
 
245
                if (body && body->isActive())
 
246
                {
 
247
                        body->applyGravity();
 
248
                }
 
249
        }
 
250
}
 
251
 
 
252
 
 
253
 
 
254
void    btDiscreteDynamicsWorld::synchronizeMotionStates()
 
255
{
 
256
        {
 
257
                //todo: iterate over awake simulation islands!
 
258
                for ( int i=0;i<m_collisionObjects.size();i++)
 
259
                {
 
260
                        btCollisionObject* colObj = m_collisionObjects[i];
 
261
                        
138
262
                        btRigidBody* body = btRigidBody::upcast(colObj);
139
263
                        if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
140
264
                        {
145
269
                                {
146
270
                                        btTransform interpolatedTransform;
147
271
                                        btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
148
 
                                                body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
 
272
                                                body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
149
273
                                        body->getMotionState()->setWorldTransform(interpolatedTransform);
150
274
                                }
151
275
                        }
158
282
                {
159
283
                        for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
160
284
                        {
161
 
                                btVector3 wheelColor(0,255,255);
162
 
                                if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
163
 
                                {
164
 
                                        wheelColor.setValue(0,0,255);
165
 
                                } else
166
 
                                {
167
 
                                        wheelColor.setValue(255,0,255);
168
 
                                }
169
 
 
170
285
                                //synchronize the wheels with the (interpolated) chassis worldtransform
171
286
                                m_vehicles[i]->updateWheelTransform(v,true);
172
 
                                        
173
 
                                btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
174
 
 
175
 
                                btVector3 axle = btVector3(     
176
 
                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
177
 
                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
178
 
                                        m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
179
 
 
180
 
 
181
 
                                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
182
 
                                //debug wheels (cylinders)
183
 
                                m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
184
 
                                m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
185
 
 
186
287
                        }
187
288
                }
188
289
        }
192
293
 
193
294
int     btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
194
295
{
 
296
        startProfiling(timeStep);
 
297
 
 
298
        BT_PROFILE("stepSimulation");
 
299
 
195
300
        int numSimulationSubSteps = 0;
196
301
 
197
302
        if (maxSubSteps)
229
334
 
230
335
                saveKinematicState(fixedTimeStep);
231
336
 
 
337
                applyGravity();
 
338
 
232
339
                //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
233
340
                int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
234
341
 
242
349
 
243
350
        synchronizeMotionStates();
244
351
 
 
352
        clearForces();
 
353
 
 
354
#ifndef BT_NO_PROFILE
 
355
        CProfileManager::Increment_Frame_Counter();
 
356
#endif //BT_NO_PROFILE
 
357
        
245
358
        return numSimulationSubSteps;
246
359
}
247
360
 
248
361
void    btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
249
362
{
250
363
        
251
 
        startProfiling(timeStep);
252
 
 
253
 
        ///update aabbs information
254
 
        updateAabbs();
 
364
        BT_PROFILE("internalSingleStepSimulation");
255
365
 
256
366
        ///apply gravity, predict motion
257
367
        predictUnconstraintMotion(timeStep);
286
396
 
287
397
        updateActivationState( timeStep );
288
398
 
289
 
        
290
 
 
 
399
        if(0 != m_internalTickCallback) {
 
400
                (*m_internalTickCallback)(this, timeStep);
 
401
        }       
291
402
}
292
403
 
293
404
void    btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
304
415
        }
305
416
}
306
417
 
 
418
btVector3 btDiscreteDynamicsWorld::getGravity () const
 
419
{
 
420
        return m_gravity;
 
421
}
 
422
 
307
423
 
308
424
void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
309
425
{
 
426
        //remove all constraints too
 
427
        while (body->getNumConstraintRefs())
 
428
        {
 
429
                btTypedConstraint* constraint = body->getConstraintRef(0);
 
430
                removeConstraint(constraint);
 
431
        }
 
432
 
310
433
        removeCollisionObject(body);
311
434
}
312
435
 
343
466
 
344
467
void    btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
345
468
{
346
 
        BEGIN_PROFILE("updateVehicles");
347
 
 
 
469
        BT_PROFILE("updateVehicles");
 
470
        
348
471
        for ( int i=0;i<m_vehicles.size();i++)
349
472
        {
350
473
                btRaycastVehicle* vehicle = m_vehicles[i];
351
474
                vehicle->updateVehicle( timeStep);
352
475
        }
353
 
        END_PROFILE("updateVehicles");
354
476
}
355
477
 
356
478
void    btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
357
479
{
358
 
        BEGIN_PROFILE("updateActivationState");
 
480
        BT_PROFILE("updateActivationState");
359
481
 
360
482
        for ( int i=0;i<m_collisionObjects.size();i++)
361
483
        {
374
496
                                {
375
497
                                        if (body->getActivationState() == ACTIVE_TAG)
376
498
                                                body->setActivationState( WANTS_DEACTIVATION );
 
499
                                        if (body->getActivationState() == ISLAND_SLEEPING) 
 
500
                                        {
 
501
                                                body->setAngularVelocity(btVector3(0,0,0));
 
502
                                                body->setLinearVelocity(btVector3(0,0,0));
 
503
                                        }
 
504
 
377
505
                                }
378
506
                        } else
379
507
                        {
382
510
                        }
383
511
                }
384
512
        }
385
 
        END_PROFILE("updateActivationState");
386
513
}
387
514
 
388
515
void    btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
412
539
        m_vehicles.remove(vehicle);
413
540
}
414
541
 
415
 
inline  int     btGetConstraintIslandId(const btTypedConstraint* lhs)
 
542
SIMD_FORCE_INLINE       int     btGetConstraintIslandId(const btTypedConstraint* lhs)
416
543
{
417
544
        int islandId;
418
545
        
442
569
 
443
570
void    btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
444
571
{
 
572
        BT_PROFILE("solveConstraints");
445
573
        
446
574
        struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
447
575
        {
452
580
                int                                             m_numConstraints;
453
581
                btIDebugDraw*                   m_debugDrawer;
454
582
                btStackAlloc*                   m_stackAlloc;
455
 
 
 
583
                btDispatcher*                   m_dispatcher;
456
584
 
457
585
                InplaceSolverIslandCallback(
458
586
                        btContactSolverInfo& solverInfo,
460
588
                        btTypedConstraint** sortedConstraints,
461
589
                        int     numConstraints,
462
590
                        btIDebugDraw*   debugDrawer,
463
 
                        btStackAlloc*                   stackAlloc)
 
591
                        btStackAlloc*                   stackAlloc,
 
592
                        btDispatcher* dispatcher)
464
593
                        :m_solverInfo(solverInfo),
465
594
                        m_solver(solver),
466
595
                        m_sortedConstraints(sortedConstraints),
467
596
                        m_numConstraints(numConstraints),
468
597
                        m_debugDrawer(debugDrawer),
469
 
                        m_stackAlloc(stackAlloc)
 
598
                        m_stackAlloc(stackAlloc),
 
599
                        m_dispatcher(dispatcher)
470
600
                {
471
601
 
472
602
                }
479
609
                }
480
610
                virtual void    ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**   manifolds,int numManifolds, int islandId)
481
611
                {
482
 
                        //also add all non-contact constraints/joints for this island
483
 
                        btTypedConstraint** startConstraint = 0;
484
 
                        int numCurConstraints = 0;
485
 
                        int i;
486
 
                        
487
 
                        //find the first constraint for this island
488
 
                        for (i=0;i<m_numConstraints;i++)
489
 
                        {
490
 
                                if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
491
 
                                {
492
 
                                        startConstraint = &m_sortedConstraints[i];
493
 
                                        break;
494
 
                                }
495
 
                        }
496
 
                        //count the number of constraints in this island
497
 
                        for (;i<m_numConstraints;i++)
498
 
                        {
499
 
                                if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
500
 
                                {
501
 
                                        numCurConstraints++;
502
 
                                }
503
 
                        }
 
612
                        if (islandId<0)
 
613
                        {
 
614
                                ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 
615
                                m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
 
616
                        } else
 
617
                        {
 
618
                                        //also add all non-contact constraints/joints for this island
 
619
                                btTypedConstraint** startConstraint = 0;
 
620
                                int numCurConstraints = 0;
 
621
                                int i;
 
622
                                
 
623
                                //find the first constraint for this island
 
624
                                for (i=0;i<m_numConstraints;i++)
 
625
                                {
 
626
                                        if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
 
627
                                        {
 
628
                                                startConstraint = &m_sortedConstraints[i];
 
629
                                                break;
 
630
                                        }
 
631
                                }
 
632
                                //count the number of constraints in this island
 
633
                                for (;i<m_numConstraints;i++)
 
634
                                {
 
635
                                        if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
 
636
                                        {
 
637
                                                numCurConstraints++;
 
638
                                        }
 
639
                                }
504
640
 
505
 
                        m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
 
641
                                ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
 
642
                                if (numManifolds + numCurConstraints)
 
643
                                {
 
644
                                        m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
 
645
                                }
 
646
                
 
647
                        }
506
648
                }
507
649
 
508
650
        };
520
662
                
521
663
        
522
664
 
523
 
        sortedConstraints.heapSort(btSortConstraintOnIslandPredicate());
 
665
        sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
524
666
        
525
667
        btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
526
668
        
527
 
        InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver, constraintsPtr,sortedConstraints.size(),    m_debugDrawer,m_stackAlloc);
528
 
 
 
669
        InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver, constraintsPtr,sortedConstraints.size(),    m_debugDrawer,m_stackAlloc,m_dispatcher1);
529
670
        
 
671
        m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
530
672
        
531
673
        /// solve all the constraints for this island
532
674
        m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
533
675
 
534
 
 
 
676
        m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
535
677
}
536
678
 
537
679
 
539
681
 
540
682
void    btDiscreteDynamicsWorld::calculateSimulationIslands()
541
683
{
542
 
        BEGIN_PROFILE("calculateSimulationIslands");
 
684
        BT_PROFILE("calculateSimulationIslands");
543
685
 
544
686
        getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
545
687
 
553
695
                        const btRigidBody* colObj0 = &constraint->getRigidBodyA();
554
696
                        const btRigidBody* colObj1 = &constraint->getRigidBodyB();
555
697
 
556
 
                        if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
557
 
                                ((colObj1) && ((colObj1)->mergesSimulationIslands())))
 
698
                        if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
 
699
                                ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
558
700
                        {
559
701
                                if (colObj0->isActive() || colObj1->isActive())
560
702
                                {
569
711
        //Store the island id in each body
570
712
        getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
571
713
 
572
 
        END_PROFILE("calculateSimulationIslands");
573
 
 
 
714
        
574
715
}
575
716
 
576
717
 
577
 
void    btDiscreteDynamicsWorld::updateAabbs()
 
718
#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
 
719
 
 
720
class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
578
721
{
579
 
        BEGIN_PROFILE("updateAabbs");
580
 
        
581
 
        btVector3 colorvec(1,0,0);
582
 
        btTransform predictedTrans;
583
 
        for ( int i=0;i<m_collisionObjects.size();i++)
584
 
        {
585
 
                btCollisionObject* colObj = m_collisionObjects[i];
586
 
                
587
 
                btRigidBody* body = btRigidBody::upcast(colObj);
588
 
                if (body)
 
722
        btCollisionObject* m_me;
 
723
        btScalar m_allowedPenetration;
 
724
        btOverlappingPairCache* m_pairCache;
 
725
 
 
726
 
 
727
public:
 
728
        btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache) : 
 
729
          btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
 
730
                m_allowedPenetration(0.0f),
 
731
                m_me(me),
 
732
                m_pairCache(pairCache)
 
733
        {
 
734
        }
 
735
 
 
736
        virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
 
737
        {
 
738
                if (convexResult.m_hitCollisionObject == m_me)
 
739
                        return 1.0;
 
740
 
 
741
                btVector3 linVelA,linVelB;
 
742
                linVelA = m_convexToWorld-m_convexFromWorld;
 
743
                linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
 
744
 
 
745
                btVector3 relativeVelocity = (linVelA-linVelB);
 
746
                //don't report time of impact for motion away from the contact normal (or causes minor penetration)
 
747
                if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
 
748
                        return 1.f;
 
749
 
 
750
                return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
 
751
        }
 
752
 
 
753
        virtual bool needsCollision(btBroadphaseProxy* proxy0) const
 
754
        {
 
755
                //don't collide with itself
 
756
                if (proxy0->m_clientObject == m_me)
 
757
                        return false;
 
758
 
 
759
                ///don't do CCD when the collision filters are not matching
 
760
                if (!btCollisionWorld::ClosestConvexResultCallback::needsCollision(proxy0))
 
761
                        return false;
 
762
 
 
763
                ///don't do CCD when there are already contact points (touching contact/penetration)
 
764
                btAlignedObjectArray<btPersistentManifold*> manifoldArray;
 
765
                btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
 
766
                if (collisionPair)
589
767
                {
590
 
                //      if (body->IsActive() && (!body->IsStatic()))
 
768
                        if (collisionPair->m_algorithm)
591
769
                        {
592
 
                                btPoint3 minAabb,maxAabb;
593
 
                                colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
594
 
                                btBroadphaseInterface* bp = (btBroadphaseInterface*)m_broadphasePairCache;
595
 
 
596
 
                                //moving objects should be moderately sized, probably something wrong if not
597
 
                                if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < btScalar(1e12)))
598
 
                                {
599
 
                                        bp->setAabb(body->getBroadphaseHandle(),minAabb,maxAabb);
600
 
                                } else
601
 
                                {
602
 
                                        //something went wrong, investigate
603
 
                                        //this assert is unwanted in 3D modelers (danger of loosing work)
604
 
                                        body->setActivationState(DISABLE_SIMULATION);
605
 
                                        
606
 
                                        static bool reportMe = true;
607
 
                                        if (reportMe && m_debugDrawer)
608
 
                                        {
609
 
                                                reportMe = false;
610
 
                                                m_debugDrawer->reportErrorWarning("Overflow in AABB, object removed from simulation");
611
 
                                                m_debugDrawer->reportErrorWarning("If you can reproduce this, please email bugs@continuousphysics.com\n");
612
 
                                                m_debugDrawer->reportErrorWarning("Please include above information, your Platform, version of OS.\n");
613
 
                                                m_debugDrawer->reportErrorWarning("Thanks.\n");
614
 
                                        }
615
 
 
616
 
 
617
 
                                }
618
 
                                if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
619
 
                                {
620
 
                                        m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
 
770
                                manifoldArray.resize(0);
 
771
                                collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
 
772
                                for (int j=0;j<manifoldArray.size();j++)
 
773
                                {
 
774
                                        btPersistentManifold* manifold = manifoldArray[j];
 
775
                                        if (manifold->getNumContacts()>0)
 
776
                                                return false;
621
777
                                }
622
778
                        }
623
779
                }
 
780
                return true;
624
781
        }
625
 
        
626
 
        END_PROFILE("updateAabbs");
627
 
}
628
 
 
 
782
 
 
783
 
 
784
};
 
785
 
 
786
///internal debugging variable. this value shouldn't be too high
 
787
int gNumClampedCcdMotions=0;
 
788
 
 
789
//#include "stdio.h"
629
790
void    btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
630
791
{
631
 
        BEGIN_PROFILE("integrateTransforms");
 
792
        BT_PROFILE("integrateTransforms");
632
793
        btTransform predictedTrans;
633
794
        for ( int i=0;i<m_collisionObjects.size();i++)
634
795
        {
636
797
                btRigidBody* body = btRigidBody::upcast(colObj);
637
798
                if (body)
638
799
                {
 
800
                        body->setHitFraction(1.f);
 
801
 
639
802
                        if (body->isActive() && (!body->isStaticOrKinematicObject()))
640
803
                        {
641
804
                                body->predictIntegratedTransform(timeStep, predictedTrans);
 
805
                                btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
 
806
 
 
807
                                if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
 
808
                                {
 
809
                                        BT_PROFILE("CCD motion clamping");
 
810
                                        if (body->getCollisionShape()->isConvex())
 
811
                                        {
 
812
                                                gNumClampedCcdMotions++;
 
813
                                                
 
814
                                                btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache());
 
815
                                                btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
 
816
                                                btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
 
817
                                                convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
 
818
                                                if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
 
819
                                                {
 
820
                                                        body->setHitFraction(sweepResults.m_closestHitFraction);
 
821
                                                        body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
 
822
                                                        body->setHitFraction(0.f);
 
823
//                                                      printf("clamped integration to hit fraction = %f\n",fraction);
 
824
                                                }
 
825
                                        }
 
826
                                }
 
827
                                
642
828
                                body->proceedToTransform( predictedTrans);
643
829
                        }
644
830
                }
645
831
        }
646
 
        END_PROFILE("integrateTransforms");
647
832
}
648
833
 
649
834
 
650
835
 
651
836
void    btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
652
837
{
653
 
        BEGIN_PROFILE("predictUnconstraintMotion");
 
838
        BT_PROFILE("predictUnconstraintMotion");
654
839
        for ( int i=0;i<m_collisionObjects.size();i++)
655
840
        {
656
841
                btCollisionObject* colObj = m_collisionObjects[i];
661
846
                        {
662
847
                                if (body->isActive())
663
848
                                {
664
 
                                        body->applyForces( timeStep);
665
849
                                        body->integrateVelocities( timeStep);
 
850
                                        //damping
 
851
                                        body->applyDamping(timeStep);
 
852
 
666
853
                                        body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
667
854
                                }
668
855
                        }
669
856
                }
670
857
        }
671
 
        END_PROFILE("predictUnconstraintMotion");
672
858
}
673
859
 
674
860
 
675
861
void    btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
676
862
{
677
863
        (void)timeStep;
678
 
        #ifdef USE_QUICKPROF
679
 
 
680
 
 
681
 
        //toggle btProfiler
682
 
        if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
683
 
        {
684
 
                if (!m_profileTimings)
685
 
                {
686
 
                        m_profileTimings = 1;
687
 
                        // To disable profiling, simply comment out the following line.
688
 
                        static int counter = 0;
689
 
 
690
 
                        char filename[128];
691
 
                        sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
692
 
                        btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
693
 
                } else
694
 
                {
695
 
                        btProfiler::endProfilingCycle();
696
 
                }
697
 
 
698
 
        } else
699
 
        {
700
 
                if (m_profileTimings)
701
 
                {
702
 
                        btProfiler::endProfilingCycle();
703
 
 
704
 
                        m_profileTimings = 0;
705
 
                        btProfiler::destroy();
706
 
                }
707
 
        }
708
 
#endif //USE_QUICKPROF
 
864
 
 
865
#ifndef BT_NO_PROFILE
 
866
        CProfileManager::Reset();
 
867
#endif //BT_NO_PROFILE
 
868
 
709
869
}
710
870
 
711
871
 
827
987
 
828
988
                                btScalar radius = capsuleShape->getRadius();
829
989
                                btScalar halfHeight = capsuleShape->getHalfHeight();
 
990
                                
 
991
                                int upAxis = capsuleShape->getUpAxis();
 
992
 
 
993
                                
 
994
                                btVector3 capStart(0.f,0.f,0.f);
 
995
                                capStart[upAxis] = -halfHeight;
 
996
 
 
997
                                btVector3 capEnd(0.f,0.f,0.f);
 
998
                                capEnd[upAxis] = halfHeight;
830
999
 
831
1000
                                // Draw the ends
832
1001
                                {
 
1002
                                        
833
1003
                                        btTransform childTransform = worldTransform;
834
 
                                        childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
 
1004
                                        childTransform.getOrigin() = worldTransform * capStart;
835
1005
                                        debugDrawSphere(radius, childTransform, color);
836
1006
                                }
837
1007
 
838
1008
                                {
839
1009
                                        btTransform childTransform = worldTransform;
840
 
                                        childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
 
1010
                                        childTransform.getOrigin() = worldTransform * capEnd;
841
1011
                                        debugDrawSphere(radius, childTransform, color);
842
1012
                                }
843
1013
 
844
1014
                                // Draw some additional lines
845
1015
                                btVector3 start = worldTransform.getOrigin();
846
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(-radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(-radius,-halfHeight,0), color);
847
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(radius,halfHeight,0),start+worldTransform.getBasis() * btVector3(radius,-halfHeight,0), color);
848
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,-radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,-radius), color);
849
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0,halfHeight,radius),start+worldTransform.getBasis() * btVector3(0,-halfHeight,radius), color);
850
 
 
 
1016
 
 
1017
                                
 
1018
                                capStart[(upAxis+1)%3] = radius;
 
1019
                                capEnd[(upAxis+1)%3] = radius;
 
1020
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
 
1021
                                capStart[(upAxis+1)%3] = -radius;
 
1022
                                capEnd[(upAxis+1)%3] = -radius;
 
1023
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
 
1024
 
 
1025
                                capStart[(upAxis+1)%3] = 0.f;
 
1026
                                capEnd[(upAxis+1)%3] = 0.f;
 
1027
 
 
1028
                                capStart[(upAxis+2)%3] = radius;
 
1029
                                capEnd[(upAxis+2)%3] = radius;
 
1030
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
 
1031
                                capStart[(upAxis+2)%3] = -radius;
 
1032
                                capEnd[(upAxis+2)%3] = -radius;
 
1033
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
 
1034
 
 
1035
                                
851
1036
                                break;
852
1037
                        }
853
1038
                case CONE_SHAPE_PROXYTYPE:
856
1041
                                btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
857
1042
                                btScalar height = coneShape->getHeight();//+coneShape->getMargin();
858
1043
                                btVector3 start = worldTransform.getOrigin();
859
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(radius,btScalar(0.),btScalar(-0.5)*height),color);
860
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(-radius,btScalar(0.),btScalar(-0.5)*height),color);
861
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),radius,btScalar(-0.5)*height),color);
862
 
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(btScalar(0.),btScalar(0.),btScalar(0.5)*height),start+worldTransform.getBasis() * btVector3(btScalar(0.),-radius,btScalar(-0.5)*height),color);
 
1044
 
 
1045
                                int upAxis= coneShape->getConeUpIndex();
 
1046
                                
 
1047
 
 
1048
                                btVector3       offsetHeight(0,0,0);
 
1049
                                offsetHeight[upAxis] = height * btScalar(0.5);
 
1050
                                btVector3       offsetRadius(0,0,0);
 
1051
                                offsetRadius[(upAxis+1)%3] = radius;
 
1052
                                btVector3       offset2Radius(0,0,0);
 
1053
                                offset2Radius[(upAxis+2)%3] = radius;
 
1054
 
 
1055
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
 
1056
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
 
1057
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
 
1058
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
 
1059
 
 
1060
 
 
1061
 
863
1062
                                break;
864
1063
 
865
1064
                        }
868
1067
                                const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
869
1068
                                int upAxis = cylinder->getUpAxis();
870
1069
                                btScalar radius = cylinder->getRadius();
871
 
                                btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
 
1070
                                btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
872
1071
                                btVector3 start = worldTransform.getOrigin();
873
1072
                                btVector3       offsetHeight(0,0,0);
874
1073
                                offsetHeight[upAxis] = halfHeight;
878
1077
                                getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
879
1078
                                break;
880
1079
                        }
 
1080
 
 
1081
                        case STATIC_PLANE_PROXYTYPE:
 
1082
                                {
 
1083
                                        const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
 
1084
                                        btScalar planeConst = staticPlaneShape->getPlaneConstant();
 
1085
                                        const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
 
1086
                                        btVector3 planeOrigin = planeNormal * planeConst;
 
1087
                                        btVector3 vec0,vec1;
 
1088
                                        btPlaneSpace1(planeNormal,vec0,vec1);
 
1089
                                        btScalar vecLen = 100.f;
 
1090
                                        btVector3 pt0 = planeOrigin + vec0*vecLen;
 
1091
                                        btVector3 pt1 = planeOrigin - vec0*vecLen;
 
1092
                                        btVector3 pt2 = planeOrigin + vec1*vecLen;
 
1093
                                        btVector3 pt3 = planeOrigin - vec1*vecLen;
 
1094
                                        getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
 
1095
                                        getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
 
1096
                                        break;
 
1097
 
 
1098
                                }
881
1099
                default:
882
1100
                        {
883
1101
 
902
1120
                                        btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
903
1121
                                        //DebugDrawcallback drawCallback;
904
1122
                                        DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
905
 
                                        convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
 
1123
                                        convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
906
1124
                                }
907
1125
 
908
1126
 
934
1152
{
935
1153
        if (m_ownsConstraintSolver)
936
1154
        {
937
 
                delete m_constraintSolver;
 
1155
                btAlignedFree( m_constraintSolver);
938
1156
        }
939
1157
        m_ownsConstraintSolver = false;
940
1158
        m_constraintSolver = solver;
941
1159
}
942
1160
 
 
1161
btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
 
1162
{
 
1163
        return m_constraintSolver;
 
1164
}
 
1165
 
 
1166
 
943
1167
int             btDiscreteDynamicsWorld::getNumConstraints() const
944
1168
{
945
1169
        return int(m_constraints.size());