Merge remote-tracking branch 'bp/master'

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -649,6 +649,24 @@ B3_SHARED_API	int	b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandl
 	return 0;
 }
 
+B3_SHARED_API int b3PhysicsParamSetDefaultGlobalCFM(b3SharedMemoryCommandHandle commandHandle, double defaultGlobalCFM)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+    command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM;
+    command->m_physSimParamArgs.m_defaultGlobalCFM = defaultGlobalCFM;
+    return 0;
+}
+
+B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandle commandHandle, double frictionCFM)
+{
+    struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS);
+    command->m_updateFlags |= SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM;
+    command->m_physSimParamArgs.m_frictionCFM = frictionCFM;
+    return 0;
+}
+
 
 B3_SHARED_API  b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient)
 {

examples/SharedMemory/PhysicsClientC_API.h

@@ -283,6 +283,8 @@ B3_SHARED_API	int	b3PhysicsParamSetTimeStep(b3SharedMemoryCommandHandle commandH
 B3_SHARED_API	int	b3PhysicsParamSetDefaultContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultContactERP);
 B3_SHARED_API	int	b3PhysicsParamSetDefaultNonContactERP(b3SharedMemoryCommandHandle commandHandle, double defaultNonContactERP);
 B3_SHARED_API	int	b3PhysicsParamSetDefaultFrictionERP(b3SharedMemoryCommandHandle commandHandle, double frictionERP);
+B3_SHARED_API int b3PhysicsParamSetDefaultGlobalCFM(b3SharedMemoryCommandHandle commandHandle, double defaultGlobalCFM);
+B3_SHARED_API int b3PhysicsParamSetDefaultFrictionCFM(b3SharedMemoryCommandHandle commandHandle, double frictionCFM);
 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);

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -6736,6 +6736,15 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
         m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP;
     }
 
+    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM)
+    {
+        m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM;
+    }
+
+    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
+    {
+        m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionCFM;
+    }
 
 	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
     {

examples/SharedMemory/SharedMemoryCommands.h

@@ -448,6 +448,8 @@ enum EnumSimParamUpdateFlags
 	SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS = 65536,
 	SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION = 131072,
 	SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE = 262144,
+	SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM = 524288,
+	SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM = 1048576,
 };
 
 enum EnumLoadSoftBodyUpdateFlags

examples/SharedMemory/SharedMemoryPublic.h

@@ -749,6 +749,8 @@ struct b3PhysicsSimulationParameters
 	double m_restitutionVelocityThreshold;
 	double m_defaultNonContactERP;
 	double m_frictionERP;
+	double m_defaultGlobalCFM;
+	double m_frictionCFM;
 	int m_enableConeFriction;
 	int m_deterministicOverlappingPairs;
 	double m_allowedCcdPenetration;

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp

@@ -738,14 +738,13 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
 {
 #if BT_THREADSAFE
     int solverBodyId = -1;
-    if ( !body.isStaticOrKinematicObject() )
+    bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL;
+    if ( isRigidBodyType && !body.isStaticOrKinematicObject() )
     {
         // dynamic body
         // Dynamic bodies can only be in one island, so it's safe to write to the companionId
         solverBodyId = body.getCompanionId();
         if ( solverBodyId < 0 )
-        {
-            if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
         {
             solverBodyId = m_tmpSolverBodyPool.size();
             btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
@@ -753,8 +752,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
             body.setCompanionId( solverBodyId );
         }
     }
-    }
+    else if (isRigidBodyType && body.isKinematicObject())
-    else if (body.isKinematicObject())
     {
         //
         // NOTE: must test for kinematic before static because some kinematic objects also
@@ -774,7 +772,6 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
         if ( solverBodyId == INVALID_SOLVER_BODY_ID )
         {
             // create a table entry for this body
-            btRigidBody* rb = btRigidBody::upcast( &body );
             solverBodyId = m_tmpSolverBodyPool.size();
             btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
             initSolverBody( &solverBody, &body, timeStep );
@@ -783,6 +780,8 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
     }
     else
     {
+        // Incorrectly set collision object flags can degrade performance in various ways.
+        btAssert( body.isStaticOrKinematicObject() );
         // all fixed bodies (inf mass) get mapped to a single solver id
         if ( m_fixedBodyId < 0 )
         {
@@ -792,7 +791,7 @@ int	btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
         }
         solverBodyId = m_fixedBodyId;
     }
-    btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
+    btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() );
 	return solverBodyId;
 #else // BT_THREADSAFE
 

src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolverMt.cpp

@@ -317,7 +317,8 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
     // getOrInitSolverBodyThreadsafe -- attempts to be fully threadsafe (however may affect determinism)
     //
     int solverBodyId = -1;
-    if ( !body.isStaticOrKinematicObject() )
+    bool isRigidBodyType = btRigidBody::upcast( &body ) != NULL;
+    if ( isRigidBodyType && !body.isStaticOrKinematicObject() )
     {
         // dynamic body
         // Dynamic bodies can only be in one island, so it's safe to write to the companionId
@@ -328,19 +329,16 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
             // now that we have the lock, check again
             solverBodyId = body.getCompanionId();
             if ( solverBodyId < 0 )
-            {
-                if ( btRigidBody* rb = btRigidBody::upcast( &body ) )
             {
                 solverBodyId = m_tmpSolverBodyPool.size();
                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                 initSolverBody( &solverBody, &body, timeStep );
                 body.setCompanionId( solverBodyId );
             }
-            }
             m_bodySolverArrayMutex.unlock();
         }
     }
