Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/gui/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @ 2974

Last change on this file since 2974 was 2662, checked in by rgrieder, 16 years ago

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