1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
|
#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;
}
} // namespace btInverseDynamics
|
