diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 862a75557..917cb3579 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1607,6 +1607,16 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_physicsDeltaTime; btScalar m_numSimulationSubSteps; + + btScalar getDeltaTimeSubStep() const + { + btScalar deltaTimeSubStep = m_numSimulationSubSteps > 0 ? + m_physicsDeltaTime / m_numSimulationSubSteps : + m_physicsDeltaTime; + return deltaTimeSubStep; + } + + btScalar m_simulationTimestamp; btAlignedObjectArray m_multiBodyJointFeedbacks; b3HashMap m_inverseDynamicsBodies; @@ -6795,10 +6805,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct //disable velocity clamp in velocity mode motor->setRhsClamp(SIMD_INFINITY); - btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f * m_data->getDeltaTimeSubStep(); if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) { - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] * m_data->m_physicsDeltaTime; + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] * m_data->getDeltaTimeSubStep(); } motor->setMaxAppliedImpulse(maxImp); } @@ -6878,10 +6888,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct } motor->setPositionTarget(desiredPosition, kp); - btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f * m_data->getDeltaTimeSubStep(); if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime; + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->getDeltaTimeSubStep(); motor->setMaxAppliedImpulse(maxImp); } @@ -6946,10 +6956,10 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct //} motor->setPositionTarget(desiredPosition, kp); - btScalar maxImp = 1000000.f * m_data->m_physicsDeltaTime; + btScalar maxImp = 1000000.f * m_data->getDeltaTimeSubStep(); if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE) != 0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->m_physicsDeltaTime; + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex] * m_data->getDeltaTimeSubStep(); motor->setMaxAppliedImpulse(maxImp); } @@ -7060,7 +7070,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct Eigen::MatrixXd M = rbdModel->GetMassMat(); //rbdModel->UpdateBiasForce(); const Eigen::VectorXd& C = rbdModel->GetBiasForce(); - M.diagonal() += m_data->m_physicsDeltaTime * mKd; + M.diagonal() += m_data->getDeltaTimeSubStep() * mKd; Eigen::VectorXd pose_inc; @@ -7462,7 +7472,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor) { btScalar impulse = motor->getAppliedImpulse(d); - btScalar force = impulse / m_data->m_physicsDeltaTime; + btScalar force = impulse / m_data->getDeltaTimeSubStep(); stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; } } @@ -7474,7 +7484,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor && m_data->m_physicsDeltaTime > btScalar(0)) { - btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; + btScalar force = motor->getAppliedImpulse(0) / m_data->getDeltaTimeSubStep(); stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force; } } @@ -7512,7 +7522,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc if (motor && m_data->m_physicsDeltaTime > btScalar(0)) { - btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime; + btScalar force = motor->getAppliedImpulse(0) / m_data->getDeltaTimeSubStep(); stateDetails->m_jointMotorForce[l] = force; //if (force>0) @@ -7814,9 +7824,9 @@ bool PhysicsServerCommandProcessor::processRequestContactpointInformationCommand pt.m_positionOnBInWS[j] = srcPt.getPositionWorldOnB()[j]; } } - pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->m_physicsDeltaTime; - pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->m_physicsDeltaTime; - pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->m_physicsDeltaTime; + pt.m_normalForce = srcPt.getAppliedImpulse() / m_data->getDeltaTimeSubStep(); + pt.m_linearFrictionForce1 = srcPt.m_appliedImpulseLateral1 / m_data->getDeltaTimeSubStep(); + pt.m_linearFrictionForce2 = srcPt.m_appliedImpulseLateral2 / m_data->getDeltaTimeSubStep(); for (int j = 0; j < 3; j++) { pt.m_linearFrictionDirection1[j] = srcPt.m_lateralFrictionDir1[j];