AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge pull request #2815 from erwincoumans/master

Browse Source 
in case of substeps use the compensated delta time / numSubSteps to c…
This commit is contained in:
erwincoumans 2020-05-20 07:20:21 -07:00 committed by GitHub
commit de7080f5cc
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 27 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
b3HashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> 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];