report constraint solver analytics data, currently for each island the number of solver iterations used and remaining residual error.

11 files changed, 144 insertions, 9 deletions

diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp
index 1e515b576..93b84bf60 100644
--- a/examples/SharedMemory/PhysicsClientC_API.cpp
+++ b/examples/SharedMemory/PhysicsClientC_API.cpp
@@ -2214,6 +2214,19 @@ B3_SHARED_API int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
 	return CMD_INVALID_STATUS;
 }
 
+B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData)
+{
+	const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
+	//b3Assert(status);
+	if (status)
+	{
+		*analyticsData = status->m_forwardDynamicsAnalyticsArgs;
+		return status->m_forwardDynamicsAnalyticsArgs.m_numIslands;
+	}
+	return 0;
+}
+
+
 B3_SHARED_API int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
 {
 	int numBodies = 0;
diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h
index 9682486ec..680e48ad4 100644
--- a/examples/SharedMemory/PhysicsClientC_API.h
+++ b/examples/SharedMemory/PhysicsClientC_API.h
@@ -360,6 +360,9 @@ extern "C"
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitStepSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 
+	B3_SHARED_API int b3GetStatusForwardDynamicsAnalyticsData(b3SharedMemoryStatusHandle statusHandle, struct b3ForwardDynamicsAnalyticsArgs* analyticsData);
+
+
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
 	B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
 
diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
index da65be89c..7beb0982d 100644
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
@@ -7605,17 +7605,17 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
 	}
 
 	btScalar deltaTimeScaled = m_data->m_physicsDeltaTime * simTimeScalingFactor;
-
+	m_data->m_dynamicsWorld->getSolverInfo().m_reportSolverAnalytics = true;
 	int numSteps = 0;
 	if (m_data->m_numSimulationSubSteps > 0)
 	{
 		numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps);
-                m_data->m_simulationTimestamp += deltaTimeScaled;
+		m_data->m_simulationTimestamp += deltaTimeScaled;
 	}
 	else
 	{
 		numSteps = m_data->m_dynamicsWorld->stepSimulation(deltaTimeScaled, 0);
-                m_data->m_simulationTimestamp += deltaTimeScaled;
+		m_data->m_simulationTimestamp += deltaTimeScaled;
 	}
 
 	if (numSteps > 0)
@@ -7624,6 +7624,24 @@ bool PhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct S
 	}
 
 	SharedMemoryStatus& serverCmd = serverStatusOut;
+
+	serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSteps = numSteps;
+
+	btAlignedObjectArray<btSolverAnalyticsData> islandAnalyticsData;
+
+	m_data->m_dynamicsWorld->getAnalyticsData(islandAnalyticsData);
+	serverCmd.m_forwardDynamicsAnalyticsArgs.m_numIslands = islandAnalyticsData.size();
+	int numIslands = btMin(islandAnalyticsData.size(), MAX_ISLANDS_ANALYTICS);
+	
+	for (int i=0;i<numIslands;i++)
+	{
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_numSolverCalls = islandAnalyticsData[i].m_numSolverCalls;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_islandId = islandAnalyticsData[i].m_islandId;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numBodies = islandAnalyticsData[i].m_numBodies;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numIterationsUsed = islandAnalyticsData[i].m_numIterationsUsed;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_remainingLeastSquaresResidual = islandAnalyticsData[i].m_remainingLeastSquaresResidual;
+		serverCmd.m_forwardDynamicsAnalyticsArgs.m_islandData[i].m_numContactManifolds = islandAnalyticsData[i].m_numContactManifolds;
+	}
 	serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
 
 	return hasStatus;
diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h
index 5be07b05a..443c4672e 100644
--- a/examples/SharedMemory/SharedMemoryCommands.h
+++ b/examples/SharedMemory/SharedMemoryCommands.h
@@ -957,6 +957,8 @@ struct b3CreateUserShapeArgs
 	b3CreateUserShapeData m_shapes[MAX_COMPOUND_COLLISION_SHAPES];
 };
 
+
+
 struct b3CreateUserShapeResultArgs
 {
 	int m_userShapeUniqueId;
@@ -1180,6 +1182,7 @@ struct SharedMemoryStatus
 		struct SyncUserDataArgs m_syncUserDataArgs;
 		struct UserDataResponseArgs m_userDataResponseArgs;
 		struct UserDataRequestArgs m_removeUserDataResponseArgs;
+		struct b3ForwardDynamicsAnalyticsArgs m_forwardDynamicsAnalyticsArgs;
 	};
 };
 
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index a569cbaf0..d0ede5768 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -931,6 +931,7 @@ struct b3PhysicsSimulationParameters
 	int m_minimumSolverIslandSize;
 };
 
+
 enum eConstraintSolverTypes
 {
 	eConstraintSolverLCP_SI = 1,
@@ -941,6 +942,25 @@ enum eConstraintSolverTypes
 	eConstraintSolverLCP_BLOCK_PGS,
 };
 
