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