//------------------------- ENSURE FORCE CACHE VALID ---------------------------
// This will also calculate potential energy since we can do it on the cheap
// simultaneously with the force. Note that if the strength of gravity was set
// to zero then we already zeroed out the forces and pe during realizeInstance()
// so all we have to do in that case is mark the cache valid now. Also, any
// immune bodies have their force set to zero in realizeInstance() so we don't
// have to do it again here.
void Force::GravityImpl::
ensureForceCacheValid(const State& state) const {
if (isForceCacheValid(state)) return;
SimTK_STAGECHECK_GE_ALWAYS(state.getSystemStage(), Stage::Position,
"Force::GravityImpl::ensureForceCacheValid()");
const Parameters& p = getParameters(state);
if (p.g == 0) { // no gravity
markForceCacheValid(state); // zeroes must have been precalculated
return;
}
// Gravity is non-zero and gravity forces are not up to date, so this counts
// as an evaluation.
++numEvaluations;
const Vec3 gravity = p.g * p.d;
const Real zeroPEOffset = p.g * p.z;
ForceCache& fc = updForceCache(state);
fc.pe = 0;
const int nb = matter.getNumBodies();
// Skip Ground since we know it is immune.
for (MobilizedBodyIndex mbx(1); mbx < nb; ++mbx) {
if (p.mobodIsImmune[mbx])
continue; // don't apply gravity to this body; F already zero
const MobilizedBody& mobod = matter.getMobilizedBody(mbx);
const MassProperties& mprops = mobod.getBodyMassProperties(state);
const Transform& X_GB = mobod.getBodyTransform(state);
Real m = mprops.getMass();
const Vec3& p_CB = mprops.getMassCenter(); // in B
const Vec3 p_CB_G = X_GB.R()*p_CB; // exp. in G; 15 flops
const Vec3 p_G_CB = X_GB.p() + p_CB_G; // meas. in G; 3 flops
const Vec3 F_CB_G = m*gravity; // force at mass center; 3 flops
fc.F_GB[mbx] = SpatialVec(p_CB_G % F_CB_G, F_CB_G); // body frc; 9 flops
// odd signs here because height is in -gravity direction.
fc.pe -= m*(~gravity*p_G_CB + zeroPEOffset); // 8 flops
}
const int np = matter.getNumParticles();
if (np) {
const Vector& m = matter.getAllParticleMasses(state);
const Vector_<Vec3>& p_GP = matter.getAllParticleLocations(state);
for (ParticleIndex px(0); px < np; ++px) {
fc.f_GP[px] = m[px] * gravity; // 3 flops
fc.pe -= m[px]*(~gravity*p_GP[px] + zeroPEOffset); // 8 flops
}
}
markForceCacheValid(state);
}
//------------------------------------------------------------------------------
// REALIZE SUBSYSTEM REPORT
//------------------------------------------------------------------------------
void Subsystem::Guts::realizeSubsystemReport(const State& s) const {
SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage(Stage::Report).prev(),
"Subsystem::Guts::realizeSubsystemReport()");
if (getStage(s) < Stage::Report) {
realizeSubsystemReportImpl(s);
// Realize this Subsystem's Measures.
for (MeasureIndex mx(0); mx < m_measures.size(); ++mx)
m_measures[mx]->realizeReport(s);
advanceToStage(s, Stage::Report);
}
}
//------------------------------------------------------------------------------
// REALIZE SUBSYSTEM MODEL
//------------------------------------------------------------------------------
void Subsystem::Guts::realizeSubsystemModel(State& s) const {
SimTK_STAGECHECK_TOPOLOGY_REALIZED_ALWAYS(subsystemTopologyHasBeenRealized(),
"Subsystem", getName(), "Subsystem::Guts::realizeSubsystemModel()");
SimTK_STAGECHECK_GE_ALWAYS(getStage(s), Stage::Topology,
"Subsystem::Guts::realizeSubsystemModel()");
if (getStage(s) < Stage::Model) {
realizeSubsystemModelImpl(s);
// Realize this Subsystem's Measures.
for (MeasureIndex mx(0); mx < m_measures.size(); ++mx)
m_measures[mx]->realizeModel(s);
advanceToStage(s, Stage::Model);
}
}
