btDeformableMultiBodyDynamicsWorld.cpp

Go to the documentation of this file.

/*
 Written by Xuchen Han <xuchenhan2015@u.northwestern.edu>
 
 Bullet Continuous Collision Detection and Physics Library
 Copyright (c) 2019 Google Inc. http://bulletphysics.org
 This software is provided 'as-is', without any express or implied warranty.
 In no event will the authors be held liable for any damages arising from the use of this software.
 Permission is granted to anyone to use this software for any purpose,
 including commercial applications, and to alter it and redistribute it freely,
 subject to the following restrictions:
 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.
 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
 3. This notice may not be removed or altered from any source distribution.
 */
 
/* ====== Overview of the Deformable Algorithm ====== */
 
/*
A single step of the deformable body simulation contains the following main components:
Call internalStepSimulation multiple times, to achieve 240Hz (4 steps of 60Hz).
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.
2. Detect discrete collisions between rigid and deformable bodies at position x_{n+1}^* = x_n + dt * v_{n+1}^*.
 
3a. Solve all constraints, including LCP. Contact, position correction due to numerical drift, friction, and anchors for deformable.
 
3b. 5 Newton steps (multiple step). Conjugent Gradient solves linear system. Deformable Damping: Then velocities of deformable bodies v_{n+1} are solved in
        M(v_{n+1} - v_{n+1}^*) = damping_force * dt / mass,
   by a conjugate gradient solver, where the damping force is implicit and depends on v_{n+1}.
   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.
4. Position is updated via x_{n+1} = x_n + dt * v_{n+1}.
 
 
The algorithm also closely resembles the one in http://physbam.stanford.edu/~fedkiw/papers/stanford2008-03.pdf
 */
 
#include <stdio.h>
#include "btDeformableMultiBodyDynamicsWorld.h"
#include "DeformableBodyInplaceSolverIslandCallback.h"
#include "btDeformableBodySolver.h"
#include "LinearMath/btQuickprof.h"
#include "btSoftBodyInternals.h"
btDeformableMultiBodyDynamicsWorld::btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver)
        : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
          m_deformableBodySolver(deformableBodySolver),
          m_solverCallback(0)
{
        m_drawFlags = fDrawFlags::Std;
        m_drawNodeTree = true;
        m_drawFaceTree = false;
        m_drawClusterTree = false;
        m_sbi.m_broadphase = pairCache;
        m_sbi.m_dispatcher = dispatcher;
        m_sbi.m_sparsesdf.Initialize();
        m_sbi.m_sparsesdf.setDefaultVoxelsz(0.005);
        m_sbi.m_sparsesdf.Reset();
 
        m_sbi.air_density = (btScalar)1.2;
        m_sbi.water_density = 0;
        m_sbi.water_offset = 0;
        m_sbi.water_normal = btVector3(0, 0, 0);
        m_sbi.m_gravity.setValue(0, -9.8, 0);
        m_internalTime = 0.0;
        m_implicit = false;
        m_lineSearch = false;
        m_useProjection = false;
        m_ccdIterations = 5;
        m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
}
 
btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
{
        delete m_solverDeformableBodyIslandCallback;
}
 
void btDeformableMultiBodyDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)
{
        BT_PROFILE("internalSingleStepSimulation");
        if (0 != m_internalPreTickCallback)
        {
                (*m_internalPreTickCallback)(this, timeStep);
        }
        reinitialize(timeStep);
 
        // add gravity to velocity of rigid and multi bodys
        applyRigidBodyGravity(timeStep);
 
        predictUnconstraintMotion(timeStep);
 
        btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
 
        btMultiBodyDynamicsWorld::calculateSimulationIslands();
 
        beforeSolverCallbacks(timeStep);
 
        solveConstraints(timeStep);
 
        afterSolverCallbacks(timeStep);
 
        performDeformableCollisionDetection();
 
        applyRepulsionForce(timeStep);
 
        performGeometricCollisions(timeStep);
 
        integrateTransforms(timeStep);
 
        btMultiBodyDynamicsWorld::updateActions(timeStep);
 
        updateActivationState(timeStep);
        // End solver-wise simulation step
        // ///////////////////////////////
}
 
