void createPlanarSystem(MultibodySystem& system) {
SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
GeneralForceSubsystem forces(system);
Force::UniformGravity gravity(forces, matter, Vec3(0, -1, 0), 0);
Body::Rigid body(MassProperties(1.0, Vec3(0), Inertia(1)));
for (int i = 0; i < NUM_BODIES; ++i) {
MobilizedBody& parent = matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
MobilizedBody::Planar b(parent, Transform(Vec3(0)), body, Transform(Vec3(BOND_LENGTH, 0, 0)));
}
}
// Component interface implementation
void extendAddToSystem(MultibodySystem& system) const override {
if (system.hasMatterSubsystem()){
matter = system.updMatterSubsystem();
}
else{
// const Sub& subc = getMemberSubcomponent<Sub>(intSubix);
SimbodyMatterSubsystem* old_matter = matter.release();
delete old_matter;
matter = new SimbodyMatterSubsystem(system);
GeneralForceSubsystem* old_forces = forces.release();
delete old_forces;
forces = new GeneralForceSubsystem(system);
SimTK::Force::UniformGravity gravity(*forces, *matter, Vec3(0, -9.816, 0));
fix = gravity.getForceIndex();
system.updMatterSubsystem().setShowDefaultGeometry(true);
}
}
void createCylinderSystem(MultibodySystem& system) {
SimbodyMatterSubsystem& matter = system.updMatterSubsystem();
GeneralForceSubsystem forces(system);
// Skew gravity so moving takes work.
Force::UniformGravity gravity(forces, matter, Vec3(0, -2, -3));
for (int i = 0; i < NUM_BODIES; ++i) {
MobilizedBody& parent =
matter.updMobilizedBody(MobilizedBodyIndex(matter.getNumBodies()-1));
const Real mass = 1 + 0.1*i;
Body::Rigid body(MassProperties(mass, Vec3(0), mass*UnitInertia(1)));
MobilizedBody::Cylinder b(parent, Transform(Vec3(.1,.2,.3)),
body, Transform(Vec3(BOND_LENGTH, 0, 0)));
}
}