diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp | 37 |
1 files changed, 30 insertions, 7 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 1557987bc3..1131e5378c 100644 --- a/thirdparty/bullet/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/thirdparty/bullet/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), @@ -235,7 +238,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: { } - MultiBodyInplaceSolverIslandCallback& operator=(MultiBodyInplaceSolverIslandCallback& other) + MultiBodyInplaceSolverIslandCallback& operator=(const MultiBodyInplaceSolverIslandCallback& other) { btAssert(0); (void)other; @@ -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&1) + { + 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&1)) + { + 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) @@ -717,13 +736,17 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) m_scratch_v.resize(bod->getNumLinks() + 1); m_scratch_m.resize(bod->getNumLinks() + 1); + if (bod->internalNeedsJointFeedback()) { 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); + } } } } |