33
33
// Take care of bodies
34
34
chassis_body = dBodyCreate(world);
35
35
dBodySetAutoDisableFlag(chassis_body, false);
36
dBodySetPosition (chassis_body,initialpos[0],initialpos[1],initialpos[2]);
36
dBodySetPosition (chassis_body,initialpos[0],initialpos[1],initialpos[2]+0.3);
39
38
dReal sizes[4][3] = { {0.1, 0.1, 1.0}, {0.1, 0.1, 1.0}, {1.0, 0.1, 0.1}, {1.0, 0.1, 0.1} };
40
dReal xlats[4][3] = { {0,-0.65,0.50}, {0,0.65,0.50}, {0,-0.65,0.05}, {0,0.65,0.05} };
39
dReal xlats[4][3] = { {0,-0.75,0.20}, {0,0.75,0.20}, {0,-0.75,-0.25}, {0,0.75,-0.25} };
42
41
// Take care of geoms
46
45
for (i=0; i<4; i++)
47
box_geom[i] = dCreateBox(0, sizes[i][0], sizes[i][1], sizes[i][2]);
48
dGeomSetData(box_geom[i], this);
49
dGeomSetBody(box_geom[i], chassis_body);
50
dGeomSetOffsetPosition(box_geom[i], xlats[i][0], xlats[i][1], xlats[i][2]);
49
53
dMassSetZero(&m_total);
50
box_geom[i] = dCreateBox(0, sizes[i][0], sizes[i][1], sizes[i][2]);
52
54
dMassSetBox(&m, 28, sizes[i][0], sizes[i][1], sizes[i][2]);
53
55
dMassTranslate(&m, xlats[i][0], xlats[i][1], xlats[i][2]);
54
56
dMassAdd (&m_total, &m);
55
dGeomSetData (box_geom[i], this);
56
trf_geom[i] = dCreateGeomTransform(0);
57
dGeomSetData (trf_geom[i], this);
58
dGeomSetBody (trf_geom[i], chassis_body);
59
dGeomTransformSetInfo(trf_geom[i], 1);
60
dGeomTransformSetGeom(trf_geom[i], box_geom[i]);
61
dGeomSetPosition(trf_geom[i], initialpos[0],initialpos[1],initialpos[2]);
62
dGeomSetPosition(box_geom[i], xlats[i][0], xlats[i][1], xlats[i][2]);
65
59
dReal *o = m_total.c;
66
60
dMassTranslate(&m_total, -o[0], -o[1], -o[2]);
67
dBodySetMass (chassis_body, &m_total);
61
dMassAdjust(&m_total, 3.0);
62
dBodySetMass(chassis_body, &m_total);
70
dReal sz[3] = { 0.1, 1.2, 1.6 };
65
dReal sz[3] = { 0.1, 1.4, 1.6 };
71
66
dReal ps[3] = { initialpos[0], initialpos[1], initialpos[2]+0.85 };
72
67
door_body = dBodyCreate(world);
73
68
dBodySetAutoDisableFlag(door_body, false);
94
89
space = dSimpleSpaceCreate(bigspace);
95
90
dSpaceSetCleanup(space,0);
96
91
for (i=0; i<4; i++)
97
dSpaceAdd (space, trf_geom[i]);
92
dSpaceAdd (space, box_geom[i]);
98
93
dSpaceAdd(space, door_geom);