dali-physics/third-party/bullet3/src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

   1 /*
   2  Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
   3
   4  Bullet Continuous Collision Detection and Physics Library
   5  Copyright (c) 2019 Google Inc. http://bulletphysics.org
   6  This software is provided 'as-is', without any express or implied warranty.
   7  In no event will the authors be held liable for any damages arising from the use of this software.
   8  Permission is granted to anyone to use this software for any purpose,
   9  including commercial applications, and to alter it and redistribute it freely,
  10  subject to the following restrictions:
  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 /* ====== Overview of the Deformable Algorithm ====== */
  17
  18 /*
  19 A single step of the deformable body simulation contains the following main components:
  20 Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
  21 1. Deformable maintaintenance of rest lengths and volume preservation. Forces only depend on position: Update velocity to a temporary state v_{n+1}^* = v_n + explicit_force * dt / mass, where explicit forces include gravity and elastic forces.
  22 2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
  23
  24 3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
  25
  26 3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
  27         M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
  28    by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
  29    Make sure contact constraints are not violated in step b by performing velocity projections as in the paper by Baraff and Witkin https://www.cs.cmu.edu/~baraff/papers/sig98.pdf. Dynamic frictions are treated as a force and added to the rhs of the CG solve, whereas static frictions are treated as constraints similar to contact.
  30 4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
  31
  32
  33 The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
  34  */
  35
  36 #include <stdio.h>
  37 #include "btDeformableMultiBodyDynamicsWorld.h"
  38 #include "DeformableBodyInplaceSolverIslandCallback.h"
  39 #include "btDeformableBodySolver.h"
  40 #include "LinearMath/btQuickprof.h"
  41 #include "btSoftBodyInternals.h"
  42 btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
  43         : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
  44           m_deformableBodySolver(deformableBodySolver),
  45           m_solverCallback(0)
  46 {
  47         m_drawFlags = fDrawFlags::Std;
  48         m_drawNodeTree = true;
  49         m_drawFaceTree = false;
  50         m_drawClusterTree = false;
  51         m_sbi.m_broadphase = pairCache;
  52         m_sbi.m_dispatcher = dispatcher;
  53         m_sbi.m_sparsesdf.Initialize();
  54         m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
  55         m_sbi.m_sparsesdf.Reset();
  56
  57         m_sbi.air_density = (btScalar)1.2;
  58         m_sbi.water_density = 0;
  59         m_sbi.water_offset = 0;
  60         m_sbi.water_normal = btVector3(0, 0, 0);
  61         m_sbi.m_gravity.setValue(0, -9.8, 0);
  62         m_internalTime = 0.0;
  63         m_implicit = false;
  64         m_lineSearch = false;
  65         m_useProjection = false;
  66         m_ccdIterations = 5;
  67         m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
  68 }
  69
  70 btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
  71 {
  72         delete m_solverDeformableBodyIslandCallback;
  73 }
  74
  75 void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
  76 {
  77         BT_PROFILE("internalSingleStepSimulation");
  78         if (0 != m_internalPreTickCallback)
  79         {
  80                 (*m_internalPreTickCallback)(this, timeStep);
  81         }
  82         reinitialize(timeStep);
  83
  84         // add gravity to velocity of rigid and multi bodys
  85         applyRigidBodyGravity(timeStep);
  86
  87         ///apply gravity and explicit force to velocity, predict motion
  88         predictUnconstraintMotion(timeStep);
  89
  90         ///perform collision detection that involves rigid/multi bodies
  91         btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
  92
  93         btMultiBodyDynamicsWorld::calculateSimulationIslands();
  94
  95         beforeSolverCallbacks(timeStep);
  96
  97         // ///solve contact constraints and then deformable bodies momemtum equation
  98         solveConstraints(timeStep);
  99
 100         afterSolverCallbacks(timeStep);
 101
 102         performDeformableCollisionDetection();
 103
 104         applyRepulsionForce(timeStep);
 105
 106         performGeometricCollisions(timeStep);
 107
 108         integrateTransforms(timeStep);
 109
 110         ///update vehicle simulation
 111         btMultiBodyDynamicsWorld::updateActions(timeStep);
 112
 113         updateActivationState(timeStep);
 114         // End solver-wise simulation step
 115         // ///////////////////////////////
 116 }
 117
 118 void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 119 {
 120         for (int i = 0; i < m_softBodies.size(); ++i)
 121         {
 122                 m_softBodies[i]->m_softSoftCollision = true;
 123         }
 124
 125         for (int i = 0; i < m_softBodies.size(); ++i)
 126         {
 127                 for (int j = i; j < m_softBodies.size(); ++j)
 128                 {
 129                         m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
 130                 }
 131         }
 132
 133         for (int i = 0; i < m_softBodies.size(); ++i)
 134         {
 135                 m_softBodies[i]->m_softSoftCollision = false;
 136         }
 137 }
 138
 139 void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
 140 {
 141         for (int i = 0; i < m_softBodies.size(); i++)
 142         {
 143                 btSoftBody* psb = m_softBodies[i];
 144                 psb->updateDeactivation(timeStep);
 145                 if (psb->wantsSleeping())
 146                 {
 147                         if (psb->getActivationState() == ACTIVE_TAG)
 148                                 psb->setActivationState(WANTS_DEACTIVATION);
 149                         if (psb->getActivationState() == ISLAND_SLEEPING)
 150                         {
 151                                 psb->setZeroVelocity();
 152                         }
 153                 }
 154                 else
 155                 {
 156                         if (psb->getActivationState() != DISABLE_DEACTIVATION)
 157                                 psb->setActivationState(ACTIVE_TAG);
 158                 }
 159         }
 160         btMultiBodyDynamicsWorld::updateActivationState(timeStep);
 161 }
 162
 163 void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep)
 164 {
 165         BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
 166         for (int i = 0; i < m_softBodies.size(); i++)
 167         {
 168                 btSoftBody* psb = m_softBodies[i];
 169                 if (psb->isActive())
 170                 {
 171                         psb->applyRepulsionForce(timeStep, true);
 172                 }
 173         }
 174 }
 175
 176 void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep)
 177 {
 178         BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
 179         // refit the BVH tree for CCD
 180         for (int i = 0; i < m_softBodies.size(); ++i)
 181         {
 182                 btSoftBody* psb = m_softBodies[i];
 183                 if (psb->isActive())
 184                 {
 185                         m_softBodies[i]->updateFaceTree(true, false);
 186                         m_softBodies[i]->updateNodeTree(true, false);
 187                         for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j)
 188                         {
 189                                 btSoftBody::Face& f = m_softBodies[i]->m_faces[j];
 190                                 f.m_n0 = (f.m_n[1]->m_x - f.m_n[0]->m_x).cross(f.m_n[2]->m_x - f.m_n[0]->m_x);
 191                         }
 192                 }
 193         }
 194
 195         // clear contact points & update DBVT
 196         for (int r = 0; r < m_ccdIterations; ++r)
 197         {
 198                 for (int i = 0; i < m_softBodies.size(); ++i)
 199                 {
 200                         btSoftBody* psb = m_softBodies[i];
 201                         if (psb->isActive())
 202                         {
 203                                 // clear contact points in the previous iteration
 204                                 psb->m_faceNodeContactsCCD.clear();
 205
 206                                 // update m_q and normals for CCD calculation
 207                                 for (int j = 0; j < psb->m_nodes.size(); ++j)
 208                                 {
 209                                         psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v;
 210                                 }
 211                                 for (int j = 0; j < psb->m_faces.size(); ++j)
 212                                 {
 213                                         btSoftBody::Face& f = psb->m_faces[j];
 214                                         f.m_n1 = (f.m_n[1]->m_q - f.m_n[0]->m_q).cross(f.m_n[2]->m_q - f.m_n[0]->m_q);
 215                                         f.m_vn = (f.m_n[1]->m_v - f.m_n[0]->m_v).cross(f.m_n[2]->m_v - f.m_n[0]->m_v) * timeStep * timeStep;
 216                                 }
 217                         }
 218                 }
 219
 220                 // apply CCD to register new contact points
 221                 for (int i = 0; i < m_softBodies.size(); ++i)
 222                 {
 223                         for (int j = i; j < m_softBodies.size(); ++j)
 224                         {
 225                                 btSoftBody* psb1 = m_softBodies[i];
 226                                 btSoftBody* psb2 = m_softBodies[j];
 227                                 if (psb1->isActive() && psb2->isActive())
 228                                 {
 229                                         m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
 230                                 }
 231                         }
 232                 }
 233
 234                 int penetration_count = 0;
 235                 for (int i = 0; i < m_softBodies.size(); ++i)
 236                 {
 237                         btSoftBody* psb = m_softBodies[i];
 238                         if (psb->isActive())
 239                         {
 240                                 penetration_count += psb->m_faceNodeContactsCCD.size();
 241                                 ;
 242                         }
 243                 }
 244                 if (penetration_count == 0)
 245                 {
 246                         break;
 247                 }
 248
 249                 // apply inelastic impulse
 250                 for (int i = 0; i < m_softBodies.size(); ++i)
 251                 {
 252                         btSoftBody* psb = m_softBodies[i];
 253                         if (psb->isActive())
 254                         {
 255                                 psb->applyRepulsionForce(timeStep, false);
 256                         }
 257                 }
 258         }
 259 }
 260
 261 void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
 262 {
 263         BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
 264         for (int i = 0; i < m_softBodies.size(); i++)
 265         {
 266                 btSoftBody* psb = m_softBodies[i];
 267                 if (psb->isActive())
 268                 {
 269                         psb->defaultCollisionHandler(psb);
 270                 }
 271         }
 272 }
 273
 274 void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
 275 {
 276         // correct the position of rigid bodies with temporary velocity generated from split impulse
 277         btContactSolverInfo infoGlobal;
 278         btVector3 zero(0, 0, 0);
 279         for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
 280         {
 281                 btRigidBody* rb = m_nonStaticRigidBodies[i];
 282                 //correct the position/orientation based on push/turn recovery
 283                 btTransform newTransform;
 284                 btVector3 pushVelocity = rb->getPushVelocity();
 285                 btVector3 turnVelocity = rb->getTurnVelocity();
 286                 if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
 287                 {
 288                         btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
 289                         rb->setWorldTransform(newTransform);
 290                         rb->setPushVelocity(zero);
 291                         rb->setTurnVelocity(zero);
 292                 }
 293         }
 294 }
 295
 296 void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
 297 {
 298         BT_PROFILE("integrateTransforms");
 299         positionCorrection(timeStep);
 300         btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
 301         m_deformableBodySolver->applyTransforms(timeStep);
 302 }
 303
 304 void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
 305 {
 306         BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
 307         // save v_{n+1}^* velocity after explicit forces
 308         m_deformableBodySolver->backupVelocity();
 309
 310         // set up constraints among multibodies and between multibodies and deformable bodies
 311         setupConstraints();
 312
 313         // solve contact constraints
 314         solveContactConstraints();
 315
 316         // set up the directions in which the velocity does not change in the momentum solve
 317         if (m_useProjection)
 318                 m_deformableBodySolver->setProjection();
 319         else
 320                 m_deformableBodySolver->setLagrangeMultiplier();
 321
 322         // for explicit scheme, m_backupVelocity = v_{n+1}^*
 323         // for implicit scheme, m_backupVelocity = v_n
 324         // Here, set dv = v_{n+1} - v_n for nodes in contact
 325         m_deformableBodySolver->setupDeformableSolve(m_implicit);
 326
 327         // At this point, dv should be golden for nodes in contact
 328         // proceed to solve deformable momentum equation
 329         m_deformableBodySolver->solveDeformableConstraints(timeStep);
 330 }
 331
 332 void btDeformableMultiBodyDynamicsWorld::setupConstraints()
 333 {
 334         // set up constraints between multibody and deformable bodies
 335         m_deformableBodySolver->setConstraints(m_solverInfo);
 336
 337         // set up constraints among multibodies
 338         {
 339                 sortConstraints();
 340                 // setup the solver callback
 341                 btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
 342                 btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
 343                 m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
 344
 345                 // build islands
 346                 m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
 347         }
 348 }
 349
 350 void btDeformableMultiBodyDynamicsWorld::sortConstraints()
 351 {
 352         m_sortedConstraints.resize(m_constraints.size());
 353         int i;
 354         for (i = 0; i < getNumConstraints(); i++)
 355         {
 356                 m_sortedConstraints[i] = m_constraints[i];
 357         }
 358         m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
 359
 360         m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
 361         for (i = 0; i < m_multiBodyConstraints.size(); i++)
 362         {
 363                 m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
 364         }
 365         m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
 366 }
 367
 368 void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
 369 {
 370         // process constraints on each island
 371         m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
 372
 373         // process deferred
 374         m_solverDeformableBodyIslandCallback->processConstraints();
 375         m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
 376
 377         // write joint feedback
 378         {
 379                 for (int i = 0; i < this->m_multiBodies.size(); i++)
 380                 {
 381                         btMultiBody* bod = m_multiBodies[i];
 382
 383                         bool isSleeping = false;
 384
 385                         if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 386                         {
 387                                 isSleeping = true;
 388                         }
 389                         for (int b = 0; b < bod->getNumLinks(); b++)
 390                         {
 391                                 if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
 392                                         isSleeping = true;
 393                         }
 394
 395                         if (!isSleeping)
 396                         {
 397                                 //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
 398                                 m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
 399                                 m_scratch_v.resize(bod->getNumLinks() + 1);
 400                                 m_scratch_m.resize(bod->getNumLinks() + 1);
 401
 402                                 if (bod->internalNeedsJointFeedback())
 403                                 {
 404                                         if (!bod->isUsingRK4Integration())
 405                                         {
 406                                                 if (bod->internalNeedsJointFeedback())
 407                                                 {
 408                                                         bool isConstraintPass = true;
 409                                                         bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
 410                                                                                                                                                                           getSolverInfo().m_jointFeedbackInWorldSpace,
 411                                                                                                                                                                           getSolverInfo().m_jointFeedbackInJointFrame);
 412                                                 }
 413                                         }
 414                                 }
 415                         }
 416                 }
 417         }
 418
 419         for (int i = 0; i < this->m_multiBodies.size(); i++)
 420         {
 421                 btMultiBody* bod = m_multiBodies[i];
 422                 bod->processDeltaVeeMultiDof2();
 423         }
 424 }
 425
 426 void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
 427 {
 428         m_softBodies.push_back(body);
 429
 430         // Set the soft body solver that will deal with this body
 431         // to be the world's solver
 432         body->setSoftBodySolver(m_deformableBodySolver);
 433
 434         btCollisionWorld::addCollisionObject(body,
 435                                                                                  collisionFilterGroup,
 436                                                                                  collisionFilterMask);
 437 }
 438
 439 void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
 440 {
 441         BT_PROFILE("predictUnconstraintMotion");
 442         btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
 443         m_deformableBodySolver->predictMotion(timeStep);
 444 }
 445
 446 void btDeformableMultiBodyDynamicsWorld::setGravity(const btVector3& gravity)
 447 {
 448         btDiscreteDynamicsWorld::setGravity(gravity);
 449         m_deformableBodySolver->setGravity(gravity);
 450 }
 451
 452 void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
 453 {
 454         m_internalTime += timeStep;
 455         m_deformableBodySolver->setImplicit(m_implicit);
 456         m_deformableBodySolver->setLineSearch(m_lineSearch);
 457         m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
 458         btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
 459         dispatchInfo.m_timeStep = timeStep;
 460         dispatchInfo.m_stepCount = 0;
 461         dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
 462         btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
 463         if (m_useProjection)
 464         {
 465                 m_deformableBodySolver->m_useProjection = true;
 466                 m_deformableBodySolver->setStrainLimiting(true);
 467                 m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::Mass_preconditioner);
 468         }
 469         else
 470         {
 471                 m_deformableBodySolver->m_useProjection = false;
 472                 m_deformableBodySolver->setStrainLimiting(false);
 473                 m_deformableBodySolver->setPreconditioner(btDeformableBackwardEulerObjective::KKT_preconditioner);
 474         }
 475 }
 476
 477 void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 478 {
 479         btMultiBodyDynamicsWorld::debugDrawWorld();
 480
 481         for (int i = 0; i < getSoftBodyArray().size(); i++)
 482         {
 483                 btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i];
 484                 {
 485                         btSoftBodyHelpers::DrawFrame(psb, getDebugDrawer());
 486                         btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
 487                 }
 488         }
 489 }
 490
 491 void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
 492 {
 493         // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
 494         // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
 495         // when there are multiple substeps
 496         btMultiBodyDynamicsWorld::applyGravity();
 497         // integrate rigid body gravity
 498         for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
 499         {
 500                 btRigidBody* rb = m_nonStaticRigidBodies[i];
 501                 rb->integrateVelocities(timeStep);
 502         }
 503
 504         // integrate multibody gravity
 505         {
 506                 forwardKinematics();
 507                 clearMultiBodyConstraintForces();
 508                 {
 509                         for (int i = 0; i < this->m_multiBodies.size(); i++)
 510                         {
 511                                 btMultiBody* bod = m_multiBodies[i];
 512
 513                                 bool isSleeping = false;
 514
 515                                 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 516                                 {
 517                                         isSleeping = true;
 518                                 }
 519                                 for (int b = 0; b < bod->getNumLinks(); b++)
 520                                 {
 521                                         if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
 522                                                 isSleeping = true;
 523                                 }
 524
 525                                 if (!isSleeping)
 526                                 {
 527                                         m_scratch_r.resize(bod->getNumLinks() + 1);
 528                                         m_scratch_v.resize(bod->getNumLinks() + 1);
 529                                         m_scratch_m.resize(bod->getNumLinks() + 1);
 530                                         bool isConstraintPass = false;
 531                                         {
 532                                                 if (!bod->isUsingRK4Integration())
 533                                                 {
 534                                                         bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
 535                                                                                                                                                                           m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
 536                                                                                                                                                                           getSolverInfo().m_jointFeedbackInWorldSpace,
 537                                                                                                                                                                           getSolverInfo().m_jointFeedbackInJointFrame);
 538                                                 }
 539                                                 else
 540                                                 {
 541                                                         btAssert(" RK4Integration is not supported");
 542                                                 }
 543                                         }
 544                                 }
 545                         }
 546                 }
 547         }
 548         clearGravity();
 549 }
 550
 551 void btDeformableMultiBodyDynamicsWorld::clearGravity()
 552 {
 553         BT_PROFILE("btMultiBody clearGravity");
 554         // clear rigid body gravity
 555         for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
 556         {
 557                 btRigidBody* body = m_nonStaticRigidBodies[i];
 558                 if (body->isActive())
 559                 {
 560                         body->clearGravity();
 561                 }
 562         }
 563         // clear multibody gravity
 564         for (int i = 0; i < this->m_multiBodies.size(); i++)
 565         {
 566                 btMultiBody* bod = m_multiBodies[i];
 567
 568                 bool isSleeping = false;
 569
 570                 if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
 571                 {
 572                         isSleeping = true;
 573                 }
 574                 for (int b = 0; b < bod->getNumLinks(); b++)
 575                 {
 576                         if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
 577                                 isSleeping = true;
 578                 }
 579
 580                 if (!isSleeping)
 581                 {
 582                         bod->addBaseForce(-m_gravity * bod->getBaseMass());
 583
 584                         for (int j = 0; j < bod->getNumLinks(); ++j)
 585                         {
 586                                 bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
 587                         }
 588                 }
 589         }
 590 }
 591
 592 void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
 593 {
 594         if (0 != m_internalTickCallback)
 595         {
 596                 (*m_internalTickCallback)(this, timeStep);
 597         }
 598
 599         if (0 != m_solverCallback)
 600         {
 601                 (*m_solverCallback)(m_internalTime, this);
 602         }
 603 }
 604
 605 void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
 606 {
 607         if (0 != m_solverCallback)
 608         {
 609                 (*m_solverCallback)(m_internalTime, this);
 610         }
 611 }
 612
 613 void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 614 {
 615         btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 616         bool added = false;
 617         for (int i = 0; i < forces.size(); ++i)
 618         {
 619                 if (forces[i]->getForceType() == force->getForceType())
 620                 {
 621                         forces[i]->addSoftBody(psb);
 622                         added = true;
 623                         break;
 624                 }
 625         }
 626         if (!added)
 627         {
 628                 force->addSoftBody(psb);
 629                 force->setIndices(m_deformableBodySolver->getIndices());
 630                 forces.push_back(force);
 631         }
 632 }
 633
 634 void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
 635 {
 636         btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 637         int removed_index = -1;
 638         for (int i = 0; i < forces.size(); ++i)
 639         {
 640                 if (forces[i]->getForceType() == force->getForceType())
 641                 {
 642                         forces[i]->removeSoftBody(psb);
 643                         if (forces[i]->m_softBodies.size() == 0)
 644                                 removed_index = i;
 645                         break;
 646                 }
 647         }
 648         if (removed_index >= 0)
 649                 forces.removeAtIndex(removed_index);
 650 }
 651
 652 void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
 653 {
 654         btAlignedObjectArray<btDeformableLagrangianForce*>& forces = *m_deformableBodySolver->getLagrangianForceArray();
 655         for (int i = 0; i < forces.size(); ++i)
 656         {
 657                 forces[i]->removeSoftBody(psb);
 658         }
 659 }
 660
 661 void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
 662 {
 663         removeSoftBodyForce(body);
 664         m_softBodies.remove(body);
 665         btCollisionWorld::removeCollisionObject(body);
 666         // force a reinitialize so that node indices get updated.
 667         m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
 668 }
 669
 670 void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
 671 {
 672         btSoftBody* body = btSoftBody::upcast(collisionObject);
 673         if (body)
 674                 removeSoftBody(body);
 675         else
 676                 btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
 677 }
 678
 679 int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
 680 {
 681         startProfiling(timeStep);
 682
 683         int numSimulationSubSteps = 0;
 684
 685         if (maxSubSteps)
 686         {
 687                 //fixed timestep with interpolation
 688                 m_fixedTimeStep = fixedTimeStep;
 689                 m_localTime += timeStep;
 690                 if (m_localTime >= fixedTimeStep)
 691                 {
 692                         numSimulationSubSteps = int(m_localTime / fixedTimeStep);
 693                         m_localTime -= numSimulationSubSteps * fixedTimeStep;
 694                 }
 695         }
 696         else
 697         {
 698                 //variable timestep
 699                 fixedTimeStep = timeStep;
 700                 m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
 701                 m_fixedTimeStep = 0;
 702                 if (btFuzzyZero(timeStep))
 703                 {
 704                         numSimulationSubSteps = 0;
 705                         maxSubSteps = 0;
 706                 }
 707                 else
 708                 {
 709                         numSimulationSubSteps = 1;
 710                         maxSubSteps = 1;
 711                 }
 712         }
 713
 714         //process some debugging flags
 715         if (getDebugDrawer())
 716         {
 717                 btIDebugDraw* debugDrawer = getDebugDrawer();
 718                 gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
 719         }
 720         if (numSimulationSubSteps)
 721         {
 722                 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
 723                 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
 724
 725                 saveKinematicState(fixedTimeStep * clampedSimulationSteps);
 726
 727                 for (int i = 0; i < clampedSimulationSteps; i++)
 728                 {
 729                         internalSingleStepSimulation(fixedTimeStep);
 730                         synchronizeMotionStates();
 731                 }
 732         }
 733         else
 734         {
 735                 synchronizeMotionStates();
 736         }
 737
 738         clearForces();
 739
 740 #ifndef BT_NO_PROFILE
 741         CProfileManager::Increment_Frame_Counter();
 742 #endif  //BT_NO_PROFILE
 743
 744         return numSimulationSubSteps;
 745 }