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 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;i0) + 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 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 m_constraints; btAlignedObjectArray m_multiBodyConstraints; + btAlignedObjectArray 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& 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& m_islandAnalyticsData) const; + }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H