// Get the value of the generalized force to be applied.
Real Force::MobilityDiscreteForceImpl::
getMobilityForce(const State& state) const {
const GeneralForceSubsystem& forces = getForceSubsystem();
const Real& fInState = Value<Real>::downcast
(forces.getDiscreteVariable(state, m_forceIx));
return fInState;
}
void Force::MobilityDiscreteForceImpl::
setMobilityForce(State& state, Real f) const {
const GeneralForceSubsystem& forces = getForceSubsystem();
Real& fInState = Value<Real>::updDowncast
(forces.updDiscreteVariable(state, m_forceIx));
fInState = f;
}
Vector_<SpatialVec>& Force::DiscreteForcesImpl::
updAllBodyForces(State& state) const {
const GeneralForceSubsystem& forces = getForceSubsystem();
Vector_<SpatialVec>& FInState = Value< Vector_<SpatialVec> >::updDowncast
(forces.updDiscreteVariable(state, m_bodyForcesIx));
return FInState;
}
Vector& Force::DiscreteForcesImpl::
updAllMobilityForces(State& state) const {
const GeneralForceSubsystem& forces = getForceSubsystem();
Vector& fInState = Value<Vector>::updDowncast
(forces.updDiscreteVariable(state, m_mobForcesIx));
return fInState;
}
void Force::DiscreteForcesImpl::
realizeTopology(State& state) const {
const GeneralForceSubsystem& forces = getForceSubsystem();
m_mobForcesIx = forces.allocateDiscreteVariable
(state, Stage::Dynamics, new Value<Vector>());
m_bodyForcesIx = forces.allocateDiscreteVariable
(state, Stage::Dynamics, new Value< Vector_<SpatialVec> >());
}
void Force::MobilityDiscreteForceImpl::
calcForce( const State& state,
Vector_<SpatialVec>& /*bodyForces*/,
Vector_<Vec3>& /*particleForces*/,
Vector& mobilityForces) const
{
const GeneralForceSubsystem& forces = getForceSubsystem();
const Real f = Value<Real>::downcast
(forces.getDiscreteVariable(state, m_forceIx));
const MobilizedBody& mobod = m_matter.getMobilizedBody(m_mobodIx);
mobod.applyOneMobilityForce(state, m_whichU, f, mobilityForces);
}
//----------------------------- REALIZE TOPOLOGY -------------------------------
// Allocate the state variables and cache entries. The force cache is a lazy-
// evaluation entry - although it can be calculated any time after
// Stage::Position, it won't be unless someone asks for it. And if it is
// evaluated early, it should not be re-evaluated when used as a force during
// Stage::Dynamics (via the calcForce() call).
//
// In addition, the force cache has a dependency on the parameter values that
// are stored in a discrete state variable. Changes to that variable
// automatically invalidate Dynamics stage, but must be carefully managed also
// to invalidate the force cache here since it is only Position-dependent.
void Force::GravityImpl::
realizeTopology(State& s) const {
GravityImpl* mThis = const_cast<GravityImpl*>(this);
const int nb=matter.getNumBodies(), np=matter.getNumParticles();
// In case more mobilized bodies were added after this Gravity element
// was constructed, make room for the rest now. Earlier default immunity
// settings are preserved.
if (defMobodIsImmune.size() != nb)
mThis->defMobodIsImmune.resize(nb, false);
// Allocate a discrete state variable to hold parameters; see above comment.
const Parameters p(defDirection,defMagnitude,defZeroHeight,
defMobodIsImmune); // initial value
mThis->parametersIx = getForceSubsystem()
.allocateDiscreteVariable(s, Stage::Dynamics, new Value<Parameters>(p));
// Don't allocate force cache space yet since we would have to copy it.
// Caution -- dependence on Parameters requires manual invalidation.
mThis->forceCacheIx = getForceSubsystem().allocateLazyCacheEntry(s,
Stage::Position, new Value<ForceCache>());
// Now allocate the appropriate amount of space, and set to zero now
// any forces that we know will end up zero so we don't have to calculate
// them at run time. Precalculated zeroes must be provided for any
// immune elements, or for all elements if g==0, and this must be kept
// up to date if there are runtime changes to the parameters.
ForceCache& fc = updForceCache(s);
if (defMagnitude == 0) {
fc.allocate(nb, np, true); // initially zero since no gravity
} else {
fc.allocate(nb, np, false); // initially NaN except for Ground
for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx)
if (defMobodIsImmune[mbx])
fc.F_GB[mbx] = SpatialVec(Vec3(0),Vec3(0));
// This doesn't mean the ForceCache is valid yet.
}
}
void Force::DiscreteForcesImpl::
calcForce( const State& state,
Vector_<SpatialVec>& bodyForces,
Vector_<Vec3>& /*particleForces*/,
Vector& mobilityForces) const
{
const GeneralForceSubsystem& forces = getForceSubsystem();
const Vector& f = Value<Vector>::downcast
(forces.getDiscreteVariable(state, m_mobForcesIx));
const Vector_<SpatialVec>& F = Value< Vector_<SpatialVec> >::downcast
(forces.getDiscreteVariable(state, m_bodyForcesIx));
if (f.size()) mobilityForces += f;
if (F.size()) bodyForces += F;
}
void Force::calcForceContribution(const State& state,
Vector_<SpatialVec>& bodyForces,
Vector_<Vec3>& particleForces,
Vector& mobilityForces) const
{
const MultibodySystem& mbs = getForceSubsystem().getMultibodySystem();
const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
// Resize if necessary.
bodyForces.resize(matter.getNumBodies());
particleForces.resize(matter.getNumParticles());
mobilityForces.resize(matter.getNumMobilities());
// Set all forces to zero.
bodyForces.setToZero();
particleForces.setToZero();
mobilityForces.setToZero();
if (isDisabled(state)) return;
// Add in force element contributions.
getImpl().calcForce(state,bodyForces,particleForces,mobilityForces);
}
void Force::disable(State& state) const
{ getForceSubsystem().setForceIsDisabled(state, getForceIndex(), true); }
void invalidateForceCache(const State& s) const
{ getForceSubsystem().markCacheValueNotRealized(s,forceCacheIx); }
void markForceCacheValid(const State& s) const
{ getForceSubsystem().markCacheValueRealized(s,forceCacheIx); }
bool isForceCacheValid(const State& s) const
{ return getForceSubsystem().isCacheValueRealized(s,forceCacheIx); }
ForceCache& updForceCache(const State& s) const
{ return Value<ForceCache>::updDowncast
(getForceSubsystem().updCacheEntry(s,forceCacheIx)); }
const ForceCache& getForceCache(const State& s) const
{ return Value<ForceCache>::downcast
(getForceSubsystem().getCacheEntry(s,forceCacheIx)); }
Parameters& updParameters(State& s) const
{ return Value<Parameters>::updDowncast
(getForceSubsystem().updDiscreteVariable(s,parametersIx)); }
const Parameters& getParameters(const State& s) const
{ return Value<Parameters>::downcast
(getForceSubsystem().getDiscreteVariable(s,parametersIx)); }
bool Force::isDisabled(const State& state) const
{ return getForceSubsystem().isForceDisabled(state, getForceIndex()); }
void Force::enable(State& state) const
{ getForceSubsystem().setForceIsDisabled(state, getForceIndex(), false); }
void Force::MobilityDiscreteForceImpl::
realizeTopology(State& state) const {
const GeneralForceSubsystem& forces = getForceSubsystem();
m_forceIx = forces.allocateDiscreteVariable
(state, Stage::Dynamics, new Value<Real>(m_defaultVal));
}
