update Bullet 2.x with latest changes, notice that the integration is not finished...
[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
20 //collision detection
21 #include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
22 #include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"
23 #include "BulletCollision/CollisionShapes/btCollisionShape.h"
24 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"
25 #include <LinearMath/btTransformUtil.h>
26
27 //rigidbody & constraints
28 #include "BulletDynamics/Dynamics/btRigidBody.h"
29 #include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
30 #include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"
31 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
32
33 //for debug rendering
34 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
35 #include "BulletCollision/CollisionShapes/btSphereShape.h"
36 #include "BulletCollision/CollisionShapes/btBoxShape.h"
37 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
38 #include "BulletCollision/CollisionShapes/btConeShape.h"
39 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
40 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
41 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
42 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
43 #include "LinearMath/btIDebugDraw.h"
44
45
46
47 //vehicle
48 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
49 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
50 #include "BulletDynamics/Vehicle/btWheelInfo.h"
51 #include "LinearMath/btIDebugDraw.h"
52 #include "LinearMath/btQuickprof.h"
53 #include "LinearMath/btMotionState.h"
54
55
56
57 #include <algorithm>
58
59 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld()
60 :btDynamicsWorld(),
61 m_constraintSolver(new btSequentialImpulseConstraintSolver),
62 m_debugDrawer(0),
63 m_gravity(0,-10,0),
64 m_localTime(1.f/60.f),
65 m_profileTimings(0)
66 {
67         m_islandManager = new btSimulationIslandManager();
68         m_ownsIslandManager = true;
69         m_ownsConstraintSolver = true;
70
71 }
72
73 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btOverlappingPairCache* pairCache,btConstraintSolver* constraintSolver)
74 :btDynamicsWorld(dispatcher,pairCache),
75 m_constraintSolver(constraintSolver? constraintSolver: new btSequentialImpulseConstraintSolver),
76 m_debugDrawer(0),
77 m_gravity(0,-10,0),
78 m_localTime(1.f/60.f),
79 m_profileTimings(0)
80 {
81         m_islandManager = new btSimulationIslandManager();
82         m_ownsIslandManager = true;
83         m_ownsConstraintSolver = (constraintSolver==0);
84 }
85
86
87 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
88 {
89         //only delete it when we created it
90         if (m_ownsIslandManager)
91                 delete m_islandManager;
92         if (m_ownsConstraintSolver)
93                  delete m_constraintSolver;
94 }
95
96 void    btDiscreteDynamicsWorld::saveKinematicState(float timeStep)
97 {
98
99         for (unsigned int i=0;i<m_collisionObjects.size();i++)
100         {
101                 btCollisionObject* colObj = m_collisionObjects[i];
102                 btRigidBody* body = btRigidBody::upcast(colObj);
103                 if (body)
104                 {
105                                 btTransform predictedTrans;
106                                 if (body->GetActivationState() != ISLAND_SLEEPING)
107                                 {
108                                         if (body->isKinematicObject())
109                                         {
110                                                 //to calculate velocities next frame
111                                                 body->saveKinematicState(timeStep);
112                                         }
113                                 }
114                 }
115         }
116 }
117
118 void    btDiscreteDynamicsWorld::synchronizeMotionStates()
119 {
120         //todo: iterate over awake simulation islands!
121         for (unsigned int i=0;i<m_collisionObjects.size();i++)
122         {
123                 btCollisionObject* colObj = m_collisionObjects[i];
124                 if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
125                 {
126                         btVector3 color(255.f,255.f,255.f);
127                         switch(colObj->GetActivationState())
128                         {
129                         case  ACTIVE_TAG:
130                                 color = btVector3(255.f,255.f,255.f); break;
131                         case ISLAND_SLEEPING:
132                                 color =  btVector3(0.f,255.f,0.f);break;
133                         case WANTS_DEACTIVATION:
134                                 color = btVector3(0.f,255.f,255.f);break;
135                         case DISABLE_DEACTIVATION:
136                                 color = btVector3(255.f,0.f,0.f);break;
137                         case DISABLE_SIMULATION:
138                                 color = btVector3(255.f,255.f,0.f);break;
139                         default:
140                                 {
141                                         color = btVector3(255.f,0.f,0.f);
142                                 }
143                         };
144
145                         debugDrawObject(colObj->m_worldTransform,colObj->m_collisionShape,color);
146                 }
147                 btRigidBody* body = btRigidBody::upcast(colObj);
148                 if (body && body->getMotionState() && !body->isStaticOrKinematicObject())
149                 {
150                         if (body->GetActivationState() != ISLAND_SLEEPING)
151                         {
152                                 btTransform interpolatedTransform;
153                                 btTransformUtil::integrateTransform(body->m_interpolationWorldTransform,
154                                         body->m_interpolationLinearVelocity,body->m_interpolationAngularVelocity,m_localTime,interpolatedTransform);
155                                 body->getMotionState()->setWorldTransform(interpolatedTransform);
156                         }
157                 }
158         }
159
160 }
161
162
163 int     btDiscreteDynamicsWorld::stepSimulation( float timeStep,int maxSubSteps, float fixedTimeStep)
164 {
165         int numSimulationSubSteps = 0;
166
167         if (maxSubSteps)
168         {
169                 //fixed timestep with interpolation
170                 m_localTime += timeStep;
171                 if (m_localTime >= fixedTimeStep)
172                 {
173                         numSimulationSubSteps = int( m_localTime / fixedTimeStep);
174                         m_localTime -= numSimulationSubSteps * fixedTimeStep;
175                 }
176         } else
177         {
178                 //variable timestep
179                 fixedTimeStep = timeStep;
180                 m_localTime = timeStep;
181                 numSimulationSubSteps = 1;
182                 maxSubSteps = 1;
183         }
184
185         //process some debugging flags
186         if (getDebugDrawer())
187         {
188                 gDisableDeactivation = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
189         }
190         if (!btFuzzyZero(timeStep) && numSimulationSubSteps)
191         {
192
193                 saveKinematicState(fixedTimeStep);
194
195                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
196                 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
197
198                 for (int i=0;i<clampedSimulationSteps;i++)
199                 {
200                         internalSingleStepSimulation(fixedTimeStep);
201                 }
202
203         } 
204
205         synchronizeMotionStates();
206
207         return numSimulationSubSteps;
208 }
209
210 void    btDiscreteDynamicsWorld::internalSingleStepSimulation(float timeStep)
211 {
212         
213         startProfiling(timeStep);
214
215         ///update aabbs information
216         updateAabbs();
217
218         ///apply gravity, predict motion
219         predictUnconstraintMotion(timeStep);
220
221         ///perform collision detection
222         performDiscreteCollisionDetection();
223
224         calculateSimulationIslands();
225
226         btContactSolverInfo infoGlobal;
227         infoGlobal.m_timeStep = timeStep;
228         
229         ///solve non-contact constraints
230         solveNoncontactConstraints(infoGlobal);
231         
232         ///solve contact constraints
233         solveContactConstraints(infoGlobal);
234
235         ///update vehicle simulation
236         updateVehicles(timeStep);
237         
238         ///CallbackTriggers();
239
240         ///integrate transforms
241         integrateTransforms(timeStep);
242                 
243         updateActivationState( timeStep );
244
245         
246
247 }
248
249 void    btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
250 {
251         m_gravity = gravity;
252         for (unsigned int i=0;i<m_collisionObjects.size();i++)
253         {
254                 btCollisionObject* colObj = m_collisionObjects[i];
255                 btRigidBody* body = btRigidBody::upcast(colObj);
256                 if (body)
257                 {
258                         body->setGravity(gravity);
259                 }
260         }
261 }
262
263
264 void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
265 {
266         removeCollisionObject(body);
267 }
268
269 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
270 {
271         body->setGravity(m_gravity);
272         bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
273         short collisionFilterGroup = isDynamic? btBroadphaseProxy::DefaultFilter : btBroadphaseProxy::StaticFilter;
274         short collisionFilterMask = isDynamic?  btBroadphaseProxy::AllFilter :  btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter;
275
276         addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
277 }
278
279
280 void    btDiscreteDynamicsWorld::updateVehicles(float timeStep)
281 {
282         BEGIN_PROFILE("updateVehicles");
283
284         for (unsigned int i=0;i<m_vehicles.size();i++)
285         {
286                 btRaycastVehicle* vehicle = m_vehicles[i];
287                 vehicle->updateVehicle( timeStep);
288         }
289         END_PROFILE("updateVehicles");
290 }
291
292 void    btDiscreteDynamicsWorld::updateActivationState(float timeStep)
293 {
294         BEGIN_PROFILE("updateActivationState");
295
296         for (unsigned 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->updateDeactivation(timeStep);
303
304                         if (body->wantsSleeping())
305                         {
306                                 if (body->GetActivationState() == ACTIVE_TAG)
307                                         body->SetActivationState( WANTS_DEACTIVATION );
308                         } else
309                         {
310                                 if (body->GetActivationState() != DISABLE_DEACTIVATION)
311                                         body->SetActivationState( ACTIVE_TAG );
312                         }
313                 }
314         }
315         END_PROFILE("updateActivationState");
316 }
317
318 void    btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint)
319 {
320         m_constraints.push_back(constraint);
321 }
322
323 void    btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
324 {
325         std::vector<btTypedConstraint*>::iterator cit = std::find(m_constraints.begin(),m_constraints.end(),constraint);
326         if (!(cit==m_constraints.end()))
327         {
328                 m_constraints.erase(cit);
329         }
330 }
331
332 void    btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
333 {
334         m_vehicles.push_back(vehicle);
335 }
336
337 void    btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
338 {
339         std::vector<btRaycastVehicle*>::iterator vit = std::find(m_vehicles.begin(),m_vehicles.end(),vehicle);
340         if (!(vit==m_vehicles.end()))
341         {
342                 m_vehicles.erase(vit);
343         }
344 }
345
346
347 void    btDiscreteDynamicsWorld::solveContactConstraints(btContactSolverInfo& solverInfo)
348 {
349         
350         BEGIN_PROFILE("solveContactConstraints");
351
352         struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
353         {
354
355                 btContactSolverInfo& m_solverInfo;
356                 btConstraintSolver*     m_solver;
357                 btIDebugDraw*   m_debugDrawer;
358
359                 InplaceSolverIslandCallback(
360                         btContactSolverInfo& solverInfo,
361                         btConstraintSolver*     solver,
362                         btIDebugDraw*   debugDrawer)
363                         :m_solverInfo(solverInfo),
364                         m_solver(solver),
365                         m_debugDrawer(debugDrawer)
366                 {
367
368                 }
369
370                 virtual void    ProcessIsland(btPersistentManifold**    manifolds,int numManifolds)
371                 {
372                         m_solver->solveGroup( manifolds, numManifolds,m_solverInfo,m_debugDrawer);
373                 }
374
375         };
376
377         
378         InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver,     m_debugDrawer);
379
380         
381         /// solve all the contact points and contact friction
382         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld()->getCollisionObjectArray(),&solverCallback);
383
384         END_PROFILE("solveContactConstraints");
385
386 }
387
388
389 void    btDiscreteDynamicsWorld::solveNoncontactConstraints(btContactSolverInfo& solverInfo)
390 {
391         BEGIN_PROFILE("solveNoncontactConstraints");
392
393         int i;
394         int numConstraints = int(m_constraints.size());
395
396         ///constraint preparation: building jacobians
397         for (i=0;i< numConstraints ; i++ )
398         {
399                 btTypedConstraint* constraint = m_constraints[i];
400                 constraint->buildJacobian();
401         }
402
403         //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
404         for (int g=0;g<solverInfo.m_numIterations;g++)
405         {
406                 //
407                 // constraint solving
408                 //
409                 for (i=0;i< numConstraints ; i++ )
410                 {
411                         btTypedConstraint* constraint = m_constraints[i];
412                         constraint->solveConstraint( solverInfo.m_timeStep );
413                 }
414         }
415
416         END_PROFILE("solveNoncontactConstraints");
417
418 }
419
420 void    btDiscreteDynamicsWorld::calculateSimulationIslands()
421 {
422         BEGIN_PROFILE("calculateSimulationIslands");
423
424         getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
425
426         {
427                 int i;
428                 int numConstraints = int(m_constraints.size());
429                 for (i=0;i< numConstraints ; i++ )
430                 {
431                         btTypedConstraint* constraint = m_constraints[i];
432
433                         const btRigidBody* colObj0 = &constraint->getRigidBodyA();
434                         const btRigidBody* colObj1 = &constraint->getRigidBodyB();
435
436                         if (((colObj0) && ((colObj0)->mergesSimulationIslands())) &&
437                                 ((colObj1) && ((colObj1)->mergesSimulationIslands())))
438                         {
439                                 if (colObj0->IsActive() || colObj1->IsActive())
440                                 {
441
442                                         getSimulationIslandManager()->getUnionFind().unite((colObj0)->m_islandTag1,
443                                                 (colObj1)->m_islandTag1);
444                                 }
445                         }
446                 }
447         }
448
449         //Store the island id in each body
450         getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
451
452         END_PROFILE("calculateSimulationIslands");
453
454 }
455
456 static void DrawAabb(btIDebugDraw* debugDrawer,const btVector3& from,const btVector3& to,const btVector3& color)
457 {
458
459         btVector3 halfExtents = (to-from)* 0.5f;
460         btVector3 center = (to+from) *0.5f;
461         int i,j;
462
463         btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
464         for (i=0;i<4;i++)
465         {
466                 for (j=0;j<3;j++)
467                 {
468                         pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],                
469                                 edgecoord[2]*halfExtents[2]);
470                         pa+=center;
471
472                         int othercoord = j%3;
473                         edgecoord[othercoord]*=-1.f;
474                         pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],        
475                                 edgecoord[2]*halfExtents[2]);
476                         pb+=center;
477
478                         debugDrawer->drawLine(pa,pb,color);
479                 }
480                 edgecoord = btVector3(-1.f,-1.f,-1.f);
481                 if (i<3)
482                         edgecoord[i]*=-1.f;
483         }
484
485
486 }
487
488 void    btDiscreteDynamicsWorld::updateAabbs()
489 {
490         BEGIN_PROFILE("updateAabbs");
491         
492         btVector3 colorvec(1,0,0);
493         btTransform predictedTrans;
494         for (unsigned int i=0;i<m_collisionObjects.size();i++)
495         {
496                 btCollisionObject* colObj = m_collisionObjects[i];
497                 
498                 btRigidBody* body = btRigidBody::upcast(colObj);
499                 if (body)
500                 {
501                 //      if (body->IsActive() && (!body->IsStatic()))
502                         {
503                                 btPoint3 minAabb,maxAabb;
504                                 colObj->m_collisionShape->getAabb(colObj->m_worldTransform, minAabb,maxAabb);
505                                 btSimpleBroadphase* bp = (btSimpleBroadphase*)m_broadphasePairCache;
506
507                                 //moving objects should be moderately sized, probably something wrong if not
508                                 if ( colObj->isStaticObject() || ((maxAabb-minAabb).length2() < 1e12f))
509                                 {
510                                         bp->setAabb(body->m_broadphaseHandle,minAabb,maxAabb);
511                                 } else
512                                 {
513                                         //something went wrong, investigate
514                                         //this assert is unwanted in 3D modelers (danger of loosing work)
515                                         assert(0);
516                                         body->SetActivationState(DISABLE_SIMULATION);
517                                         
518                                         static bool reportMe = true;
519                                         if (reportMe)
520                                         {
521                                                 reportMe = false;
522                                                 printf("Overflow in AABB, object removed from simulation \n");
523                                                 printf("If you can reproduce this, please email bugs@continuousphysics.com\n");
524                                                 printf("Please include above information, your Platform, version of OS.\n");
525                                                 printf("Thanks.\n");
526                                         }
527
528
529                                 }
530                                 if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
531                                 {
532                                         DrawAabb(m_debugDrawer,minAabb,maxAabb,colorvec);
533                                 }
534                         }
535                 }
536         }
537         
538         END_PROFILE("updateAabbs");
539 }
540
541 void    btDiscreteDynamicsWorld::integrateTransforms(float timeStep)
542 {
543         BEGIN_PROFILE("integrateTransforms");
544         btTransform predictedTrans;
545         for (unsigned int i=0;i<m_collisionObjects.size();i++)
546         {
547                 btCollisionObject* colObj = m_collisionObjects[i];
548                 btRigidBody* body = btRigidBody::upcast(colObj);
549                 if (body)
550                 {
551                         if (body->IsActive() && (!body->isStaticOrKinematicObject()))
552                         {
553                                 body->predictIntegratedTransform(timeStep, predictedTrans);
554                                 body->proceedToTransform( predictedTrans);
555                         }
556                 }
557         }
558         END_PROFILE("integrateTransforms");
559 }
560
561
562
563 void    btDiscreteDynamicsWorld::predictUnconstraintMotion(float timeStep)
564 {
565         BEGIN_PROFILE("predictUnconstraintMotion");
566         for (unsigned int i=0;i<m_collisionObjects.size();i++)
567         {
568                 btCollisionObject* colObj = m_collisionObjects[i];
569                 btRigidBody* body = btRigidBody::upcast(colObj);
570                 if (body)
571                 {
572                         if (!body->isStaticOrKinematicObject())
573                         {
574                                 if (body->IsActive())
575                                 {
576                                         body->applyForces( timeStep);
577                                         body->integrateVelocities( timeStep);
578                                         body->predictIntegratedTransform(timeStep,body->m_interpolationWorldTransform);
579                                 }
580                         }
581                 }
582         }
583         END_PROFILE("predictUnconstraintMotion");
584 }
585
586
587 void    btDiscreteDynamicsWorld::startProfiling(float timeStep)
588 {
589         #ifdef USE_QUICKPROF
590
591
592         //toggle btProfiler
593         if ( m_debugDrawer && m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_ProfileTimings)
594         {
595                 if (!m_profileTimings)
596                 {
597                         m_profileTimings = 1;
598                         // To disable profiling, simply comment out the following line.
599                         static int counter = 0;
600
601                         char filename[128];
602                         sprintf(filename,"quickprof_bullet_timings%i.csv",counter++);
603                         btProfiler::init(filename, btProfiler::BLOCK_CYCLE_SECONDS);//BLOCK_TOTAL_MICROSECONDS
604                 } else
605                 {
606                         btProfiler::endProfilingCycle();
607                 }
608
609         } else
610         {
611                 if (m_profileTimings)
612                 {
613                         btProfiler::endProfilingCycle();
614
615                         m_profileTimings = 0;
616                         btProfiler::destroy();
617                 }
618         }
619 #endif //USE_QUICKPROF
620 }
621
622
623
624
625         
626
627 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
628 {
629         btIDebugDraw*   m_debugDrawer;
630         btVector3       m_color;
631         btTransform     m_worldTrans;
632
633 public:
634
635         DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color)
636                 : m_debugDrawer(debugDrawer),
637                 m_worldTrans(worldTrans),
638                 m_color(color)
639         {
640         }
641
642         virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
643         {
644                 processTriangle(triangle,partId,triangleIndex);
645         }
646
647         virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
648         {
649                 btVector3 wv0,wv1,wv2;
650                 wv0 = m_worldTrans*triangle[0];
651                 wv1 = m_worldTrans*triangle[1];
652                 wv2 = m_worldTrans*triangle[2];
653                 m_debugDrawer->drawLine(wv0,wv1,m_color);
654                 m_debugDrawer->drawLine(wv1,wv2,m_color);
655                 m_debugDrawer->drawLine(wv2,wv0,m_color);
656         }
657 };
658
659
660
661 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
662 {
663
664         if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
665         {
666                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
667                 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
668                 {
669                         btTransform childTrans = compoundShape->getChildTransform(i);
670                         const btCollisionShape* colShape = compoundShape->getChildShape(i);
671                         debugDrawObject(worldTransform*childTrans,colShape,color);
672                 }
673
674         } else
675         {
676                 switch (shape->getShapeType())
677                 {
678
679                 case SPHERE_SHAPE_PROXYTYPE:
680                         {
681                                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
682                                 float radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
683                                 btVector3 start = worldTransform.getOrigin();
684                                 getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(radius,0,0),color);
685                                 getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,radius,0),color);
686                                 getDebugDrawer()->drawLine(start,start+worldTransform.getBasis() * btVector3(0,0,radius),color);
687                                 //drawSphere                                    
688                                 break;
689                         }
690                 case MULTI_SPHERE_SHAPE_PROXYTYPE:
691                 case CONE_SHAPE_PROXYTYPE:
692                         {
693                                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
694                                 float radius = coneShape->getRadius();//+coneShape->getMargin();
695                                 float height = coneShape->getHeight();//+coneShape->getMargin();
696                                 btVector3 start = worldTransform.getOrigin();
697                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(radius,0.f,-0.5f*height),color);
698                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(-radius,0.f,-0.5f*height),color);
699                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,radius,-0.5f*height),color);
700                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * btVector3(0.f,0.f,0.5f*height),start+worldTransform.getBasis() * btVector3(0.f,-radius,-0.5f*height),color);
701                                 break;
702
703                         }
704                 case CYLINDER_SHAPE_PROXYTYPE:
705                         {
706                                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
707                                 int upAxis = cylinder->getUpAxis();
708                                 float radius = cylinder->getRadius();
709                                 float halfHeight = cylinder->getHalfExtents()[upAxis];
710                                 btVector3 start = worldTransform.getOrigin();
711                                 btVector3       offsetHeight(0,0,0);
712                                 offsetHeight[upAxis] = halfHeight;
713                                 btVector3       offsetRadius(0,0,0);
714                                 offsetRadius[(upAxis+1)%3] = radius;
715                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
716                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
717                                 break;
718                         }
719                 default:
720                         {
721
722                                 if (shape->getShapeType() == TRIANGLE_MESH_SHAPE_PROXYTYPE)
723                                 {
724                                         btTriangleMeshShape* concaveMesh = (btTriangleMeshShape*) shape;
725                                         //btVector3 aabbMax(1e30f,1e30f,1e30f);
726                                         //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
727
728                                         //todo pass camera, for some culling
729                                         btVector3 aabbMax(1e30f,1e30f,1e30f);
730                                         btVector3 aabbMin(-1e30f,-1e30f,-1e30f);
731
732                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
733                                         concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
734
735                                 }
736
737                                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
738                                 {
739                                         btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
740                                         //todo: pass camera for some culling                    
741                                         btVector3 aabbMax(1e30f,1e30f,1e30f);
742                                         btVector3 aabbMin(-1e30f,-1e30f,-1e30f);
743                                         //DebugDrawcallback drawCallback;
744                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
745                                         convexMesh->getStridingMesh()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
746                                 }
747
748
749                                 /// for polyhedral shapes
750                                 if (shape->isPolyhedral())
751                                 {
752                                         btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
753
754                                         int i;
755                                         for (i=0;i<polyshape->getNumEdges();i++)
756                                         {
757                                                 btPoint3 a,b;
758                                                 polyshape->getEdge(i,a,b);
759                                                 btVector3 wa = worldTransform * a;
760                                                 btVector3 wb = worldTransform * b;
761                                                 getDebugDrawer()->drawLine(wa,wb,color);
762
763                                         }
764
765                                         
766                                 }
767                         }
768                 }
769         }
770 }