void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
{
        for (int i = 0; i < m_softBodies.size(); ++i)
        {
                m_softBodies[i]->m_softSoftCollision = true;
        }
 
        for (int i = 0; i < m_softBodies.size(); ++i)
        {
                for (int j = i; j < m_softBodies.size(); ++j)
                {
                        m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
                }
        }
 
        for (int i = 0; i < m_softBodies.size(); ++i)
        {
                m_softBodies[i]->m_softSoftCollision = false;
        }
}
 
void btDeformableMultiBodyDynamicsWorld::updateActivationState(btScalar timeStep)
{
        for (int i = 0; i < m_softBodies.size(); i++)
        {
                btSoftBody* psb = m_softBodies[i];
                psb->updateDeactivation(timeStep);
                if (psb->wantsSleeping())
                {
                        if (psb->getActivationState() == ACTIVE_TAG)
                                psb->setActivationState(WANTS_DEACTIVATION);
                        if (psb->getActivationState() == ISLAND_SLEEPING)
                        {
                                psb->setZeroVelocity();
                        }
                }
                else
                {
                        if (psb->getActivationState() != DISABLE_DEACTIVATION)
                                psb->setActivationState(ACTIVE_TAG);
                }
        }
        btMultiBodyDynamicsWorld::updateActivationState(timeStep);
}
 
void btDeformableMultiBodyDynamicsWorld::applyRepulsionForce(btScalar timeStep)
{
        BT_PROFILE("btDeformableMultiBodyDynamicsWorld::applyRepulsionForce");
        for (int i = 0; i < m_softBodies.size(); i++)
        {
                btSoftBody* psb = m_softBodies[i];
                if (psb->isActive())
                {
                        psb->applyRepulsionForce(timeStep, true);
                }
        }
}
 
void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar timeStep)
{
        BT_PROFILE("btDeformableMultiBodyDynamicsWorld::performGeometricCollisions");
        // refit the BVH tree for CCD
        for (int i = 0; i < m_softBodies.size(); ++i)
        {
                btSoftBody* psb = m_softBodies[i];
                if (psb->isActive())
                {
                        m_softBodies[i]->updateFaceTree(true, false);
                        m_softBodies[i]->updateNodeTree(true, false);
                        for (int j = 0; j < m_softBodies[i]->m_faces.size(); ++j)
                        {
                                btSoftBody::Face& f = m_softBodies[i]->m_faces[j];
                                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);
                        }
                }
        }
 
        // clear contact points & update DBVT
        for (int r = 0; r < m_ccdIterations; ++r)
        {
                for (int i = 0; i < m_softBodies.size(); ++i)
                {
                        btSoftBody* psb = m_softBodies[i];
                        if (psb->isActive())
                        {
                                // clear contact points in the previous iteration
                                psb->m_faceNodeContacts.clear();
 
                                // update m_q and normals for CCD calculation
                                for (int j = 0; j < psb->m_nodes.size(); ++j)
                                {
                                        psb->m_nodes[j].m_q = psb->m_nodes[j].m_x + timeStep * psb->m_nodes[j].m_v;
                                }
                                for (int j = 0; j < psb->m_faces.size(); ++j)
                                {
                                        btSoftBody::Face& f = psb->m_faces[j];
                                        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);
                                        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;
                                }
                        }
                }
 
                // apply CCD to register new contact points
                for (int i = 0; i < m_softBodies.size(); ++i)
                {
                        for (int j = i; j < m_softBodies.size(); ++j)
                        {
                                btSoftBody* psb1 = m_softBodies[i];
                                btSoftBody* psb2 = m_softBodies[j];
                                if (psb1->isActive() && psb2->isActive())
                                {
                                        m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
                                }
                        }
                }
 
                int penetration_count = 0;
                for (int i = 0; i < m_softBodies.size(); ++i)
                {
                        btSoftBody* psb = m_softBodies[i];
                        if (psb->isActive())
                        {
                                penetration_count += psb->m_faceNodeContacts.size();
                        }
                }
                if (penetration_count == 0)
                {
                        break;
                }
 
                // apply inelastic impulse
                for (int i = 0; i < m_softBodies.size(); ++i)
                {
                        btSoftBody* psb = m_softBodies[i];
                        if (psb->isActive())
                        {
                                psb->applyRepulsionForce(timeStep, false);
                        }
                }
        }
}
 
