State.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_STATE_H_
#define SimTK_SimTKCOMMON_STATE_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTKcommon                               *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2005-7 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors: Peter Eastman                                                *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon/basics.h"
#include "SimTKcommon/Simmatrix.h"

#include <ostream>
#include <cassert>
#include <set>

namespace SimTK {

SimTK_DEFINE_UNIQUE_INDEX_TYPE(SubsystemIndex)

// TODO: these need an option to have associated "update" variables in the cache,
// analogous to the derivative variables qdot,udot,zdot that we create
// for the continuous variables. Consider whether "discrete variable" should
// be reserved for those that are updated in time, with something else like
// "parameter variable" for those that just hold externally set data.

class SimTK_SimTKCOMMON_EXPORT DiscreteVariable {
public:
    DiscreteVariable() : rep(0) { }
    DiscreteVariable(const DiscreteVariable&);
    DiscreteVariable& operator=(const DiscreteVariable&);
    ~DiscreteVariable();

    // This takes ownership of the AbstractValue pointer.
    DiscreteVariable(Stage, AbstractValue* vp);

    Stage getStage() const;
    const AbstractValue& getValue() const;
    AbstractValue&       updValue();

private:
    class DiscreteVariableRep* rep;
};

class SimTK_SimTKCOMMON_EXPORT CacheEntry : public DiscreteVariable {
public:
    CacheEntry() : DiscreteVariable() { }

    // This takes ownership of the AbstractValue pointer.
    CacheEntry(Stage g, AbstractValue* vp)
        : DiscreteVariable(g,vp) { }

    CacheEntry(const CacheEntry& ce) : DiscreteVariable(ce) { }
    CacheEntry& operator=(const CacheEntry& ce) {
        DiscreteVariable::operator=(ce);
        return *this;
    }
};

class EventStatus {
public:
    EventStatus() { initialize(); }
    // default destructor, copy constructor, copy assignment

    // Event trigger (which zero crossings cause triggering). Can be
    // OR'ed together to make a mask.
    enum EventTrigger {
        NoEventTrigger          =0x0000,    // must be 0

        PositiveToNegative      =0x0001,    // 1
        NegativeToPositive      =0x0002,    // 2

        Falling                 =(PositiveToNegative), // 1
        Rising                  =(NegativeToPositive), // 2
        AnySignChange           =(PositiveToNegative|NegativeToPositive)    // 3
    };

    bool isEventPending() const {return transitionSeen != NoEventTrigger;}
    EventTrigger getEventTrigger() const {return transitionSeen;}
    Real getLastTriggerTime() const {return lastTriggerTime;}
    Real getLastTriggerTimeBestGuess() const {return lastTriggerTimeBestGuess;}
    Real getBeforeValue() const {return beforeValue;}
    Real getAfterValue() const {return afterValue;}
    Real getLocalizationWindow() const {return localizationWindow;}

    void setEventTriggered(EventTrigger transition, Real triggerTime,
                           Real actualTimeEst, Real window,
                           Real before, Real after)
    {
        assert(transition != NoEventTrigger);
        assert(triggerTime >= 0 && actualTimeEst >= 0 
               && triggerTime >= actualTimeEst);

        transitionSeen = transition;
        lastTriggerTime = triggerTime;
        lastTriggerTimeBestGuess = actualTimeEst;
        localizationWindow = window;
        beforeValue = before;
        afterValue  = after;
    }

    void clearEventTrigger() {
        transitionSeen = NoEventTrigger;
    }

    // Classify a before/after sign transition.
    static EventTrigger classifyTransition(int before, int after) {
        if (before==after)
            return NoEventTrigger;
        if (before==0)
            return NoEventTrigger; // Do not report transitions away from zero.
        if (before==1)
            return PositiveToNegative;
        // before==-1
        return NegativeToPositive;
    }

    static EventTrigger maskTransition(EventTrigger transition, EventTrigger mask) {
        return EventTrigger(transition & mask); // we're depending on NoEventTrigger==0
    }

    SimTK_SimTKCOMMON_EXPORT static String eventTriggerString(EventTrigger e);
private:
    void initialize() {
        transitionSeen = NoEventTrigger;
        lastTriggerTime = lastTriggerTimeBestGuess = localizationWindow
            = beforeValue = afterValue = NaN;
    }

