Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp @ 1963

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

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