diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 45ccf3196..c943a360c 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6201,6 +6201,16 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS"); + if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION) + { + if (clientCmd.m_physSimParamArgs.m_enableConeFriction) + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } else + { + m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION; + } + } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) { m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c16733e45..b3d8d8c49 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -394,6 +394,8 @@ enum EnumSimDesiredStateUpdateFlags }; + + enum EnumSimParamUpdateFlags { SIM_PARAM_UPDATE_DELTA_TIME=1, @@ -407,7 +409,7 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256, SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512, SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024, - SIM_PARAM_MAX_CMD_PER_1MS = 2048, + SIM_PARAM_ENABLE_CONE_FRICTION = 2048, SIM_PARAM_ENABLE_FILE_CACHING = 4096, SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD = 8192, SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP=16384, diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b3e1b779f..a7e128e62 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -692,6 +692,7 @@ struct b3PhysicsSimulationParameters double m_restitutionVelocityThreshold; double m_defaultNonContactERP; double m_frictionERP; + int m_enableConeFriction; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 71526de92..bde55bce6 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1,4 +1,3 @@ - #include "../SharedMemory/PhysicsClientC_API.h" #include "../SharedMemory/PhysicsDirectC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 28d0c1dd4..93865cbc5 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -29,7 +29,8 @@ enum btSolverMode SOLVER_CACHE_FRIENDLY = 128, SOLVER_SIMD = 256, SOLVER_INTERLEAVE_CONTACT_AND_FRICTION_CONSTRAINTS = 512, - SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024 + SOLVER_ALLOW_ZERO_LENGTH_FRICTION_DIRECTIONS = 1024, + SOLVER_DISABLE_IMPLICIT_CONE_FRICTION = 2048 }; struct btContactSolverInfoData diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 7d8d21ef7..3f3a73153 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -78,7 +78,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl btMultiBodySolverConstraint& frictionConstraint = m_multiBodyLateralFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS) + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION)==0)) { j1++; int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; @@ -91,7 +91,11 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse); frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse; - resolveConeFrictionConstraintRows(frictionConstraint,frictionConstraintB); + leastSquaredResidual += resolveConeFrictionConstraintRows(frictionConstraint,frictionConstraintB); + if(frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if(frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); } } else @@ -112,6 +116,30 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } } } + + for (int j1=0;j1m_multiBodyTorsionalFrictionContactConstraints.size();j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual += residual*residual; + + if(frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if(frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } return leastSquaredResidual; }