From 84117b84409eda20ac209c04c3e89f12aaff77f7 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 26 Feb 2020 14:11:41 -0800 Subject: [PATCH 1/2] separate motor constraint iterations and expose API --- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 6 +++++ examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 1 + .../ConstraintSolver/btContactSolverInfo.h | 2 ++ .../btMultiBodyConstraintSolver.cpp | 22 ++++++++++--------- 7 files changed, 32 insertions(+), 10 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 3a0a8b994..bcb573348 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f1d911266..ba1fae67c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 07bb9d13e..504321980 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index a39ebfa08..7dba9a47c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -487,6 +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, }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index d3e797af3..461f0b4c5 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -1006,6 +1006,7 @@ struct b3PhysicsSimulationParameters int m_minimumSolverIslandSize; int m_reportSolverAnalytics; double m_sparseSdfVoxelSize; + int m_numMotorIterations; }; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 50c09d83e..e64cc1c5a 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -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 } }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 6c1ed0959..7322b2021 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -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 From 7ad19fde6f7e77a7d98b7ecd34e321c910e3341b Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Wed, 26 Feb 2020 14:16:51 -0800 Subject: [PATCH 2/2] formatting --- examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 8 ++++---- examples/SharedMemory/SharedMemoryCommands.h | 3 +-- examples/SharedMemory/SharedMemoryPublic.h | 2 +- 4 files changed, 7 insertions(+), 8 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ba1fae67c..66938e0ca 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -341,7 +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 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 504321980..c6db34cac 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9455,10 +9455,10 @@ 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_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) { diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 7dba9a47c..98744a543 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -487,8 +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, - + SIM_PARAM_UPDATE_NUM_MOTOR_ITERATIONS = 1 << 30, }; enum EnumLoadSoftBodyUpdateFlags diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 461f0b4c5..7855b3df4 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -1006,7 +1006,7 @@ struct b3PhysicsSimulationParameters int m_minimumSolverIslandSize; int m_reportSolverAnalytics; double m_sparseSdfVoxelSize; - int m_numMotorIterations; + int m_numMotorIterations; };