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