From 913400eba1ab808a3fbf43c393425344ef66cee4 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Thu, 17 Oct 2019 16:59:15 -0700 Subject: [PATCH] add SOLVER_USE_ARTICULATED_WARMSTARTING option and APIs fix compile --- examples/SharedMemory/PhysicsClientC_API.cpp | 8 +++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 6 ++++ examples/SharedMemory/SharedMemoryCommands.h | 1 + examples/SharedMemory/SharedMemoryPublic.h | 1 + .../NarrowPhaseCollision/btManifoldPoint.h | 4 +-- .../btPersistentManifold.cpp | 2 +- .../btPersistentManifold.h | 2 +- .../ConstraintSolver/btContactSolverInfo.h | 8 +++-- .../btMultiBodyConstraintSolver.cpp | 30 ++++--------------- 10 files changed, 33 insertions(+), 30 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 085a0384b..9677197ca 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -752,6 +752,14 @@ B3_SHARED_API int b3PhysicsParamSetWarmStartingFactor(b3SharedMemoryCommandHandl return 0; } +B3_SHARED_API int b3PhysicsParamSetArticulatedWarmStartingFactor(b3SharedMemoryCommandHandle commandHandle, double warmStartingFactor) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_articulatedWarmStartingFactor = warmStartingFactor; + command->m_updateFlags |= SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR; + return 0; +} B3_SHARED_API int b3PhysicsParamSetSolverResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double solverResidualThreshold) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 752789428..4e0e0eb63 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -339,6 +339,7 @@ extern "C" B3_SHARED_API int b3PhysicsParamSetRealTimeSimulation(b3SharedMemoryCommandHandle commandHandle, int enableRealTimeSimulation); B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations); 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); B3_SHARED_API int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse); B3_SHARED_API int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 9e5799c36..437584951 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -9338,6 +9338,12 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st { m_data->m_dynamicsWorld->getSolverInfo().m_warmstartingFactor = clientCmd.m_physSimParamArgs.m_warmStartingFactor; } + + if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR) + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |= SOLVER_USE_ARTICULATED_WARMSTARTING; + m_data->m_dynamicsWorld->getSolverInfo().m_articulatedWarmstartingFactor = clientCmd.m_physSimParamArgs.m_articulatedWarmStartingFactor; + } SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; return hasStatus; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index d23ac1594..610bfe275 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -483,6 +483,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_CONSTRAINT_MIN_SOLVER_ISLAND_SIZE = 1 << 25, SIM_PARAM_REPORT_CONSTRAINT_SOLVER_ANALYTICS = 1 << 26, SIM_PARAM_UPDATE_WARM_STARTING_FACTOR = 1 << 27, + SIM_PARAM_UPDATE_ARTICULATED_WARM_STARTING_FACTOR = 1 << 28, }; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 78c0c0fcb..3db75d397 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -947,6 +947,7 @@ struct b3PhysicsSimulationParameters int m_numSimulationSubSteps; int m_numSolverIterations; double m_warmStartingFactor; + double m_articulatedWarmStartingFactor; int m_useRealTimeSimulation; int m_useSplitImpulse; double m_splitImpulsePenetrationThreshold; diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 66f38c9dd..573fc86bf 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -56,7 +56,7 @@ public: m_contactPointFlags(0), m_appliedImpulse(0.f), m_prevRHS(0.f), - m_appliedImpulseLateral1(0.f), + m_appliedImpulseLateral1(0.f), m_appliedImpulseLateral2(0.f), m_contactMotion1(0.f), m_contactMotion2(0.f), @@ -117,7 +117,7 @@ public: btScalar m_appliedImpulse; btScalar m_prevRHS; - btScalar m_appliedImpulseLateral1; + btScalar m_appliedImpulseLateral1; btScalar m_appliedImpulseLateral2; btScalar m_contactMotion1; btScalar m_contactMotion2; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index d1fad7a9d..dca3e0926 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -326,7 +326,7 @@ const char* btPersistentManifold::serialize(const class btPersistentManifold* ma const btManifoldPoint& pt = manifold->getContactPoint(i); dataOut->m_pointCacheAppliedImpulse[i] = pt.m_appliedImpulse; dataOut->m_pointCachePrevRHS[i] = pt.m_prevRHS; - dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; + dataOut->m_pointCacheAppliedImpulseLateral1[i] = pt.m_appliedImpulseLateral1; dataOut->m_pointCacheAppliedImpulseLateral2[i] = pt.m_appliedImpulseLateral2; pt.m_localPointA.serialize(dataOut->m_pointCacheLocalPointA[i]); pt.m_localPointB.serialize(dataOut->m_pointCacheLocalPointB[i]); diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index feb149af6..0e26da0eb 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -280,7 +280,7 @@ struct btPersistentManifoldDoubleData double m_pointCacheDistance[4]; double m_pointCacheAppliedImpulse[4]; double m_pointCachePrevRHS[4]; - double m_pointCacheCombinedFriction[4]; + double m_pointCacheCombinedFriction[4]; double m_pointCacheCombinedRollingFriction[4]; double m_pointCacheCombinedSpinningFriction[4]; double m_pointCacheCombinedRestitution[4]; diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 63d7c98e1..072b0463a 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -30,7 +30,8 @@ enum btSolverMode SOLVER_SIMD = 256, SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024, - SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048 + SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048, + SOLVER_USE_ARTICULATED_WARMSTARTING = 4096, }; struct btContactSolverInfoData @@ -54,7 +55,7 @@ struct btContactSolverInfoData btScalar m_splitImpulseTurnErp; btScalar m_linearSlop; btScalar m_warmstartingFactor; - + btScalar m_articulatedWarmstartingFactor; int m_solverMode; int m_restingContactRestitutionThreshold; int m_minimumSolverBatchSize; @@ -89,6 +90,7 @@ struct btContactSolverInfo : public btContactSolverInfoData m_splitImpulseTurnErp = 0.1f; m_linearSlop = btScalar(0.0); m_warmstartingFactor = btScalar(0.85); + m_articulatedWarmstartingFactor = btScalar(0); //m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD | SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION|SOLVER_USE_2_FRICTION_DIRECTIONS|SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;// | SOLVER_RANDMIZE_ORDER; m_solverMode = SOLVER_USE_WARMSTARTING | SOLVER_SIMD; // | SOLVER_RANDMIZE_ORDER; m_restingContactRestitutionThreshold = 2; //unused as of 2.81 @@ -120,6 +122,7 @@ struct btContactSolverInfoDoubleData double m_splitImpulseTurnErp; double m_linearSlop; double m_warmstartingFactor; + double m_articulatedWarmstartingFactor; double m_maxGyroscopicForce; ///it is only used for 'explicit' version of gyroscopic force double m_singleAxisRollingFrictionThreshold; @@ -150,6 +153,7 @@ struct btContactSolverInfoFloatData float m_linearSlop; float m_warmstartingFactor; + float m_articulatedWarmstartingFactor; float m_maxGyroscopicForce; float m_singleAxisRollingFrictionThreshold; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index e10fea5a6..b9493896a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -835,7 +835,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { if (btFabs(cp.m_prevRHS) > 1e-5) { - solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_warmstartingFactor; + solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse / cp.m_prevRHS * solverConstraint.m_rhs * infoGlobal.m_articulatedWarmstartingFactor; if (solverConstraint.m_appliedImpulse < 0) solverConstraint.m_appliedImpulse = 0; } @@ -1438,7 +1438,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol } // Warmstart for noncontact constraints - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) { for (int i = 0; i < m_multiBodyNonContactConstraints.size(); i++) { @@ -1446,7 +1446,7 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol m_multiBodyNonContactConstraints[i]; solverConstraint.m_appliedImpulse = solverConstraint.m_orgConstraint->getAppliedImpulse(solverConstraint.m_orgDofIndex) * - infoGlobal.m_warmstartingFactor; + infoGlobal.m_articulatedWarmstartingFactor; btMultiBody* multiBodyA = solverConstraint.m_multiBodyA; btMultiBody* multiBodyB = solverConstraint.m_multiBodyB; @@ -1603,28 +1603,10 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO writeBackSolverBodyToMultiBody(solverConstraint, infoGlobal.m_timeStep); } - if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING) + if (infoGlobal.m_solverMode & SOLVER_USE_ARTICULATED_WARMSTARTING) { BT_PROFILE("warm starting write back"); - for (int j = 0; j < numPoolConstraints; j++) - { - const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j]; - btManifoldPoint* pt = (btManifoldPoint*)solverConstraint.m_originalContactPoint; - btAssert(pt); - pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; - pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse; - - //printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1); - if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) - { - pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex + 1].m_appliedImpulse; - } else - { - pt->m_appliedImpulseLateral2 = 0; - } - } - - //do a callback here? + writeBackMultiBodyContacts(0, numPoolConstraints); } #if 0 //multibody joint feedback @@ -1725,7 +1707,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO } #endif #endif - writeBackMultiBodyContacts(0, numPoolConstraints); + return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies, numBodies, infoGlobal); }