SubsystemGuts.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_
#define SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_
 
/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2006-14 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */
 
#include "SimTKcommon/basics.h"
#include "SimTKcommon/Simmatrix.h"
#include "SimTKcommon/internal/State.h"
 
#include <cassert>
 
namespace SimTK {
 
class System;
class DecorativeGeometry;
 
//==============================================================================
//                           SUBSYSTEM :: GUTS
//==============================================================================
class SimTK_SimTKCOMMON_EXPORT Subsystem::Guts {
public:
 
explicit Guts(const String& name="<NONAME>", const String& version="0.0.0");
 
virtual ~Guts();
 
Guts(const Guts&);
 
const String& getName()    const {return m_subsystemName;}
const String& getVersion() const {return m_subsystemVersion;}
 
QIndex allocateQ(State& s, const Vector& qInit) const 
{   return s.allocateQ(getMySubsystemIndex(), qInit); }
UIndex allocateU(State& s, const Vector& uInit) const 
{   return s.allocateU(getMySubsystemIndex(), uInit); }
ZIndex allocateZ(State& s, const Vector& zInit) const 
{   return s.allocateZ(getMySubsystemIndex(), zInit); }
 
DiscreteVariableIndex 
allocateDiscreteVariable(State& s, Stage g, AbstractValue* v) const 
{   return s.allocateDiscreteVariable(getMySubsystemIndex(), g, v); }
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
   (State& s, Stage invalidates, AbstractValue* v, Stage updateDependsOn) const
{   return s.allocateAutoUpdateDiscreteVariable
               (getMySubsystemIndex(),invalidates,v,updateDependsOn); }
 
CacheEntryIndex allocateCacheEntry
   (const State& s, Stage dependsOn, Stage computedBy, AbstractValue* v) const 
{   return s.allocateCacheEntry
               (getMySubsystemIndex(), dependsOn, computedBy, v); }
CacheEntryIndex allocateCacheEntry
   (const State& state, Stage g, AbstractValue* v) const 
{   return allocateCacheEntry(state, g, g, v); }
CacheEntryIndex allocateLazyCacheEntry   
   (const State& state, Stage earliest, AbstractValue* v) const 
{   return allocateCacheEntry(state, earliest, Stage::Infinity, v); }
 
QErrIndex allocateQErr(const State& s, int nqerr) const 
{   return s.allocateQErr(getMySubsystemIndex(), nqerr); }
UErrIndex allocateUErr(const State& s, int nuerr) const 
{   return s.allocateUErr(getMySubsystemIndex(), nuerr); }
UDotErrIndex allocateUDotErr(const State& s, int nudoterr) const 
{   return s.allocateUDotErr(getMySubsystemIndex(), nudoterr); }
EventTriggerByStageIndex 
allocateEventTriggersByStage(const State& s, Stage g, int ntriggers) const 
{   return s.allocateEventTrigger(getMySubsystemIndex(),g,ntriggers); }
 
const Vector& getQ(const State& s) const 
{   return s.getQ(getMySubsystemIndex()); }
const Vector& getU(const State& s) const 
{   return s.getU(getMySubsystemIndex()); }
const Vector& getZ(const State& s) const 
{   return s.getZ(getMySubsystemIndex()); }
const Vector& getUWeights(const State& s) const 
{   return s.getUWeights(getMySubsystemIndex()); }
const Vector& getZWeights(const State& s) const 
{   return s.getZWeights(getMySubsystemIndex()); }
 
Vector& updQ(State& s) const {return s.updQ(getMySubsystemIndex());}
Vector& updU(State& s) const {return s.updU(getMySubsystemIndex());}
Vector& updZ(State& s) const {return s.updZ(getMySubsystemIndex());}
 
const Vector& getQDot   (const State& s) const 
{   return s.getQDot(getMySubsystemIndex()); }
const Vector& getUDot   (const State& s) const 
{   return s.getUDot(getMySubsystemIndex()); }
const Vector& getZDot   (const State& s) const 
{   return s.getZDot(getMySubsystemIndex()); }
const Vector& getQDotDot(const State& s) const 
{   return s.getQDotDot(getMySubsystemIndex()); }
 
Vector& updQDot   (const State& s) const 
{   return s.updQDot(getMySubsystemIndex()); }
Vector& updUDot   (const State& s) const 
{   return s.updUDot(getMySubsystemIndex()); }
Vector& updZDot   (const State& s) const 
{   return s.updZDot(getMySubsystemIndex()); }
Vector& updQDotDot(const State& s) const 
{   return s.updQDotDot(getMySubsystemIndex()); }
 
const Vector& getQErr(const State& s) const 
{   return s.getQErr(getMySubsystemIndex()); }
const Vector& getUErr(const State& s) const 
{   return s.getUErr(getMySubsystemIndex()); }
const Vector& getQErrWeights(const State& s) const 
{   return s.getQErrWeights(getMySubsystemIndex()); }
const Vector& getUErrWeights(const State& s) const 
{   return s.getUErrWeights(getMySubsystemIndex()); }
 
const Vector& getUDotErr(const State& s) const 
{   return s.getUDotErr(getMySubsystemIndex()); }
const Vector& getMultipliers(const State& s) const 
{   return s.getMultipliers(getMySubsystemIndex()); }
const Vector& getEventTriggersByStage(const State& s, Stage g) const
{   return s.getEventTriggersByStage(getMySubsystemIndex(),g); }
 
Vector& updQErr(const State& s) const 
{   return s.updQErr(getMySubsystemIndex()); }
Vector& updUErr(const State& s) const 
{   return s.updUErr(getMySubsystemIndex()); }
Vector& updUDotErr(const State& s) const 
{   return s.updUDotErr(getMySubsystemIndex()); }
Vector& updMultipliers(const State& s) const 
{   return s.updMultipliers(getMySubsystemIndex()); }
Vector& updEventTriggersByStage(const State& s, Stage g) const
{   return s.updEventTriggersByStage(getMySubsystemIndex(),g); }
 
SystemQIndex getQStart(const State& s) const 
{   return s.getQStart(getMySubsystemIndex()); }
int getNQ(const State& s)     const 
{   return s.getNQ(getMySubsystemIndex()); }
 
SystemUIndex getUStart(const State& s) const 
{   return s.getUStart(getMySubsystemIndex()); }
int getNU(const State& s)     const 
{   return s.getNU(getMySubsystemIndex()); }
 
SystemZIndex getZStart(const State& s) const 
{   return s.getZStart(getMySubsystemIndex()); }
int getNZ(const State& s)     const 
{   return s.getNZ(getMySubsystemIndex()); }
 
SystemQErrIndex getQErrStart(const State& s) const 
{   return s.getQErrStart(getMySubsystemIndex()); }
int getNQErr(const State& s) const 
{   return s.getNQErr(getMySubsystemIndex()); }
 
SystemUErrIndex getUErrStart(const State& s) const 
{   return s.getUErrStart(getMySubsystemIndex()); }
int getNUErr(const State& s)     const 
{   return s.getNUErr(getMySubsystemIndex()); }
 
SystemUDotErrIndex getUDotErrStart(const State& s) const 
{   return s.getUDotErrStart(getMySubsystemIndex()); }
int getNUDotErr(const State& s)     const 
{   return s.getNUDotErr(getMySubsystemIndex()); }
 
SystemMultiplierIndex getMultipliersStart(const State& s) const 
{   return s.getMultipliersStart(getMySubsystemIndex()); }
int getNMultipliers(const State& s)     const 
{   return s.getNMultipliers(getMySubsystemIndex()); }
 
SystemEventTriggerByStageIndex getEventTriggerStartByStage(const State& s, Stage g) const 
{   return s.getEventTriggerStartByStage(getMySubsystemIndex(),g); }
int getNEventTriggersByStage   (const State& s, Stage g) const 
{   return s.getNEventTriggersByStage(getMySubsystemIndex(),g); }
 
 
// For convenience.
void setQ(State& s, const Vector& q) const {
    SimTK_ASSERT(q.size() == getNQ(s), "Subsystem::Guts::setQ()");
    updQ(s) = q;
}
void setU(State& s, const Vector& u) const {
    SimTK_ASSERT(u.size() == getNU(s), "Subsystem::Guts::setU()");
    updU(s) = u;
}
void setZ(State& s, const Vector& z) const {
    SimTK_ASSERT(z.size() == getNZ(s), "Subsystem::Guts::setZ()");
    updZ(s) = z;
}
 
Stage getStage(const State& s) const 
{   return s.getSubsystemStage(getMySubsystemIndex()); }
void advanceToStage(const State& s, Stage g) const 
{   s.advanceSubsystemToStage(getMySubsystemIndex(), g); }
 
const AbstractValue& 
getDiscreteVariable(const State& s, DiscreteVariableIndex index) const 
{   return s.getDiscreteVariable(getMySubsystemIndex(), index); }
AbstractValue& updDiscreteVariable(State& s, DiscreteVariableIndex index) const 
{   return s.updDiscreteVariable(getMySubsystemIndex(), index); }
const AbstractValue& getCacheEntry(const State& s, CacheEntryIndex index) const 
{   return s.getCacheEntry(getMySubsystemIndex(), index); }
AbstractValue& updCacheEntry(const State& s, CacheEntryIndex index) const 
{   return s.updCacheEntry(getMySubsystemIndex(), index); }
Real getDiscreteVarLastUpdateTime(const State& s, DiscreteVariableIndex dx) const
{   return s.getDiscreteVarLastUpdateTime(getMySubsystemIndex(),dx); }
CacheEntryIndex 
getDiscreteVarUpdateIndex(const State& s, DiscreteVariableIndex dx) const
{   return s.getDiscreteVarUpdateIndex(getMySubsystemIndex(),dx); }
const AbstractValue& 
getDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const
{   return s.getDiscreteVarUpdateValue(getMySubsystemIndex(),dx); }
AbstractValue& 
updDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const
{   return s.updDiscreteVarUpdateValue(getMySubsystemIndex(),dx); }
bool isDiscreteVarUpdateValueRealized
   (const State& s, DiscreteVariableIndex dx) const
{   return s.isDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); }
void markDiscreteVarUpdateValueRealized
   (const State& s, DiscreteVariableIndex dx) const
{   return s.markDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); }
 
