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