    EventTrigger transitionSeen;
    Real         lastTriggerTime; // digital
    Real         lastTriggerTimeBestGuess; // analog, <=lastTriggerTime
    Real         localizationWindow;
    Real         beforeValue, afterValue;
};

class SimTK_SimTKCOMMON_EXPORT State {
public:
    State();
    ~State();

    void clear();

    void setNSubsystems(int i);

    void initializeSubsystem(SubsystemIndex, const String& name, const String& version);

    State(const State&);

    State& operator=(const State&);

    int addSubsystem(const String& name, const String& version);

    int getNSubsystems() const;
    const String& getSubsystemName   (SubsystemIndex) const;
    const String& getSubsystemVersion(SubsystemIndex) const;
    const Stage&  getSubsystemStage  (SubsystemIndex) const;

    const Stage& getSystemStage() const;

    void invalidateAll(Stage) const;  // cache is mutable

    void advanceSubsystemToStage(SubsystemIndex, Stage) const;
    void advanceSystemToStage(Stage) const;


    int allocateQ(SubsystemIndex, const Vector& qInit); // qdot, qdotdot also allocated in cache
    int allocateU(SubsystemIndex, const Vector& uInit); // udot                    "
    int allocateZ(SubsystemIndex, const Vector& zInit); // zdot                    "


    int allocateQErr   (SubsystemIndex, int nqerr) const;    // these are cache entries
    int allocateUErr   (SubsystemIndex, int nuerr) const;
    int allocateUDotErr(SubsystemIndex, int nudoterr) const;

    int allocateEvent(SubsystemIndex, Stage, int nevent) const;

    int allocateDiscreteVariable(SubsystemIndex, Stage, AbstractValue* v);
    int allocateCacheEntry      (SubsystemIndex, Stage, AbstractValue* v);
    

    int getNY() const; // = nq+nu+nz
    int getQStart() const; int getNQ() const;
    int getUStart() const; int getNU() const;
    int getZStart() const; int getNZ() const;

    int getNYErr() const; // = nqerr+nuerr
    int getQErrStart() const; int getNQErr() const; // =mp + #quaternions
    int getUErrStart() const; int getNUErr() const; // =mp+mv

    int getNUDotErr() const;     // =mp+mv+ma
    int getNMultipliers() const; // =mp+mv+ma, necessarily the same as NUDotErr

    int getQStart(SubsystemIndex)       const; int getNQ(SubsystemIndex)       const;
    int getUStart(SubsystemIndex)       const; int getNU(SubsystemIndex)       const;
    int getZStart(SubsystemIndex)       const; int getNZ(SubsystemIndex)       const;

    int getQErrStart(SubsystemIndex)    const; int getNQErr(SubsystemIndex)    const;
    int getUErrStart(SubsystemIndex)    const; int getNUErr(SubsystemIndex)    const;
    int getUDotErrStart(SubsystemIndex) const; int getNUDotErr(SubsystemIndex) const;
    int getMultipliersStart(SubsystemIndex i) const;
    int getNMultipliers(SubsystemIndex i)     const;

        // Event handling
    int getNEvents() const; // total
    int getEventStartByStage(Stage) const; // per-stage
    int getNEventsByStage(Stage) const;
    int getEventStartByStage(SubsystemIndex, Stage) const;
    int getNEventsByStage(SubsystemIndex, Stage) const;

    const Vector& getEvents() const;
    const Vector& getEventsByStage(Stage) const;
    const Vector& getEventsByStage(SubsystemIndex, Stage) const;

    Vector& updEvents() const; // mutable
    Vector& updEventsByStage(Stage) const;
    Vector& updEventsByStage(SubsystemIndex, Stage) const;

    const Vector& getQ(SubsystemIndex) const;
    const Vector& getU(SubsystemIndex) const;
    const Vector& getZ(SubsystemIndex) const;

    Vector& updQ(SubsystemIndex);
    Vector& updU(SubsystemIndex);
    Vector& updZ(SubsystemIndex);

