From aa87e47d2d76ac2523809fcedf119e32c6382cc3 Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 01:14:48 +0100 Subject: [PATCH] preparing for stabilization investigation: useRK4 is now a btMultiBody flag (not world's), reenabled global velocities (as a flag-controlled option), made the test application easier to handle for multiple multibodiez and added a max coordinate multibody (created from btMultiBody) --- .../Featherstone/btMultiBody.cpp | 60 +++++++++---------- src/BulletDynamics/Featherstone/btMultiBody.h | 7 +++ .../Featherstone/btMultiBodyDynamicsWorld.cpp | 4 +- .../Featherstone/btMultiBodyDynamicsWorld.h | 4 -- 4 files changed, 39 insertions(+), 36 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 6b594d085..91b575bed 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -104,7 +104,8 @@ btMultiBody::btMultiBody(int n_links, m_dofCount(0), __posUpdated(false), m_isMultiDof(multiDof), - m_posVarCnt(0) + m_posVarCnt(0), + m_useRK4(false), m_useGlobalVelocities(false) { if(!m_isMultiDof) @@ -1155,7 +1156,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, rot_from_world[0] = rot_from_parent[0]; // - bool useGlobalVelocities = false; for (int i = 0; i < num_links; ++i) { const int parent = m_links[i].m_parent; rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); @@ -1201,7 +1201,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, // now set vhat_i to its true value by doing // vhat_i += qidot * shat_i - if(!useGlobalVelocities) + if(!m_useGlobalVelocities) { spatJointVel.setZero(); @@ -1494,37 +1494,37 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt, ///// ///////////////////// - //if(useGlobalVelocities) - //{ - // for (int i = 0; i < num_links; ++i) - // { - // const int parent = m_links[i].m_parent; - // //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done - // //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done - // - // fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; - // fromWorld.m_rotMat = rot_from_world[i+1]; - // - // // vhat_i = i_xhat_p(i) * vhat_p(i) - // fromParent.transform(spatVel[parent+1], spatVel[i+1]); - // //nice alternative below (using operator *) but it generates temps - // ///////////////////////////////////////////////////////////// + if(m_useGlobalVelocities) + { + for (int i = 0; i < num_links; ++i) + { + const int parent = m_links[i].m_parent; + //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done + //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done + + fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector; + fromWorld.m_rotMat = rot_from_world[i+1]; + + // vhat_i = i_xhat_p(i) * vhat_p(i) + fromParent.transform(spatVel[parent+1], spatVel[i+1]); + //nice alternative below (using operator *) but it generates temps + ///////////////////////////////////////////////////////////// - // // now set vhat_i to its true value by doing - // // vhat_i += qidot * shat_i - // spatJointVel.setZero(); + // now set vhat_i to its true value by doing + // vhat_i += qidot * shat_i + spatJointVel.setZero(); - // for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - // spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; - // - // // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint - // spatVel[i+1] += spatJointVel; + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof]; + + // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint + spatVel[i+1] += spatJointVel; - // fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); - // fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); - // } - //} + fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity); + fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity); + } + } } #endif diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 4c99aea4c..8f6b0b851 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -489,6 +489,11 @@ public: bool isMultiDof() { return m_isMultiDof; } void finalizeMultiDof(); + void useRK4Integration(bool use) { m_useRK4 = use; } + bool isUsingRK4Integration() const { return m_useRK4; } + void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } + bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } + bool __posUpdated; private: @@ -530,6 +535,8 @@ private: btAlignedObjectArray m_links; // array of m_links, excluding the base. index from 0 to num_links-1. btAlignedObjectArray m_colliders; int m_dofCount, m_posVarCnt; + + bool m_useRK4, m_useGlobalVelocities; // // realBuf: diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 9c2e0387d..90bd896a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -381,7 +381,7 @@ struct MultiBodyInplaceSolverIslandCallback : public btSimulationIslandManager:: btMultiBodyDynamicsWorld::btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration) :btDiscreteDynamicsWorld(dispatcher,pairCache,constraintSolver,collisionConfiguration), - m_multiBodyConstraintSolver(constraintSolver), m_useRK4ForMultiBodies(false) + m_multiBodyConstraintSolver(constraintSolver) { //split impulse is not yet supported for Featherstone hierarchies getSolverInfo().m_splitImpulse = false; @@ -467,7 +467,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo) if(bod->isMultiDof()) { - if(!m_useRK4ForMultiBodies) + if(!bod->isUsingRK4Integration()) bod->stepVelocitiesMultiDof(solverInfo.m_timeStep, scratch_r, scratch_v, scratch_m); else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index 590906e16..a4d1a1161 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -34,7 +34,6 @@ protected: btAlignedObjectArray m_sortedMultiBodyConstraints; btMultiBodyConstraintSolver* m_multiBodyConstraintSolver; MultiBodyInplaceSolverIslandCallback* m_solverMultiBodyIslandCallback; - bool m_useRK4ForMultiBodies; virtual void calculateSimulationIslands(); virtual void updateActivationState(btScalar timeStep); @@ -53,8 +52,5 @@ public: virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint); virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint); - - void useRK4IntegrationForMultiBodies(bool use) { m_useRK4ForMultiBodies = use; } - bool isUsingRK4IntegrationForMultiBodies() const { return m_useRK4ForMultiBodies; } }; #endif //BT_MULTIBODY_DYNAMICS_WORLD_H