Merge pull request #2645 from xhan0619/master

Separate motor constraint iterations and expose API.
This commit is contained in:
erwincoumans
2020-03-01 21:48:22 -08:00
committed by GitHub
7 changed files with 32 additions and 11 deletions
@@ -771,6 +771,15 @@ B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHand
return 0;
}
B3_SHARED_API int b3PhysicsParamSetNumMotorIterations(b3SharedMemoryCommandHandle commandHandle, int numMotorIterations)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
command->m_physSimParamArgs.m_numMotorIterations = numMotorIterations;
command->m_updateFlags |= SIM_PARAM_UPDATE_NUM_MOTOR_ITERATIONS;
return 0;
}
B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
@@ -341,6 +341,7 @@ extern "C"
B3_SHARED_API int b3PhysicsParamSetNumSubSteps(b3SharedMemoryCommandHandle commandHandle, int numSubSteps);
B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation);
B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations);
B3_SHARED_API int b3PhysicsParamSetNumMotorIterations(b3SharedMemoryCommandHandle commandHandle, int numMotorIterations);
B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor);
B3_SHARED_API int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHandle, int filterMode);
@@ -9355,6 +9355,7 @@ bool PhysicsServerCommandProcessor::processRequestPhysicsSimulationParametersCom
serverCmd.m_simulationParameterResultArgs.m_numSimulationSubSteps = m_data->m_numSimulationSubSteps;
serverCmd.m_simulationParameterResultArgs.m_numSolverIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numIterations;
serverCmd.m_simulationParameterResultArgs.m_numMotorIterations = m_data->m_dynamicsWorld->getSolverInfo().m_numMotorIterations;
serverCmd.m_simulationParameterResultArgs.m_restitutionVelocityThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold;
serverCmd.m_simulationParameterResultArgs.m_solverResidualThreshold = m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold;
@@ -9453,6 +9454,11 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
{
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_NUM_MOTOR_ITERATIONS)
{
m_data->m_dynamicsWorld->getSolverInfo().m_numMotorIterations = clientCmd.m_physSimParamArgs.m_numMotorIterations;
}
if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD)
{
+1 -1
View File
@@ -487,7 +487,7 @@ enum EnumSimParamUpdateFlags
SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27,
SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28,
SIM_PARAM_UPDATE_SPARSE_SDF = 1 << 29,
SIM_PARAM_UPDATE_NUM_MOTOR_ITERATIONS = 1 << 30,
};
enum EnumLoadSoftBodyUpdateFlags
@@ -1006,6 +1006,7 @@ struct b3PhysicsSimulationParameters
int m_minimumSolverIslandSize;
int m_reportSolverAnalytics;
double m_sparseSdfVoxelSize;
int m_numMotorIterations;
};
@@ -67,6 +67,7 @@ struct btContactSolverInfoData
bool m_jointFeedbackInWorldSpace;
bool m_jointFeedbackInJointFrame;
int m_reportSolverAnalytics;
int m_numMotorIterations;
};
struct btContactSolverInfo : public btContactSolverInfoData
@@ -104,6 +105,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_jointFeedbackInWorldSpace = false;
m_jointFeedbackInJointFrame = false;
m_reportSolverAnalytics = 0;
m_numMotorIterations = 1; // the number of inner iterations for solving motor constraint in a single iteration of the constraint solve
}
};
@@ -32,20 +32,22 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone non-contact constraints
//printf("m_multiBodyNonContactConstraints = %d\n",m_multiBodyNonContactConstraints.size());
for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
for (int i = 0; i < infoGlobal.m_numMotorIterations; ++i)
{
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
for (int j = 0; j < m_multiBodyNonContactConstraints.size(); j++)
{
int index = iteration & 1 ? j : m_multiBodyNonContactConstraints.size() - 1 - j;
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
btMultiBodySolverConstraint& constraint = m_multiBodyNonContactConstraints[index];
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
btScalar residual = resolveSingleConstraintRowGeneric(constraint);
leastSquaredResidual = btMax(leastSquaredResidual, residual * residual);
if (constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if (constraint.m_multiBodyB)
constraint.m_multiBodyB->setPosUpdated(false);
if (constraint.m_multiBodyA)
constraint.m_multiBodyA->setPosUpdated(false);
if (constraint.m_multiBodyB)
constraint.m_multiBodyB->setPosUpdated(false);
}
}
//solve featherstone normal contact