bool isCacheValueRealized(const State& s, CacheEntryIndex cx) const 
{   return s.isCacheValueRealized(getMySubsystemIndex(), cx); }
 
void markCacheValueRealized(const State& s, CacheEntryIndex cx) const 
{   s.markCacheValueRealized(getMySubsystemIndex(), cx); }
 
void markCacheValueNotRealized(const State& s, CacheEntryIndex cx) const 
{   s.markCacheValueNotRealized(getMySubsystemIndex(), cx); }
MeasureIndex adoptMeasure(AbstractMeasure& m);
 
template <class T> Measure_<T> getMeasure_(MeasureIndex mx) const
{   return Measure_<T>::getAs(getMeasure(mx)); }
 
AbstractMeasure getMeasure(MeasureIndex mx) const {
    SimTK_ASSERT(0 <= mx && mx < m_measures.size(), 
                 "Subsystem::Guts::getMeasure()");
    return AbstractMeasure(m_measures[mx]);
}
 
bool isInSystem() const {return m_mySystem != 0;}
bool isInSameSystem(const Subsystem& otherSubsystem) const;
 
const System& getSystem() const {
    SimTK_ASSERT(isInSystem(), "Subsystem::getSystem()");
    return *m_mySystem;
}
System& updSystem() {
    SimTK_ASSERT(isInSystem(), "Subsystem::updSystem()");
    return *m_mySystem;
}
void setSystem(System& sys, SubsystemIndex id) {
    SimTK_ASSERT(!isInSystem(), "Subsystem::setSystem()");
    SimTK_ASSERT(id.isValid(), "Subsystem::setSystem()");
    m_mySystem = &sys;
    m_mySubsystemIndex = id;
}
SubsystemIndex getMySubsystemIndex() const {
    SimTK_ASSERT(isInSystem(), "Subsystem::getMySubsystemIndex()");
    return m_mySubsystemIndex;
}
 
 
bool subsystemTopologyHasBeenRealized() const
{   return m_subsystemTopologyRealized; }
 