void btDeformableMultiBodyDynamicsWorld::softBodySelfCollision()
{
        BT_PROFILE("btDeformableMultiBodyDynamicsWorld::softBodySelfCollision");
        for (int i = 0; i < m_softBodies.size(); i++)
        {
                btSoftBody* psb = m_softBodies[i];
                if (psb->isActive())
                {
                        psb->defaultCollisionHandler(psb);
                }
        }
}
 
void btDeformableMultiBodyDynamicsWorld::positionCorrection(btScalar timeStep)
{
        // correct the position of rigid bodies with temporary velocity generated from split impulse
        btContactSolverInfo infoGlobal;
        btVector3 zero(0, 0, 0);
        for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
        {
                btRigidBody* rb = m_nonStaticRigidBodies[i];
                //correct the position/orientation based on push/turn recovery
                btTransform newTransform;
                btVector3 pushVelocity = rb->getPushVelocity();
                btVector3 turnVelocity = rb->getTurnVelocity();
                if (pushVelocity[0] != 0.f || pushVelocity[1] != 0 || pushVelocity[2] != 0 || turnVelocity[0] != 0.f || turnVelocity[1] != 0 || turnVelocity[2] != 0)
                {
                        btTransformUtil::integrateTransform(rb->getWorldTransform(), pushVelocity, turnVelocity * infoGlobal.m_splitImpulseTurnErp, timeStep, newTransform);
                        rb->setWorldTransform(newTransform);
                        rb->setPushVelocity(zero);
                        rb->setTurnVelocity(zero);
                }
        }
}
 
void btDeformableMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
        BT_PROFILE("integrateTransforms");
        positionCorrection(timeStep);
        btMultiBodyDynamicsWorld::integrateTransforms(timeStep);
        for (int i = 0; i < m_softBodies.size(); ++i)
        {
                btSoftBody* psb = m_softBodies[i];
                for (int j = 0; j < psb->m_nodes.size(); ++j)
                {
                        btSoftBody::Node& node = psb->m_nodes[j];
                        btScalar maxDisplacement = psb->getWorldInfo()->m_maxDisplacement;
                        btScalar clampDeltaV = maxDisplacement / timeStep;
                        for (int c = 0; c < 3; c++)
                        {
                                if (node.m_v[c] > clampDeltaV)
                                {
                                        node.m_v[c] = clampDeltaV;
                                }
                                if (node.m_v[c] < -clampDeltaV)
                                {
                                        node.m_v[c] = -clampDeltaV;
                                }
                        }
                        node.m_x = node.m_x + timeStep * (node.m_v + node.m_splitv);
                        node.m_q = node.m_x;
                        node.m_vn = node.m_v;
                }
                // enforce anchor constraints
                for (int j = 0; j < psb->m_deformableAnchors.size(); ++j)
                {
                        btSoftBody::DeformableNodeRigidAnchor& a = psb->m_deformableAnchors[j];
                        btSoftBody::Node* n = a.m_node;
                        n->m_x = a.m_cti.m_colObj->getWorldTransform() * a.m_local;
 
                        // update multibody anchor info
                        if (a.m_cti.m_colObj->getInternalType() == btCollisionObject::CO_FEATHERSTONE_LINK)
                        {
                                btMultiBodyLinkCollider* multibodyLinkCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(a.m_cti.m_colObj);
                                if (multibodyLinkCol)
                                {
                                        btVector3 nrm;
                                        const btCollisionShape* shp = multibodyLinkCol->getCollisionShape();
                                        const btTransform& wtr = multibodyLinkCol->getWorldTransform();
                                        psb->m_worldInfo->m_sparsesdf.Evaluate(
                                                wtr.invXform(n->m_x),
                                                shp,
                                                nrm,
                                                0);
                                        a.m_cti.m_normal = wtr.getBasis() * nrm;
                                        btVector3 normal = a.m_cti.m_normal;
                                        btVector3 t1 = generateUnitOrthogonalVector(normal);
                                        btVector3 t2 = btCross(normal, t1);
                                        btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
                                        findJacobian(multibodyLinkCol, jacobianData_normal, a.m_node->m_x, normal);
                                        findJacobian(multibodyLinkCol, jacobianData_t1, a.m_node->m_x, t1);
                                        findJacobian(multibodyLinkCol, jacobianData_t2, a.m_node->m_x, t2);
 
                                        btScalar* J_n = &jacobianData_normal.m_jacobians[0];
                                        btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];
                                        btScalar* J_t2 = &jacobianData_t2.m_jacobians[0];
 
                                        btScalar* u_n = &jacobianData_normal.m_deltaVelocitiesUnitImpulse[0];
                                        btScalar* u_t1 = &jacobianData_t1.m_deltaVelocitiesUnitImpulse[0];
                                        btScalar* u_t2 = &jacobianData_t2.m_deltaVelocitiesUnitImpulse[0];
 
                                        btMatrix3x3 rot(normal.getX(), normal.getY(), normal.getZ(),
                                                                        t1.getX(), t1.getY(), t1.getZ(),
                                                                        t2.getX(), t2.getY(), t2.getZ());  // world frame to local frame
                                        const int ndof = multibodyLinkCol->m_multiBody->getNumDofs() + 6;
                                        btMatrix3x3 local_impulse_matrix = (Diagonal(n->m_im) + OuterProduct(J_n, J_t1, J_t2, u_n, u_t1, u_t2, ndof)).inverse();
                                        a.m_c0 = rot.transpose() * local_impulse_matrix * rot;
                                        a.jacobianData_normal = jacobianData_normal;
                                        a.jacobianData_t1 = jacobianData_t1;
                                        a.jacobianData_t2 = jacobianData_t2;
                                        a.t1 = t1;
                                        a.t2 = t2;
                                }
                        }
                }
                psb->interpolateRenderMesh();
        }
}
 
