summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp')
-rw-r--r--thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp131
1 files changed, 0 insertions, 131 deletions
diff --git a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
deleted file mode 100644
index a718db051e..0000000000
--- a/thirdparty/bullet/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-#include "MultiBodyTreeInitCache.hpp"
-
-namespace btInverseDynamics
-{
-MultiBodyTree::InitCache::InitCache()
-{
- m_inertias.resize(0);
- m_joints.resize(0);
- m_num_dofs = 0;
- m_root_index = -1;
-}
-
-int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
- const JointType joint_type,
- const vec3& parent_r_parent_body_ref,
- 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)
- {
- case REVOLUTE:
- case PRISMATIC:
- m_num_dofs += 1;
- break;
- case FIXED:
- // 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;
- default:
- bt_id_error_message("unknown joint type %d\n", joint_type);
- return -1;
- }
-
- 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);
- return -1;
- }
- m_root_index = body_index;
- }
-
- JointData joint;
- joint.m_child = body_index;
- joint.m_parent = parent_index;
- joint.m_type = joint_type;
- joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref;
- joint.m_child_T_parent_ref = body_T_parent_ref;
- joint.m_child_axis_of_motion = body_axis_of_motion;
-
- InertiaData body;
- body.m_mass = mass;
- body.m_body_pos_body_com = body_r_body_com;
- body.m_body_I_body = body_I_body;
-
- m_inertias.push_back(body);
- m_joints.push_back(joint);
- m_user_int.push_back(user_int);
- 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()))
- {
- bt_id_error_message("index out of range\n");
- return -1;
- }
-
- *inertia = m_inertias[index];
- return 0;
-}
-
-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;
- }
- *user_int = m_user_int[index];
- return 0;
-}
-
-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;
- }
- *user_ptr = m_user_ptr[index];
- return 0;
-}
-
-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;
- }
- *joint = m_joints[index];
- return 0;
-}
-
-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++)
- {
- const JointData& joint = m_joints[j];
- m_parent_index[joint.m_child] = joint.m_parent;
- }
-
- return 0;
-}
-} // namespace btInverseDynamics