BGE Bullet fix: physics debug representation of cone shape does not take Up axis...
[blender.git] / extern / bullet2 / src / BulletDynamics / Dynamics / btDiscreteDynamicsWorld.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 #include "btDiscreteDynamicsWorld.h"
18
19 //collision detection
20 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
21 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
22 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
23 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
24 #include <LinearMath/btTransformUtil.h>
25
26 //rigidbody & constraints
27 #include "BulletDynamics/Dynamics/btRigidBody.h"
28 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
29 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
30 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
31
32 //for debug rendering
33 #include "BulletCollision/CollisionShapes/btBoxShape.h"
34 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
35 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
36 #include "BulletCollision/CollisionShapes/btConeShape.h"
37 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
38 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
39 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
40 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
41 #include "BulletCollision/CollisionShapes/btSphereShape.h"
42 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
43 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
44 #include "LinearMath/btIDebugDraw.h"
45
46
47
48 //vehicle
49 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
50 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
51 #include "BulletDynamics/Vehicle/btWheelInfo.h"
52 #include "LinearMath/btIDebugDraw.h"
53 #include "LinearMath/btQuickprof.h"
54 #include "LinearMath/btMotionState.h"
55
56
57
58
59
60 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
61 :btDynamicsWorld(dispatcher,pairCache),
62 m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
63 m_debugDrawer(0),
64 m_gravity(0,-10,0),
65 m_localTime(btScalar(1.)/btScalar(60.)),
66 m_profileTimings(0)
67 {
68         m_islandManager = new btSimulationIslandManager();
69         m_ownsIslandManager = true;
70         m_ownsConstraintSolver = (constraintSolver==0);
71 }
72
73
74 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
75 {
76         //only delete it when we created it
77         if (m_ownsIslandManager)
78                 delete m_islandManager;
79         if (m_ownsConstraintSolver)
80                  delete m_constraintSolver;
81 }
82
83 void    btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
84 {
85
86         for (int i=0;i<m_collisionObjects.size();i++)
87         {
88                 btCollisionObject* colObj = m_collisionObjects[i];
89                 btRigidBody* body = btRigidBody::upcast(colObj);
90                 if (body)
91                 {
92                                 btTransform predictedTrans;
93                                 if (body->getActivationState() != ISLAND_SLEEPING)
94                                 {
95                                         if (body->isKinematicObject())
96                                         {
97                                                 //to calculate velocities next frame
98                                                 body->saveKinematicState(timeStep);
99                                         }
100                                 }
101                 }
102         }
103 }
104
105 void    btDiscreteDynamicsWorld::synchronizeMotionStates()
106 {
107         //debug vehicle wheels
108         
109         
110         {
111                 //todo: iterate over awake simulation islands!
112                 for ( int i=0;i<m_collisionObjects.size();i++)
113                 {
114                         btCollisionObject* colObj = m_collisionObjects[i];
115                         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
116                         {
117                                 btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
118                                 switch(colObj->getActivationState())
119                                 {
120                                 case  ACTIVE_TAG:
121                                         color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
122                                 case ISLAND_SLEEPING:
123                                         color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
124                                 case WANTS_DEACTIVATION:
125                                         color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
126                                 case DISABLE_DEACTIVATION:
127                                         color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
128                                 case DISABLE_SIMULATION:
129                                         color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
130                                 default:
131                                         {
132                                                 color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
133                                         }
134                                 };
135
136                                 debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
137                         }
138                         btRigidBody* body = btRigidBody::upcast(colObj);
139                         if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
140                         {
141                                 //we need to call the update at least once, even for sleeping objects
142                                 //otherwise the 'graphics' transform never updates properly
143                                 //so todo: add 'dirty' flag
144                                 //if (body->getActivationState() != ISLAND_SLEEPING)
145                                 {
146                                         btTransform interpolatedTransform;
147                                         btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
148                                                 body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime,interpolatedTransform);
149                                         body->getMotionState()->setWorldTransform(interpolatedTransform);
150                                 }
151                         }
152                 }
153         }
154
155         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
156         {
157                 for ( int i=0;i<this->m_vehicles.size();i++)
158                 {
159                         for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
160                         {
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                                 //synchronize the wheels with the (interpolated) chassis worldtransform
171                                 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                         }
187                 }
188         }
189
190 }
191
192
193 int     btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
194 {
195         int numSimulationSubSteps = 0;
196
197         if (maxSubSteps)
198         {
199                 //fixed timestep with interpolation
200                 m_localTime += timeStep;
201                 if (m_localTime >= fixedTimeStep)
202                 {
203                         numSimulationSubSteps = int( m_localTime / fixedTimeStep);
204                         m_localTime -= numSimulationSubSteps * fixedTimeStep;
205                 }
206         } else
207         {
208                 //variable timestep
209                 fixedTimeStep = timeStep;
210                 m_localTime = timeStep;
211                 if (btFuzzyZero(timeStep))
212                 {
213                         numSimulationSubSteps = 0;
214                         maxSubSteps = 0;
215                 } else
216                 {
217                         numSimulationSubSteps = 1;
218                         maxSubSteps = 1;
219                 }
220         }
221
222         //process some debugging flags
223         if (getDebugDrawer())
224         {
225                 gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
226         }
227         if (numSimulationSubSteps)
228         {
229
230                 saveKinematicState(fixedTimeStep);
231
232                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
233                 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
234
235                 for (int i=0;i<clampedSimulationSteps;i++)
236                 {
237                         internalSingleStepSimulation(fixedTimeStep);
238                         synchronizeMotionStates();
239                 }
240
241         } 
242
243         synchronizeMotionStates();
244
245         return numSimulationSubSteps;
246 }
247
248 void    btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
249 {
250         
251         startProfiling(timeStep);
252
253         ///update aabbs information
254         updateAabbs();
255
256         ///apply gravity, predict motion
257         predictUnconstraintMotion(timeStep);
258
259         btDispatcherInfo& dispatchInfo = getDispatchInfo();
260
261         dispatchInfo.m_timeStep = timeStep;
262         dispatchInfo.m_stepCount = 0;
263         dispatchInfo.m_debugDraw = getDebugDrawer();
264
265         ///perform collision detection
266         performDiscreteCollisionDetection();
267
268         calculateSimulationIslands();
269
270         
271         getSolverInfo().m_timeStep = timeStep;
272         
273
274
275         ///solve contact and other joint constraints
276         solveConstraints(getSolverInfo());
277         
278         ///CallbackTriggers();
279
280         ///integrate transforms
281         integrateTransforms(timeStep);
282
283         ///update vehicle simulation
284         updateVehicles(timeStep);
285
286
287         updateActivationState( timeStep );
288
289         
290
291 }
292
293 void    btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
294 {
295         m_gravity = gravity;
296         for ( int i=0;i<m_collisionObjects.size();i++)
297         {
298                 btCollisionObject* colObj = m_collisionObjects[i];
299                 btRigidBody* body = btRigidBody::upcast(colObj);
300                 if (body)
301                 {
302                         body->setGravity(gravity);
303                 }
304         }
305 }
306
307
308 void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
309 {
310         removeCollisionObject(body);
311 }
312
313 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
314 {
315         if (!body->isStaticOrKinematicObject())
316         {
317                 body->setGravity(m_gravity);
318         }
319
320         if (body->getCollisionShape())
321         {
322                 bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
323                 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
324                 short collisionFilterMask = isDynamic?  short(btBroadphaseProxy::AllFilter) :   short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
325
326                 addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
327         }
328 }
329
330 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
331 {
332         if (!body->isStaticOrKinematicObject())
333         {
334                 body->setGravity(m_gravity);
335         }
336
337         if (body->getCollisionShape())
338         {
339                 addCollisionObject(body,group,mask);
340         }
341 }
342
343
344 void    btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
345 {
346         BEGIN_PROFILE("updateVehicles");
347
348         for ( int i=0;i<m_vehicles.size();i++)
349         {
350                 btRaycastVehicle* vehicle = m_vehicles[i];
351                 vehicle->updateVehicle( timeStep);
352         }
353         END_PROFILE("updateVehicles");
354 }
355
356 void    btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
357 {
358         BEGIN_PROFILE("updateActivationState");
359
360         for ( int i=0;i<m_collisionObjects.size();i++)
361         {
362                 btCollisionObject* colObj = m_collisionObjects[i];
363                 btRigidBody* body = btRigidBody::upcast(colObj);
364                 if (body)
365                 {
366                         body->updateDeactivation(timeStep);
367
368                         if (body->wantsSleeping())
369                         {
370                                 if (body->isStaticOrKinematicObject())
371                                 {
372                                         body->setActivationState(ISLAND_SLEEPING);
373                                 } else
374                                 {
375                                         if (body->getActivationState() == ACTIVE_TAG)
376                                                 body->setActivationState( WANTS_DEACTIVATION );
377                                 }
378                         } else
379                         {
380                                 if (body->getActivationState() != DISABLE_DEACTIVATION)
381                                         body->setActivationState( ACTIVE_TAG );
382                         }
383                 }
384         }
385         END_PROFILE("updateActivationState");
386 }
387
388 void    btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
389 {
390         m_constraints.push_back(constraint);
391         if (disableCollisionsBetweenLinkedBodies)
392         {
393                 constraint->getRigidBodyA().addConstraintRef(constraint);
394                 constraint->getRigidBodyB().addConstraintRef(constraint);
395         }
396 }
397
398 void    btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
399 {
400         m_constraints.remove(constraint);
401         constraint->getRigidBodyA().removeConstraintRef(constraint);
402         constraint->getRigidBodyB().removeConstraintRef(constraint);
403 }
404
405 void    btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
406 {
407         m_vehicles.push_back(vehicle);
408 }
409
410 void    btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
411 {
412         m_vehicles.remove(vehicle);
413 }
414
415 inline  int     btGetConstraintIslandId(const btTypedConstraint* lhs)
416 {
417         int islandId;
418         
419         const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
420         const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
421         islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
422         return islandId;
423
424 }
425
426
427 class btSortConstraintOnIslandPredicate
428 {
429         public:
430
431                 bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
432                 {
433                         int rIslandId0,lIslandId0;
434                         rIslandId0 = btGetConstraintIslandId(rhs);
435                         lIslandId0 = btGetConstraintIslandId(lhs);
436                         return lIslandId0 < rIslandId0;
437                 }
438 };
439
440
441
442
443 void    btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
444 {
445         
446         struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
447         {
448
449                 btContactSolverInfo&    m_solverInfo;
450                 btConstraintSolver*             m_solver;
451                 btTypedConstraint**             m_sortedConstraints;
452                 int                                             m_numConstraints;
453                 btIDebugDraw*                   m_debugDrawer;
454                 btStackAlloc*                   m_stackAlloc;
455
456
457                 InplaceSolverIslandCallback(
458                         btContactSolverInfo& solverInfo,
459                         btConstraintSolver*     solver,
460                         btTypedConstraint** sortedConstraints,
461                         int     numConstraints,
462                         btIDebugDraw*   debugDrawer,
463                         btStackAlloc*                   stackAlloc)
464                         :m_solverInfo(solverInfo),
465                         m_solver(solver),
466                         m_sortedConstraints(sortedConstraints),
467                         m_numConstraints(numConstraints),
468                         m_debugDrawer(debugDrawer),
469                         m_stackAlloc(stackAlloc)
470                 {
471
472                 }
473
474                 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
475                 {
476                         btAssert(0);
477                         (void)other;
478                         return *this;
479                 }
480                 virtual void    ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**   manifolds,int numManifolds, int islandId)
481                 {
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                         }
504
505                         m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc);
506                 }
507
508         };
509
510         //sorted version of all btTypedConstraint, based on islandId
511         btAlignedObjectArray<btTypedConstraint*>        sortedConstraints;
512         sortedConstraints.resize( m_constraints.size());
513         int i; 
514         for (i=0;i<getNumConstraints();i++)
515         {
516                 sortedConstraints[i] = m_constraints[i];
517         }
518
519 //      assert(0);
520                 
521         
522
523         sortedConstraints.heapSort(btSortConstraintOnIslandPredicate());
524         
525         btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
526         
527         InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver, constraintsPtr,sortedConstraints.size(),    m_debugDrawer,m_stackAlloc);
528
529         
530         
531         /// solve all the constraints for this island
532         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
533
534
535 }
536
537
538
539
540 void    btDiscreteDynamicsWorld::calculateSimulationIslands()
541 {
542         BEGIN_PROFILE("calculateSimulationIslands");
543
544         getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
545
546         {
547                 int i;
548                 int numConstraints = int(m_constraints.size());
549                 for (i=0;i< numConstraints ; i++ )
550                 {
551                         btTypedConstraint* constraint = m_constraints[i];
552
553                         const btRigidBody* colObj0 = &constraint->getRigidBodyA();
554                         const btRigidBody* colObj1 = &constraint->getRigidBodyB();
555
556                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
557                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
558                         {
559                                 if (colObj0->isActive() || colObj1->isActive())
560                                 {
561
562                                         getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
563                                                 (colObj1)->getIslandTag());
564                                 }
565                         }
566                 }
567         }
568
569         //Store the island id in each body
570         getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
571
572         END_PROFILE("calculateSimulationIslands");
573
574 }
575
576
577 void    btDiscreteDynamicsWorld::updateAabbs()
578 {
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)
589                 {
590                 //      if (body->IsActive() && (!body->IsStatic()))
591                         {
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);
621                                 }
622                         }
623                 }
624         }
625         
626         END_PROFILE("updateAabbs");
627 }
628
629 void    btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
630 {
631         BEGIN_PROFILE("integrateTransforms");
632         btTransform predictedTrans;
633         for ( int i=0;i<m_collisionObjects.size();i++)
634         {
635                 btCollisionObject* colObj = m_collisionObjects[i];
636                 btRigidBody* body = btRigidBody::upcast(colObj);
637                 if (body)
638                 {
639                         if (body->isActive() && (!body->isStaticOrKinematicObject()))
640                         {
641                                 body->predictIntegratedTransform(timeStep, predictedTrans);
642                                 body->proceedToTransform( predictedTrans);
643                         }
644                 }
645         }
646         END_PROFILE("integrateTransforms");
647 }
648
649
650
651 void    btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
652 {
653         BEGIN_PROFILE("predictUnconstraintMotion");
654         for ( int i=0;i<m_collisionObjects.size();i++)
655         {
656                 btCollisionObject* colObj = m_collisionObjects[i];
657                 btRigidBody* body = btRigidBody::upcast(colObj);
658                 if (body)
659                 {
660                         if (!body->isStaticOrKinematicObject())
661                         {
662                                 if (body->isActive())
663                                 {
664                                         body->applyForces( timeStep);
665                                         body->integrateVelocities( timeStep);
666                                         body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
667                                 }
668                         }
669                 }
670         }
671         END_PROFILE("predictUnconstraintMotion");
672 }
673
674
675 void    btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
676 {
677         (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
709 }
710
711
712
713
714         
715
716 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
717 {
718         btIDebugDraw*   m_debugDrawer;
719         btVector3       m_color;
720         btTransform     m_worldTrans;
721
722 public:
723
724         DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
725                 m_debugDrawer(debugDrawer),
726                 m_color(color),
727                 m_worldTrans(worldTrans)
728         {
729         }
730
731         virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
732         {
733                 processTriangle(triangle,partId,triangleIndex);
734         }
735
736         virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
737         {
738                 (void)partId;
739                 (void)triangleIndex;
740
741                 btVector3 wv0,wv1,wv2;
742                 wv0 = m_worldTrans*triangle[0];
743                 wv1 = m_worldTrans*triangle[1];
744                 wv2 = m_worldTrans*triangle[2];
745                 m_debugDrawer->drawLine(wv0,wv1,m_color);
746                 m_debugDrawer->drawLine(wv1,wv2,m_color);
747                 m_debugDrawer->drawLine(wv2,wv0,m_color);
748         }
749 };
750
751 void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
752 {
753         btVector3 start = transform.getOrigin();
754
755         const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
756         const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
757         const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
758
759         // XY 
760         getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
761         getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
762         getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
763         getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
764
765         // XZ
766         getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
767         getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
768         getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
769         getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
770
771         // YZ
772         getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
773         getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
774         getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
775         getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
776 }
777
778 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
779 {
780         // Draw a small simplex at the center of the object
781         {
782                 btVector3 start = worldTransform.getOrigin();
783                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
784                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
785                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
786         }
787
788         if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
789         {
790                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
791                 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
792                 {
793                         btTransform childTrans = compoundShape->getChildTransform(i);
794                         const btCollisionShape* colShape = compoundShape->getChildShape(i);
795                         debugDrawObject(worldTransform*childTrans,colShape,color);
796                 }
797
798         } else
799         {
800                 switch (shape->getShapeType())
801                 {
802
803                 case SPHERE_SHAPE_PROXYTYPE:
804                         {
805                                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
806                                 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
807                                 
808                                 debugDrawSphere(radius, worldTransform, color);
809                                 break;
810                         }
811                 case MULTI_SPHERE_SHAPE_PROXYTYPE:
812                         {
813                                 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
814
815                                 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
816                                 {
817                                         btTransform childTransform = worldTransform;
818                                         childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
819                                         debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
820                                 }
821
822                                 break;
823                         }
824                 case CAPSULE_SHAPE_PROXYTYPE:
825                         {
826                                 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
827
828                                 btScalar radius = capsuleShape->getRadius();
829                                 btScalar halfHeight = capsuleShape->getHalfHeight();
830
831                                 // Draw the ends
832                                 {
833                                         btTransform childTransform = worldTransform;
834                                         childTransform.getOrigin() = worldTransform * btVector3(0,halfHeight,0);
835                                         debugDrawSphere(radius, childTransform, color);
836                                 }
837
838                                 {
839                                         btTransform childTransform = worldTransform;
840                                         childTransform.getOrigin() = worldTransform * btVector3(0,-halfHeight,0);
841                                         debugDrawSphere(radius, childTransform, color);
842                                 }
843
844                                 // Draw some additional lines
845                                 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
851                                 break;
852                         }
853                 case CONE_SHAPE_PROXYTYPE:
854                         {
855                                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
856                                 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
857                                 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
858                                 btVector3 start = worldTransform.getOrigin();
859                                 // insert here Bullet 2.69 that fixes representation of cone
860                                 int upAxis= coneShape->getConeUpIndex();
861                                 
862                                 btVector3       offsetHeight(0,0,0);
863                                 offsetHeight[upAxis] = height * btScalar(0.5);
864                                 btVector3       offsetRadius(0,0,0);
865                                 offsetRadius[(upAxis+1)%3] = radius;
866                                 btVector3       offset2Radius(0,0,0);
867                                 offset2Radius[(upAxis+2)%3] = radius;
868
869                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
870                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
871                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
872                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
873
874                                 // buggy code that does not take into account the direction of the cone
875                                 //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);
876                                 //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);
877                                 //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);
878                                 //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);
879                                 break;
880
881                         }
882                 case CYLINDER_SHAPE_PROXYTYPE:
883                         {
884                                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
885                                 int upAxis = cylinder->getUpAxis();
886                                 btScalar radius = cylinder->getRadius();
887                                 btScalar halfHeight = cylinder->getHalfExtents()[upAxis];
888                                 btVector3 start = worldTransform.getOrigin();
889                                 btVector3       offsetHeight(0,0,0);
890                                 offsetHeight[upAxis] = halfHeight;
891                                 btVector3       offsetRadius(0,0,0);
892                                 offsetRadius[(upAxis+1)%3] = radius;
893                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
894                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
895                                 break;
896                         }
897                 default:
898                         {
899
900                                 if (shape->isConcave())
901                                 {
902                                         btConcaveShape* concaveMesh = (btConcaveShape*) shape;
903                                         
904                                         //todo pass camera, for some culling
905                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
906                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
907
908                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
909                                         concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
910
911                                 }
912
913                                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
914                                 {
915                                         btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
916                                         //todo: pass camera for some culling                    
917                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
918                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
919                                         //DebugDrawcallback drawCallback;
920                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
921                                         convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
922                                 }
923
924
925                                 /// for polyhedral shapes
926                                 if (shape->isPolyhedral())
927                                 {
928                                         btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
929
930                                         int i;
931                                         for (i=0;i<polyshape->getNumEdges();i++)
932                                         {
933                                                 btPoint3 a,b;
934                                                 polyshape->getEdge(i,a,b);
935                                                 btVector3 wa = worldTransform * a;
936                                                 btVector3 wb = worldTransform * b;
937                                                 getDebugDrawer()->drawLine(wa,wb,color);
938
939                                         }
940
941                                         
942                                 }
943                         }
944                 }
945         }
946 }
947
948
949 void    btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
950 {
951         if (m_ownsConstraintSolver)
952         {
953                 delete m_constraintSolver;
954         }
955         m_ownsConstraintSolver = false;
956         m_constraintSolver = solver;
957 }
958
959 int             btDiscreteDynamicsWorld::getNumConstraints() const
960 {
961         return int(m_constraints.size());
962 }
963 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
964 {
965         return m_constraints[index];
966 }
967 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
968 {
969         return m_constraints[index];
970 }