dali-physics/third-party/bullet3/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp

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