diff options
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp | 60 |
1 files changed, 39 insertions, 21 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index e9511b7076..a718db051e 100644 --- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp +++ b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp @@ -1,12 +1,13 @@ #include "MultiBodyTreeInitCache.hpp" -namespace btInverseDynamics { - -MultiBodyTree::InitCache::InitCache() { +namespace btInverseDynamics +{ +MultiBodyTree::InitCache::InitCache() +{ m_inertias.resize(0); m_joints.resize(0); m_num_dofs = 0; - m_root_index=-1; + m_root_index = -1; } int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index, @@ -15,8 +16,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind const mat33& body_T_parent_ref, const vec3& body_axis_of_motion, const idScalar mass, const vec3& body_r_body_com, const mat33& body_I_body, - const int user_int, void* user_ptr) { - switch (joint_type) { + const int user_int, void* user_ptr) +{ + switch (joint_type) + { case REVOLUTE: case PRISMATIC: m_num_dofs += 1; @@ -25,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind // does not add a degree of freedom // m_num_dofs+=0; break; + case SPHERICAL: + m_num_dofs += 3; + break; case FLOATING: m_num_dofs += 6; break; @@ -33,13 +39,15 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind return -1; } - if(-1 == parent_index) { - if(m_root_index>=0) { + if (-1 == parent_index) + { + if (m_root_index >= 0) + { bt_id_error_message("trying to add body %d as root, but already added %d as root body\n", - body_index, m_root_index); + body_index, m_root_index); return -1; } - m_root_index=body_index; + m_root_index = body_index; } JointData joint; @@ -61,8 +69,10 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind m_user_ptr.push_back(user_ptr); return 0; } -int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const { - if (index < 0 || index > static_cast<int>(m_inertias.size())) { +int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const +{ + if (index < 0 || index > static_cast<int>(m_inertias.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -71,8 +81,10 @@ int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inert return 0; } -int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { - if (index < 0 || index > static_cast<int>(m_user_int.size())) { +int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const +{ + if (index < 0 || index > static_cast<int>(m_user_int.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -80,8 +92,10 @@ int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const { return 0; } -int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const { - if (index < 0 || index > static_cast<int>(m_user_ptr.size())) { +int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const +{ + if (index < 0 || index > static_cast<int>(m_user_ptr.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -89,8 +103,10 @@ int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const return 0; } -int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const { - if (index < 0 || index > static_cast<int>(m_joints.size())) { +int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const +{ + if (index < 0 || index > static_cast<int>(m_joints.size())) + { bt_id_error_message("index out of range\n"); return -1; } @@ -98,16 +114,18 @@ int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) co return 0; } -int MultiBodyTree::InitCache::buildIndexSets() { +int MultiBodyTree::InitCache::buildIndexSets() +{ // NOTE: This function assumes that proper indices were provided // User2InternalIndex from utils can be used to facilitate this. m_parent_index.resize(numBodies()); - for (idArrayIdx j = 0; j < m_joints.size(); j++) { + for (idArrayIdx j = 0; j < m_joints.size(); j++) + { const JointData& joint = m_joints[j]; m_parent_index[joint.m_child] = joint.m_parent; } return 0; } -} +} // namespace btInverseDynamics |