void invalidateSubsystemTopologyCache() const;
 
// These are wrappers for the virtual methods defined below. They
// are used to ensure good behavior. Most of them deal automatically with
// the Subsystem's Measures, as well as invoking the corresponding virtual
// for the Subsystem's own processing.
 
Subsystem::Guts* clone() const;
 
// Realize this subsystem's part of the State from Stage-1 to Stage
// for the indicated stage. After doing some checking, these routines
// call the concrete subsystem's corresponding virtual method, and
// on return they make sure the stage has been properly updated.
// Note that these will do nothing if the Subsystem stage is already
// at or greater than the indicated stage.
void realizeSubsystemTopology    (State&) const;
void realizeSubsystemModel       (State&) const;
void realizeSubsystemInstance    (const State&) const;
void realizeSubsystemTime        (const State&) const;
void realizeSubsystemPosition    (const State&) const;
void realizeSubsystemVelocity    (const State&) const;
void realizeSubsystemDynamics    (const State&) const;
void realizeSubsystemAcceleration(const State&) const;
void realizeSubsystemReport      (const State&) const;
 
// Generate decorative geometry computable at a specific stage. This will
// throw an exception if this subsystem's state hasn't already been realized
// to that stage. Note that the list is not inclusive -- you have to
// request geometry from each stage to get all of it.
// The generated geometry will be *appended* to the supplied output vector.
void calcDecorativeGeometryAndAppend
    (const State&, Stage, Array_<DecorativeGeometry>&) const;
    
