diff options
| author | Rémi Verschelde <rverschelde@gmail.com> | 2019-06-11 13:18:05 +0200 |
|---|---|---|
| committer | Rémi Verschelde <rverschelde@gmail.com> | 2019-06-11 13:19:42 +0200 |
| commit | 99acec63f175fecd7172c927263ed3787cb082d6 (patch) | |
| tree | 183329f17c5c74b49d60058c5511d532f70b6e35 /thirdparty/bullet/BulletInverseDynamics/details | |
| parent | 7b883d7d50fdcac09afe09de859816da07ddc1e7 (diff) | |
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.
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/details')
| -rw-r--r-- | thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp | 33 |
1 files changed, 30 insertions, 3 deletions
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--) { |