moved more attributes from getattr into PyAttributeDef's
[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
729                                         getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),
730                                                 (colObj1)->getIslandTag());
731                                 }
732                         }
733                 }
734         }
735
736         //Store the island id in each body
737         getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());
738
739         
740 }
741
742
743 #include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"
744
745 class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
746 {
747         btCollisionObject* m_me;
748         btScalar m_allowedPenetration;
749         btOverlappingPairCache* m_pairCache;
750         btDispatcher* m_dispatcher;
751
752
753 public:
754         btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) : 
755           btCollisionWorld::ClosestConvexResultCallback(fromA,toA),
756                 m_allowedPenetration(0.0f),
757                 m_me(me),
758                 m_pairCache(pairCache),
759                 m_dispatcher(dispatcher)
760         {
761         }
762
763         virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)
764         {
765                 if (convexResult.m_hitCollisionObject == m_me)
766                         return 1.0f;
767
768                 //ignore result if there is no contact response
769                 if(!convexResult.m_hitCollisionObject->hasContactResponse())
770                         return 1.0f;
771
772                 btVector3 linVelA,linVelB;
773                 linVelA = m_convexToWorld-m_convexFromWorld;
774                 linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();
775
776                 btVector3 relativeVelocity = (linVelA-linVelB);
777                 //don't report time of impact for motion away from the contact normal (or causes minor penetration)
778                 if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)
779                         return 1.f;
780
781                 return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);
782         }
783
784         virtual bool needsCollision(btBroadphaseProxy* proxy0) const
785         {
786                 //don't collide with itself
787                 if (proxy0->m_clientObject == m_me)
788                         return false;
789
790                 ///don't do CCD when the collision filters are not matching
791                 if (!ClosestConvexResultCallback::needsCollision(proxy0))
792                         return false;
793
794                 btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;
795
796                 //call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179
797                 if (m_dispatcher->needsResponse(m_me,otherObj))
798                 {
799                         ///don't do CCD when there are already contact points (touching contact/penetration)
800                         btAlignedObjectArray<btPersistentManifold*> manifoldArray;
801                         btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);
802                         if (collisionPair)
803                         {
804                                 if (collisionPair->m_algorithm)
805                                 {
806                                         manifoldArray.resize(0);
807                                         collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);
808                                         for (int j=0;j<manifoldArray.size();j++)
809                                         {
810                                                 btPersistentManifold* manifold = manifoldArray[j];
811                                                 if (manifold->getNumContacts()>0)
812                                                         return false;
813                                         }
814                                 }
815                         }
816                 }
817                 return true;
818         }
819
820
821 };
822
823 ///internal debugging variable. this value shouldn't be too high
824 int gNumClampedCcdMotions=0;
825
826 //#include "stdio.h"
827 void    btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)
828 {
829         BT_PROFILE("integrateTransforms");
830         btTransform predictedTrans;
831         for ( int i=0;i<m_collisionObjects.size();i++)
832         {
833                 btCollisionObject* colObj = m_collisionObjects[i];
834                 btRigidBody* body = btRigidBody::upcast(colObj);
835                 if (body)
836                 {
837                         body->setHitFraction(1.f);
838
839                         if (body->isActive() && (!body->isStaticOrKinematicObject()))
840                         {
841                                 body->predictIntegratedTransform(timeStep, predictedTrans);
842                                 btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();
843
844                                 if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)
845                                 {
846                                         BT_PROFILE("CCD motion clamping");
847                                         if (body->getCollisionShape()->isConvex())
848                                         {
849                                                 gNumClampedCcdMotions++;
850                                                 
851                                                 btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());
852                                                 btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
853                                                 btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());
854
855                                                 sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;
856                                                 sweepResults.m_collisionFilterMask  = body->getBroadphaseProxy()->m_collisionFilterMask;
857
858                                                 convexSweepTest(&tmpSphere,body->getWorldTransform(),predictedTrans,sweepResults);
859                                                 if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))
860                                                 {
861                                                         body->setHitFraction(sweepResults.m_closestHitFraction);
862                                                         body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);
863                                                         body->setHitFraction(0.f);
864 //                                                      printf("clamped integration to hit fraction = %f\n",fraction);
865                                                 }
866                                         }
867                                 }
868                                 
869                                 body->proceedToTransform( predictedTrans);
870                         }
871                 }
872         }
873 }
874
875
876
877
878
879 void    btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
880 {
881         BT_PROFILE("predictUnconstraintMotion");
882         for ( int i=0;i<m_collisionObjects.size();i++)
883         {
884                 btCollisionObject* colObj = m_collisionObjects[i];
885                 btRigidBody* body = btRigidBody::upcast(colObj);
886                 if (body)
887                 {
888                         if (!body->isStaticOrKinematicObject())
889                         {
890                                 
891                                 body->integrateVelocities( timeStep);
892                                 //damping
893                                 body->applyDamping(timeStep);
894
895                                 body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());
896                         }
897                 }
898         }
899 }
900
901
902 void    btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)
903 {
904         (void)timeStep;
905
906 #ifndef BT_NO_PROFILE
907         CProfileManager::Reset();
908 #endif //BT_NO_PROFILE
909
910 }
911
912
913
914
915         
916
917 class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
918 {
919         btIDebugDraw*   m_debugDrawer;
920         btVector3       m_color;
921         btTransform     m_worldTrans;
922
923 public:
924
925         DebugDrawcallback(btIDebugDraw* debugDrawer,const btTransform& worldTrans,const btVector3& color) :
926                 m_debugDrawer(debugDrawer),
927                 m_color(color),
928                 m_worldTrans(worldTrans)
929         {
930         }
931
932         virtual void internalProcessTriangleIndex(btVector3* triangle,int partId,int  triangleIndex)
933         {
934                 processTriangle(triangle,partId,triangleIndex);
935         }
936
937         virtual void processTriangle(btVector3* triangle,int partId, int triangleIndex)
938         {
939                 (void)partId;
940                 (void)triangleIndex;
941
942                 btVector3 wv0,wv1,wv2;
943                 wv0 = m_worldTrans*triangle[0];
944                 wv1 = m_worldTrans*triangle[1];
945                 wv2 = m_worldTrans*triangle[2];
946                 m_debugDrawer->drawLine(wv0,wv1,m_color);
947                 m_debugDrawer->drawLine(wv1,wv2,m_color);
948                 m_debugDrawer->drawLine(wv2,wv0,m_color);
949         }
950 };
951
952 void btDiscreteDynamicsWorld::debugDrawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
953 {
954         btVector3 start = transform.getOrigin();
955
956         const btVector3 xoffs = transform.getBasis() * btVector3(radius,0,0);
957         const btVector3 yoffs = transform.getBasis() * btVector3(0,radius,0);
958         const btVector3 zoffs = transform.getBasis() * btVector3(0,0,radius);
959
960         // XY 
961         getDebugDrawer()->drawLine(start-xoffs, start+yoffs, color);
962         getDebugDrawer()->drawLine(start+yoffs, start+xoffs, color);
963         getDebugDrawer()->drawLine(start+xoffs, start-yoffs, color);
964         getDebugDrawer()->drawLine(start-yoffs, start-xoffs, color);
965
966         // XZ
967         getDebugDrawer()->drawLine(start-xoffs, start+zoffs, color);
968         getDebugDrawer()->drawLine(start+zoffs, start+xoffs, color);
969         getDebugDrawer()->drawLine(start+xoffs, start-zoffs, color);
970         getDebugDrawer()->drawLine(start-zoffs, start-xoffs, color);
971
972         // YZ
973         getDebugDrawer()->drawLine(start-yoffs, start+zoffs, color);
974         getDebugDrawer()->drawLine(start+zoffs, start+yoffs, color);
975         getDebugDrawer()->drawLine(start+yoffs, start-zoffs, color);
976         getDebugDrawer()->drawLine(start-zoffs, start-yoffs, color);
977 }
978
979 void btDiscreteDynamicsWorld::debugDrawObject(const btTransform& worldTransform, const btCollisionShape* shape, const btVector3& color)
980 {
981         // Draw a small simplex at the center of the object
982         {
983                 btVector3 start = worldTransform.getOrigin();
984                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(1,0,0), btVector3(1,0,0));
985                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,1,0), btVector3(0,1,0));
986                 getDebugDrawer()->drawLine(start, start+worldTransform.getBasis() * btVector3(0,0,1), btVector3(0,0,1));
987         }
988
989         if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
990         {
991                 const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
992                 for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
993                 {
994                         btTransform childTrans = compoundShape->getChildTransform(i);
995                         const btCollisionShape* colShape = compoundShape->getChildShape(i);
996                         debugDrawObject(worldTransform*childTrans,colShape,color);
997                 }
998
999         } else
1000         {
1001                 switch (shape->getShapeType())
1002                 {
1003
1004                 case SPHERE_SHAPE_PROXYTYPE:
1005                         {
1006                                 const btSphereShape* sphereShape = static_cast<const btSphereShape*>(shape);
1007                                 btScalar radius = sphereShape->getMargin();//radius doesn't include the margin, so draw with margin
1008                                 
1009                                 debugDrawSphere(radius, worldTransform, color);
1010                                 break;
1011                         }
1012                 case MULTI_SPHERE_SHAPE_PROXYTYPE:
1013                         {
1014                                 const btMultiSphereShape* multiSphereShape = static_cast<const btMultiSphereShape*>(shape);
1015
1016                                 for (int i = multiSphereShape->getSphereCount()-1; i>=0;i--)
1017                                 {
1018                                         btTransform childTransform = worldTransform;
1019                                         childTransform.getOrigin() += multiSphereShape->getSpherePosition(i);
1020                                         debugDrawSphere(multiSphereShape->getSphereRadius(i), childTransform, color);
1021                                 }
1022
1023                                 break;
1024                         }
1025                 case CAPSULE_SHAPE_PROXYTYPE:
1026                         {
1027                                 const btCapsuleShape* capsuleShape = static_cast<const btCapsuleShape*>(shape);
1028
1029                                 btScalar radius = capsuleShape->getRadius();
1030                                 btScalar halfHeight = capsuleShape->getHalfHeight();
1031                                 
1032                                 int upAxis = capsuleShape->getUpAxis();
1033
1034                                 
1035                                 btVector3 capStart(0.f,0.f,0.f);
1036                                 capStart[upAxis] = -halfHeight;
1037
1038                                 btVector3 capEnd(0.f,0.f,0.f);
1039                                 capEnd[upAxis] = halfHeight;
1040
1041                                 // Draw the ends
1042                                 {
1043                                         
1044                                         btTransform childTransform = worldTransform;
1045                                         childTransform.getOrigin() = worldTransform * capStart;
1046                                         debugDrawSphere(radius, childTransform, color);
1047                                 }
1048
1049                                 {
1050                                         btTransform childTransform = worldTransform;
1051                                         childTransform.getOrigin() = worldTransform * capEnd;
1052                                         debugDrawSphere(radius, childTransform, color);
1053                                 }
1054
1055                                 // Draw some additional lines
1056                                 btVector3 start = worldTransform.getOrigin();
1057
1058                                 
1059                                 capStart[(upAxis+1)%3] = radius;
1060                                 capEnd[(upAxis+1)%3] = radius;
1061                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1062                                 capStart[(upAxis+1)%3] = -radius;
1063                                 capEnd[(upAxis+1)%3] = -radius;
1064                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1065
1066                                 capStart[(upAxis+1)%3] = 0.f;
1067                                 capEnd[(upAxis+1)%3] = 0.f;
1068
1069                                 capStart[(upAxis+2)%3] = radius;
1070                                 capEnd[(upAxis+2)%3] = radius;
1071                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1072                                 capStart[(upAxis+2)%3] = -radius;
1073                                 capEnd[(upAxis+2)%3] = -radius;
1074                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * capStart,start+worldTransform.getBasis() * capEnd, color);
1075
1076                                 
1077                                 break;
1078                         }
1079                 case CONE_SHAPE_PROXYTYPE:
1080                         {
1081                                 const btConeShape* coneShape = static_cast<const btConeShape*>(shape);
1082                                 btScalar radius = coneShape->getRadius();//+coneShape->getMargin();
1083                                 btScalar height = coneShape->getHeight();//+coneShape->getMargin();
1084                                 btVector3 start = worldTransform.getOrigin();
1085
1086                                 int upAxis= coneShape->getConeUpIndex();
1087                                 
1088
1089                                 btVector3       offsetHeight(0,0,0);
1090                                 offsetHeight[upAxis] = height * btScalar(0.5);
1091                                 btVector3       offsetRadius(0,0,0);
1092                                 offsetRadius[(upAxis+1)%3] = radius;
1093                                 btVector3       offset2Radius(0,0,0);
1094                                 offset2Radius[(upAxis+2)%3] = radius;
1095
1096                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
1097                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
1098                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight+offset2Radius),color);
1099                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight),start+worldTransform.getBasis() * (-offsetHeight-offset2Radius),color);
1100
1101
1102
1103                                 break;
1104
1105                         }
1106                 case CYLINDER_SHAPE_PROXYTYPE:
1107                         {
1108                                 const btCylinderShape* cylinder = static_cast<const btCylinderShape*>(shape);
1109                                 int upAxis = cylinder->getUpAxis();
1110                                 btScalar radius = cylinder->getRadius();
1111                                 btScalar halfHeight = cylinder->getHalfExtentsWithMargin()[upAxis];
1112                                 btVector3 start = worldTransform.getOrigin();
1113                                 btVector3       offsetHeight(0,0,0);
1114                                 offsetHeight[upAxis] = halfHeight;
1115                                 btVector3       offsetRadius(0,0,0);
1116                                 offsetRadius[(upAxis+1)%3] = radius;
1117                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight+offsetRadius),start+worldTransform.getBasis() * (-offsetHeight+offsetRadius),color);
1118                                 getDebugDrawer()->drawLine(start+worldTransform.getBasis() * (offsetHeight-offsetRadius),start+worldTransform.getBasis() * (-offsetHeight-offsetRadius),color);
1119                                 break;
1120                         }
1121
1122                         case STATIC_PLANE_PROXYTYPE:
1123                                 {
1124                                         const btStaticPlaneShape* staticPlaneShape = static_cast<const btStaticPlaneShape*>(shape);
1125                                         btScalar planeConst = staticPlaneShape->getPlaneConstant();
1126                                         const btVector3& planeNormal = staticPlaneShape->getPlaneNormal();
1127                                         btVector3 planeOrigin = planeNormal * planeConst;
1128                                         btVector3 vec0,vec1;
1129                                         btPlaneSpace1(planeNormal,vec0,vec1);
1130                                         btScalar vecLen = 100.f;
1131                                         btVector3 pt0 = planeOrigin + vec0*vecLen;
1132                                         btVector3 pt1 = planeOrigin - vec0*vecLen;
1133                                         btVector3 pt2 = planeOrigin + vec1*vecLen;
1134                                         btVector3 pt3 = planeOrigin - vec1*vecLen;
1135                                         getDebugDrawer()->drawLine(worldTransform*pt0,worldTransform*pt1,color);
1136                                         getDebugDrawer()->drawLine(worldTransform*pt2,worldTransform*pt3,color);
1137                                         break;
1138
1139                                 }
1140                 default:
1141                         {
1142
1143                                 if (shape->isConcave())
1144                                 {
1145                                         btConcaveShape* concaveMesh = (btConcaveShape*) shape;
1146                                         
1147                                         ///@todo pass camera, for some culling? no -> we are not a graphics lib
1148                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
1149                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
1150
1151                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1152                                         concaveMesh->processAllTriangles(&drawCallback,aabbMin,aabbMax);
1153
1154                                 }
1155
1156                                 if (shape->getShapeType() == CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE)
1157                                 {
1158                                         btConvexTriangleMeshShape* convexMesh = (btConvexTriangleMeshShape*) shape;
1159                                         //todo: pass camera for some culling                    
1160                                         btVector3 aabbMax(btScalar(1e30),btScalar(1e30),btScalar(1e30));
1161                                         btVector3 aabbMin(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
1162                                         //DebugDrawcallback drawCallback;
1163                                         DebugDrawcallback drawCallback(getDebugDrawer(),worldTransform,color);
1164                                         convexMesh->getMeshInterface()->InternalProcessAllTriangles(&drawCallback,aabbMin,aabbMax);
1165                                 }
1166
1167
1168                                 /// for polyhedral shapes
1169                                 if (shape->isPolyhedral())
1170                                 {
1171                                         btPolyhedralConvexShape* polyshape = (btPolyhedralConvexShape*) shape;
1172
1173                                         int i;
1174                                         for (i=0;i<polyshape->getNumEdges();i++)
1175                                         {
1176                                                 btVector3 a,b;
1177                                                 polyshape->getEdge(i,a,b);
1178                                                 btVector3 wa = worldTransform * a;
1179                                                 btVector3 wb = worldTransform * b;
1180                                                 getDebugDrawer()->drawLine(wa,wb,color);
1181
1182                                         }
1183
1184                                         
1185                                 }
1186                         }
1187                 }
1188         }
1189 }
1190
1191
1192 void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)
1193 {
1194         bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;
1195         bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;
1196         btScalar dbgDrawSize = constraint->getDbgDrawSize();
1197         if(dbgDrawSize <= btScalar(0.f))
1198         {
1199                 return;
1200         }
1201
1202         switch(constraint->getConstraintType())
1203         {
1204                 case POINT2POINT_CONSTRAINT_TYPE:
1205                         {
1206                                 btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;
1207                                 btTransform tr;
1208                                 tr.setIdentity();
1209                                 btVector3 pivot = p2pC->getPivotInA();
1210                                 pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot; 
1211                                 tr.setOrigin(pivot);
1212                                 getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1213                                 // that ideally should draw the same frame      
1214                                 pivot = p2pC->getPivotInB();
1215                                 pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot; 
1216                                 tr.setOrigin(pivot);
1217                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1218                         }
1219                         break;
1220                 case HINGE_CONSTRAINT_TYPE:
1221                         {
1222                                 btHingeConstraint* pHinge = (btHingeConstraint*)constraint;
1223                                 btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();
1224                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1225                                 tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();
1226                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1227                                 btScalar minAng = pHinge->getLowerLimit();
1228                                 btScalar maxAng = pHinge->getUpperLimit();
1229                                 if(minAng == maxAng)
1230                                 {
1231                                         break;
1232                                 }
1233                                 bool drawSect = true;
1234                                 if(minAng > maxAng)
1235                                 {
1236                                         minAng = btScalar(0.f);
1237                                         maxAng = SIMD_2_PI;
1238                                         drawSect = false;
1239                                 }
1240                                 if(drawLimits) 
1241                                 {
1242                                         btVector3& center = tr.getOrigin();
1243                                         btVector3 normal = tr.getBasis().getColumn(2);
1244                                         btVector3 axis = tr.getBasis().getColumn(0);
1245                                         getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);
1246                                 }
1247                         }
1248                         break;
1249                 case CONETWIST_CONSTRAINT_TYPE:
1250                         {
1251                                 btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;
1252                                 btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1253                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1254                                 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1255                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1256                                 if(drawLimits)
1257                                 {
1258                                         //const btScalar length = btScalar(5);
1259                                         const btScalar length = dbgDrawSize;
1260                                         static int nSegments = 8*4;
1261                                         btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);
1262                                         btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);
1263                                         pPrev = tr * pPrev;
1264                                         for (int i=0; i<nSegments; i++)
1265                                         {
1266                                                 fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);
1267                                                 btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);
1268                                                 pCur = tr * pCur;
1269                                                 getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));
1270
1271                                                 if (i%(nSegments/8) == 0)
1272                                                         getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));
1273
1274                                                 pPrev = pCur;
1275                                         }                                               
1276                                         btScalar tws = pCT->getTwistSpan();
1277                                         btScalar twa = pCT->getTwistAngle();
1278                                         bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));
1279                                         if(useFrameB)
1280                                         {
1281                                                 tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();
1282                                         }
1283                                         else
1284                                         {
1285                                                 tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();
1286                                         }
1287                                         btVector3 pivot = tr.getOrigin();
1288                                         btVector3 normal = tr.getBasis().getColumn(0);
1289                                         btVector3 axis1 = tr.getBasis().getColumn(1);
1290                                         getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);
1291
1292                                 }
1293                         }
1294                         break;
1295                 case D6_CONSTRAINT_TYPE:
1296                         {
1297                                 btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;
1298                                 btTransform tr = p6DOF->getCalculatedTransformA();
1299                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1300                                 tr = p6DOF->getCalculatedTransformB();
1301                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1302                                 if(drawLimits) 
1303                                 {
1304                                         tr = p6DOF->getCalculatedTransformA();
1305                                         const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();
1306                                         btVector3 up = tr.getBasis().getColumn(2);
1307                                         btVector3 axis = tr.getBasis().getColumn(0);
1308                                         btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;
1309                                         btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;
1310                                         btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;
1311                                         btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;
1312                                         getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));
1313                                         axis = tr.getBasis().getColumn(1);
1314                                         btScalar ay = p6DOF->getAngle(1);
1315                                         btScalar az = p6DOF->getAngle(2);
1316                                         btScalar cy = btCos(ay);
1317                                         btScalar sy = btSin(ay);
1318                                         btScalar cz = btCos(az);
1319                                         btScalar sz = btSin(az);
1320                                         btVector3 ref;
1321                                         ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];
1322                                         ref[1] = -sz*axis[0] + cz*axis[1];
1323                                         ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];
1324                                         tr = p6DOF->getCalculatedTransformB();
1325                                         btVector3 normal = -tr.getBasis().getColumn(0);
1326                                         btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;
1327                                         btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;
1328                                         if(minFi > maxFi)
1329                                         {
1330                                                 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);
1331                                         }
1332                                         else if(minFi < maxFi)
1333                                         {
1334                                                 getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);
1335                                         }
1336                                         tr = p6DOF->getCalculatedTransformA();
1337                                         btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;
1338                                         btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;
1339                                         getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));
1340                                 }
1341                         }
1342                         break;
1343                 case SLIDER_CONSTRAINT_TYPE:
1344                         {
1345                                 btSliderConstraint* pSlider = (btSliderConstraint*)constraint;
1346                                 btTransform tr = pSlider->getCalculatedTransformA();
1347                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1348                                 tr = pSlider->getCalculatedTransformB();
1349                                 if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);
1350                                 if(drawLimits)
1351                                 {
1352                                         btTransform tr = pSlider->getCalculatedTransformA();
1353                                         btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);
1354                                         btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);
1355                                         getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));
1356                                         btVector3 normal = tr.getBasis().getColumn(0);
1357                                         btVector3 axis = tr.getBasis().getColumn(1);
1358                                         btScalar a_min = pSlider->getLowerAngLimit();
1359                                         btScalar a_max = pSlider->getUpperAngLimit();
1360                                         const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();
1361                                         getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);
1362                                 }
1363                         }
1364                         break;
1365                 default : 
1366                         break;
1367         }
1368         return;
1369 } // btDiscreteDynamicsWorld::debugDrawConstraint()
1370
1371
1372
1373
1374
1375 void    btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)
1376 {
1377         if (m_ownsConstraintSolver)
1378         {
1379                 btAlignedFree( m_constraintSolver);
1380         }
1381         m_ownsConstraintSolver = false;
1382         m_constraintSolver = solver;
1383 }
1384
1385 btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
1386 {
1387         return m_constraintSolver;
1388 }
1389
1390
1391 int             btDiscreteDynamicsWorld::getNumConstraints() const
1392 {
1393         return int(m_constraints.size());
1394 }
1395 btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
1396 {
1397         return m_constraints[index];
1398 }
1399 const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
1400 {
1401         return m_constraints[index];
1402 }
1403
1404