src/BulletDynamics/Dynamics/btDiscreteDynamicsWorld.cpp

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