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