Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @ 12175

Last change on this file since 12175 was 12175, checked in by siramesh, 5 years ago

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