From 02b0e34bdae22b94734f4c72520030bc67110f91 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 3 Aug 2018 17:53:07 -0700 Subject: [PATCH 1/2] v_ptr --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 6bb0a2f8d..a4014adde 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // hhat is NOT stored for the base (but ahat is) btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; - // v_ptr += num_links * 2 + 2; // Disabled since v_ptr is not used in the rest of the code + v_ptr += num_links * 2 + 2; // // Y_i, invD_i btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; From 684b69f4ddb48748b0eb2ab5ea9fd49418bd0c6a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 3 Aug 2018 17:57:09 -0700 Subject: [PATCH 2/2] fix applied too fast --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a4014adde..34fe4b8de 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -967,12 +967,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - - spatCoriolisAcc[i].dot(hDof) - ; + - spatCoriolisAcc[i].dot(hDof); } for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - } + { btScalar *D_row = &D[dof * m_links[i].m_dofCount]; for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) {