dali-physics/third-party/bullet3/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

   1 #include "MultiBodyTreeInitCache.hpp"
   2
   3 namespace btInverseDynamics
   4 {
   5 MultiBodyTree::InitCache::InitCache()
   6 {
   7         m_inertias.resize(0);
   8         m_joints.resize(0);
   9         m_num_dofs = 0;
  10         m_root_index = -1;
  11 }
  12
  13 int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_index,
  14                                                                           const JointType joint_type,
  15                                                                           const vec3& parent_r_parent_body_ref,
  16                                                                           const mat33& body_T_parent_ref,
  17                                                                           const vec3& body_axis_of_motion, const idScalar mass,
  18                                                                           const vec3& body_r_body_com, const mat33& body_I_body,
  19                                                                           const int user_int, void* user_ptr)
  20 {
  21         switch (joint_type)
  22         {
  23                 case REVOLUTE:
  24                 case PRISMATIC:
  25                         m_num_dofs += 1;
  26                         break;
  27                 case FIXED:
  28                         // does not add a degree of freedom
  29                         // m_num_dofs+=0;
  30                         break;
  31                 case SPHERICAL:
  32                         m_num_dofs += 3;
  33                         break;
  34                 case FLOATING:
  35                         m_num_dofs += 6;
  36                         break;
  37                 default:
  38                         bt_id_error_message("unknown joint type %d\n", joint_type);
  39                         return -1;
  40         }
  41
  42         if (-1 == parent_index)
  43         {
  44                 if (m_root_index >= 0)
  45                 {
  46                         bt_id_error_message("trying to add body %d as root, but already added %d as root body\n",
  47                                                                 body_index, m_root_index);
  48                         return -1;
  49                 }
  50                 m_root_index = body_index;
  51         }
  52
  53         JointData joint;
  54         joint.m_child = body_index;
  55         joint.m_parent = parent_index;
  56         joint.m_type = joint_type;
  57         joint.m_parent_pos_parent_child_ref = parent_r_parent_body_ref;
  58         joint.m_child_T_parent_ref = body_T_parent_ref;
  59         joint.m_child_axis_of_motion = body_axis_of_motion;
  60
  61         InertiaData body;
  62         body.m_mass = mass;
  63         body.m_body_pos_body_com = body_r_body_com;
  64         body.m_body_I_body = body_I_body;
  65
  66         m_inertias.push_back(body);
  67         m_joints.push_back(joint);
  68         m_user_int.push_back(user_int);
  69         m_user_ptr.push_back(user_ptr);
  70         return 0;
  71 }
  72 int MultiBodyTree::InitCache::getInertiaData(const int index, InertiaData* inertia) const
  73 {
  74         if (index < 0 || index > static_cast<int>(m_inertias.size()))
  75         {
  76                 bt_id_error_message("index out of range\n");
  77                 return -1;
  78         }
  79
  80         *inertia = m_inertias[index];
  81         return 0;
  82 }
  83
  84 int MultiBodyTree::InitCache::getUserInt(const int index, int* user_int) const
  85 {
  86         if (index < 0 || index > static_cast<int>(m_user_int.size()))
  87         {
  88                 bt_id_error_message("index out of range\n");
  89                 return -1;
  90         }
  91         *user_int = m_user_int[index];
  92         return 0;
  93 }
  94
  95 int MultiBodyTree::InitCache::getUserPtr(const int index, void** user_ptr) const
  96 {
  97         if (index < 0 || index > static_cast<int>(m_user_ptr.size()))
  98         {
  99                 bt_id_error_message("index out of range\n");
 100                 return -1;
 101         }
 102         *user_ptr = m_user_ptr[index];
 103         return 0;
 104 }
 105
 106 int MultiBodyTree::InitCache::getJointData(const int index, JointData* joint) const
 107 {
 108         if (index < 0 || index > static_cast<int>(m_joints.size()))
 109         {
 110                 bt_id_error_message("index out of range\n");
 111                 return -1;
 112         }
 113         *joint = m_joints[index];
 114         return 0;
 115 }
 116
 117 int MultiBodyTree::InitCache::buildIndexSets()
 118 {
 119         // NOTE: This function assumes that proper indices were provided
 120         //         User2InternalIndex from utils can be used to facilitate this.
 121
 122         m_parent_index.resize(numBodies());
 123         for (idArrayIdx j = 0; j < m_joints.size(); j++)
 124         {
 125                 const JointData& joint = m_joints[j];
 126                 m_parent_index[joint.m_child] = joint.m_parent;
 127         }
 128
 129         return 0;
 130 }
 131 }  // namespace btInverseDynamics