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; }