1 #include "MultiBodyTreeCreator.hpp"
2
3 namespace btInverseDynamics
4 {
CreateMultiBodyTree(const MultiBodyTreeCreator & creator)5 MultiBodyTree* CreateMultiBodyTree(const MultiBodyTreeCreator& creator)
6 {
7 int num_bodies;
8 int parent_index;
9 JointType joint_type;
10 vec3 body_r_parent_body_ref;
11 mat33 body_R_parent_ref;
12 vec3 body_axis_of_motion;
13 idScalar mass;
14 vec3 body_r_body_com;
15 mat33 body_I_body;
16 int user_int;
17 void* user_ptr;
18
19 MultiBodyTree* tree = new MultiBodyTree();
20 if (0x0 == tree)
21 {
22 bt_id_error_message("cannot allocate tree\n");
23 return 0x0;
24 }
25
26 // TODO: move to some policy argument
27 tree->setAcceptInvalidMassParameters(false);
28
29 // get number of bodies in the system
30 if (-1 == creator.getNumBodies(&num_bodies))
31 {
32 bt_id_error_message("getting body indices\n");
33 delete tree;
34 return 0x0;
35 }
36
37 // get data for all bodies
38 for (int index = 0; index < num_bodies; index++)
39 {
40 // get body parameters from user callbacks
41 if (-1 ==
42 creator.getBody(index, &parent_index, &joint_type, &body_r_parent_body_ref,
43 &body_R_parent_ref, &body_axis_of_motion, &mass, &body_r_body_com,
44 &body_I_body, &user_int, &user_ptr))
45 {
46 bt_id_error_message("getting data for body %d\n", index);
47 delete tree;
48 return 0x0;
49 }
50 // add body to system
51 if (-1 ==
52 tree->addBody(index, parent_index, joint_type, body_r_parent_body_ref,
53 body_R_parent_ref, body_axis_of_motion, mass, body_r_body_com,
54 body_I_body, user_int, user_ptr))
55 {
56 bt_id_error_message("adding body %d\n", index);
57 delete tree;
58 return 0x0;
59 }
60 }
61 // finalize initialization
62 if (-1 == tree->finalize())
63 {
64 bt_id_error_message("building system\n");
65 delete tree;
66 return 0x0;
67 }
68
69 return tree;
70 }
71 } // namespace btInverseDynamics
72