add joint damping in PhysicsServerCommandProcessor

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -1415,6 +1415,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 					{
 						b3Printf("Step simulation request");
 					}
+                    ///todo(erwincoumans) move this damping inside Bullet
+                    for (int i=0;i<m_data->m_bodyHandles.size();i++)
+                    {
+                        applyJointDamping(i);
+                    }
                     m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0);
                     
 					SharedMemoryStatus& serverCmd =serverStatusOut;
@@ -1883,3 +1888,20 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
 	m_data->m_logPlayback = pb;
 }
 
+void PhysicsServerCommandProcessor::applyJointDamping(int bodyUniqueId)
+{
+    InteralBodyData* body = m_data->getHandle(bodyUniqueId);
+    if (body) {
+        btMultiBody* mb = body->m_multiBody;
+        if (mb) {
+            for (int l=0;l<mb->getNumLinks();l++) {
+                for (int d=0;d<mb->getLink(l).m_dofCount;d++) {
+                    double damping_coefficient = mb->getLink(l).m_jointDamping;
+                    double damping = -damping_coefficient*mb->getJointVelMultiDof(l)[d];
+                    mb->addJointTorqueMultiDof(l, d, damping);
+                }
+            }
+        }
+    }
+}
+

examples/SharedMemory/PhysicsServerCommandProcessor.h

@@ -48,7 +48,7 @@ public:
 	void enableCommandLogging(bool enable, const char* fileName);
 	void replayFromLogFile(const char* fileName);
     void replayLogCommand(char* bufferServerToClient, int bufferSizeInBytes );
-
+	void applyJointDamping(int bodyUniqueId);
 };
 
 #endif //PHYSICS_SERVER_COMMAND_PROCESSOR_H