1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2013 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 "btMultiBodyDynamicsWorld.h" 17 #include "btMultiBodyConstraintSolver.h" 18 #include "btMultiBody.h" 19 #include "btMultiBodyLinkCollider.h" 20 #include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h" 21 #include "LinearMath/btQuickprof.h" 22 #include "btMultiBodyConstraint.h" 23 #include "LinearMath/btIDebugDraw.h" 24 #include "LinearMath/btSerializer.h" 25 26 27 void btMultiBodyDynamicsWorld::addMultiBody(btMultiBody* body, short group, short mask) 28 { 29 m_multiBodies.push_back(body); 30 31 } 32 33 void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body) 34 { 35 m_multiBodies.remove(body); 36 } 37 38 void btMultiBodyDynamicsWorld::calculateSimulationIslands() 39 { 40 BT_PROFILE("calculateSimulationIslands"); 41 42 getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher()); 43 44 { 45 //merge islands based on speculative contact manifolds too 46 for (int i=0;i<this->m_predictiveManifolds.size();i++) 47 { 48 btPersistentManifold* manifold = m_predictiveManifolds[i]; 49 50 const btCollisionObject* colObj0 = manifold->getBody0(); 51 const btCollisionObject* colObj1 = manifold->getBody1(); 52 53 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && 54 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) 55 { 56 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); 57 } 58 } 59 } 60 61 { 62 int i; 63 int numConstraints = int(m_constraints.size()); 64 for (i=0;i< numConstraints ; i++ ) 65 { 66 btTypedConstraint* constraint = m_constraints[i]; 67 if (constraint->isEnabled()) 68 { 69 const btRigidBody* colObj0 = &constraint->getRigidBodyA(); 70 const btRigidBody* colObj1 = &constraint->getRigidBodyB(); 71 72 if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) && 73 ((colObj1) && (!(colObj1)->isStaticOrKinematicObject()))) 74 { 75 getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),(colObj1)->getIslandTag()); 76 } 77 } 78 } 79 } 80 81 //merge islands linked by Featherstone link colliders 82 for (int i=0;i<m_multiBodies.size();i++) 83 { 84 btMultiBody* body = m_multiBodies[i]; 85 { 86 btMultiBodyLinkCollider* prev = body->getBaseCollider(); 87 88 for (int b=0;b<body->getNumLinks();b++) 89 { 90 btMultiBodyLinkCollider* cur = body->getLink(b).m_collider; 91 92 if (((cur) && (!(cur)->isStaticOrKinematicObject())) && 93 ((prev) && (!(prev)->isStaticOrKinematicObject()))) 94 { 95 int tagPrev = prev->getIslandTag(); 96 int tagCur = cur->getIslandTag(); 97 getSimulationIslandManager()->getUnionFind().unite(tagPrev, tagCur); 98 } 99 if (cur && !cur->isStaticOrKinematicObject()) 100 prev = cur; 101 102 } 103 } 104 } 105 106 //merge islands linked by multibody constraints 107 { 108 for (int i=0;i<this->m_multiBodyConstraints.size();i++) 109 { 110 btMultiBodyConstraint* c = m_multiBodyConstraints[i]; 111 int tagA = c->getIslandIdA(); 112 int tagB = c->getIslandIdB(); 113 if (tagA>=0 && tagB>=0) 114 getSimulationIslandManager()->getUnionFind().unite(tagA, tagB); 115 } 116 } 117 118 //Store the island id in each body 119 getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld()); 120 121 } 122 123 124 void btMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep) 125 { 126 BT_PROFILE("btMultiBodyDynamicsWorld::updateActivationState"); 127 128 129 130 for ( int i=0;i<m_multiBodies.size();i++) 131 { 132 btMultiBody* body = m_multiBodies[i]; 133 if (body) 134 { 135 body->checkMotionAndSleepIfRequired(timeStep); 136 if (!body->isAwake()) 137 { 138 btMultiBodyLinkCollider* col = body->getBaseCollider(); 139 if (col && col->getActivationState() == ACTIVE_TAG) 140 { 141 col->setActivationState( WANTS_DEACTIVATION); 142 col->setDeactivationTime(0.f); 143 } 144 for (int b=0;b<body->getNumLinks();b++) 145 { 146 btMultiBodyLinkCollider* col = body->getLink(b).m_collider; 147 if (col && col->getActivationState() == ACTIVE_TAG) 148 { 149 col->setActivationState( WANTS_DEACTIVATION); 150 col->setDeactivationTime(0.f); 151 } 152 } 153 } else 154 { 155 btMultiBodyLinkCollider* col = body->getBaseCollider(); 156 if (col && col->getActivationState() != DISABLE_DEACTIVATION) 157 col->setActivationState( ACTIVE_TAG ); 158 159 for (int b=0;b<body->getNumLinks();b++) 160 { 161 btMultiBodyLinkCollider* col = body->getLink(b).m_collider; 162 if (col && col->getActivationState() != DISABLE_DEACTIVATION) 163 col->setActivationState( ACTIVE_TAG ); 164 } 165 } 166 167 } 168 } 169 170 btDiscreteDynamicsWorld::updateActivationState(timeStep); 171 } 172 173 174 SIMD_FORCE_INLINE int btGetConstraintIslandId2(const btTypedConstraint* lhs) 175 { 176 int islandId; 177 178 const btCollisionObject& rcolObj0 = lhs->getRigidBodyA(); 179 const btCollisionObject& rcolObj1 = lhs->getRigidBodyB(); 180 islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag(); 181 return islandId; 182 183 } 184 185 186 class btSortConstraintOnIslandPredicate2 187 { 188 public: 189 190 bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs ) const 191 { 192 int rIslandId0,lIslandId0; 193 rIslandId0 = btGetConstraintIslandId2(rhs); 194 lIslandId0 = btGetConstraintIslandId2(lhs); 195 return lIslandId0 < rIslandId0; 196 } 197 }; 198 199 200 201 SIMD_FORCE_INLINE int btGetMultiBodyConstraintIslandId(const btMultiBodyConstraint* lhs) 202 { 203 int islandId; 204 205 int islandTagA = lhs->getIslandIdA(); 206 int islandTagB = lhs->getIslandIdB(); 207 islandId= islandTagA>=0?islandTagA:islandTagB; 208 return islandId; 209 210 } 211 212 213 class btSortMultiBodyConstraintOnIslandPredicate 214 { 215 public: 216 217 bool operator() ( const btMultiBodyConstraint* lhs, const btMultiBodyConstraint* rhs ) const 218 { 219 int rIslandId0,lIslandId0; 220 rIslandId0 = btGetMultiBodyConstraintIslandId(rhs); 221 lIslandId0 = btGetMultiBodyConstraintIslandId(lhs); 222 return lIslandId0 < rIslandId0; 223 } 224 }; 225 226 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback 227 { 228 btContactSolverInfo* m_solverInfo; 229 btMultiBodyConstraintSolver* m_solver; 230 btMultiBodyConstraint** m_multiBodySortedConstraints; 231 int m_numMultiBodyConstraints; 232 233 btTypedConstraint** m_sortedConstraints; 234 int m_numConstraints; 235 btIDebugDraw* m_debugDrawer; 236 btDispatcher* m_dispatcher; 237 238 btAlignedObjectArray<btCollisionObject*> m_bodies; 239 btAlignedObjectArray<btPersistentManifold*> m_manifolds; 240 btAlignedObjectArray<btTypedConstraint*> m_constraints; 241 btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints; 242 243 244 MultiBodyInplaceSolverIslandCallback( btMultiBodyConstraintSolver* solver, 245 btDispatcher* dispatcher) 246 :m_solverInfo(NULL), 247 m_solver(solver), 248 m_multiBodySortedConstraints(NULL), 249 m_numConstraints(0), 250 m_debugDrawer(NULL), 251 m_dispatcher(dispatcher) 252 { 253 254 } 255 256 MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) 257 { 258 btAssert(0); 259 (void)other; 260 return *this; 261 } 262 263 SIMD_FORCE_INLINE void setup ( btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer) 264 { 265 btAssert(solverInfo); 266 m_solverInfo = solverInfo; 267 268 m_multiBodySortedConstraints = sortedMultiBodyConstraints; 269 m_numMultiBodyConstraints = numMultiBodyConstraints; 270 m_sortedConstraints = sortedConstraints; 271 m_numConstraints = numConstraints; 272 273 m_debugDrawer = debugDrawer; 274 m_bodies.resize (0); 275 m_manifolds.resize (0); 276 m_constraints.resize (0); 277 m_multiBodyConstraints.resize(0); 278 } 279 280 281 virtual void processIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId) 282 { 283 if (islandId<0) 284 { 285 ///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id 286 m_solver->solveMultiBodyGroup( bodies,numBodies,manifolds, numManifolds,m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0],m_numConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); 287 } else 288 { 289 //also add all non-contact constraints/joints for this island 290 btTypedConstraint** startConstraint = 0; 291 btMultiBodyConstraint** startMultiBodyConstraint = 0; 292 293 int numCurConstraints = 0; 294 int numCurMultiBodyConstraints = 0; 295 296 int i; 297 298 //find the first constraint for this island 299 300 for (i=0;i<m_numConstraints;i++) 301 { 302 if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) 303 { 304 startConstraint = &m_sortedConstraints[i]; 305 break; 306 } 307 } 308 //count the number of constraints in this island 309 for (;i<m_numConstraints;i++) 310 { 311 if (btGetConstraintIslandId2(m_sortedConstraints[i]) == islandId) 312 { 313 numCurConstraints++; 314 } 315 } 316 317 for (i=0;i<m_numMultiBodyConstraints;i++) 318 { 319 if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) 320 { 321 322 startMultiBodyConstraint = &m_multiBodySortedConstraints[i]; 323 break; 324 } 325 } 326 //count the number of multi body constraints in this island 327 for (;i<m_numMultiBodyConstraints;i++) 328 { 329 if (btGetMultiBodyConstraintIslandId(m_multiBodySortedConstraints[i]) == islandId) 330 { 331 numCurMultiBodyConstraints++; 332 } 333 } 334 335 if (m_solverInfo->m_minimumSolverBatchSize<=1) 336 { 337 m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,*m_solverInfo,m_debugDrawer,m_dispatcher); 338 } else 339 { 340 341 for (i=0;i<numBodies;i++) 342 m_bodies.push_back(bodies[i]); 343 for (i=0;i<numManifolds;i++) 344 m_manifolds.push_back(manifolds[i]); 345 for (i=0;i<numCurConstraints;i++) 346 m_constraints.push_back(startConstraint[i]); 347 348 for (i=0;i<numCurMultiBodyConstraints;i++) 349 m_multiBodyConstraints.push_back(startMultiBodyConstraint[i]); 350 351 if ((m_constraints.size()+m_manifolds.size())>m_solverInfo->m_minimumSolverBatchSize) 352 { 353 processConstraints(); 354 } else 355 { 356 //printf("deferred\n"); 357 } 358 } 359 } 360 } 361 void processConstraints() 362 { 363 364 btCollisionObject** bodies = m_bodies.size()? &m_bodies[0]:0; 365 btPersistentManifold** manifold = m_manifolds.size()?&m_manifolds[0]:0; 366 btTypedConstraint** constraints = m_constraints.size()?&m_constraints[0]:0; 367 btMultiBodyConstraint** multiBodyConstraints = m_multiBodyConstraints.size() ? &m_multiBodyConstraints[0] : 0; 368 369 //printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size()); 370 371 m_solver->solveMultiBodyGroup( bodies,m_bodies.size(),manifold, m_manifolds.size(),constraints, m_constraints.size() ,multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo,m_debugDrawer,m_dispatcher); 372 m_bodies.resize(0); 373 m_manifolds.resize(0); 374 m_constraints.resize(0); 375 m_multiBodyConstraints.resize(0); 376 } 377 378 }; 379 380 381 382 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) 383 :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), 384 m_multiBodyConstraintSolver(constraintSolver) 385 { 386 //split impulse is not yet supported for Featherstone hierarchies 387 getSolverInfo().m_splitImpulse = false; 388 getSolverInfo().m_solverMode |=SOLVER_USE_2_FRICTION_DIRECTIONS; 389 m_solverMultiBodyIslandCallback = new MultiBodyInplaceSolverIslandCallback(constraintSolver,dispatcher); 390 } 391 392 btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () 393 { 394 delete m_solverMultiBodyIslandCallback; 395 } 396 397 void btMultiBodyDynamicsWorld::forwardKinematics() 398 { 399 btAlignedObjectArray<btQuaternion> world_to_local; 400 btAlignedObjectArray<btVector3> local_origin; 401 402 for (int b=0;b<m_multiBodies.size();b++) 403 { 404 btMultiBody* bod = m_multiBodies[b]; 405 bod->forwardKinematics(world_to_local,local_origin); 406 } 407 } 408 void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) 409 { 410 forwardKinematics(); 411 412 btAlignedObjectArray<btScalar> scratch_r; 413 btAlignedObjectArray<btVector3> scratch_v; 414 btAlignedObjectArray<btMatrix3x3> scratch_m; 415 416 417 BT_PROFILE("solveConstraints"); 418 419 m_sortedConstraints.resize( m_constraints.size()); 420 int i; 421 for (i=0;i<getNumConstraints();i++) 422 { 423 m_sortedConstraints[i] = m_constraints[i]; 424 } 425 m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2()); 426 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0; 427 428 m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size()); 429 for (i=0;i<m_multiBodyConstraints.size();i++) 430 { 431 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i]; 432 } 433 m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate()); 434 435 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0; 436 437 438 m_solverMultiBodyIslandCallback->setup(&solverInfo,constraintsPtr,m_sortedConstraints.size(),sortedMultiBodyConstraints,m_sortedMultiBodyConstraints.size(), getDebugDrawer()); 439 m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds()); 440 441 /// solve all the constraints for this island 442 m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),m_solverMultiBodyIslandCallback); 443 444 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 445 { 446 BT_PROFILE("btMultiBody addForce"); 447 for (int i=0;i<this->m_multiBodies.size();i++) 448 { 449 btMultiBody* bod = m_multiBodies[i]; 450 451 bool isSleeping = false; 452 453 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) 454 { 455 isSleeping = true; 456 } 457 for (int b=0;b<bod->getNumLinks();b++) 458 { 459 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) 460 isSleeping = true; 461 } 462 463 if (!isSleeping) 464 { 465 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) 466 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) 467 scratch_v.resize(bod->getNumLinks()+1); 468 scratch_m.resize(bod->getNumLinks()+1); 469 470 bod->addBaseForce(m_gravity * bod->getBaseMass()); 471 472 for (int j = 0; j < bod->getNumLinks(); ++j) 473 { 474 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); 475 } 476 }//if (!isSleeping) 477 } 478 } 479 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 480 481 482 { 483 BT_PROFILE("btMultiBody stepVelocities"); 484 for (int i=0;i<this->m_multiBodies.size();i++) 485 { 486 btMultiBody* bod = m_multiBodies[i]; 487 488 bool isSleeping = false; 489 490 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) 491 { 492 isSleeping = true; 493 } 494 for (int b=0;b<bod->getNumLinks();b++) 495 { 496 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) 497 isSleeping = true; 498 } 499 500 if (!isSleeping) 501 { 502 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) 503 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) 504 scratch_v.resize(bod->getNumLinks()+1); 505 scratch_m.resize(bod->getNumLinks()+1); 506 bool doNotUpdatePos = false; 507 508 if(bod->isMultiDof()) 509 { 510 if(!bod->isUsingRK4Integration()) 511 { 512 bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); 513 } 514 else 515 { 516 // 517 int numDofs = bod->getNumDofs() + 6; 518 int numPosVars = bod->getNumPosVars() + 7; 519 btAlignedObjectArray<btScalar> scratch_r2; scratch_r2.resize(2*numPosVars + 8*numDofs); 520 //convenience 521 btScalar *pMem = &scratch_r2[0]; 522 btScalar *scratch_q0 = pMem; pMem += numPosVars; 523 btScalar *scratch_qx = pMem; pMem += numPosVars; 524 btScalar *scratch_qd0 = pMem; pMem += numDofs; 525 btScalar *scratch_qd1 = pMem; pMem += numDofs; 526 btScalar *scratch_qd2 = pMem; pMem += numDofs; 527 btScalar *scratch_qd3 = pMem; pMem += numDofs; 528 btScalar *scratch_qdd0 = pMem; pMem += numDofs; 529 btScalar *scratch_qdd1 = pMem; pMem += numDofs; 530 btScalar *scratch_qdd2 = pMem; pMem += numDofs; 531 btScalar *scratch_qdd3 = pMem; pMem += numDofs; 532 btAssert((pMem - (2*numPosVars + 8*numDofs)) == &scratch_r2[0]); 533 534 ///// 535 //copy q0 to scratch_q0 and qd0 to scratch_qd0 536 scratch_q0[0] = bod->getWorldToBaseRot().x(); 537 scratch_q0[1] = bod->getWorldToBaseRot().y(); 538 scratch_q0[2] = bod->getWorldToBaseRot().z(); 539 scratch_q0[3] = bod->getWorldToBaseRot().w(); 540 scratch_q0[4] = bod->getBasePos().x(); 541 scratch_q0[5] = bod->getBasePos().y(); 542 scratch_q0[6] = bod->getBasePos().z(); 543 // 544 for(int link = 0; link < bod->getNumLinks(); ++link) 545 { 546 for(int dof = 0; dof < bod->getLink(link).m_posVarCount; ++dof) 547 scratch_q0[7 + bod->getLink(link).m_cfgOffset + dof] = bod->getLink(link).m_jointPos[dof]; 548 } 549 // 550 for(int dof = 0; dof < numDofs; ++dof) 551 scratch_qd0[dof] = bod->getVelocityVector()[dof]; 552 //// 553 struct 554 { 555 btMultiBody *bod; 556 btScalar *scratch_qx, *scratch_q0; 557 558 void operator()() 559 { 560 for(int dof = 0; dof < bod->getNumPosVars() + 7; ++dof) 561 scratch_qx[dof] = scratch_q0[dof]; 562 } 563 } pResetQx = {bod, scratch_qx, scratch_q0}; 564 // 565 struct 566 { 567 void operator()(btScalar dt, const btScalar *pDer, const btScalar *pCurVal, btScalar *pVal, int size) 568 { 569 for(int i = 0; i < size; ++i) 570 pVal[i] = pCurVal[i] + dt * pDer[i]; 571 } 572 573 } pEulerIntegrate; 574 // 575 struct 576 { 577 void operator()(btMultiBody *pBody, const btScalar *pData) 578 { 579 btScalar *pVel = const_cast<btScalar*>(pBody->getVelocityVector()); 580 581 for(int i = 0; i < pBody->getNumDofs() + 6; ++i) 582 pVel[i] = pData[i]; 583 584 } 585 } pCopyToVelocityVector; 586 // 587 struct 588 { 589 void operator()(const btScalar *pSrc, btScalar *pDst, int start, int size) 590 { 591 for(int i = 0; i < size; ++i) 592 pDst[i] = pSrc[start + i]; 593 } 594 } pCopy; 595 // 596 597 btScalar h = solverInfo.m_timeStep; 598 #define output &scratch_r[bod->getNumDofs()] 599 //calc qdd0 from: q0 & qd0 600 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); 601 pCopy(output, scratch_qdd0, 0, numDofs); 602 //calc q1 = q0 + h/2 * qd0 603 pResetQx(); 604 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd0); 605 //calc qd1 = qd0 + h/2 * qdd0 606 pEulerIntegrate(btScalar(.5)*h, scratch_qdd0, scratch_qd0, scratch_qd1, numDofs); 607 // 608 //calc qdd1 from: q1 & qd1 609 pCopyToVelocityVector(bod, scratch_qd1); 610 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); 611 pCopy(output, scratch_qdd1, 0, numDofs); 612 //calc q2 = q0 + h/2 * qd1 613 pResetQx(); 614 bod->stepPositionsMultiDof(btScalar(.5)*h, scratch_qx, scratch_qd1); 615 //calc qd2 = qd0 + h/2 * qdd1 616 pEulerIntegrate(btScalar(.5)*h, scratch_qdd1, scratch_qd0, scratch_qd2, numDofs); 617 // 618 //calc qdd2 from: q2 & qd2 619 pCopyToVelocityVector(bod, scratch_qd2); 620 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); 621 pCopy(output, scratch_qdd2, 0, numDofs); 622 //calc q3 = q0 + h * qd2 623 pResetQx(); 624 bod->stepPositionsMultiDof(h, scratch_qx, scratch_qd2); 625 //calc qd3 = qd0 + h * qdd2 626 pEulerIntegrate(h, scratch_qdd2, scratch_qd0, scratch_qd3, numDofs); 627 // 628 //calc qdd3 from: q3 & qd3 629 pCopyToVelocityVector(bod, scratch_qd3); 630 bod->stepVelocitiesMultiDof(0., scratch_r, scratch_v, scratch_m); 631 pCopy(output, scratch_qdd3, 0, numDofs); 632 633 // 634 //calc q = q0 + h/6(qd0 + 2*(qd1 + qd2) + qd3) 635 //calc qd = qd0 + h/6(qdd0 + 2*(qdd1 + qdd2) + qdd3) 636 btAlignedObjectArray<btScalar> delta_q; delta_q.resize(numDofs); 637 btAlignedObjectArray<btScalar> delta_qd; delta_qd.resize(numDofs); 638 for(int i = 0; i < numDofs; ++i) 639 { 640 delta_q[i] = h/btScalar(6.)*(scratch_qd0[i] + 2*scratch_qd1[i] + 2*scratch_qd2[i] + scratch_qd3[i]); 641 delta_qd[i] = h/btScalar(6.)*(scratch_qdd0[i] + 2*scratch_qdd1[i] + 2*scratch_qdd2[i] + scratch_qdd3[i]); 642 //delta_q[i] = h*scratch_qd0[i]; 643 //delta_qd[i] = h*scratch_qdd0[i]; 644 } 645 // 646 pCopyToVelocityVector(bod, scratch_qd0); 647 bod->applyDeltaVeeMultiDof(&delta_qd[0], 1); 648 // 649 if(!doNotUpdatePos) 650 { 651 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector()); 652 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); 653 654 for(int i = 0; i < numDofs; ++i) 655 pRealBuf[i] = delta_q[i]; 656 657 //bod->stepPositionsMultiDof(1, 0, &delta_q[0]); 658 bod->setPosUpdated(true); 659 } 660 661 //ugly hack which resets the cached data to t0 (needed for constraint solver) 662 { 663 for(int link = 0; link < bod->getNumLinks(); ++link) 664 bod->getLink(link).updateCacheMultiDof(); 665 bod->stepVelocitiesMultiDof(0, scratch_r, scratch_v, scratch_m); 666 } 667 668 } 669 } 670 else//if(bod->isMultiDof()) 671 { 672 bod->stepVelocities(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); 673 } 674 #ifndef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 675 bod->clearForcesAndTorques(); 676 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 677 }//if (!isSleeping) 678 } 679 } 680 681 clearMultiBodyConstraintForces(); 682 683 m_solverMultiBodyIslandCallback->processConstraints(); 684 685 m_constraintSolver->allSolved(solverInfo, m_debugDrawer); 686 687 { 688 BT_PROFILE("btMultiBody stepVelocities"); 689 for (int i=0;i<this->m_multiBodies.size();i++) 690 { 691 btMultiBody* bod = m_multiBodies[i]; 692 693 bool isSleeping = false; 694 695 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) 696 { 697 isSleeping = true; 698 } 699 for (int b=0;b<bod->getNumLinks();b++) 700 { 701 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) 702 isSleeping = true; 703 } 704 705 if (!isSleeping) 706 { 707 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY) 708 scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd) 709 scratch_v.resize(bod->getNumLinks()+1); 710 scratch_m.resize(bod->getNumLinks()+1); 711 712 if(bod->isMultiDof()) 713 { 714 if(!bod->isUsingRK4Integration()) 715 { 716 bool isConstraintPass = true; 717 bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m, isConstraintPass); 718 } 719 } 720 } 721 } 722 } 723 724 for (int i=0;i<this->m_multiBodies.size();i++) 725 { 726 btMultiBody* bod = m_multiBodies[i]; 727 bod->processDeltaVeeMultiDof2(); 728 } 729 730 } 731 732 void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep) 733 { 734 btDiscreteDynamicsWorld::integrateTransforms(timeStep); 735 736 { 737 BT_PROFILE("btMultiBody stepPositions"); 738 //integrate and update the Featherstone hierarchies 739 btAlignedObjectArray<btQuaternion> world_to_local; 740 btAlignedObjectArray<btVector3> local_origin; 741 742 for (int b=0;b<m_multiBodies.size();b++) 743 { 744 btMultiBody* bod = m_multiBodies[b]; 745 bool isSleeping = false; 746 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) 747 { 748 isSleeping = true; 749 } 750 for (int b=0;b<bod->getNumLinks();b++) 751 { 752 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) 753 isSleeping = true; 754 } 755 756 757 if (!isSleeping) 758 { 759 int nLinks = bod->getNumLinks(); 760 761 ///base + num m_links 762 763 if(bod->isMultiDof()) 764 { 765 if(!bod->isPosUpdated()) 766 bod->stepPositionsMultiDof(timeStep); 767 else 768 { 769 btScalar *pRealBuf = const_cast<btScalar *>(bod->getVelocityVector()); 770 pRealBuf += 6 + bod->getNumDofs() + bod->getNumDofs()*bod->getNumDofs(); 771 772 bod->stepPositionsMultiDof(1, 0, pRealBuf); 773 bod->setPosUpdated(false); 774 } 775 } 776 else 777 { 778 bod->stepPositions(timeStep); 779 } 780 world_to_local.resize(nLinks+1); 781 local_origin.resize(nLinks+1); 782 783 bod->updateCollisionObjectWorldTransforms(world_to_local,local_origin); 784 785 } else 786 { 787 bod->clearVelocities(); 788 } 789 } 790 } 791 } 792 793 794 795 void btMultiBodyDynamicsWorld::addMultiBodyConstraint( btMultiBodyConstraint* constraint) 796 { 797 m_multiBodyConstraints.push_back(constraint); 798 } 799 800 void btMultiBodyDynamicsWorld::removeMultiBodyConstraint( btMultiBodyConstraint* constraint) 801 { 802 m_multiBodyConstraints.remove(constraint); 803 } 804 805 void btMultiBodyDynamicsWorld::debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint) 806 { 807 constraint->debugDraw(getDebugDrawer()); 808 } 809 810 811 void btMultiBodyDynamicsWorld::debugDrawWorld() 812 { 813 BT_PROFILE("btMultiBodyDynamicsWorld debugDrawWorld"); 814 815 bool drawConstraints = false; 816 if (getDebugDrawer()) 817 { 818 int mode = getDebugDrawer()->getDebugMode(); 819 if (mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits)) 820 { 821 drawConstraints = true; 822 } 823 824 if (drawConstraints) 825 { 826 BT_PROFILE("btMultiBody debugDrawWorld"); 827 828 btAlignedObjectArray<btQuaternion> world_to_local1; 829 btAlignedObjectArray<btVector3> local_origin1; 830 831 for (int c=0;c<m_multiBodyConstraints.size();c++) 832 { 833 btMultiBodyConstraint* constraint = m_multiBodyConstraints[c]; 834 debugDrawMultiBodyConstraint(constraint); 835 } 836 837 for (int b = 0; b<m_multiBodies.size(); b++) 838 { 839 btMultiBody* bod = m_multiBodies[b]; 840 bod->forwardKinematics(world_to_local1,local_origin1); 841 842 getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1); 843 844 845 for (int m = 0; m<bod->getNumLinks(); m++) 846 { 847 848 const btTransform& tr = bod->getLink(m).m_cachedWorldTransform; 849 850 getDebugDrawer()->drawTransform(tr, 0.1); 851 852 //draw the joint axis 853 if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute) 854 { 855 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec); 856 857 btVector4 color(0,0,0,1);//1,1,1); 858 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); 859 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); 860 getDebugDrawer()->drawLine(from,to,color); 861 } 862 if (bod->getLink(m).m_jointType==btMultibodyLink::eFixed) 863 { 864 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); 865 866 btVector4 color(0,0,0,1);//1,1,1); 867 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); 868 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); 869 getDebugDrawer()->drawLine(from,to,color); 870 } 871 if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic) 872 { 873 btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec); 874 875 btVector4 color(0,0,0,1);//1,1,1); 876 btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); 877 btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector); 878 getDebugDrawer()->drawLine(from,to,color); 879 } 880 881 } 882 } 883 } 884 } 885 886 btDiscreteDynamicsWorld::debugDrawWorld(); 887 } 888 889 890 891 void btMultiBodyDynamicsWorld::applyGravity() 892 { 893 btDiscreteDynamicsWorld::applyGravity(); 894 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 895 BT_PROFILE("btMultiBody addGravity"); 896 for (int i=0;i<this->m_multiBodies.size();i++) 897 { 898 btMultiBody* bod = m_multiBodies[i]; 899 900 bool isSleeping = false; 901 902 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) 903 { 904 isSleeping = true; 905 } 906 for (int b=0;b<bod->getNumLinks();b++) 907 { 908 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) 909 isSleeping = true; 910 } 911 912 if (!isSleeping) 913 { 914 bod->addBaseForce(m_gravity * bod->getBaseMass()); 915 916 for (int j = 0; j < bod->getNumLinks(); ++j) 917 { 918 bod->addLinkForce(j, m_gravity * bod->getLinkMass(j)); 919 } 920 }//if (!isSleeping) 921 } 922 #endif //BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 923 } 924 925 void btMultiBodyDynamicsWorld::clearMultiBodyConstraintForces() 926 { 927 for (int i=0;i<this->m_multiBodies.size();i++) 928 { 929 btMultiBody* bod = m_multiBodies[i]; 930 bod->clearConstraintForces(); 931 } 932 } 933 void btMultiBodyDynamicsWorld::clearMultiBodyForces() 934 { 935 { 936 BT_PROFILE("clearMultiBodyForces"); 937 for (int i=0;i<this->m_multiBodies.size();i++) 938 { 939 btMultiBody* bod = m_multiBodies[i]; 940 941 bool isSleeping = false; 942 943 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING) 944 { 945 isSleeping = true; 946 } 947 for (int b=0;b<bod->getNumLinks();b++) 948 { 949 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING) 950 isSleeping = true; 951 } 952 953 if (!isSleeping) 954 { 955 btMultiBody* bod = m_multiBodies[i]; 956 bod->clearForcesAndTorques(); 957 } 958 } 959 } 960 961 } 962 void btMultiBodyDynamicsWorld::clearForces() 963 { 964 btDiscreteDynamicsWorld::clearForces(); 965 966 #ifdef BT_USE_VIRTUAL_CLEARFORCES_AND_GRAVITY 967 clearMultiBodyForces(); 968 #endif 969 } 970 971 972 973 974 void btMultiBodyDynamicsWorld::serialize(btSerializer* serializer) 975 { 976 977 serializer->startSerialization(); 978 979 serializeDynamicsWorldInfo( serializer); 980 981 serializeMultiBodies(serializer); 982 983 serializeRigidBodies(serializer); 984 985 serializeCollisionObjects(serializer); 986 987 serializer->finishSerialization(); 988 } 989 990 void btMultiBodyDynamicsWorld::serializeMultiBodies(btSerializer* serializer) 991 { 992 int i; 993 //serialize all collision objects 994 for (i=0;i<m_multiBodies.size();i++) 995 { 996 btMultiBody* mb = m_multiBodies[i]; 997 { 998 int len = mb->calculateSerializeBufferSize(); 999 btChunk* chunk = serializer->allocate(len,1); 1000 const char* structType = mb->serialize(chunk->m_oldPtr, serializer); 1001 serializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb); 1002 } 1003 } 1004 1005 }
