From 32277c7bd5ad87bb744cb360d9b161908424aed0 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 19 May 2020 11:00:53 -0700 Subject: [PATCH 1/2] in case of substeps use the compensated delta time / numSubSteps to convert between force and impulse. --- .../PhysicsServerCommandProcessor.cpp | 36 ++++++++++++------- 1 file changed, 23 insertions(+), 13 deletions(-) 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]; From 7d61b3514c6be6a9cb24e9626e152aea5f32d36c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 20 May 2020 07:19:26 -0700 Subject: [PATCH 2/2] use memcpy instead of manual copy, alignment issue? --- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 917cb3579..4c2d92e35 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5322,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;