fixes related to torsional friction, due to recent cone friction update.

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;

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,

examples/SharedMemory/SharedMemoryPublic.h

@@ -692,6 +692,7 @@ struct b3PhysicsSimulationParameters
 	double m_restitutionVelocityThreshold;
 	double 	m_defaultNonContactERP;
 	double m_frictionERP;
+	int m_enableConeFriction;
 };
 
 

examples/pybullet/pybullet.c

@@ -1,4 +1,3 @@
-
 #include "../SharedMemory/PhysicsClientC_API.h"
 #include "../SharedMemory/PhysicsDirectC_API.h"
 #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"

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

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;j1<this->m_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;
 }