    const Vector& getQDot(SubsystemIndex) const;
    const Vector& getUDot(SubsystemIndex) const;
    const Vector& getZDot(SubsystemIndex) const;
    const Vector& getQDotDot(SubsystemIndex) const;

    Vector& updQDot(SubsystemIndex) const;    // these are mutable
    Vector& updUDot(SubsystemIndex) const;
    Vector& updZDot(SubsystemIndex) const;
    Vector& updQDotDot(SubsystemIndex) const;

    const Vector& getQErr(SubsystemIndex) const;
    const Vector& getUErr(SubsystemIndex) const;
    const Vector& getUDotErr(SubsystemIndex) const;
    const Vector& getMultipliers(SubsystemIndex) const;
    Vector& updQErr(SubsystemIndex) const;    // these are mutable
    Vector& updUErr(SubsystemIndex) const;
    Vector& updUDotErr(SubsystemIndex) const;
    Vector& updMultipliers(SubsystemIndex) const;

    const Real&   getTime() const;
    const Vector& getY() const; // {Q,U,Z} packed and in that order

    const Vector& getQ() const;
    const Vector& getU() const;
    const Vector& getZ() const;

    Real&   updTime();  // Back up to Stage::Time-1
    Vector& updY();     // Back up to Stage::Congfigured-1

    void setTime(Real t);
    void setY(const Vector& y);

    Vector& updQ();     // Back up to Stage::Position-1
    Vector& updU();     // Back up to Stage::Velocity-1
    Vector& updZ();     // Back up to Stage::Dynamics-1

    void setQ(const Vector& q);
    void setU(const Vector& u);
    void setZ(const Vector& z);

    const Vector& getYDot()    const; // Stage::Acceleration

    const Vector& getQDot()    const; // Stage::Velocity
    const Vector& getZDot()    const; // Stage::Dynamics
    const Vector& getUDot()    const; // Stage::Acceleration

    const Vector& getQDotDot() const; // Stage::Acceleration

    Vector& updYDot() const;    // Stage::Acceleration-1
    Vector& updQDot() const;    // Stage::Velocity-1     (view into YDot)
    Vector& updZDot() const;    // Stage::Dynamics-1            "
    Vector& updUDot() const;    // Stage::Acceleration-1        "

    Vector& updQDotDot() const; // Stage::Acceleration-1

    const Vector& getYErr() const; // {QErr,UErr} packed and in that order

    const Vector& getQErr() const;  // Stage::Position (index 3 constraints)
    const Vector& getUErr() const;  // Stage::Velocity (index 2 constraints)

    const Vector& getUDotErr()     const; // Stage::Acceleration (index 1 constraints)
    const Vector& getMultipliers() const; // Stage::Acceleration

    Vector& updYErr() const; // Stage::Dynamics-1
    Vector& updQErr() const; // Stage::Position-1 (view into YErr)
    Vector& updUErr() const; // Stage::Velocity-1        "

    Vector& updUDotErr()     const; // Stage::Acceleration-1 (not a view)
    Vector& updMultipliers() const; // Stage::Acceleration-1 (not a view)

    const AbstractValue& getDiscreteVariable(SubsystemIndex, int index) const;

    AbstractValue&       updDiscreteVariable(SubsystemIndex, int index);

    void setDiscreteVariable(SubsystemIndex i, int index, const AbstractValue& v);

    const AbstractValue& getCacheEntry(SubsystemIndex, int index) const;

    AbstractValue& updCacheEntry(SubsystemIndex, int index) const; // mutable
    
    void createRestrictedState(State& restrictedState, EnumerationSet<Stage> restrictedStages, std::set<SubsystemIndex> restrictedSubsystems);

    EnumerationSet<Stage> getRestrictedStages() const;

    std::set<SubsystemIndex> getRestrictedSubsystems() const;

    String toString() const;
    String cacheToString() const;

private:
    class StateRep* rep;
    const StateRep& getRep() const {assert(rep); return *rep;}
    StateRep&       updRep()       {assert(rep); return *rep;}
};

SimTK_SimTKCOMMON_EXPORT std::ostream& 
operator<<(std::ostream& o, const State& s);

} // namespace SimTK

#endif // SimTK_SimTKCOMMON_STATE_H_