void btDeformableMultiBodyDynamicsWorld::solveConstraints(btScalar timeStep)
{
        BT_PROFILE("btDeformableMultiBodyDynamicsWorld::solveConstraints");
        // save v_{n+1}^* velocity after explicit forces
        m_deformableBodySolver->backupVelocity();
 
        // set up constraints among multibodies and between multibodies and deformable bodies
        setupConstraints();
 
        // solve contact constraints
        solveContactConstraints();
 
        // set up the directions in which the velocity does not change in the momentum solve
        if (m_useProjection)
                m_deformableBodySolver->m_objective->m_projection.setProjection();
        else
                m_deformableBodySolver->m_objective->m_projection.setLagrangeMultiplier();
 
        // for explicit scheme, m_backupVelocity = v_{n+1}^*
        // for implicit scheme, m_backupVelocity = v_n
        // Here, set dv = v_{n+1} - v_n for nodes in contact
        m_deformableBodySolver->setupDeformableSolve(m_implicit);
 
        // At this point, dv should be golden for nodes in contact
        // proceed to solve deformable momentum equation
        m_deformableBodySolver->solveDeformableConstraints(timeStep);
}
 
void btDeformableMultiBodyDynamicsWorld::setupConstraints()
{
        // set up constraints between multibody and deformable bodies
        m_deformableBodySolver->setConstraints(m_solverInfo);
 
        // set up constraints among multibodies
        {
                sortConstraints();
                // setup the solver callback
                btMultiBodyConstraint** sortedMultiBodyConstraints = m_sortedMultiBodyConstraints.size() ? &m_sortedMultiBodyConstraints[0] : 0;
                btTypedConstraint** constraintsPtr = getNumConstraints() ? &m_sortedConstraints[0] : 0;
                m_solverDeformableBodyIslandCallback->setup(&m_solverInfo, constraintsPtr, m_sortedConstraints.size(), sortedMultiBodyConstraints, m_sortedMultiBodyConstraints.size(), getDebugDrawer());
 
                // build islands
                m_islandManager->buildIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld());
        }
}
 
