diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index bfd4705c6..116357390 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/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;im_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;lgetNumLinks();l++) { + for (int d=0;dgetLink(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); + } + } + } + } +} + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.h b/examples/SharedMemory/PhysicsServerCommandProcessor.h index 34b731de8..dca5f8930 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.h +++ b/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