void createScheduledEvent(const State& state, EventId& eventId) const;
void createTriggeredEvent(const State& state, EventId& eventId, 
                            EventTriggerByStageIndex& triggerFunctionIndex,
                            Stage stage) const;
 
// These methods are called by the corresponding methods of System.
// Each subsystem is responsible for defining its own events, and
// System then combines the information from them, and dispatches events
// to the appropriate subsystems for handling when they occur.
void calcEventTriggerInfo
    (const State&, Array_<EventTriggerInfo>&) const;
void calcTimeOfNextScheduledEvent
    (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 
    bool includeCurrentTime) const;
void calcTimeOfNextScheduledReport
    (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 
    bool includeCurrentTime) const;
void handleEvents
    (State&, Event::Cause, const Array_<EventId>& eventIds,
    const HandleEventsOptions& options, HandleEventsResults& results) const;
void reportEvents
    (const State&, Event::Cause, const Array_<EventId>& eventIds) const;
 
protected:
// These virtual methods should be overridden in concrete Subsystems as
// necessary. They should never be called directly; instead call the
// wrapper routines above, which have the same name but without the "Impl"
// (implementation) at the end.
    
// The "realize..." wrappers will call the "realize...Impl" methods below
// only when the current stage for the Subsystem is the one just prior
// to the stage being realized. For example, realizeSubsystemVelocityImpl()
// is called by realizeSubsystemVelocity() only when the passed-in State
// shows this subsystem's stage to be exactly Stage::Position.
//
// The default implementations provided here do nothing. That means the
// wrappers will simply check that the current stage is correct and
// advance it if necessary.
 
// The destructor is already virtual; see above.
 
virtual Subsystem::Guts* cloneImpl() const = 0;
 
virtual int realizeSubsystemTopologyImpl(State& s)       const {return 0;}
virtual int realizeSubsystemModelImpl   (State& s)       const {return 0;}
virtual int realizeSubsystemInstanceImpl(const State& s) const {return 0;}
virtual int realizeSubsystemTimeImpl    (const State& s) const {return 0;}
virtual int realizeSubsystemPositionImpl(const State& s) const {return 0;}
virtual int realizeSubsystemVelocityImpl(const State& s) const {return 0;}
virtual int realizeSubsystemDynamicsImpl(const State& s) const {return 0;}
virtual int realizeSubsystemAccelerationImpl(const State& s)const{return 0;}
virtual int realizeSubsystemReportImpl  (const State& s) const {return 0;}
 
