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
60
61
62
63
64
65
66
67
68
69
70
71
|
#include "MultiBodyTreeCreator.hpp"
namespace btInverseDynamics
{
MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator)
{
int num_bodies;
int parent_index;
JointType joint_type;
vec3 body_r_parent_body_ref;
mat33 body_R_parent_ref;
vec3 body_axis_of_motion;
idScalar mass;
vec3 body_r_body_com;
mat33 body_I_body;
int user_int;
void* user_ptr;
MultiBodyTree* tree = new MultiBodyTree();
if (0x0 == tree)
{
bt_id_error_message("cannot allocate tree\n");
return 0x0;
}
// TODO: move to some policy argument
tree->setAcceptInvalidMassParameters(false);
// get number of bodies in the system
if (-1 == creator.getNumBodies(&num_bodies))
{
bt_id_error_message("getting body indices\n");
delete tree;
return 0x0;
}
// get data for all bodies
for (int index = 0; index < num_bodies; index++)
{
// get body parameters from user callbacks
if (-1 ==
creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
&body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
&body_I_body, &user_int, &user_ptr))
{
bt_id_error_message("getting data for body %d\n", index);
delete tree;
return 0x0;
}
// add body to system
if (-1 ==
tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
body_I_body, user_int, user_ptr))
{
bt_id_error_message("adding body %d\n", index);
delete tree;
return 0x0;
}
}
// finalize initialization
if (-1 == tree->finalize())
{
bt_id_error_message("building system\n");
delete tree;
return 0x0;
}
return tree;
}
} // namespace btInverseDynamics
|