+struct b3ForwardDynamicsAnalyticsIslandData
+{
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
+
+#define MAX_ISLANDS_ANALYTICS 1024
+
+struct b3ForwardDynamicsAnalyticsArgs
+{
+	int m_numSteps;
+	int m_numIslands;
+	int m_numSolverCalls;
+	struct b3ForwardDynamicsAnalyticsIslandData m_islandData[MAX_ISLANDS_ANALYTICS];
+};
+
 enum eFileIOActions
 {
 	eAddFileIOAction = 1024,//avoid collision with eFileIOTypes
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index fe3b18799..9f59c79d8 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -330,6 +330,28 @@ static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args, PyObjec
 			statusHandle = b3SubmitClientCommandAndWaitStatus(
 				sm, b3InitStepSimulationCommand(sm));
 			statusType = b3GetStatusType(statusHandle);
+
+			if (statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED)
+			{
+				struct b3ForwardDynamicsAnalyticsArgs analyticsData;
+				int numIslands = 0;
+				int i;
+				numIslands = b3GetStatusForwardDynamicsAnalyticsData(statusHandle, &analyticsData);
+
+				PyObject* pyAnalyticsData = PyTuple_New(numIslands);
+				for (i=0;i<numIslands;i++)
+				{
+					int numFields = 4;
+					PyObject* pyIslandData = PyTuple_New(numFields);
+					PyTuple_SetItem(pyIslandData, 0, PyLong_FromLong(analyticsData.m_islandData[i].m_islandId));
+					PyTuple_SetItem(pyIslandData, 1, PyLong_FromLong(analyticsData.m_islandData[i].m_numBodies));
+					PyTuple_SetItem(pyIslandData, 2, PyLong_FromLong(analyticsData.m_islandData[i].m_numIterationsUsed));
+					PyTuple_SetItem(pyIslandData, 3, PyFloat_FromDouble(analyticsData.m_islandData[i].m_remainingLeastSquaresResidual));
+					PyTuple_SetItem(pyAnalyticsData, i, pyIslandData);
+				}
+
+				return pyAnalyticsData;
+			}
 		}
 	}
 
diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
index 89f8db8b1..0b6ac54c8 100644
--- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
+++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
@@ -64,6 +64,7 @@ struct btContactSolverInfoData
 	btScalar m_restitutionVelocityThreshold;
 	bool m_jointFeedbackInWorldSpace;
 	bool m_jointFeedbackInJointFrame;
+	bool m_reportSolverAnalytics;
 };
 
 struct btContactSolverInfo : public btContactSolverInfoData
@@ -98,6 +99,7 @@ struct btContactSolverInfo : public btContactSolverInfoData
 		m_restitutionVelocityThreshold = 0.2f;  //if the relative velocity is below this threshold, there is zero restitution
 		m_jointFeedbackInWorldSpace = false;
 		m_jointFeedbackInJointFrame = false;
+		m_reportSolverAnalytics = false;
 	}
 };
 
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
index 9f76713db..d3b71e458 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
@@ -2239,6 +2239,14 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyIterations(
 #ifdef VERBOSE_RESIDUAL_PRINTF
 				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
 #endif
+				m_analyticsData.m_numSolverCalls++;
+				m_analyticsData.m_numIterationsUsed = iteration+1;
+				m_analyticsData.m_islandId = -2;
+				if (numBodies>0)
+					m_analyticsData.m_islandId = bodies[0]->getCompanionId();
+				m_analyticsData.m_numBodies = numBodies;
+				m_analyticsData.m_numContactManifolds = numManifolds;
+				m_analyticsData.m_remainingLeastSquaresResidual = m_leastSquaresResidual;
 				break;
 			}
 		}
diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
index aca1d0988..542ea23c9 100644
--- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
+++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
@@ -91,10 +91,29 @@ struct btSISolverSingleIterationData
 	}
 };
 
+struct btSolverAnalyticsData
+{
+	btSolverAnalyticsData()
+	{
+		m_numSolverCalls = 0;
+		m_numIterationsUsed = -1;
+		m_remainingLeastSquaresResidual = -1;
+		m_islandId = -2;
+	}
+	int m_islandId;
+	int m_numBodies;
+	int m_numContactManifolds;
+	int m_numSolverCalls;
+	int m_numIterationsUsed;
+	double m_remainingLeastSquaresResidual;
+};
+
 ///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
 ATTRIBUTE_ALIGNED16(class)
 btSequentialImpulseConstraintSolver : public btConstraintSolver
 {
+	
+
 protected:
 	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
 	btConstraintArray m_tmpSolverContactConstraintPool;
@@ -283,6 +302,8 @@ public:
 		m_resolveSingleConstraintRowLowerLimit = rowSolver;
 	}
 
+
+
 	///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
 	static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
 	static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
@@ -296,6 +317,7 @@ public:
 	static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
 	static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
 
+	btSolverAnalyticsData m_analyticsData;
 };
 
 #endif  //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
\ No newline at end of file
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
index 1557987bc..28b569935 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
@@ -207,6 +207,7 @@ public:
 	}
 };
 
