Fix build: don't use Character stuff, it wasn't meant to be included in Blender/exter...
[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 #include "LinearMath/btQuickprof.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 #include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
33 #include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"
34 #include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"
35 #include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"
36 #include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"
37
38 //for debug rendering
39 #include "BulletCollision/CollisionShapes/btBoxShape.h"
40 #include "BulletCollision/CollisionShapes/btCapsuleShape.h"
41 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
42 #include "BulletCollision/CollisionShapes/btConeShape.h"
43 #include "BulletCollision/CollisionShapes/btConvexTriangleMeshShape.h"
44 #include "BulletCollision/CollisionShapes/btCylinderShape.h"
45 #include "BulletCollision/CollisionShapes/btMultiSphereShape.h"
46 #include "BulletCollision/CollisionShapes/btPolyhedralConvexShape.h"
47 #include "BulletCollision/CollisionShapes/btSphereShape.h"
48 #include "BulletCollision/CollisionShapes/btTriangleCallback.h"
49 #include "BulletCollision/CollisionShapes/btTriangleMeshShape.h"
50 #include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
51 #include "LinearMath/btIDebugDraw.h"
52
53
54
55 //vehicle
56 #include "BulletDynamics/Vehicle/btRaycastVehicle.h"
57 #include "BulletDynamics/Vehicle/btVehicleRaycaster.h"
58 #include "BulletDynamics/Vehicle/btWheelInfo.h"
59 #ifdef USE_CHARACTER
60 //character
61 #include "BulletDynamics/Character/btCharacterControllerInterface.h"
62 #endif //USE_CHARACTER
63
64 #include "LinearMath/btIDebugDraw.h"
65 #include "LinearMath/btQuickprof.h"
66 #include "LinearMath/btMotionState.h"
67
68
69
70
71
72 btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
73 :btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
74 m_constraintSolver(constraintSolver),
75 m_gravity(0,-10,0),
76 m_localTime(btScalar(1.)/btScalar(60.)),
77 m_profileTimings(0)
78 {
79         if (!m_constraintSolver)
80         {
81                 void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);
82                 m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;
83                 m_ownsConstraintSolver = true;
84         } else
85         {
86                 m_ownsConstraintSolver = false;
87         }
88
89         {
90                 void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);
91                 m_islandManager = new (mem) btSimulationIslandManager();
92         }
93
94         m_ownsIslandManager = true;
95 }
96
97
98 btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()
99 {
100         //only delete it when we created it
101         if (m_ownsIslandManager)
102         {
103                 m_islandManager->~btSimulationIslandManager();
104                 btAlignedFree( m_islandManager);
105         }
106         if (m_ownsConstraintSolver)
107         {
108
109                 m_constraintSolver->~btConstraintSolver();
110                 btAlignedFree(m_constraintSolver);
111         }
112 }
113
114 void    btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)
115 {
116
117         for (int i=0;i<m_collisionObjects.size();i++)
118         {
119                 btCollisionObject* colObj = m_collisionObjects[i];
120                 btRigidBody* body = btRigidBody::upcast(colObj);
121                 if (body)
122                 {
123                                 if (body->getActivationState() != ISLAND_SLEEPING)
124                                 {
125                                         if (body->isKinematicObject())
126                                         {
127                                                 //to calculate velocities next frame
128                                                 body->saveKinematicState(timeStep);
129                                         }
130                                 }
131                 }
132         }
133 }
134
135 void    btDiscreteDynamicsWorld::debugDrawWorld()
136 {
137         BT_PROFILE("debugDrawWorld");
138
139         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawContactPoints)
140         {
141                 int numManifolds = getDispatcher()->getNumManifolds();
142                 btVector3 color(0,0,0);
143                 for (int i=0;i<numManifolds;i++)
144                 {
145                         btPersistentManifold* contactManifold = getDispatcher()->getManifoldByIndexInternal(i);
146                         //btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
147                         //btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
148
149                         int numContacts = contactManifold->getNumContacts();
150                         for (int j=0;j<numContacts;j++)
151                         {
152                                 btManifoldPoint& cp = contactManifold->getContactPoint(j);
153                                 getDebugDrawer()->drawContactPoint(cp.m_positionWorldOnB,cp.m_normalWorldOnB,cp.getDistance(),cp.getLifeTime(),color);
154                         }
155                 }
156         }
157         bool drawConstraints = false;
158         if (getDebugDrawer())
159         {
160                 int mode = getDebugDrawer()->getDebugMode();
161                 if(mode  & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))
162                 {
163                         drawConstraints = true;
164                 }
165         }
166         if(drawConstraints)
167         {
168                 for(int i = getNumConstraints()-1; i>=0 ;i--)
169                 {
170                         btTypedConstraint* constraint = getConstraint(i);
171                         debugDrawConstraint(constraint);
172                 }
173         }
174
175
176
177         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))
178         {
179                 int i;
180
181                 for (  i=0;i<m_collisionObjects.size();i++)
182                 {
183                         btCollisionObject* colObj = m_collisionObjects[i];
184                         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
185                         {
186                                 btVector3 color(btScalar(255.),btScalar(255.),btScalar(255.));
187                                 switch(colObj->getActivationState())
188                                 {
189                                 case  ACTIVE_TAG:
190                                         color = btVector3(btScalar(255.),btScalar(255.),btScalar(255.)); break;
191                                 case ISLAND_SLEEPING:
192                                         color =  btVector3(btScalar(0.),btScalar(255.),btScalar(0.));break;
193                                 case WANTS_DEACTIVATION:
194                                         color = btVector3(btScalar(0.),btScalar(255.),btScalar(255.));break;
195                                 case DISABLE_DEACTIVATION:
196                                         color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));break;
197                                 case DISABLE_SIMULATION:
198                                         color = btVector3(btScalar(255.),btScalar(255.),btScalar(0.));break;
199                                 default:
200                                         {
201                                                 color = btVector3(btScalar(255.),btScalar(0.),btScalar(0.));
202                                         }
203                                 };
204
205                                 debugDrawObject(colObj->getWorldTransform(),colObj->getCollisionShape(),color);
206                         }
207                         if (m_debugDrawer && (m_debugDrawer->getDebugMode() & btIDebugDraw::DBG_DrawAabb))
208                         {
209                                 btVector3 minAabb,maxAabb;
210                                 btVector3 colorvec(1,0,0);
211                                 colObj->getCollisionShape()->getAabb(colObj->getWorldTransform(), minAabb,maxAabb);
212                                 m_debugDrawer->drawAabb(minAabb,maxAabb,colorvec);
213                         }
214
215                 }
216         
217                 for (  i=0;i<this->m_vehicles.size();i++)
218                 {
219                         for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
220                         {
221                                 btVector3 wheelColor(0,255,255);
222                                 if (m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_isInContact)
223                                 {
224                                         wheelColor.setValue(0,0,255);
225                                 } else
226                                 {
227                                         wheelColor.setValue(255,0,255);
228                                 }
229                 
230                                 btVector3 wheelPosWS = m_vehicles[i]->getWheelInfo(v).m_worldTransform.getOrigin();
231
232                                 btVector3 axle = btVector3(     
233                                         m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[0][m_vehicles[i]->getRightAxis()],
234                                         m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[1][m_vehicles[i]->getRightAxis()],
235                                         m_vehicles[i]->getWheelInfo(v).m_worldTransform.getBasis()[2][m_vehicles[i]->getRightAxis()]);
236
237
238                                 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
239                                 //debug wheels (cylinders)
240                                 m_debugDrawer->drawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
241                                 m_debugDrawer->drawLine(wheelPosWS,m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);
242
243                         }
244                 }
245         }
246 }
247
248 void    btDiscreteDynamicsWorld::clearForces()
249 {
250         ///@todo: iterate over awake simulation islands!
251         for ( int i=0;i<m_collisionObjects.size();i++)
252         {
253                 btCollisionObject* colObj = m_collisionObjects[i];
254                 
255                 btRigidBody* body = btRigidBody::upcast(colObj);
256                 if (body)
257                 {
258                         body->clearForces();
259                 }
260         }
261 }       
262
263 ///apply gravity, call this once per timestep
264 void    btDiscreteDynamicsWorld::applyGravity()
265 {
266         ///@todo: iterate over awake simulation islands!
267         for ( int i=0;i<m_collisionObjects.size();i++)
268         {
269                 btCollisionObject* colObj = m_collisionObjects[i];
270                 
271                 btRigidBody* body = btRigidBody::upcast(colObj);
272                 if (body && body->isActive())
273                 {
274                         body->applyGravity();
275                 }
276         }
277 }
278
279
280 void    btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
281 {
282         btAssert(body);
283
284         if (body->getMotionState() && !body->isStaticOrKinematicObject())
285         {
286                 //we need to call the update at least once, even for sleeping objects
287                 //otherwise the 'graphics' transform never updates properly
288                 ///@todo: add 'dirty' flag
289                 //if (body->getActivationState() != ISLAND_SLEEPING)
290                 {
291                         btTransform interpolatedTransform;
292                         btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),
293                                 body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);
294                         body->getMotionState()->setWorldTransform(interpolatedTransform);
295                 }
296         }
297 }
298
299
300 void    btDiscreteDynamicsWorld::synchronizeMotionStates()
301 {
302         BT_PROFILE("synchronizeMotionStates");
303         {
304                 //todo: iterate over awake simulation islands!
305                 for ( int i=0;i<m_collisionObjects.size();i++)
306                 {
307                         btCollisionObject* colObj = m_collisionObjects[i];
308                         
309                         btRigidBody* body = btRigidBody::upcast(colObj);
310                         if (body)
311                                 synchronizeSingleMotionState(body);
312                 }
313         }
314
315         if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawWireframe)
316         {
317                 for ( int i=0;i<this->m_vehicles.size();i++)
318                 {
319                         for (int v=0;v<m_vehicles[i]->getNumWheels();v++)
320                         {
321                                 //synchronize the wheels with the (interpolated) chassis worldtransform
322                                 m_vehicles[i]->updateWheelTransform(v,true);
323                         }
324                 }
325         }
326
327 }
328
329
330 int     btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)
331 {
332         startProfiling(timeStep);
333
334         BT_PROFILE("stepSimulation");
335
336         int numSimulationSubSteps = 0;
337
338         if (maxSubSteps)
339         {
340                 //fixed timestep with interpolation
341                 m_localTime += timeStep;
342                 if (m_localTime >= fixedTimeStep)
343                 {
344                         numSimulationSubSteps = int( m_localTime / fixedTimeStep);
345                         m_localTime -= numSimulationSubSteps * fixedTimeStep;
346                 }
347         } else
348         {
349                 //variable timestep
350                 fixedTimeStep = timeStep;
351                 m_localTime = timeStep;
352                 if (btFuzzyZero(timeStep))
353                 {
354                         numSimulationSubSteps = 0;
355                         maxSubSteps = 0;
356                 } else
357                 {
358                         numSimulationSubSteps = 1;
359                         maxSubSteps = 1;
360                 }
361         }
362
363         //process some debugging flags
364         if (getDebugDrawer())
365         {
366                 btIDebugDraw* debugDrawer = getDebugDrawer ();
367                 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
368         }
369         if (numSimulationSubSteps)
370         {
371
372                 saveKinematicState(fixedTimeStep);
373
374                 applyGravity();
375
376                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
377                 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;
378
379                 for (int i=0;i<clampedSimulationSteps;i++)
380                 {
381                         internalSingleStepSimulation(fixedTimeStep);
382                         synchronizeMotionStates();
383                 }
384
385         } 
386
387         synchronizeMotionStates();
388
389         clearForces();
390
391 #ifndef BT_NO_PROFILE
392         CProfileManager::Increment_Frame_Counter();
393 #endif //BT_NO_PROFILE
394         
395         return numSimulationSubSteps;
396 }
397
398 void    btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
399 {
400         
401         BT_PROFILE("internalSingleStepSimulation");
402
403         ///apply gravity, predict motion
404         predictUnconstraintMotion(timeStep);
405
406         btDispatcherInfo& dispatchInfo = getDispatchInfo();
407
408         dispatchInfo.m_timeStep = timeStep;
409         dispatchInfo.m_stepCount = 0;
410         dispatchInfo.m_debugDraw = getDebugDrawer();
411
412         ///perform collision detection
413         performDiscreteCollisionDetection();
414
415         calculateSimulationIslands();
416
417         
418         getSolverInfo().m_timeStep = timeStep;
419         
420
421
422         ///solve contact and other joint constraints
423         solveConstraints(getSolverInfo());
424         
425         ///CallbackTriggers();
426
427         ///integrate transforms
428         integrateTransforms(timeStep);
429
430         ///update vehicle simulation
431         updateVehicles(timeStep);
432         
433         updateCharacters(timeStep);
434
435         updateActivationState( timeStep );
436
437         if(0 != m_internalTickCallback) {
438                 (*m_internalTickCallback)(this, timeStep);
439         }       
440 }
441
442 void    btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)
443 {
444         m_gravity = gravity;
445         for ( int i=0;i<m_collisionObjects.size();i++)
446         {
447                 btCollisionObject* colObj = m_collisionObjects[i];
448                 btRigidBody* body = btRigidBody::upcast(colObj);
449                 if (body)
450                 {
451                         body->setGravity(gravity);
452                 }
453         }
454 }
455
456 btVector3 btDiscreteDynamicsWorld::getGravity () const
457 {
458         return m_gravity;
459 }
460
461
462 void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
463 {
464         removeCollisionObject(body);
465 }
466
467 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)
468 {
469         if (!body->isStaticOrKinematicObject())
470         {
471                 body->setGravity(m_gravity);
472         }
473
474         if (body->getCollisionShape())
475         {
476                 bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());
477                 short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
478                 short collisionFilterMask = isDynamic?  short(btBroadphaseProxy::AllFilter) :   short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
479
480                 addCollisionObject(body,collisionFilterGroup,collisionFilterMask);
481         }
482 }
483
484 void    btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)
485 {
486         if (!body->isStaticOrKinematicObject())
487         {
488                 body->setGravity(m_gravity);
489         }
490
491         if (body->getCollisionShape())
492         {
493                 addCollisionObject(body,group,mask);
494         }
495 }
496
497
498 void    btDiscreteDynamicsWorld::updateVehicles(btScalar timeStep)
499 {
500         BT_PROFILE("updateVehicles");
501         
502         for ( int i=0;i<m_vehicles.size();i++)
503         {
504                 btRaycastVehicle* vehicle = m_vehicles[i];
505                 vehicle->updateVehicle( timeStep);
506         }
507 }
508
509 void    btDiscreteDynamicsWorld::updateCharacters(btScalar timeStep)
510 {
511 #ifdef USE_CHARACTER
512         BT_PROFILE("updateCharacters");
513         
514         for ( int i=0;i<m_characters.size();i++)
515         {
516                 btCharacterControllerInterface* character = m_characters[i];
517                 character->preStep (this);
518                 character->playerStep (this,timeStep);
519         }
520 #endif //USE_CHARACTER
521 }
522
523         
524         
525 void    btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)
526 {
527         BT_PROFILE("updateActivationState");
528
529         for ( int i=0;i<m_collisionObjects.size();i++)
530         {
531                 btCollisionObject* colObj = m_collisionObjects[i];
532                 btRigidBody* body = btRigidBody::upcast(colObj);
533                 if (body)
534                 {
535                         body->updateDeactivation(timeStep);
536
537                         if (body->wantsSleeping())
538                         {
539                                 if (body->isStaticOrKinematicObject())
540                                 {
541                                         body->setActivationState(ISLAND_SLEEPING);
542                                 } else
543                                 {
544                                         if (body->getActivationState() == ACTIVE_TAG)
545                                                 body->setActivationState( WANTS_DEACTIVATION );
546                                         if (body->getActivationState() == ISLAND_SLEEPING) 
547                                         {
548                                                 body->setAngularVelocity(btVector3(0,0,0));
549                                                 body->setLinearVelocity(btVector3(0,0,0));
550                                         }
551
552                                 }
553                         } else
554                         {
555                                 if (body->getActivationState() != DISABLE_DEACTIVATION)
556                                         body->setActivationState( ACTIVE_TAG );
557                         }
558                 }
559         }
560 }
561
562 void    btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)
563 {
564         m_constraints.push_back(constraint);
565         if (disableCollisionsBetweenLinkedBodies)
566         {
567                 constraint->getRigidBodyA().addConstraintRef(constraint);
568                 constraint->getRigidBodyB().addConstraintRef(constraint);
569         }
570 }
571
572 void    btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
573 {
574         m_constraints.remove(constraint);
575         constraint->getRigidBodyA().removeConstraintRef(constraint);
576         constraint->getRigidBodyB().removeConstraintRef(constraint);
577 }
578
579 void    btDiscreteDynamicsWorld::addVehicle(btRaycastVehicle* vehicle)
580 {
581         m_vehicles.push_back(vehicle);
582 }
583
584 void    btDiscreteDynamicsWorld::removeVehicle(btRaycastVehicle* vehicle)
585 {
586         m_vehicles.remove(vehicle);
587 }
588
589 void    btDiscreteDynamicsWorld::addCharacter(btCharacterControllerInterface* character)
590 {
591 #ifdef USE_CHARACTER
592         m_characters.push_back(character);
593 #endif //USE_CHARACTER
594 }
595
596 void    btDiscreteDynamicsWorld::removeCharacter(btCharacterControllerInterface* character)
597 {
598 #ifdef USE_CHARACTER
599         m_characters.remove(character);
600 #endif //USE_CHARACTER
601 }
602
603
604 SIMD_FORCE_INLINE       int     btGetConstraintIslandId(const btTypedConstraint* lhs)
605 {
606         int islandId;
607         
608         const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();
609         const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();
610         islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();
611         return islandId;
612
613 }
614
615
616 class btSortConstraintOnIslandPredicate
617 {
618         public:
619
620                 bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )
621                 {
622                         int rIslandId0,lIslandId0;
623                         rIslandId0 = btGetConstraintIslandId(rhs);
624                         lIslandId0 = btGetConstraintIslandId(lhs);
625                         return lIslandId0 < rIslandId0;
626                 }
627 };
628
629
630
631
632 void    btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
633 {
634         BT_PROFILE("solveConstraints");
635         
636         struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
637         {
638
639                 btContactSolverInfo&    m_solverInfo;
640                 btConstraintSolver*             m_solver;
641                 btTypedConstraint**             m_sortedConstraints;
642                 int                                             m_numConstraints;
643                 btIDebugDraw*                   m_debugDrawer;
644                 btStackAlloc*                   m_stackAlloc;
645                 btDispatcher*                   m_dispatcher;
646
647                 InplaceSolverIslandCallback(
648                         btContactSolverInfo& solverInfo,
649                         btConstraintSolver*     solver,
650                         btTypedConstraint** sortedConstraints,
651                         int     numConstraints,
652                         btIDebugDraw*   debugDrawer,
653                         btStackAlloc*                   stackAlloc,
654                         btDispatcher* dispatcher)
655                         :m_solverInfo(solverInfo),
656                         m_solver(solver),
657                         m_sortedConstraints(sortedConstraints),
658                         m_numConstraints(numConstraints),
659                         m_debugDrawer(debugDrawer),
660                         m_stackAlloc(stackAlloc),
661                         m_dispatcher(dispatcher)
662                 {
663
664                 }
665
666                 InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)
667                 {
668                         btAssert(0);
669                         (void)other;
670                         return *this;
671                 }
672                 virtual void    ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold**   manifolds,int numManifolds, int islandId)
673                 {
674                         if (islandId<0)
675                         {
676                                 if (numManifolds + m_numConstraints)
677                                 {
678                                         ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
679                                         m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
680                                 }
681                         } else
682                         {
683                                         //also add all non-contact constraints/joints for this island
684                                 btTypedConstraint** startConstraint = 0;
685                                 int numCurConstraints = 0;
686                                 int i;
687                                 
688                                 //find the first constraint for this island
689                                 for (i=0;i<m_numConstraints;i++)
690                                 {
691                                         if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
692                                         {
693                                                 startConstraint = &m_sortedConstraints[i];
694                                                 break;
695                                         }
696                                 }
697                                 //count the number of constraints in this island
698                                 for (;i<m_numConstraints;i++)
699                                 {
700                                         if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)
701                                         {
702                                                 numCurConstraints++;
703                                         }
704                                 }
705
706                                 ///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive
707                                 if (numManifolds + numCurConstraints)
708                                 {
709                                         m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);
710                                 }
711                 
712                         }
713                 }
714
715         };
716
717         //sorted version of all btTypedConstraint, based on islandId
718         btAlignedObjectArray<btTypedConstraint*>        sortedConstraints;
719         sortedConstraints.resize( m_constraints.size());
720         int i; 
721         for (i=0;i<getNumConstraints();i++)
722         {
723                 sortedConstraints[i] = m_constraints[i];
724         }
725
726 //      btAssert(0);
727                 
728         
729
730         sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());
731         
732         btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;
733         
734         InplaceSolverIslandCallback     solverCallback( solverInfo,     m_constraintSolver, constraintsPtr,sortedConstraints.size(),    m_debugDrawer,m_stackAlloc,m_dispatcher1);
735         
736         m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());
737         
738         /// solve all the constraints for this island
739         m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);
740
741         m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);
742 }
743
744
745
746
747 void    btDiscreteDynamicsWorld::calculateSimulationIslands()
748 {
749         BT_PROFILE("calculateSimulationIslands");
750
751         getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());
752
753         {
754                 int i;
755                 int numConstraints = int(m_constraints.size());
756                 for (i=0;i< numConstraints ; i++ )
757                 {
758                         btTypedConstraint* constraint = m_constraints[i];
759
760                         const btRigidBody* colObj0 = &constraint->getRigidBodyA();
761                         const btRigidBody* colObj1 = &constraint->getRigidBodyB();
762
763                         if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&
764                                 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))
765                         {
766                                 if (colObj0->isActive() || colObj1->isActive())
767                                 {
768
769                                         getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
770                                                 (colObj1)->getIslandTag());
771                                 }
772                         }
773                 }
774         }
775
776         //Store the island id in each body
777         getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
778
779         
780 }
781
782
783 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
784
785 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
786 {
787         btCollisionObject* m_me;
788         btScalar m_allowedPenetration;
789         btOverlappingPairCache* m_pairCache;
790         btDispatcher* m_dispatcher;
791
792
793 public:
794         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
795           btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
796                 m_allowedPenetration(0.0f),
797                 m_me(me),
798                 m_pairCache(pairCache),
799                 m_dispatcher(dispatcher)
800         {
801         }
802
803         virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
804         {
805                 if (convexResult.m_hitCollisionObject == m_me)
806                         return 1.0f;
807
808                 //ignore result if there is no contact response
809                 if(!convexResult.m_hitCollisionObject->hasContactResponse())
810                         return 1.0f;
811
812                 btVector3 linVelA,linVelB;
813                 linVelA = m_convexToWorld-m_convexFromWorld;
814                 linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
815
816                 btVector3 relativeVelocity = (linVelA-linVelB);
817                 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
818                 if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
819                         return 1.f;
820
821                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
822         }
823
824         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
825         {
826                 //don't collide with itself
827                 if (proxy0->m_clientObject == m_me)
828                         return false;
829
830                 ///don't do CCD when the collision filters are not matching
831                 if (!ClosestConvexResultCallback::needsCollision(proxy0))
832                         return false;
833
834                 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
835
836                 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
837                 if (m_dispatcher->needsResponse(m_me,otherObj))
838                 {
839                         ///don't do CCD when there are already contact points (touching contact/penetration)
840                         btAlignedObjectArray<btPersistentManifold*> manifoldArray;
841                         btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
842                         if (collisionPair)
843                         {
844                                 if (collisionPair->m_algorithm)
845                                 {
846                                         manifoldArray.resize(0);
847                                         collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
848                                         for (int j=0;j<manifoldArray.size();j++)
849                                         {
850                                                 btPersistentManifold* manifold = manifoldArray[j];
851                                                 if (manifold->getNumContacts()>0)
852                                                         return false;
853                                         }
854                                 }
855                         }
856                 }
857                 return true;
858         }
859
860
861 };
862
863 ///internal debugging variable. this value shouldn't be too high
864 int gNumClampedCcdMotions=0;
865
866 //#include "stdio.h"
867 void    btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
868 {
869         BT_PROFILE("integrateTransforms");
870         btTransform predictedTrans;
871         for ( int i=0;i<m_collisionObjects.size();i++)
872         {
873                 btCollisionObject* colObj = m_collisionObjects[i];
874                 btRigidBody* body = btRigidBody::upcast(colObj);
875                 if (body)
876                 {
877                         body->setHitFraction(1.f);
878
879                         if (body->isActive() && (!body->isStaticOrKinematicObject()))
880                         {
881                                 body->predictIntegratedTransform(timeStep, predictedTrans);
882                                 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
883
884                                 if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
885                                 {
886                                         BT_PROFILE("CCD motion clamping");
887                                         if (body->getCollisionShape()->isConvex())
888                                         {
889                                                 gNumClampedCcdMotions++;
890                                                 
891                                                 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
892                                                 btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
893                                                 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
894
895                                                 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
896                                                 sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
897
898                                                 convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
899                                                 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
900                                                 {
901                                                         body->setHitFraction(sweepResults.m_closestHitFraction);
902                                                         body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
903                                                         body->setHitFraction(0.f);
904 //                                                      printf("clamped integration to hit fraction = %f\n",fraction);
905                                                 }
906                                         }
907                                 }
908                                 
909                                 body->proceedToTransform( predictedTrans);
910                         }
911                 }
912         }
913 }
914
915
916
917
918
919 void    btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
920 {
921         BT_PROFILE("predictUnconstraintMotion");
922         for ( int i=0;i<m_collisionObjects.size();i++)
923         {
924                 btCollisionObject* colObj = m_collisionObjects[i];
925                 btRigidBody* body = btRigidBody::upcast(colObj);
926                 if (body)
927                 {
928                         if (!body->isStaticOrKinematicObject())
929                         {
930                                 
931                                 body->integrateVelocities( timeStep);
932                                 //damping
933                                 body->applyDamping(timeStep);
934
935                                 body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
936                         }
937                 }
938         }
939 }
940
941
942 void    btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
943 {
944         (void)timeStep;
945
946 #ifndef BT_NO_PROFILE
947         CProfileManager::Reset();
948 #endif //BT_NO_PROFILE
949
950 }
951
952
953
954
955         
956
957 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
958 {
959         btIDebugDraw*   m_debugDrawer;
960         btVector3       m_color;
961         btTransform     m_worldTrans;
962
963 public:
964
965         DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
966                 m_debugDrawer(debugDrawer),
967                 m_color(color),
968                 m_worldTrans(worldTrans)
969         {
970         }
971
972         virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
973         {
974                 processTriangle(triangle,partId,triangleIndex);
975         }
976
977         virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
978         {
979                 (void)partId;
980                 (void)triangleIndex;
981
982                 btVector3 wv0,wv1,wv2;
983                 wv0 = m_worldTrans*triangle[0];
984                 wv1 = m_worldTrans*triangle[1];
985                 wv2 = m_worldTrans*triangle[2];
986                 m_debugDrawer->drawLine(wv0,wv1,m_color);
987                 m_debugDrawer->drawLine(wv1,wv2,m_color);
988                 m_debugDrawer->drawLine(wv2,wv0,m_color);
989         }
990 };
991
992 void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
993 {
994         btVector3 start = transform.getOrigin();
995
996         const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
997         const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
998         const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
999
1000         // XY 
1001         getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
1002         getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
1003         getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
1004         getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
1005
1006         // XZ
1007         getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
1008         getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
1009         getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
1010         getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
1011
1012         // YZ
1013         getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
1014         getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
1015         getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
1016         getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
1017 }
1018
1019 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
1020 {
1021         // Draw a small simplex at the center of the object
1022         {
1023                 btVector3 start = worldTransform.getOrigin();
1024                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
1025                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
1026                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
1027         }
1028
1029         if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
1030         {
1031                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
1032                 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
1033                 {
1034                         btTransform childTrans = compoundShape->getChildTransform(i);
1035                         const btCollisionShape* colShape = compoundShape->getChildShape(i);
1036                         debugDrawObject(worldTransform*childTrans,colShape,color);
1037                 }
1038
1039         } else
1040         {
1041                 switch (shape->getShapeType())
1042                 {
1043
1044                 case SPHERE_SHAPE_PROXYTYPE:
1045                         {
1046                                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1047                                 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1048                                 
1049                                 debugDrawSphere(radius, worldTransform, color);
1050                                 break;
1051                         }
1052                 case MULTI_SPHERE_SHAPE_PROXYTYPE:
1053                         {
1054                                 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1055
1056                                 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1057                                 {
1058                                         btTransform childTransform = worldTransform;
1059                                         childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
1060                                         debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
1061                                 }
1062
1063                                 break;
1064                         }
1065                 case CAPSULE_SHAPE_PROXYTYPE:
1066                         {
1067                                 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1068
1069                                 btScalar radius = capsuleShape->getRadius();
1070                                 btScalar halfHeight = capsuleShape->getHalfHeight();
1071                                 
1072                                 int upAxis = capsuleShape->getUpAxis();
1073
1074                                 
1075                                 btVector3 capStart(0.f,0.f,0.f);
1076                                 capStart[upAxis] = -halfHeight;
1077
1078                                 btVector3 capEnd(0.f,0.f,0.f);
1079                                 capEnd[upAxis] = halfHeight;
1080
1081                                 // Draw the ends
1082                                 {
1083                                         
1084                                         btTransform childTransform = worldTransform;
1085                                         childTransform.getOrigin() = worldTransform * capStart;
1086                                         debugDrawSphere(radius, childTransform, color);
1087                                 }
1088
1089                                 {
1090                                         btTransform childTransform = worldTransform;
1091                                         childTransform.getOrigin() = worldTransform * capEnd;
1092                                         debugDrawSphere(radius, childTransform, color);
1093                                 }
1094
1095                                 // Draw some additional lines
1096                                 btVector3 start = worldTransform.getOrigin();
1097
1098                                 
1099                                 capStart[(upAxis+1)%3] = radius;
1100                                 capEnd[(upAxis+1)%3] = radius;
1101                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1102                                 capStart[(upAxis+1)%3] = -radius;
1103                                 capEnd[(upAxis+1)%3] = -radius;
1104                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1105
1106                                 capStart[(upAxis+1)%3] = 0.f;
1107                                 capEnd[(upAxis+1)%3] = 0.f;
1108
1109                                 capStart[(upAxis+2)%3] = radius;
1110                                 capEnd[(upAxis+2)%3] = radius;
1111                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1112                                 capStart[(upAxis+2)%3] = -radius;
1113                                 capEnd[(upAxis+2)%3] = -radius;
1114                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1115
1116                                 
1117                                 break;
1118                         }
1119                 case CONE_SHAPE_PROXYTYPE:
1120                         {
1121                                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1122                                 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1123                                 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1124                                 btVector3 start = worldTransform.getOrigin();
1125
1126                                 int upAxis= coneShape->getConeUpIndex();
1127                                 
1128
1129                                 btVector3       offsetHeight(0,0,0);
1130                                 offsetHeight[upAxis] = height * btScalar(0.5);
1131                                 btVector3       offsetRadius(0,0,0);
1132                                 offsetRadius[(upAxis+1)%3] = radius;
1133                                 btVector3       offset2Radius(0,0,0);
1134                                 offset2Radius[(upAxis+2)%3] = radius;
1135
1136                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
1137                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
1138                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
1139                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
1140
1141
1142
1143                                 break;
1144
1145                         }
1146                 case CYLINDER_SHAPE_PROXYTYPE:
1147                         {
1148                                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1149                                 int upAxis = cylinder->getUpAxis();
1150                                 btScalar radius = cylinder->getRadius();
1151                                 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1152                                 btVector3 start = worldTransform.getOrigin();
1153                                 btVector3       offsetHeight(0,0,0);
1154                                 offsetHeight[upAxis] = halfHeight;
1155                                 btVector3       offsetRadius(0,0,0);
1156                                 offsetRadius[(upAxis+1)%3] = radius;
1157                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
1158                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
1159                                 break;
1160                         }
1161
1162                         case STATIC_PLANE_PROXYTYPE:
1163                                 {
1164                                         const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1165                                         btScalar planeConst = staticPlaneShape->getPlaneConstant();
1166                                         const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1167                                         btVector3 planeOrigin = planeNormal * planeConst;
1168                                         btVector3 vec0,vec1;
1169                                         btPlaneSpace1(planeNormal,vec0,vec1);
1170                                         btScalar vecLen = 100.f;
1171                                         btVector3 pt0 = planeOrigin + vec0*vecLen;
1172                                         btVector3 pt1 = planeOrigin - vec0*vecLen;
1173                                         btVector3 pt2 = planeOrigin + vec1*vecLen;
1174                                         btVector3 pt3 = planeOrigin - vec1*vecLen;
1175                                         getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
1176                                         getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
1177                                         break;
1178
1179                                 }
1180                 default:
1181                         {
1182
1183                                 if (shape->isConcave())
1184                                 {
1185                                         btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1186                                         
1187                                         ///@todo pass camera, for some culling? no -> we are not a graphics lib
1188                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
1189                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
1190
1191                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1192                                         concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1193
1194                                 }
1195
1196                                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1197                                 {
1198                                         btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1199                                         //todo: pass camera for some culling                    
1200                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
1201                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
1202                                         //DebugDrawcallback drawCallback;
1203                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1204                                         convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1205                                 }
1206
1207
1208                                 /// for polyhedral shapes
1209                                 if (shape->isPolyhedral())
1210                                 {
1211                                         btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1212
1213                                         int i;
1214                                         for (i=0;i<polyshape->getNumEdges();i++)
1215                                         {
1216                                                 btVector3 a,b;
1217                                                 polyshape->getEdge(i,a,b);
1218                                                 btVector3 wa = worldTransform * a;
1219                                                 btVector3 wb = worldTransform * b;
1220                                                 getDebugDrawer()->drawLine(wa,wb,color);
1221
1222                                         }
1223
1224                                         
1225                                 }
1226                         }
1227                 }
1228         }
1229 }
1230
1231
1232 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
1233 {
1234         bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
1235         bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
1236         btScalar dbgDrawSize = constraint->getDbgDrawSize();
1237         if(dbgDrawSize <= btScalar(0.f))
1238         {
1239                 return;
1240         }
1241
1242         switch(constraint->getConstraintType())
1243         {
1244                 case POINT2POINT_CONSTRAINT_TYPE:
1245                         {
1246                                 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
1247                                 btTransform tr;
1248                                 tr.setIdentity();
1249                                 btVector3 pivot = p2pC->getPivotInA();
1250                                 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
1251                                 tr.setOrigin(pivot);
1252                                 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1253                                 // that ideally should draw the same frame      
1254                                 pivot = p2pC->getPivotInB();
1255                                 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
1256                                 tr.setOrigin(pivot);
1257                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1258                         }
1259                         break;
1260                 case HINGE_CONSTRAINT_TYPE:
1261                         {
1262                                 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
1263                                 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
1264                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1265                                 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
1266                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1267                                 btScalar minAng = pHinge->getLowerLimit();
1268                                 btScalar maxAng = pHinge->getUpperLimit();
1269                                 if(minAng == maxAng)
1270                                 {
1271                                         break;
1272                                 }
1273                                 bool drawSect = true;
1274                                 if(minAng > maxAng)
1275                                 {
1276                                         minAng = btScalar(0.f);
1277                                         maxAng = SIMD_2_PI;
1278                                         drawSect = false;
1279                                 }
1280                                 if(drawLimits) 
1281                                 {
1282                                         btVector3& center = tr.getOrigin();
1283                                         btVector3 normal = tr.getBasis().getColumn(2);
1284                                         btVector3 axis = tr.getBasis().getColumn(0);
1285                                         getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
1286                                 }
1287                         }
1288                         break;
1289                 case CONETWIST_CONSTRAINT_TYPE:
1290                         {
1291                                 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
1292                                 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1293                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1294                                 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1295                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1296                                 if(drawLimits)
1297                                 {
1298                                         //const btScalar length = btScalar(5);
1299                                         const btScalar length = dbgDrawSize;
1300                                         static int nSegments = 8*4;
1301                                         btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
1302                                         btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
1303                                         pPrev = tr * pPrev;
1304                                         for (int i=0; i<nSegments; i++)
1305                                         {
1306                                                 fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
1307                                                 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
1308                                                 pCur = tr * pCur;
1309                                                 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
1310
1311                                                 if (i%(nSegments/8) == 0)
1312                                                         getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
1313
1314                                                 pPrev = pCur;
1315                                         }                                               
1316                                         btScalar tws = pCT->getTwistSpan();
1317                                         btScalar twa = pCT->getTwistAngle();
1318                                         bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
1319                                         if(useFrameB)
1320                                         {
1321                                                 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1322                                         }
1323                                         else
1324                                         {
1325                                                 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1326                                         }
1327                                         btVector3 pivot = tr.getOrigin();
1328                                         btVector3 normal = tr.getBasis().getColumn(0);
1329                                         btVector3 axis1 = tr.getBasis().getColumn(1);
1330                                         getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
1331
1332                                 }
1333                         }
1334                         break;
1335                 case D6_CONSTRAINT_TYPE:
1336                         {
1337                                 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
1338                                 btTransform tr = p6DOF->getCalculatedTransformA();
1339                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1340                                 tr = p6DOF->getCalculatedTransformB();
1341                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1342                                 if(drawLimits) 
1343                                 {
1344                                         tr = p6DOF->getCalculatedTransformA();
1345                                         const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1346                                         btVector3 up = tr.getBasis().getColumn(2);
1347                                         btVector3 axis = tr.getBasis().getColumn(0);
1348                                         btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1349                                         btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1350                                         btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1351                                         btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1352                                         getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
1353                                         axis = tr.getBasis().getColumn(1);
1354                                         btScalar ay = p6DOF->getAngle(1);
1355                                         btScalar az = p6DOF->getAngle(2);
1356                                         btScalar cy = btCos(ay);
1357                                         btScalar sy = btSin(ay);
1358                                         btScalar cz = btCos(az);
1359                                         btScalar sz = btSin(az);
1360                                         btVector3 ref;
1361                                         ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
1362                                         ref[1] = -sz*axis[0] + cz*axis[1];
1363                                         ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
1364                                         tr = p6DOF->getCalculatedTransformB();
1365                                         btVector3 normal = -tr.getBasis().getColumn(0);
1366                                         btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1367                                         btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1368                                         if(minFi > maxFi)
1369                                         {
1370                                                 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
1371                                         }
1372                                         else if(minFi < maxFi)
1373                                         {
1374                                                 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
1375                                         }
1376                                         tr = p6DOF->getCalculatedTransformA();
1377                                         btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1378                                         btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1379                                         getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
1380                                 }
1381                         }
1382                         break;
1383                 case SLIDER_CONSTRAINT_TYPE:
1384                         {
1385                                 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
1386                                 btTransform tr = pSlider->getCalculatedTransformA();
1387                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1388                                 tr = pSlider->getCalculatedTransformB();
1389                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1390                                 if(drawLimits)
1391                                 {
1392                                         btTransform tr = pSlider->getCalculatedTransformA();
1393                                         btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
1394                                         btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
1395                                         getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
1396                                         btVector3 normal = tr.getBasis().getColumn(0);
1397                                         btVector3 axis = tr.getBasis().getColumn(1);
1398                                         btScalar a_min = pSlider->getLowerAngLimit();
1399                                         btScalar a_max = pSlider->getUpperAngLimit();
1400                                         const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
1401                                         getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
1402                                 }
1403                         }
1404                         break;
1405                 default : 
1406                         break;
1407         }
1408         return;
1409 } // btDiscreteDynamicsWorld::debugDrawConstraint()
1410
1411
1412
1413
1414
1415 void    btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
1416 {
1417         if (m_ownsConstraintSolver)
1418         {
1419                 btAlignedFree( m_constraintSolver);
1420         }
1421         m_ownsConstraintSolver = false;
1422         m_constraintSolver = solver;
1423 }
1424
1425 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
1426 {
1427         return m_constraintSolver;
1428 }
1429
1430
1431 int             btDiscreteDynamicsWorld::getNumConstraints() const
1432 {
1433         return int(m_constraints.size());
1434 }
1435 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
1436 {
1437         return m_constraints[index];
1438 }
1439 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
1440 {
1441         return m_constraints[index];
1442 }
1443
1444