text: #include "CloneTreeCreator.hpp"
  
  #include <cstdio>
  
  namespace btInverseDynamics {
  #define CHECK_NULLPTR()                                                                            \
      do {                                                                                           \
          if (m_reference == 0x0) {                                                                      \
              bt_id_error_message("m_reference == 0x0\n");                                                     \
              return -1;                                                                             \
          }                                                                                          \
      } while (0)
  
  #define TRY(x)                                                                                     \
      do {                                                                                           \
          if (x == -1) {                                                                             \
              bt_id_error_message("error calling " #x "\n");                                               \
              return -1;                                                                             \
          }                                                                                          \
      } while (0)
  CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference) { m_reference = reference; }
  
  CloneTreeCreator::~CloneTreeCreator() {}
  
  int CloneTreeCreator::getNumBodies(int* num_bodies) const {
      CHECK_NULLPTR();
      *num_bodies = m_reference->numBodies();
      return 0;
  }
  
  int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
                                vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
                                vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
                                mat33* body_I_body, int* user_int, void** user_ptr) const {
      CHECK_NULLPTR();
      TRY(m_reference->getParentIndex(body_index, parent_index));
      TRY(m_reference->getJointType(body_index, joint_type));
      TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
      TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
      TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
      TRY(m_reference->getBodyMass(body_index, mass));
      TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
      TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
      TRY(m_reference->getUserInt(body_index, user_int));
      TRY(m_reference->getUserPtr(body_index, user_ptr));
  
      return 0;
  }
  }
  