void btDeformableMultiBodyDynamicsWorld::sortConstraints()
{
        m_sortedConstraints.resize(m_constraints.size());
        int i;
        for (i = 0; i < getNumConstraints(); i++)
        {
                m_sortedConstraints[i] = m_constraints[i];
        }
        m_sortedConstraints.quickSort(btSortConstraintOnIslandPredicate2());
 
        m_sortedMultiBodyConstraints.resize(m_multiBodyConstraints.size());
        for (i = 0; i < m_multiBodyConstraints.size(); i++)
        {
                m_sortedMultiBodyConstraints[i] = m_multiBodyConstraints[i];
        }
        m_sortedMultiBodyConstraints.quickSort(btSortMultiBodyConstraintOnIslandPredicate());
}
 
void btDeformableMultiBodyDynamicsWorld::solveContactConstraints()
{
        // process constraints on each island
        m_islandManager->processIslands(getCollisionWorld()->getDispatcher(), getCollisionWorld(), m_solverDeformableBodyIslandCallback);
 
        // process deferred
        m_solverDeformableBodyIslandCallback->processConstraints();
        m_constraintSolver->allSolved(m_solverInfo, m_debugDrawer);
 
        // write joint feedback
        {
                for (int i = 0; i < this->m_multiBodies.size(); i++)
                {
                        btMultiBody* bod = m_multiBodies[i];
 
                        bool isSleeping = false;
 
                        if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
                        {
                                isSleeping = true;
                        }
                        for (int b = 0; b < bod->getNumLinks(); b++)
                        {
                                if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
                                        isSleeping = true;
                        }
 
                        if (!isSleeping)
                        {
                                //useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
                                m_scratch_r.resize(bod->getNumLinks() + 1);  //multidof? ("Y"s use it and it is used to store qdd)
                                m_scratch_v.resize(bod->getNumLinks() + 1);
                                m_scratch_m.resize(bod->getNumLinks() + 1);
 
                                if (bod->internalNeedsJointFeedback())
                                {
                                        if (!bod->isUsingRK4Integration())
                                        {
                                                if (bod->internalNeedsJointFeedback())
                                                {
                                                        bool isConstraintPass = true;
                                                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
                                                                                                                                                                          getSolverInfo().m_jointFeedbackInWorldSpace,
                                                                                                                                                                          getSolverInfo().m_jointFeedbackInJointFrame);
                                                }
                                        }
                                }
                        }
                }
        }
 
        for (int i = 0; i < this->m_multiBodies.size(); i++)
        {
                btMultiBody* bod = m_multiBodies[i];
                bod->processDeltaVeeMultiDof2();
        }
}
 
void btDeformableMultiBodyDynamicsWorld::addSoftBody(btSoftBody* body, int collisionFilterGroup, int collisionFilterMask)
{
        m_softBodies.push_back(body);
 
        // Set the soft body solver that will deal with this body
        // to be the world's solver
        body->setSoftBodySolver(m_deformableBodySolver);
 
        btCollisionWorld::addCollisionObject(body,
                                                                                 collisionFilterGroup,
                                                                                 collisionFilterMask);
}
 
void btDeformableMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
        BT_PROFILE("predictUnconstraintMotion");
        btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
        m_deformableBodySolver->predictMotion(timeStep);
}
 
void btDeformableMultiBodyDynamicsWorld::reinitialize(btScalar timeStep)
{
        m_internalTime += timeStep;
        m_deformableBodySolver->setImplicit(m_implicit);
        m_deformableBodySolver->setLineSearch(m_lineSearch);
        m_deformableBodySolver->reinitialize(m_softBodies, timeStep);
        btDispatcherInfo& dispatchInfo = btMultiBodyDynamicsWorld::getDispatchInfo();
        dispatchInfo.m_timeStep = timeStep;
        dispatchInfo.m_stepCount = 0;
        dispatchInfo.m_debugDraw = btMultiBodyDynamicsWorld::getDebugDrawer();
        btMultiBodyDynamicsWorld::getSolverInfo().m_timeStep = timeStep;
        if (m_useProjection)
        {
                m_deformableBodySolver->m_useProjection = true;
                m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = true;
                m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_massPreconditioner;
        }
        else
        {
                m_deformableBodySolver->m_useProjection = false;
                m_deformableBodySolver->m_objective->m_projection.m_useStrainLimiting = false;
                m_deformableBodySolver->m_objective->m_preconditioner = m_deformableBodySolver->m_objective->m_KKTPreconditioner;
        }
}
 
