Merge pull request #44455 from akien-mga/bullet-3.07

bullet: Sync with upstream 3.07

1 files changed, 538 insertions, 532 deletions

diff --git a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
index 6b742978ef..983e622b5f 100644
--- a/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
+++ b/thirdparty/bullet/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp
@@ -40,8 +40,9 @@ The algorithm also closely resembles the one in http://physbam.stanford.edu/~fed
 #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)
+	: btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
+	  m_deformableBodySolver(deformableBodySolver),
+	  m_solverCallback(0)
 {
 	m_drawFlags = fDrawFlags::Std;
 	m_drawNodeTree = true;
@@ -52,7 +53,7 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
 	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;
@@ -61,57 +62,57 @@ m_deformableBodySolver(deformableBodySolver), m_solverCallback(0)
 	m_internalTime = 0.0;
 	m_implicit = false;
 	m_lineSearch = false;
-    m_useProjection = true;
+	m_useProjection = false;
 	m_ccdIterations = 5;
 	m_solverDeformableBodyIslandCallback = new DeformableBodyInplaceSolverIslandCallback(constraintSolver, dispatcher);
 }
 
 btDeformableMultiBodyDynamicsWorld::~btDeformableMultiBodyDynamicsWorld()
 {
-    delete m_solverDeformableBodyIslandCallback;
+	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);
-    
-    ///apply gravity and explicit force to velocity, predict motion
-    predictUnconstraintMotion(timeStep);
-    
-    ///perform collision detection that involves rigid/multi bodies
-    btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
-    
-    btMultiBodyDynamicsWorld::calculateSimulationIslands();
-    
-    beforeSolverCallbacks(timeStep);
-    
-    ///solve contact constraints and then deformable bodies momemtum equation
-    solveConstraints(timeStep);
-    
-    afterSolverCallbacks(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);
+
+	///apply gravity and explicit force to velocity, predict motion
+	predictUnconstraintMotion(timeStep);
+
+	///perform collision detection that involves rigid/multi bodies
+	btMultiBodyDynamicsWorld::performDiscreteCollisionDetection();
+
+	btMultiBodyDynamicsWorld::calculateSimulationIslands();
+
+	beforeSolverCallbacks(timeStep);
+
+	///solve contact constraints and then deformable bodies momemtum equation
+	solveConstraints(timeStep);
+
+	afterSolverCallbacks(timeStep);
 
 	performDeformableCollisionDetection();
 
-    applyRepulsionForce(timeStep);
+	applyRepulsionForce(timeStep);
+
+	performGeometricCollisions(timeStep);
+
+	integrateTransforms(timeStep);
 
-    performGeometricCollisions(timeStep);
+	///update vehicle simulation
+	btMultiBodyDynamicsWorld::updateActions(timeStep);
 
-    integrateTransforms(timeStep);
-    
-    ///update vehicle simulation
-    btMultiBodyDynamicsWorld::updateActions(timeStep);
-    
-    updateActivationState(timeStep);
-    // End solver-wise simulation step
-    // ///////////////////////////////
+	updateActivationState(timeStep);
+	// End solver-wise simulation step
+	// ///////////////////////////////
 }
 
 void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
@@ -120,7 +121,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 	{
 		m_softBodies[i]->m_softSoftCollision = true;
 	}
-	
+
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		for (int j = i; j < m_softBodies.size(); ++j)
@@ -128,7 +129,7 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 			m_softBodies[i]->defaultCollisionHandler(m_softBodies[j]);
 		}
 	}
-	
+
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		m_softBodies[i]->m_softSoftCollision = false;
@@ -137,45 +138,45 @@ void btDeformableMultiBodyDynamicsWorld::performDeformableCollisionDetection()
 
 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);
+	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())
-        {
+	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
+	// refit the BVH tree for CCD
 	for (int i = 0; i < m_softBodies.size(); ++i)
 	{
 		btSoftBody* psb = m_softBodies[i];
@@ -214,7 +215,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 					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)
@@ -228,7 +229,7 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 					m_softBodies[i]->geometricCollisionHandler(m_softBodies[j]);
 				}
 			}
-        }
+		}
 
 		int penetration_count = 0;
 		for (int i = 0; i < m_softBodies.size(); ++i)
@@ -258,294 +259,292 @@ void btDeformableMultiBodyDynamicsWorld::performGeometricCollisions(btScalar tim
 
 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);
-        }
-    }
+	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);
-        }
-    }
+	// 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_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();
-    }
+	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);
+	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());
-    }
+	// 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());
+	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();
-    }
+	// 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);
+	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);
+	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_objective->m_preconditioner =  m_deformableBodySolver->m_objective->m_KKTPreconditioner;
-    }
-        
+	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++)
@@ -556,253 +555,260 @@ void btDeformableMultiBodyDynamicsWorld::debugDrawWorld()
 			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();
+	// 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));
-            }
-        }
-    }
+	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);
-    }
+	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);
-    }
+	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);
-    }
+	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);
+	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)
 {
-    m_softBodies.remove(body);
-    btCollisionWorld::removeCollisionObject(body);
-    // force a reinitialize so that node indices get updated.
-    m_deformableBodySolver->reinitialize(m_softBodies, btScalar(-1));
+	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);
+	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();
-    
+	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();
+	CProfileManager::Increment_Frame_Counter();
 #endif  //BT_NO_PROFILE
-    
-    return numSimulationSubSteps;
+
+	return numSimulationSubSteps;
 }