diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 862a75557..4c2d92e35 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; @@ -5312,10 +5322,12 @@ bool PhysicsServerCommandProcessor::processRequestMeshDataCommand(const struct S int maxNumVertices = bufferSizeInBytes / totalBytesPerVertex - 1; int numVerticesRemaining = numVertices - clientCmd.m_requestMeshDataArgs.m_startingVertex; int verticesCopied = btMin(maxNumVertices, numVerticesRemaining); - for (int i = 0; i < verticesCopied; ++i) + + if (verticesCopied > 0) { - verticesOut[i] = vertices[i]; + memcpy(verticesOut, &vertices[0], sizeof(btVector3) * verticesCopied); } + sizeInBytes = verticesCopied * sizeof(btVector3); serverStatusOut.m_type = CMD_REQUEST_MESH_DATA_COMPLETED; serverStatusOut.m_sendMeshDataArgs.m_numVerticesCopied = verticesCopied; @@ -6795,10 +6807,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 +6890,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 +6958,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 +7072,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 +7474,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 +7486,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 +7524,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 +7826,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];