Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/toonshader/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @ 5910

Last change on this file since 5910 was 2882, checked in by rgrieder, 16 years ago

Update from Bullet 2.73 to 2.74.
  • Property svn:eol-style set to native
File size: 42.8 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
62btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
63:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),
64m_constraintSolver(constraintSolver),
65m_gravity(0,-10,0),
66m_localTime(btScalar(1.)/btScalar(60.)),
67m_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
88btDiscreteDynamicsWorld::~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
104void    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
125void    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
217void    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
233void    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
249void    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
269void    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
301int     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
369void    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
411void    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
425btVector3 btDiscreteDynamicsWorld::getGravity () const
426{
427        return m_gravity;
428}
429
430
431void    btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)
432{
433        removeCollisionObject(body);
434}
435
436void    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
453void    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
467void    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       
478void    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
515void    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
525void    btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)
526{
527        m_constraints.remove(constraint);
528        constraint->getRigidBodyA().removeConstraintRef(constraint);
529        constraint->getRigidBodyB().removeConstraintRef(constraint);
530}
531
532void    btDiscreteDynamicsWorld::addAction(btActionInterface* action)
533{
534        m_actions.push_back(action);
535}
536
537void    btDiscreteDynamicsWorld::removeAction(btActionInterface* action)
538{
539        m_actions.remove(action);
540}
541
542
543void    btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)
544{
545        addAction(vehicle);
546}
547
548void    btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)
549{
550        removeAction(vehicle);
551}
552
553void    btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)
554{
555        addAction(character);
556}
557
558void    btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)
559{
560        removeAction(character);
561}
562
563
564SIMD_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
576class 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
592void    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
707void    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
745class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback
746{
747        btCollisionObject* m_me;
748        btScalar m_allowedPenetration;
749        btOverlappingPairCache* m_pairCache;
750        btDispatcher* m_dispatcher;
751
752
753public:
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
824int gNumClampedCcdMotions=0;
825
826//#include "stdio.h"
827void    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
879void    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
902void    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
917class DebugDrawcallback : public btTriangleCallback, public btInternalTriangleIndexCallback
918{
919        btIDebugDraw*   m_debugDrawer;
920        btVector3       m_color;
921        btTransform     m_worldTrans;
922
923public:
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
952void 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
979void 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
1192void 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
1375void    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
1385btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()
1386{
1387        return m_constraintSolver;
1388}
1389
1390
1391int             btDiscreteDynamicsWorld::getNumConstraints() const
1392{
1393        return int(m_constraints.size());
1394}
1395btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)
1396{
1397        return m_constraints[index];
1398}
1399const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const
1400{
1401        return m_constraints[index];
1402}
1403
1404
Note: See TracBrowser for help on using the repository browser.