void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
{
        btMultiBodyDynamicsWorld::debugDrawWorld();
 
        for (int i = 0; i < getSoftBodyArray().size(); i++)
        {
                btSoftBody* psb = (btSoftBody*)getSoftBodyArray()[i];
                {
                        btSoftBodyHelpers::DrawFrame(psb, getDebugDrawer());
                        btSoftBodyHelpers::Draw(psb, getDebugDrawer(), getDrawFlags());
                }
        }
}
 
void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep)
{
        // Gravity is applied in stepSimulation and then cleared here and then applied here and then cleared here again
        // so that 1) gravity is applied to velocity before constraint solve and 2) gravity is applied in each substep
        // when there are multiple substeps
        btMultiBodyDynamicsWorld::applyGravity();
        // integrate rigid body gravity
        for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
        {
                btRigidBody* rb = m_nonStaticRigidBodies[i];
                rb->integrateVelocities(timeStep);
        }
 
        // integrate multibody gravity
        {
                forwardKinematics();
                clearMultiBodyConstraintForces();
                {
                        for (int i = 0; i < this->m_multiBodies.size(); i++)
                        {
                                btMultiBody* bod = m_multiBodies[i];
 
                                bool isSleeping = false;
 
                                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
                                {
                                        isSleeping = true;
                                }
                                for (int b = 0; b < bod->getNumLinks(); b++)
                                {
                                        if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
                                                isSleeping = true;
                                }
 
                                if (!isSleeping)
                                {
                                        m_scratch_r.resize(bod->getNumLinks() + 1);
                                        m_scratch_v.resize(bod->getNumLinks() + 1);
                                        m_scratch_m.resize(bod->getNumLinks() + 1);
                                        bool isConstraintPass = false;
                                        {
                                                if (!bod->isUsingRK4Integration())
                                                {
                                                        bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(m_solverInfo.m_timeStep,
                                                                                                                                                                          m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
                                                                                                                                                                          getSolverInfo().m_jointFeedbackInWorldSpace,
                                                                                                                                                                          getSolverInfo().m_jointFeedbackInJointFrame);
                                                }
                                                else
                                                {
                                                        btAssert(" RK4Integration is not supported");
                                                }
                                        }
                                }
                        }
                }
        }
        clearGravity();
}
 
void btDeformableMultiBodyDynamicsWorld::clearGravity()
{
        BT_PROFILE("btMultiBody clearGravity");
        // clear rigid body gravity
        for (int i = 0; i < m_nonStaticRigidBodies.size(); i++)
        {
                btRigidBody* body = m_nonStaticRigidBodies[i];
                if (body->isActive())
                {
                        body->clearGravity();
                }
        }
        // clear multibody gravity
        for (int i = 0; i < this->m_multiBodies.size(); i++)
        {
                btMultiBody* bod = m_multiBodies[i];
 
                bool isSleeping = false;
 
                if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
                {
                        isSleeping = true;
                }
                for (int b = 0; b < bod->getNumLinks(); b++)
                {
                        if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState() == ISLAND_SLEEPING)
                                isSleeping = true;
                }
 
                if (!isSleeping)
                {
                        bod->addBaseForce(-m_gravity * bod->getBaseMass());
 
                        for (int j = 0; j < bod->getNumLinks(); ++j)
                        {
                                bod->addLinkForce(j, -m_gravity * bod->getLinkMass(j));
                        }
                }
        }
}
 
void btDeformableMultiBodyDynamicsWorld::beforeSolverCallbacks(btScalar timeStep)
{
        if (0 != m_internalTickCallback)
        {
                (*m_internalTickCallback)(this, timeStep);
        }
 
        if (0 != m_solverCallback)
        {
                (*m_solverCallback)(m_internalTime, this);
        }
}
 
