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