Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

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

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