+
 struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback
 {
 	btContactSolverInfo* m_solverInfo;
@@ -224,6 +225,8 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	btAlignedObjectArray<btTypedConstraint*> m_constraints;
 	btAlignedObjectArray<btMultiBodyConstraint*> m_multiBodyConstraints;
 
+	btAlignedObjectArray<btSolverAnalyticsData> m_islandAnalyticsData;
+
 	MultiBodyInplaceSolverIslandCallback(btMultiBodyConstraintSolver* solver,
 										 btDispatcher* dispatcher)
 		: m_solverInfo(NULL),
@@ -244,6 +247,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 	SIMD_FORCE_INLINE void setup(btContactSolverInfo* solverInfo, btTypedConstraint** sortedConstraints, int numConstraints, btMultiBodyConstraint** sortedMultiBodyConstraints, int numMultiBodyConstraints, btIDebugDraw* debugDrawer)
 	{
+		m_islandAnalyticsData.clear();
 		btAssert(solverInfo);
 		m_solverInfo = solverInfo;
 
@@ -270,6 +274,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		{
 			///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id
 			m_solver->solveMultiBodyGroup(bodies, numBodies, manifolds, numManifolds, m_sortedConstraints, m_numConstraints, &m_multiBodySortedConstraints[0], m_numConstraints, *m_solverInfo, m_debugDrawer, m_dispatcher);
+			if (m_solverInfo->m_reportSolverAnalytics)
+			{
+				m_solver->m_analyticsData.m_islandId = islandId;
+				m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+			}
 		}
 		else
 		{
@@ -335,7 +344,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 
 				if ((m_multiBodyConstraints.size() + m_constraints.size() + m_manifolds.size()) > m_solverInfo->m_minimumSolverBatchSize)
 				{
-					processConstraints();
+					processConstraints(islandId);
 				}
 				else
 				{
@@ -344,7 +353,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 			}
 		}
 	}
-	void processConstraints()
+	void processConstraints(int islandId=-1)
 	{
 		btCollisionObject** bodies = m_bodies.size() ? &m_bodies[0] : 0;
 		btPersistentManifold** manifold = m_manifolds.size() ? &m_manifolds[0] : 0;
@@ -354,6 +363,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 		//printf("mb contacts = %d, mb constraints = %d\n", mbContacts, m_multiBodyConstraints.size());
 
 		m_solver->solveMultiBodyGroup(bodies, m_bodies.size(), manifold, m_manifolds.size(), constraints, m_constraints.size(), multiBodyConstraints, m_multiBodyConstraints.size(), *m_solverInfo, m_debugDrawer, m_dispatcher);
+		if (m_bodies.size() && m_solverInfo->m_reportSolverAnalytics)
+		{
+			m_solver->m_analyticsData.m_islandId = islandId;
+			m_islandAnalyticsData.push_back(m_solver->m_analyticsData);
+		}
 		m_bodies.resize(0);
 		m_manifolds.resize(0);
 		m_constraints.resize(0);
@@ -361,6 +375,11 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager::
 	}
 };
 
+void btMultiBodyDynamicsWorld::getAnalyticsData(btAlignedObjectArray<btSolverAnalyticsData>& islandAnalyticsData) const
+{
+	islandAnalyticsData = m_solverMultiBodyIslandCallback->m_islandAnalyticsData;
+}
+
 btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher, btBroadphaseInterface* pairCache, btMultiBodyConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)
 	: btDiscreteDynamicsWorld(dispatcher, pairCache, constraintSolver, collisionConfiguration),
 	  m_multiBodyConstraintSolver(constraintSolver)
@@ -720,10 +739,13 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
 				{
 					if (!bod->isUsingRK4Integration())
 					{
-						bool isConstraintPass = true;
-						bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
-						getSolverInfo().m_jointFeedbackInWorldSpace,
-						getSolverInfo().m_jointFeedbackInJointFrame);
+						if (bod->internalNeedsJointFeedback())
+						{
+							bool isConstraintPass = true;
+							bod->computeAccelerationsArticulatedBodyAlgorithmMultiDof(solverInfo.m_timeStep, m_scratch_r, m_scratch_v, m_scratch_m, isConstraintPass,
+								getSolverInfo().m_jointFeedbackInWorldSpace,
+								getSolverInfo().m_jointFeedbackInJointFrame);
+						}
 					}
 				}
 			}
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
index 641238f3b..e36c2f7aa 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
+++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
@@ -109,5 +109,7 @@ public:
 	virtual void serialize(btSerializer* serializer);
 	virtual void setMultiBodyConstraintSolver(btMultiBodyConstraintSolver* solver);
 	virtual void setConstraintSolver(btConstraintSolver* solver);
+	virtual void getAnalyticsData(btAlignedObjectArray<struct btSolverAnalyticsData>& m_islandAnalyticsData) const;
+
 };
 #endif  //BT_MULTIBODY_DYNAMICS_WORLD_H