-    else if (body.isKinematicObject())
+    else if (isRigidBodyType && body.isKinematicObject())
     {
         //
         // NOTE: must test for kinematic before static because some kinematic objects also
@@ -373,7 +371,6 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
             if ( INVALID_SOLVER_BODY_ID == solverBodyId )
             {
                 // create a table entry for this body
-                btRigidBody* rb = btRigidBody::upcast( &body );
                 solverBodyId = m_tmpSolverBodyPool.size();
                 btSolverBody& solverBody = m_tmpSolverBodyPool.expand();
                 initSolverBody( &solverBody, &body, timeStep );
@@ -400,7 +397,7 @@ int	btSequentialImpulseConstraintSolverMt::getOrInitSolverBodyThreadsafe(btColli
         }
         solverBodyId = m_fixedBodyId;
     }
-    btAssert( solverBodyId < m_tmpSolverBodyPool.size() );
+    btAssert( solverBodyId >= 0 && solverBodyId < m_tmpSolverBodyPool.size() );
 	return solverBodyId;
 }
 
@@ -425,9 +422,10 @@ void btSequentialImpulseConstraintSolverMt::internalCollectContactManifoldCached
         btSolverBody* solverBodyA = &m_tmpSolverBodyPool[ solverBodyIdA ];
         btSolverBody* solverBodyB = &m_tmpSolverBodyPool[ solverBodyIdB ];
 
-        ///avoid collision response between two static objects
+        // A contact manifold between 2 static object should not exist!
-        if ( solverBodyA->m_invMass.fuzzyZero() && solverBodyB->m_invMass.fuzzyZero() )
+        // check the collision flags of your objects if this assert fires.
-            break;
+        // Incorrectly set collision object flags can degrade performance in various ways.
+        btAssert( !m_tmpSolverBodyPool[ solverBodyIdA ].m_invMass.isZero() || !m_tmpSolverBodyPool[ solverBodyIdB ].m_invMass.isZero() );
 
         int iContact = 0;
         for ( int j = 0; j < manifold->getNumContacts(); j++ )

src/LinearMath/btMatrix3x3.h

@@ -289,7 +289,7 @@ public:
 	/** @brief Set the matrix from euler angles YPR around ZYX axes
 	* @param eulerX Roll about X axis
 	* @param eulerY Pitch around Y axis
-	* @param eulerZ Yaw aboud Z axis
+	* @param eulerZ Yaw about Z axis
 	* 
 	* These angles are used to produce a rotation matrix. The euler
 	* angles are applied in ZYX order. I.e a vector is first rotated 
@@ -514,7 +514,7 @@ public:
 
 
 	/**@brief Get the matrix represented as euler angles around ZYX
-	* @param yaw Yaw around X axis
+	* @param yaw Yaw around Z axis
 	* @param pitch Pitch around Y axis
 	* @param roll around X axis 
 	* @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/