// Add underly Simbody elements to the System after subcomponents
void BushingForce::
extendAddToSystemAfterSubcomponents(SimTK::MultibodySystem& system) const
{
Super::extendAddToSystemAfterSubcomponents(system);
const SimTK::Vec3& rotStiffness = get_rotational_stiffness();
const SimTK::Vec3& transStiffness = get_translational_stiffness();
const SimTK::Vec3& rotDamping = get_rotational_damping();
const SimTK::Vec3& transDamping = get_translational_damping();
// get connected frames
const PhysicalFrame& frame1 = getFrame1();
const PhysicalFrame& frame2 = getFrame2();
// Get underlying mobilized bodies
const SimTK::MobilizedBody& b1 = frame1.getMobilizedBody();
const SimTK::MobilizedBody& b2 = frame2.getMobilizedBody();
Vec6 stiffness(rotStiffness[0], rotStiffness[1], rotStiffness[2],
transStiffness[0], transStiffness[1], transStiffness[2]);
Vec6 damping(rotDamping[0], rotDamping[1], rotDamping[2],
transDamping[0], transDamping[1], transDamping[2]);
// Now create a Simbody Force::LinearBushing
SimTK::Force::LinearBushing simtkForce
(_model->updForceSubsystem(), b1, frame1.findTransformInBaseFrame(),
b2, frame2.findTransformInBaseFrame(),
stiffness, damping );
// Beyond the const Component get the index so we can access the
// SimTK::Force later.
BushingForce* mutableThis = const_cast<BushingForce *>(this);
mutableThis->_index = simtkForce.getForceIndex();
}
void BushingForce::extendAddToSystem(SimTK::MultibodySystem& system) const
{
Super::extendAddToSystem(system);
const string& body1Name = get_body_1();
const string& body2Name = get_body_2();
const SimTK::Vec3& locationInBody1 = get_location_body_1();
const SimTK::Vec3& orientationInBody1 = get_orientation_body_1();
const SimTK::Vec3& locationInBody2 = get_location_body_2();
const SimTK::Vec3& orientationInBody2 = get_orientation_body_2();
const SimTK::Vec3& rotStiffness = get_rotational_stiffness();
const SimTK::Vec3& transStiffness = get_translational_stiffness();
const SimTK::Vec3& rotDamping = get_rotational_damping();
const SimTK::Vec3& transDamping = get_translational_damping();
// Get underlying mobilized bodies
const SimTK::MobilizedBody& b1 = _body1->getMobilizedBody();
const SimTK::MobilizedBody& b2 = _body2->getMobilizedBody();
// Build the transforms
SimTK::Rotation r1; r1.setRotationToBodyFixedXYZ(orientationInBody1);
SimTK::Rotation r2; r2.setRotationToBodyFixedXYZ(orientationInBody2);
SimTK::Transform inb1(r1, locationInBody1);
SimTK::Transform inb2(r2, locationInBody2);
Vec6 stiffness(rotStiffness[0], rotStiffness[1], rotStiffness[2],
transStiffness[0], transStiffness[1], transStiffness[2]);
Vec6 damping(rotDamping[0], rotDamping[1], rotDamping[2],
transDamping[0], transDamping[1], transDamping[2]);
// Now create a Simbody Force::LinearBushing
SimTK::Force::LinearBushing simtkForce
(_model->updForceSubsystem(), b1, inb1, b2, inb2, stiffness, damping);
// Beyond the const Component get the index so we can access the
// SimTK::Force later.
BushingForce* mutableThis = const_cast<BushingForce *>(this);
mutableThis->_index = simtkForce.getForceIndex();
}