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

   1 #ifndef MULTIBODYTREEINITCACHE_HPP_
   2 #define MULTIBODYTREEINITCACHE_HPP_
   3
   4 #include "../IDConfig.hpp"
   5 #include "../IDMath.hpp"
   6 #include "../MultiBodyTree.hpp"
   7
   8 namespace btInverseDynamics
   9 {
  10 /// Mass properties of a rigid body
  11 struct InertiaData
  12 {
  13         ID_DECLARE_ALIGNED_ALLOCATOR();
  14
  15         /// mass
  16         idScalar m_mass;
  17         /// vector from body-fixed frame to center of mass,
  18         /// in body-fixed frame, multiplied by the mass
  19         vec3 m_body_pos_body_com;
  20         /// moment of inertia w.r.t. the origin of the body-fixed
  21         /// frame, represented in that frame
  22         mat33 m_body_I_body;
  23 };
  24
  25 /// Joint properties
  26 struct JointData
  27 {
  28         ID_DECLARE_ALIGNED_ALLOCATOR();
  29
  30         /// type of joint
  31         JointType m_type;
  32         /// index of parent body
  33         int m_parent;
  34         /// index of child body
  35         int m_child;
  36         /// vector from parent's body-fixed frame to child's body-fixed
  37         /// frame for q=0, written in the parent's body fixed frame
  38         vec3 m_parent_pos_parent_child_ref;
  39         /// Transform matrix converting vectors written in the parent's frame
  40         /// into vectors written in the child's frame for q=0
  41         /// ie, child_vector = child_T_parent_ref * parent_vector;
  42         mat33 m_child_T_parent_ref;
  43         /// Axis of motion for 1 degree-of-freedom joints,
  44         /// written in the child's frame
  45         /// For revolute joints, the q-value is positive for a positive
  46         /// rotation about this axis.
  47         /// For prismatic joints, the q-value is positive for a positive
  48         /// translation is this direction.
  49         vec3 m_child_axis_of_motion;
  50 };
  51
  52 /// Data structure to store data passed by the user.
  53 /// This is used in MultiBodyTree::finalize to build internal data structures.
  54 class MultiBodyTree::InitCache
  55 {
  56 public:
  57         ID_DECLARE_ALIGNED_ALLOCATOR();
  58         /// constructor
  59         InitCache();
  60         ///\copydoc MultiBodyTree::addBody
  61         int addBody(const int body_index, const int parent_index, const JointType joint_type,
  62                                 const vec3 &parent_r_parent_body_ref, const mat33 &body_T_parent_ref,
  63                                 const vec3 &body_axis_of_motion, idScalar mass, const vec3 &body_r_body_com,
  64                                 const mat33 &body_I_body, const int user_int, void *user_ptr);
  65         /// build index arrays
  66         /// @return 0 on success, -1 on failure
  67         int buildIndexSets();
  68         /// @return number of degrees of freedom
  69         int numDoFs() const { return m_num_dofs; }
  70         /// @return number of bodies
  71         int numBodies() const { return m_inertias.size(); }
  72         /// get inertia data for index
  73         /// @param index of the body
  74         /// @param inertia pointer for return data
  75         /// @return 0 on success, -1 on failure
  76         int getInertiaData(const int index, InertiaData *inertia) const;
  77         /// get joint data for index
  78         /// @param index of the body
  79         /// @param joint pointer for return data
  80         /// @return 0 on success, -1 on failure
  81         int getJointData(const int index, JointData *joint) const;
  82         /// get parent index array (paren_index[i] is the index of the parent of i)
  83         /// @param parent_index pointer for return data
  84         void getParentIndexArray(idArray<int>::type *parent_index) { *parent_index = m_parent_index; }
  85         /// get user integer
  86         /// @param index body index
  87         /// @param user_int user integer
  88         /// @return 0 on success, -1 on failure
  89         int getUserInt(const int index, int *user_int) const;
  90         /// get user pointer
  91         /// @param index body index
  92         /// @param user_int user pointer
  93         /// @return 0 on success, -1 on failure
  94         int getUserPtr(const int index, void **user_ptr) const;
  95
  96 private:
  97         // vector of bodies
  98         idArray<InertiaData>::type m_inertias;
  99         // vector of joints
 100         idArray<JointData>::type m_joints;
 101         // number of mechanical degrees of freedom
 102         int m_num_dofs;
 103         // parent index array
 104         idArray<int>::type m_parent_index;
 105         // user integers
 106         idArray<int>::type m_user_int;
 107         // user pointers
 108         idArray<void *>::type m_user_ptr;
 109         // index of root body (or -1 if not set)
 110         int m_root_index;
 111 };
 112 }  // namespace btInverseDynamics
 113 #endif  // MULTIBODYTREEINITCACHE_HPP_