Merge pull request #2478 from xhan0619/fix-pd

fix the bug that prevents the pd control forces/torques being added

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -9261,8 +9261,7 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 				{
 				}
 			};
-
-#ifdef SKIP_DEFORMABLE_BODY
+            
 			if (newSolver)
 			{
 				delete oldSolver;
@@ -9271,7 +9270,6 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
 				m_data->m_solver = newSolver;
 				printf("switched solver\n");
 			}
-#endif
 		}
 	}
 

src/BulletDynamics/Dynamics/btRigidBody.cpp

@@ -206,6 +206,14 @@ void btRigidBody::applyGravity()
 	applyCentralForce(m_gravity);
 }
 
+void btRigidBody::clearGravity()
+{
+    if (isStaticOrKinematicObject())
+        return;
+    
+    applyCentralForce(-m_gravity);
+}
+
 void btRigidBody::proceedToTransform(const btTransform& newTrans)
 {
 	setCenterOfMassTransform(newTrans);

src/BulletDynamics/Dynamics/btRigidBody.h

@@ -205,6 +205,8 @@ public:
 	void saveKinematicState(btScalar step);
 
 	void applyGravity();
+    
+    void clearGravity();
 
 	void setGravity(const btVector3& acceleration);
 

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.cpp

@@ -383,8 +383,6 @@ 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
-    clearForces();
-    clearMultiBodyForces();
     btMultiBodyDynamicsWorld::applyGravity();
     // integrate rigid body gravity
     for (int i = 0; i < m_nonStaticRigidBodies.size(); ++i)
@@ -437,8 +435,48 @@ void btDeformableMultiBodyDynamicsWorld::applyRigidBodyGravity(btScalar timeStep
             }
         }
     }
-    clearForces();
-    clearMultiBodyForces();
+    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)
@@ -499,3 +537,72 @@ void btDeformableMultiBodyDynamicsWorld::removeCollisionObject(btCollisionObject
     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;
+}

src/BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h

@@ -63,7 +63,9 @@ protected:
     void solveConstraints(btScalar timeStep);
     
     void updateActivationState(btScalar timeStep);
-
+    
+    void clearGravity();
+    
 public:
     btDeformableMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btDeformableMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration, btDeformableBodySolver* deformableBodySolver = 0)
     : btMultiBodyDynamicsWorld(dispatcher, pairCache, (btMultiBodyConstraintSolver*)constraintSolver, collisionConfiguration),
@@ -90,6 +92,8 @@ public:
         m_selfCollision = true;
     }
 
+    virtual int stepSimulation(btScalar timeStep, int maxSubSteps = 1, btScalar fixedTimeStep = btScalar(1.) / btScalar(60.));
+    
     void setSolverCallback(btSolverCallback cb)
     {
         m_solverCallback = cb;