void btDeformableMultiBodyDynamicsWorld::afterSolverCallbacks(btScalar timeStep)
{
        if (0 != m_solverCallback)
        {
                (*m_solverCallback)(m_internalTime, this);
        }
}
 
void btDeformableMultiBodyDynamicsWorld::addForce(btSoftBody* psb, btDeformableLagrangianForce* force)
{
        btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
        bool added = false;
        for (int i = 0; i < forces.size(); ++i)
        {
                if (forces[i]->getForceType() == force->getForceType())
                {
                        forces[i]->addSoftBody(psb);
                        added = true;
                        break;
                }
        }
        if (!added)
        {
                force->addSoftBody(psb);
                force->setIndices(m_deformableBodySolver->m_objective->getIndices());
                forces.push_back(force);
        }
}
 
void btDeformableMultiBodyDynamicsWorld::removeForce(btSoftBody* psb, btDeformableLagrangianForce* force)
{
        btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
        int removed_index = -1;
        for (int i = 0; i < forces.size(); ++i)
        {
                if (forces[i]->getForceType() == force->getForceType())
                {
                        forces[i]->removeSoftBody(psb);
                        if (forces[i]->m_softBodies.size() == 0)
                                removed_index = i;
                        break;
                }
        }
        if (removed_index >= 0)
                forces.removeAtIndex(removed_index);
}
 
void btDeformableMultiBodyDynamicsWorld::removeSoftBodyForce(btSoftBody* psb)
{
        btAlignedObjectArray<btDeformableLagrangianForce*>& forces = m_deformableBodySolver->m_objective->m_lf;
        for (int i = 0; i < forces.size(); ++i)
        {
                forces[i]->removeSoftBody(psb);
        }
}
 
void btDeformableMultiBodyDynamicsWorld::removeSoftBody(btSoftBody* body)
{
        removeSoftBodyForce(body);
        m_softBodies.remove(body);
        btCollisionWorld::removeCollisionObject(body);
        // force a reinitialize so that node indices get updated.
        m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
}
 
void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)
{
        btSoftBody* body = btSoftBody::upcast(collisionObject);
        if (body)
                removeSoftBody(body);
        else
                btDiscreteDynamicsWorld::removeCollisionObject(collisionObject);
}
 
int btDeformableMultiBodyDynamicsWorld::stepSimulation(btScalar timeStep, int maxSubSteps, btScalar fixedTimeStep)
{
        startProfiling(timeStep);
 
        int numSimulationSubSteps = 0;
 
        if (maxSubSteps)
        {
                //fixed timestep with interpolation
                m_fixedTimeStep = fixedTimeStep;
                m_localTime += timeStep;
                if (m_localTime >= fixedTimeStep)
                {
                        numSimulationSubSteps = int(m_localTime / fixedTimeStep);
                        m_localTime -= numSimulationSubSteps * fixedTimeStep;
                }
        }
        else
        {
                //variable timestep
                fixedTimeStep = timeStep;
                m_localTime = m_latencyMotionStateInterpolation ? 0 : timeStep;
                m_fixedTimeStep = 0;
                if (btFuzzyZero(timeStep))
                {
                        numSimulationSubSteps = 0;
                        maxSubSteps = 0;
                }
                else
                {
                        numSimulationSubSteps = 1;
                        maxSubSteps = 1;
                }
        }
 
        //process some debugging flags
        if (getDebugDrawer())
        {
                btIDebugDraw* debugDrawer = getDebugDrawer();
                gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;
        }
        if (numSimulationSubSteps)
        {
                //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
                int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
 
                saveKinematicState(fixedTimeStep * clampedSimulationSteps);
 
                for (int i = 0; i < clampedSimulationSteps; i++)
                {
                        internalSingleStepSimulation(fixedTimeStep);
                        synchronizeMotionStates();
                }
        }
        else
        {
                synchronizeMotionStates();
        }
 
        clearForces();
 
#ifndef BT_NO_PROFILE
        CProfileManager::Increment_Frame_Counter();
#endif  //BT_NO_PROFILE
 
        return numSimulationSubSteps;
}