Engine/lib/bullet/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp

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