Force::Gravity& Force::Gravity::
setDefaultGravityVector(const Vec3& gravity) {
const Real g = gravity.norm();
getImpl().invalidateTopologyCache();
updImpl().defMagnitude = g;
// Don't change the direction if the magnitude is zero.
if (g > 0)
updImpl().defDirection = UnitVec3(gravity/g, true);
return *this;
}
Constraint::LineOnLineContact::LineOnLineContact
(MobilizedBody& mobod_F,
const Transform& defaultEdgeFrameF,
Real defaultHalfLengthF,
MobilizedBody& mobod_B,
const Transform& defaultEdgeFrameB,
Real defaultHalfLengthB,
bool enforceRolling)
: Constraint(new LineOnLineContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(),
"Constraint::LineOnLineContact","LineOnLineContact",
"Both mobilized bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B),
"Constraint::LineOnLineContact","LineOnLineContact",
"The two mobilized bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
mobod_F.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F);
updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B);
updImpl().m_def_X_FEf = defaultEdgeFrameF;
updImpl().m_def_hf = defaultHalfLengthF;
updImpl().m_def_X_BEb = defaultEdgeFrameB;
updImpl().m_def_hb = defaultHalfLengthB;
}
Constraint::SphereOnSphereContact::SphereOnSphereContact
(MobilizedBody& mobod_F,
const Vec3& defaultCenter_F,
Real defaultRadius_F,
MobilizedBody& mobod_B,
const Vec3& defaultCenter_B,
Real defaultRadius_B,
bool enforceRolling)
: Constraint(new SphereOnSphereContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSubsystem() && mobod_B.isInSubsystem(),
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"Both mobilized bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(mobod_F.isInSameSubsystem(mobod_B),
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"The two mobilized bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
SimTK_APIARGCHECK2_ALWAYS(defaultRadius_F > 0 && defaultRadius_B > 0,
"Constraint::SphereOnSphereContact","SphereOnSphereContact",
"The sphere radii must be greater than zero; they were %g and %g.",
defaultRadius_F, defaultRadius_B);
mobod_F.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_mobod_F = updImpl().addConstrainedBody(mobod_F);
updImpl().m_mobod_B = updImpl().addConstrainedBody(mobod_B);
updImpl().m_def_p_FSf = defaultCenter_F;
updImpl().m_def_radius_F = defaultRadius_F;
updImpl().m_def_p_BSb = defaultCenter_B;
updImpl().m_def_radius_B = defaultRadius_B;
}
Force::MobilityLinearStop::MobilityLinearStop
(GeneralForceSubsystem& forces,
const MobilizedBody& mobod,
MobilizerQIndex whichQ,
Real defaultStiffness,
Real defaultDissipation,
Real defaultQLow,
Real defaultQHigh)
: Force(new MobilityLinearStopImpl(mobod,whichQ,
defaultStiffness, defaultDissipation,
defaultQLow, defaultQHigh))
{
SimTK_ERRCHK2_ALWAYS(defaultStiffness >= 0 && defaultDissipation >= 0,
"Force::MobilityLinearStop::MobilityLinearStop()",
"Stiffness and dissipation coefficient must be nonnegative "
"(defaultStiffness=%g, defaultDissipation=%g).",
defaultStiffness, defaultDissipation);
SimTK_ERRCHK2_ALWAYS(defaultQLow <= defaultQHigh,
"Force::MobilityLinearStop::MobilityLinearStop()",
"Lower bound can't be larger than upper bound "
"(defaultQLow=%g, defaultQHigh=%g).",
defaultQLow, defaultQHigh);
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::MobilityConstantForce::MobilityConstantForce
(GeneralForceSubsystem& forces,
const MobilizedBody& mobod,
MobilizerUIndex whichU,
Real defaultForce)
: Force(new MobilityConstantForceImpl(mobod, whichU, defaultForce)) {
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::Gravity::Gravity
(GeneralForceSubsystem& forces,
const SimbodyMatterSubsystem& matter,
const Vec3& defGravity)
: Force(new GravityImpl(matter,defGravity,0))
{
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Constraint::SphereOnPlaneContact::SphereOnPlaneContact
(MobilizedBody& planeBody,
const Transform& defaultPlaneFrame,
MobilizedBody& sphereBody,
const Vec3& defaultSphereCenter,
Real defaultSphereRadius,
bool enforceRolling)
: Constraint(new SphereOnPlaneContactImpl(enforceRolling))
{
SimTK_APIARGCHECK_ALWAYS( planeBody.isInSubsystem()
&& sphereBody.isInSubsystem(),
"Constraint::SphereOnPlaneContact","SphereOnPlaneContact",
"Both bodies must already be in a SimbodyMatterSubsystem.");
SimTK_APIARGCHECK_ALWAYS(planeBody.isInSameSubsystem(sphereBody),
"Constraint::SphereOnPlaneContact","SphereOnPlaneContact",
"The two bodies to be connected must be in the same "
"SimbodyMatterSubsystem.");
SimTK_APIARGCHECK1_ALWAYS(defaultSphereRadius > 0,
"Constraint::SphereOnPlaneContact","SphereOnPlaneContact",
"The sphere radius must be greater than zero but was %g.",
defaultSphereRadius);
planeBody.updMatterSubsystem().adoptConstraint(*this);
updImpl().m_planeBody_F = updImpl().addConstrainedBody(planeBody);
updImpl().m_ballBody_B = updImpl().addConstrainedBody(sphereBody);
updImpl().m_def_X_FP = defaultPlaneFrame;
updImpl().m_def_p_BO = defaultSphereCenter;
updImpl().m_def_radius = defaultSphereRadius;
}
Force::MobilityLinearSpring& Force::MobilityLinearSpring::
setDefaultQZero(Real defaultQZero) {
MobilityLinearSpringImpl& impl = updImpl();
if (impl.m_defaultQZero != defaultQZero) {
impl.m_defaultQZero = defaultQZero;
impl.invalidateTopologyCache();
}
return *this;
}
Force::MobilityConstantForce& Force::MobilityConstantForce::
setDefaultForce(Real defaultForce) {
MobilityConstantForceImpl& impl = updImpl();
if (impl.m_defaultForce != defaultForce) {
impl.m_defaultForce = defaultForce;
impl.invalidateTopologyCache();
}
return *this;
}
Force::Gravity& Force::Gravity::
setDefaultDownDirection(const UnitVec3& down) {
SimTK_ERRCHK_ALWAYS(down.isFinite(),
"Force::Gravity::setDefaultDownDirection()",
"A non-finite 'down' direction was received; did you specify a zero-"
"length Vec3? The direction must be non-zero.");
getImpl().invalidateTopologyCache();
updImpl().defDirection = down;
return *this;
}
Force::Gravity& Force::Gravity::
setDefaultMagnitude(Real g) {
SimTK_ERRCHK1_ALWAYS(g >= 0,
"Force::Gravity::setDefaultMagnitude()",
"The gravity magnitude g must be nonnegative but was specified as %g.",
g);
getImpl().invalidateTopologyCache();
updImpl().defMagnitude = g;
return *this;
}
Force::Gravity::Gravity
(GeneralForceSubsystem& forces,
const SimbodyMatterSubsystem& matter,
Real defMagnitude)
: Force(new GravityImpl(matter,defMagnitude,0))
{
SimTK_ERRCHK1_ALWAYS(defMagnitude >= 0,
"Force::Gravity::Gravity(magnitude)",
"The gravity magnitude g must be nonnegative but was specified as %g.",
defMagnitude);
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::MobilityLinearSpring& Force::MobilityLinearSpring::
setDefaultStiffness(Real defaultStiffness) {
SimTK_ERRCHK1_ALWAYS(defaultStiffness >= 0,
"Force::MobilityLinearSpring::setDefaultStiffness()",
"Stiffness coefficient must be nonnegative "
"(defaultStiffness=%g).", defaultStiffness);
MobilityLinearSpringImpl& impl = updImpl();
if (impl.m_defaultStiffness != defaultStiffness) {
impl.m_defaultStiffness = defaultStiffness;
impl.invalidateTopologyCache();
}
return *this;
}
Force::MobilityLinearDamper::
MobilityLinearDamper(GeneralForceSubsystem& forces,
const MobilizedBody& mobod,
MobilizerUIndex whichU,
Real defaultDamping)
: Force(new MobilityLinearDamperImpl(mobod, whichU, defaultDamping))
{
SimTK_ERRCHK1_ALWAYS(defaultDamping >= 0,
"Force::MobilityLinearDamper::MobilityLinearDamper()",
"Damping coefficient must be nonnegative "
"(defaultDamping=%g).", defaultDamping);
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::MobilityLinearDamper& Force::MobilityLinearDamper::
setDefaultDamping(Real defaultDamping) {
SimTK_ERRCHK1_ALWAYS(defaultDamping >= 0,
"Force::MobilityLinearDamper::setDefaultDamping()",
"Damping coefficient must be nonnegative "
"(defaultDamping=%g).", defaultDamping);
MobilityLinearDamperImpl& impl = updImpl();
if (impl.m_defaultDamping != defaultDamping) {
impl.m_defaultDamping = defaultDamping;
impl.invalidateTopologyCache();
}
return *this;
}
Force::MobilityLinearStop& Force::MobilityLinearStop::
setDefaultBounds(Real defaultQLow, Real defaultQHigh) {
SimTK_ERRCHK2_ALWAYS(defaultQLow <= defaultQHigh,
"Force::MobilityLinearStop::setDefaultBounds()",
"Lower bound can't be larger than upper bound "
"(defaultQLow=%g, defaultQHigh=%g).",
defaultQLow, defaultQHigh);
MobilityLinearStopImpl& impl = updImpl();
if (impl.m_defQLow != defaultQLow || impl.m_defQHigh != defaultQHigh) {
impl.m_defQLow = defaultQLow;
impl.m_defQHigh = defaultQHigh;
impl.invalidateTopologyCache();
}
return *this;
}
Force::MobilityLinearSpring::
MobilityLinearSpring(GeneralForceSubsystem& forces,
const MobilizedBody& mobod,
MobilizerQIndex whichQ,
Real defaultStiffness,
Real defaultQZero)
: Force(new MobilityLinearSpringImpl(mobod, whichQ,
defaultStiffness, defaultQZero))
{
SimTK_ERRCHK1_ALWAYS(defaultStiffness >= 0,
"Force::MobilityLinearSpring::MobilityLinearSpring()",
"Stiffness coefficient must be nonnegative "
"(defaultStiffness=%g).", defaultStiffness);
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::MobilityLinearStop& Force::MobilityLinearStop::
setDefaultMaterialProperties(Real defaultStiffness, Real defaultDissipation) {
SimTK_ERRCHK2_ALWAYS(defaultStiffness >= 0 && defaultDissipation >= 0,
"Force::MobilityLinearStop::setDefaultMaterialProperties()",
"Stiffness and dissipation coefficient must be nonnegative "
"(defaultStiffness=%g, defaultDissipation=%g).",
defaultStiffness, defaultDissipation);
MobilityLinearStopImpl& impl = updImpl();
if ( impl.m_defStiffness != defaultStiffness
|| impl.m_defDissipation != defaultDissipation) {
impl.m_defStiffness = defaultStiffness;
impl.m_defDissipation = defaultDissipation;
impl.invalidateTopologyCache();
}
return *this;
}
Force::Gravity::Gravity
(GeneralForceSubsystem& forces,
const SimbodyMatterSubsystem& matter,
const UnitVec3& defDirection,
Real defMagnitude,
Real defZeroHeight)
: Force(new GravityImpl(matter,defDirection,defMagnitude,defZeroHeight))
{
SimTK_ERRCHK1_ALWAYS(defMagnitude >= 0,
"Force::Gravity::Gravity(downDirection,magnitude)",
"The gravity magnitude g must be nonnegative but was specified as %g.",
defMagnitude);
SimTK_ERRCHK_ALWAYS(defDirection.isFinite(),
"Force::Gravity::Gravity(downDirection,magnitude)",
"A non-finite 'down' direction was received; did you specify a zero-"
"length Vec3? The direction must be non-zero.");
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::Gravity& Force::Gravity::
setDefaultBodyIsExcluded(MobilizedBodyIndex mobod, bool isExcluded) {
// Invalidates topology cache.
updImpl().setMobodIsImmuneByDefault(mobod, isExcluded);
return *this;
}
void ElasticFoundationForce::setTransitionVelocity(Real v) {
updImpl().transitionVelocity = v;
}
void ElasticFoundationForce::setBodyParameters
(ContactSurfaceIndex bodyIndex, Real stiffness, Real dissipation,
Real staticFriction, Real dynamicFriction, Real viscousFriction) {
updImpl().setBodyParameters(bodyIndex, stiffness, dissipation, staticFriction, dynamicFriction, viscousFriction);
}
Force::GlobalDamper::GlobalDamper(GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter,
Real damping) : Force(new GlobalDamperImpl(matter, damping)) {
assert(damping >= 0);
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::ConstantTorque::ConstantTorque(GeneralForceSubsystem& forces, const MobilizedBody& body, const Vec3& torque) :
Force(new ConstantTorqueImpl(body, torque)) {
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::ConstantForce::ConstantForce(GeneralForceSubsystem& forces, const MobilizedBody& body, const Vec3& station, const Vec3& force) :
Force(new ConstantForceImpl(body, station, force)) {
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::MobilityDiscreteForce& Force::MobilityDiscreteForce::
setDefaultMobilityForce(Real defaultForce) {
updImpl().m_defaultVal = defaultForce;
getImpl().invalidateTopologyCache();
return *this;
}
Force::DiscreteForces::DiscreteForces
(GeneralForceSubsystem& forces, const SimbodyMatterSubsystem& matter)
: Force(new DiscreteForcesImpl(matter)) {
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
ElasticFoundationForce::ElasticFoundationForce(GeneralForceSubsystem& forces, GeneralContactSubsystem& contacts, ContactSetIndex set) :
Force(new ElasticFoundationForceImpl(contacts, set)) {
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
Force::Gravity& Force::Gravity::
setDefaultZeroHeight(Real zeroHeight) {
getImpl().invalidateTopologyCache();
updImpl().defZeroHeight = zeroHeight;
return *this;
}
Force::TwoPointLinearSpring::TwoPointLinearSpring(GeneralForceSubsystem& forces, const MobilizedBody& body1, const Vec3& station1,
const MobilizedBody& body2, const Vec3& station2, Real k, Real x0) : Force(new TwoPointLinearSpringImpl(
body1, station1, body2, station2, k, x0)) {
updImpl().setForceSubsystem(forces, forces.adoptForce(*this));
}
