// initialize necessary variables
this.numberOfModules = numberOfModules;
servoRefPos = new double[numberOfModules];
// masses = new DMass[1];
DMass mass = OdeHelper.createMass();
masses.add(mass);
// bodies = new DBody[numberOfModules + 1];
// geoms = new DGeom[numberOfModules * 2];
joints = new DHingeJoint[numberOfModules];
// create bodies and geoms for the modules
{
// create left body
DBody body = OdeHelper.createBody(env.getWorld());
body.setPosition(0, -L / 4, H / 2);
mass.setBoxTotal(MASS / 2, W, L / 2, H);
body.setMass(mass);
DGeom geom = OdeHelper.createBox(env.getSpace(), W, L / 2, H);
geom.setBody(body);
bodies.add(body);
geoms.add(geom);
// create central bodies
for (int i = 1; i < numberOfModules; i++) {
body = OdeHelper.createBody(env.getWorld());
body.setPosition(0, -L * (i), H / 2);
mass.setBoxTotal(MASS, W, L / 2, H);
body.setMass(mass);
geom = OdeHelper.createBox(env.getSpace(), W, L / 2, H);
DGeom geom2 = OdeHelper.createBox(
env.getSpace(), W, L / 2, H);
geom.setBody(body);
geom2.setBody(body);
geom.setOffsetPosition(0, L / 4, 0);
geom2.setOffsetPosition(0, -L / 4, 0);
bodies.add(body);
geoms.add(geom);
geoms.add(geom2);
}
// create right body
body = OdeHelper.createBody(env.getWorld());
body.setPosition(0,
(-(numberOfModules) * L + L / 4), H / 2);
mass.setBoxTotal(MASS / 2, W, L / 2, H);
body.setMass(mass);
geom = OdeHelper.createBox(
env.getSpace(), W, L / 2, H);
geom.setBody(body);
bodies.add(body);