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));
}
void Force::DiscreteForces::
setAllMobilityForces(State& state, const Vector& f) const {
if (f.size()==0) {clearAllMobilityForces(state); return;}
SimTK_ERRCHK2_ALWAYS(f.size() == state.getNU(),
"Force::DiscreteForces::setAllMobilityForces()",
"Mobility force vector f had wrong size %d; should have been %d.",
f.size(), state.getNU());
getImpl().updAllMobilityForces(state) = f;
}
void Force::MobilityLinearStop::
setBounds(State& state, Real qLow, Real qHigh) const {
SimTK_ERRCHK2_ALWAYS(qLow <= qHigh,
"Force::MobilityLinearStop::setBounds()",
"Lower bound can't be larger than upper bound (qLow=%g, qHigh=%g).",
qLow, qHigh);
MobilityLinearStopImpl::Parameters& params =
getImpl().updParameters(state); // invalidates Dynamics stage
params.qLow = qLow; params.qHigh = qHigh;
}
void Force::DiscreteForces::
setAllBodyForces(State& state, const Vector_<SpatialVec>& bodyForcesInG) const {
if (bodyForcesInG.size()==0) {clearAllBodyForces(state); return;}
const int numBodies = getImpl().m_matter.getNumBodies();
SimTK_ERRCHK2_ALWAYS(bodyForcesInG.size() == numBodies,
"Force::DiscreteForces::setAllBodyForces()",
"Body force vector bodyForcesInG had wrong size %d; "
"should have been %d (0th entry is for Ground).",
bodyForcesInG.size(), numBodies);
getImpl().updAllBodyForces(state) = bodyForcesInG;
}
void Force::MobilityLinearStop::
setMaterialProperties(State& state, Real stiffness, Real dissipation) const {
SimTK_ERRCHK2_ALWAYS(stiffness >= 0 && dissipation >= 0,
"Force::MobilityLinearStop::setMaterialProperties()",
"Stiffness and dissipation coefficient must be nonnegative "
"(stiffness=%g, dissipation=%g).",
stiffness, dissipation);
MobilityLinearStopImpl::Parameters& params =
getImpl().updParameters(state); // invalidates Dynamics stage
params.k = stiffness; params.d = dissipation;
}
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::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;
}
const Force::Gravity& Force::Gravity::
setBodyIsExcluded(State& state, MobilizedBodyIndex mobod,
bool isExcluded) const
{
const GravityImpl& impl = getImpl();
SimTK_ERRCHK2_ALWAYS(mobod < impl.matter.getNumBodies(),
"Force::Gravity::setBodyIsExcluded()",
"Attemped to exclude mobilized body with index %d but only mobilized"
" bodies with indices between 0 and %d exist in this System.",
(int)mobod, impl.matter.getNumBodies()-1);
if (getBodyIsExcluded(state, mobod) != isExcluded) {
// Invalidates force cache.
impl.setMobodIsImmune(state, mobod, isExcluded);
// The zero must be precalculated if the body is immune to gravity.
SpatialVec& F = impl.updForceCache(state).F_GB[mobod];
if (isExcluded || getMagnitude(state) == 0)
F.setToZero();
else
F.setToNaN();
}
return *this;
}