virtual int calcDecorativeGeometryAndAppendImpl
    (const State&, Stage, Array_<DecorativeGeometry>&) const {return 0;}
 
virtual void calcEventTriggerInfoImpl
    (const State&, Array_<EventTriggerInfo>&) const {}
virtual void calcTimeOfNextScheduledEventImpl
    (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 
    bool includeCurrentTime) const {}
virtual void calcTimeOfNextScheduledReportImpl
    (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 
    bool includeCurrentTime) const {}
virtual void handleEventsImpl
    (State&, Event::Cause, const Array_<EventId>& eventIds,
    const HandleEventsOptions& options, 
    HandleEventsResults& results) const {}
virtual void reportEventsImpl
    (const State&, Event::Cause, const Array_<EventId>& eventIds) const {}
 
 
public:
const Subsystem& getOwnerSubsystemHandle() const {
    SimTK_ASSERT(m_myHandle, "Subsystem::getOwnerSubsystemHandle()");
    return *m_myHandle;
}
Subsystem& updOwnerSubsystemHandle() {
    SimTK_ASSERT(m_myHandle, "Subsystem::getOwnerSubsystemHandle()");
    return *m_myHandle;
}
 
void setOwnerSubsystemHandle(Subsystem& subsys) {m_myHandle=&subsys;}
 
bool hasOwnerSubsystemHandle() const {return m_myHandle != 0;}
 
private:
// Suppressed.
Guts& operator=(const Guts&);
 
//------------------------------------------------------------------------------
                                    private:
 
    // TOPOLOGY STATE INFORMATION
String          m_subsystemName;
String          m_subsystemVersion;
System*         m_mySystem;       // the System to which this Subsystem belongs
SubsystemIndex  m_mySubsystemIndex;  // Subsystem # within System
 
friend class Subsystem;
Subsystem*      m_myHandle;    // the owner handle of this Guts object
 
// This is the list of Measures belonging to this Subsystem.
Array_<AbstractMeasure::Implementation*> 
                m_measures;
 
    // TOPOLOGY CACHE INFORMATION
mutable bool    m_subsystemTopologyRealized;
};
 
 
//==============================================================================
//                           SUBSYSTEM INLINES
//==============================================================================
// These had to wait for Subsystem::Guts to be defined.
 
inline SubsystemIndex Subsystem::getMySubsystemIndex() const
{   return getSubsystemGuts().getMySubsystemIndex(); }
 
inline const String& Subsystem::getName()    const {return getSubsystemGuts().getName();}
inline const String& Subsystem::getVersion() const {return getSubsystemGuts().getVersion();}
 
inline bool Subsystem::subsystemTopologyHasBeenRealized() const {
    return getSubsystemGuts().subsystemTopologyHasBeenRealized();
}
 
inline void Subsystem::invalidateSubsystemTopologyCache() const {
    getSubsystemGuts().invalidateSubsystemTopologyCache(); // mutable
}
 
inline MeasureIndex Subsystem::adoptMeasure(AbstractMeasure& m)
{   return updSubsystemGuts().adoptMeasure(m); }
inline AbstractMeasure Subsystem::getMeasure(MeasureIndex mx) const
{   return getSubsystemGuts().getMeasure(mx); }
 
 
inline bool Subsystem::isInSystem() const 
{   return getSubsystemGuts().isInSystem(); }
inline bool Subsystem::isInSameSystem(const Subsystem& otherSubsystem) const 
{   return getSubsystemGuts().isInSameSystem(otherSubsystem); }
 
inline const System& Subsystem::getSystem() const 
{   return getSubsystemGuts().getSystem(); }
inline System& Subsystem::updSystem()       
{   return updSubsystemGuts().updSystem(); }
inline void Subsystem::setSystem(System& sys, SubsystemIndex id) 
{   updSubsystemGuts().setSystem(sys,id); }
 
inline bool Subsystem::isOwnerHandle() const 
{   return guts==0 || &guts->getOwnerSubsystemHandle()==this; }
 
} // namespace SimTK
 
#endif // SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_