From 4eac9a11f355bad2fef6cda99cc67322ec50e23b Mon Sep 17 00:00:00 2001 From: kubas Date: Thu, 9 Jan 2014 00:58:31 +0100 Subject: [PATCH] made the multiDof-singleDof disctinction a bit cleaner --- .../Featherstone/btMultiBody.cpp | 83 ++++++++----------- src/BulletDynamics/Featherstone/btMultiBody.h | 41 ++++----- 2 files changed, 51 insertions(+), 73 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 8e69ba679..d4cafe36d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -85,7 +85,8 @@ btMultiBody::btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, - bool canSleep) + bool canSleep, + bool multiDof) : m_baseQuat(0, 0, 0, 1), m_baseMass(mass), m_baseInertia(inertia), @@ -101,54 +102,24 @@ btMultiBody::btMultiBody(int n_links, m_maxAppliedImpulse(1000.f), m_hasSelfCollision(true), m_dofCount(n_links), - __posUpdated(false), - m_posVarCnt(-1) //-1 => not calculated yet/invalid + __posUpdated(false), + m_isMultiDof(multiDof), + m_posVarCnt(0) { + + if(!m_isMultiDof) + { + m_vectorBuf.resize(2*n_links); + m_realBuf.resize(6 + 2*n_links); + m_posVarCnt = n_links; + } + m_links.resize(n_links); - - m_vectorBuf.resize(2*n_links); - m_matrixBuf.resize(n_links + 1); - m_realBuf.resize(6 + 2*n_links); - m_basePos.setValue(0, 0, 0); - m_baseForce.setValue(0, 0, 0); - m_baseTorque.setValue(0, 0, 0); - - m_isMultiDof = false; -} - -btMultiBody::btMultiBody(int n_links, int n_dofs, btScalar mass, - const btVector3 &inertia, - bool fixedBase, - bool canSleep) - : m_baseQuat(0, 0, 0, 1), - m_baseMass(mass), - m_baseInertia(inertia), - - m_fixedBase(fixedBase), - m_awake(true), - m_canSleep(canSleep), - m_sleepTimer(0), - m_baseCollider(0), - m_linearDamping(0.04f), - m_angularDamping(0.04f), - m_useGyroTerm(true), - m_maxAppliedImpulse(1000.f), - m_hasSelfCollision(true), - m_dofCount(n_dofs), - __posUpdated(false), - m_posVarCnt(-1) //-1 => not calculated yet/invalid -{ - m_links.resize(n_links); - - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) m_matrixBuf.resize(n_links + 1); - m_basePos.setValue(0, 0, 0); + m_basePos.setValue(0, 0, 0); m_baseForce.setValue(0, 0, 0); m_baseTorque.setValue(0, 0, 0); - - m_isMultiDof = n_links != n_dofs; } btMultiBody::~btMultiBody() @@ -164,6 +135,12 @@ void btMultiBody::setupPrismatic(int i, const btVector3 &parentComToThisComOffset, bool disableParentCollision) { + if(m_isMultiDof) + { + m_dofCount += 1; + m_posVarCnt += 1; + } + m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; m_links[i].m_parent = parent; @@ -201,6 +178,12 @@ void btMultiBody::setupRevolute(int i, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision) { + if(m_isMultiDof) + { + m_dofCount += 1; + m_posVarCnt += 1; + } + m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; m_links[i].m_parent = parent; @@ -240,6 +223,8 @@ void btMultiBody::setupSpherical(int i, bool disableParentCollision) { btAssert(m_isMultiDof); + m_dofCount += 3; + m_posVarCnt += 4; m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; @@ -280,6 +265,8 @@ void btMultiBody::setupPlanar(int i, bool disableParentCollision) { btAssert(m_isMultiDof); + m_dofCount += 3; + m_posVarCnt += 3; m_links[i].m_mass = mass; m_links[i].m_inertia = inertia; @@ -315,14 +302,12 @@ void btMultiBody::setupPlanar(int i, } #endif -void btMultiBody::forceMultiDof() +void btMultiBody::finalizeMultiDof() { - if(m_isMultiDof) return; + btAssert(m_isMultiDof); - m_isMultiDof = true; - - m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices - m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) + m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices) updateLinksDofOffsets(); } diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 8b3b33bc9..41cb783d3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -49,14 +49,9 @@ public: btScalar mass, // mass of base const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep); - - btMultiBody(int n_links, // NOT including the base - int n_dofs, // NOT including 6 floating base dofs - btScalar mass, // mass of base - const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal - bool fixedBase, // whether the base is fixed (true) or can move (false) - bool canSleep); + bool canSleep, + bool multiDof = false + ); ~btMultiBody(); @@ -356,20 +351,20 @@ public: // printf("%.4f ", delta_vee[dof]*multiplier); //printf("\n"); - btScalar sum = 0; - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { - sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; - } - btScalar l = btSqrt(sum); + //btScalar sum = 0; + //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + //{ + // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; + //} + //btScalar l = btSqrt(sum); - if (l>m_maxAppliedImpulse) - { - multiplier *= m_maxAppliedImpulse/l; - } + //if (l>m_maxAppliedImpulse) + //{ + // multiplier *= m_maxAppliedImpulse/l; + //} - for (int dof = 0; dof < 6 + getNumDofs(); ++dof) - { + for (int dof = 0; dof < 6 + getNumDofs(); ++dof) + { m_realBuf[dof] += delta_vee[dof] * multiplier; } } @@ -480,7 +475,7 @@ public: } bool isMultiDof() { return m_isMultiDof; } - void forceMultiDof(); + void finalizeMultiDof(); bool __posUpdated; @@ -503,8 +498,6 @@ private: m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; } - - m_posVarCnt = cfgOffset; } void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; @@ -529,7 +522,7 @@ private: // // realBuf: // offset size array - // 0 6 + num_links v (base_omega; base_vel; joint_vels) + // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] // 6+num_links num_links D // // vectorBuf: