From 99acec63f175fecd7172c927263ed3787cb082d6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?R=C3=A9mi=20Verschelde?= Date: Tue, 11 Jun 2019 13:18:05 +0200 Subject: bullet: Sync with current upstream master branch This updates our local copy to commit 5ec8339b6fc491e3f09a34a4516e82787f053fcc. We need a recent master commit for some new features that we use in Godot (see #25543 and #28909). To avoid warnings generated by Bullet headers included in our own module, we include those headers with -isystem on GCC and Clang. Fixes #29503. --- .../details/MultiBodyTreeImpl.cpp | 33 ++++++++++++++++++++-- 1 file changed, 30 insertions(+), 3 deletions(-) (limited to 'thirdparty/bullet/BulletInverseDynamics/details') diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index befbc2e2a4..ec9a562295 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -479,9 +479,17 @@ int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx //todo: review RigidBody &body = m_body_list[m_body_spherical_list[i]]; - body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) * - transformY(q(body.m_q_index + 1)) * - transformX(q(body.m_q_index)); + mat33 T; + + T = transformX(q(body.m_q_index)) * + transformY(q(body.m_q_index + 1)) * + transformZ(q(body.m_q_index + 2)); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; if (type >= POSITION_VELOCITY) @@ -832,6 +840,25 @@ int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; } + + for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++) + { + //todo: review + RigidBody &body = m_body_list[m_body_spherical_list[i]]; + + mat33 T; + + T = transformX(q(body.m_q_index)) * + transformY(q(body.m_q_index + 1)) * + transformZ(q(body.m_q_index + 2)); + body.m_body_T_parent = T * body.m_body_T_parent_ref; + + body.m_parent_pos_parent_body(0)=0; + body.m_parent_pos_parent_body(1)=0; + body.m_parent_pos_parent_body(2)=0; + + body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body; + } } for (int i = m_body_list.size() - 1; i >= 0; i--) { -- cgit v1.2.3