Simbody  3.5
StateImpl.h
Go to the documentation of this file.
00001 #ifndef SimTK_SimTKCOMMON_STATE_IMPL_H_
00002 #define SimTK_SimTKCOMMON_STATE_IMPL_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                       Simbody(tm): SimTKcommon                             *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2005-14 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors: Peter Eastman                                                *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026    // Hide from Doxygen
00032 
00033 // This header is logically an extension of State.h and is intended to be
00034 // included from within State.h and nowhere else.
00035 
00036 namespace SimTK {
00037 
00038 // These local classes
00039 //      DiscreteVarInfo
00040 //      CacheVarInfo
00041 //      EventInfo
00042 // contain the information needed for each discrete variable and cache entry,
00043 // including a reference to its current value and everything needed to 
00044 // understand its dependencies, as well as information for each Event allocated
00045 // by a Subsystem.
00046 //
00047 // These are intended as elements in an allocation stack as described above,
00048 // so it is expected that they will get reaallocated, copied, and destructed 
00049 // during allocation as the Array_ gets resized from time to time. However, the
00050 // discrete variable and cache entry *values* must remain in a fixed location in 
00051 // memory once allocated, because callers are permitted to retain references to
00052 // these values once they have been allocated. So pointers to the AbstractValues
00053 // are kept in these objects, and only the pointers are copied around. That 
00054 // means the actual value object will not be deleted by the destructor; be sure
00055 // to do that explicitly in the higher-level destructor or you'll have a nasty
00056 // leak.
00057 
00058 //==============================================================================
00059 //                           DISCRETE VAR INFO
00060 //==============================================================================
00061 class DiscreteVarInfo {
00062 public:
00063     DiscreteVarInfo()
00064     :   allocationStage(Stage::Empty), invalidatedStage(Stage::Empty),
00065         value(0), timeLastUpdated(NaN) {}
00066 
00067     DiscreteVarInfo(Stage allocation, Stage invalidated, AbstractValue* v)
00068     :   allocationStage(allocation), invalidatedStage(invalidated),
00069         autoUpdateEntry(), value(v), timeLastUpdated(NaN) 
00070     {   assert(isReasonable()); }
00071 
00072     // Default copy constructor, copy assignment, destructor are shallow.
00073 
00074     // Use this to make this entry contain a *copy* of the source value.
00075     // If the destination already has a value, the new value must be
00076     // assignment compatible.
00077     DiscreteVarInfo& deepAssign(const DiscreteVarInfo& src) {
00078         assert(src.isReasonable());
00079          
00080         allocationStage   = src.allocationStage;
00081         invalidatedStage  = src.invalidatedStage;
00082         autoUpdateEntry   = src.autoUpdateEntry;
00083         if (value) *value = *src.value;
00084         else        value = src.value->clone();
00085         timeLastUpdated   = src.timeLastUpdated;
00086         return *this;
00087     }
00088 
00089     // For use in the containing class's destructor.
00090     void deepDestruct() {delete value; value=0;}
00091     const Stage& getAllocationStage()  const {return allocationStage;}
00092 
00093     // Exchange value pointers (should be from this dv's update cache entry).
00094     void swapValue(Real updTime, AbstractValue*& other) 
00095     {   std::swap(value, other); timeLastUpdated=updTime; }
00096 
00097     const AbstractValue& getValue() const {assert(value); return *value;}
00098     Real                 getTimeLastUpdated() const 
00099     {   assert(value); return timeLastUpdated; }
00100     AbstractValue&       updValue(Real updTime)
00101     {   assert(value); timeLastUpdated=updTime; return *value; }
00102 
00103     const Stage&    getInvalidatedStage() const {return invalidatedStage;}
00104     CacheEntryIndex getAutoUpdateEntry()  const {return autoUpdateEntry;}
00105     void setAutoUpdateEntry(CacheEntryIndex cx) {autoUpdateEntry = cx;}
00106 
00107 private:
00108     // These are fixed at construction.
00109     Stage           allocationStage;
00110     Stage           invalidatedStage;
00111     CacheEntryIndex autoUpdateEntry;
00112 
00113     // These change at run time.
00114     AbstractValue*  value;
00115     Real            timeLastUpdated;
00116 
00117     bool isReasonable() const
00118     {    return (allocationStage==Stage::Topology 
00119                  || allocationStage==Stage::Model)
00120              && (invalidatedStage > allocationStage)
00121              && (value != 0); }
00122 };
00123 
00124 
00125 
00126 //==============================================================================
00127 //                            CACHE ENTRY INFO
00128 //==============================================================================
00129 class CacheEntryInfo {
00130 public:
00131     CacheEntryInfo()
00132     :   allocationStage(Stage::Empty), dependsOnStage(Stage::Empty), 
00133         computedByStage(Stage::Empty), value(0), versionWhenLastComputed(-1) {}
00134 
00135     CacheEntryInfo
00136        (Stage allocation, Stage dependsOn, Stage computedBy, AbstractValue* v)
00137     :   allocationStage(allocation), dependsOnStage(dependsOn), 
00138         computedByStage(computedBy), value(v), versionWhenLastComputed(0) 
00139     {   assert(isReasonable()); }
00140 
00141     bool isCurrent(const Stage& current, const StageVersion versions[]) const 
00142     {   if (current >= computedByStage) return true;
00143         if (current <  dependsOnStage)  return false;
00144         return versions[dependsOnStage] == versionWhenLastComputed;}
00145 
00146     StageVersion getVersionWhenLastComputed() const 
00147     {   return versionWhenLastComputed; }
00148 
00149     // These affect only the explicit "last computed" flag which does not fully
00150     // determine whether the value is current; see isCurrent() above.
00151     void invalidate() {versionWhenLastComputed = 0;}
00152     void markAsComputed(const StageVersion versions[])
00153     {   versionWhenLastComputed = versions[dependsOnStage];}
00154 
00155     // Default copy constructor, copy assignment, destructor are shallow.
00156 
00157     // Use this to make this entry contain a *copy* of the source value.
00158     CacheEntryInfo& deepAssign(const CacheEntryInfo& src) {
00159         assert(src.isReasonable());
00160 
00161         allocationStage   = src.allocationStage;
00162         dependsOnStage    = src.dependsOnStage;
00163         computedByStage   = src.computedByStage;
00164         associatedVar     = src.associatedVar;
00165         if (value) *value = *src.value;
00166         else        value = src.value->clone();
00167         versionWhenLastComputed = src.versionWhenLastComputed;
00168         return *this;
00169     }
00170 
00171     // For use in the containing class's destructor.
00172     void deepDestruct() {delete value; value=0;}
00173     const Stage& getAllocationStage() const {return allocationStage;}
00174 
00175     // Exchange values with a discrete variable (presumably this
00176     // cache entry has been determined to be that variable's update
00177     // entry but we're not checking here).
00178     void swapValue(Real updTime, DiscreteVarInfo& dv) 
00179     {   dv.swapValue(updTime, value); }
00180     const AbstractValue& getValue() const {assert(value); return *value;}
00181     AbstractValue&       updValue()       {assert(value); return *value;}
00182 
00183     const Stage&          getDependsOnStage()  const {return dependsOnStage;}
00184     const Stage&          getComputedByStage() const {return computedByStage;}
00185     DiscreteVariableIndex getAssociatedVar()   const {return associatedVar;}
00186     void setAssociatedVar(DiscreteVariableIndex dx)  {associatedVar=dx;}
00187 private:
00188     // These are fixed at construction.
00189     Stage                   allocationStage;
00190     Stage                   dependsOnStage;
00191     Stage                   computedByStage;
00192     DiscreteVariableIndex   associatedVar;  // if this is an auto-update entry
00193 
00194     // These change at run time.
00195     AbstractValue*          value;
00196     StageVersion            versionWhenLastComputed;//version of Stage dependsOn
00197 
00198     bool isReasonable() const
00199     {    return (   allocationStage==Stage::Topology
00200                  || allocationStage==Stage::Model
00201                  || allocationStage==Stage::Instance)
00202              && (computedByStage >= dependsOnStage)
00203              && (value != 0)
00204              && (versionWhenLastComputed >= 0); }
00205 };
00206 
00207 
00208 
00209 //==============================================================================
00210 //                               TRIGGER INFO
00211 //==============================================================================
00212 class TriggerInfo {
00213 public:
00214     TriggerInfo() 
00215     :   allocationStage(Stage::Empty), firstIndex(-1), nslots(0) {}
00216 
00217     TriggerInfo(Stage allocation, int index, int n)
00218     :   allocationStage(allocation), firstIndex(index), nslots(n)
00219     {   assert(isReasonable()); assert(n>0);}
00220 
00221     // Default copy constructor, copy assignment, destructor are fine since 
00222     // there is no heap object owned here.
00223 
00224     int getFirstIndex() const {return firstIndex;}
00225     int getNumSlots() const {return nslots;}
00226 
00227     // These the the "virtual" methods required by template methods elsewhere.
00228     TriggerInfo& deepAssign(const TriggerInfo& src) 
00229     {   return operator=(src); }
00230     void         deepDestruct() {}
00231     const Stage& getAllocationStage() const {return allocationStage;}
00232 private:
00233     // These are fixed at construction.
00234     Stage                   allocationStage;
00235     int                     firstIndex;
00236     int                     nslots;
00237 
00238     bool isReasonable() const {
00239         return (    allocationStage==Stage::Topology
00240                  || allocationStage==Stage::Model
00241                  || allocationStage==Stage::Instance);
00242     }
00243 };
00244 
00245 
00246 
00247 //==============================================================================
00248 //                           CONTINUOUS VAR INFO
00249 //==============================================================================
00250 // Used for q, u, and z (in separate stacks).
00251 // These accumulate default values for this subsystem's use of shared
00252 // global state variables. After the System is advanced to Stage::Model,
00253 // the state will allocate those globals and copy the initial
00254 // values stored here into them. Some of these are allocated at Topology
00255 // stage, and some at Model stage. If Model stage is invalidated, variables
00256 // allocated then are forgotten while the ones allocated at Topology stage
00257 // remain.
00258 class ContinuousVarInfo {
00259 public:
00260     ContinuousVarInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
00261 
00262     ContinuousVarInfo(Stage         allocation, 
00263                       int           index,  // QIndex, UIndex, or ZIndex
00264                       const Vector& initVals, 
00265                       const Vector& varWeights=Vector())
00266     :   allocationStage(allocation), firstIndex(index), initialValues(initVals)
00267     {   assert(isReasonable());
00268         assert(varWeights.size()==0 || varWeights.size()==initVals.size());
00269         assert(weightsAreOK(varWeights));
00270         if (varWeights.size()) weights=varWeights;
00271         else weights=Vector(initVals.size(), Real(1));
00272     }
00273 
00274     int getFirstIndex() const {return firstIndex;}
00275     int getNumVars() const {return initialValues.size();}
00276     const Vector& getInitialValues() const {return initialValues;}
00277     const Vector& getWeights() const {return weights;}
00278 
00279     // Default copy constructor, copy assignment, destructor are fine since 
00280     // there is no heap object owned here.
00281 
00282     // These the the "virtual" methods required by template methods elsewhere.
00283     ContinuousVarInfo& deepAssign(const ContinuousVarInfo& src) 
00284     {   return operator=(src); }
00285     void               deepDestruct() {}
00286     const Stage&       getAllocationStage() const {return allocationStage;}
00287 private:
00288     // These are fixed at construction.
00289     Stage     allocationStage;
00290     int       firstIndex;   // a QIndex, UIndex, or ZIndex
00291     Vector    initialValues;
00292     Vector    weights; // only used for u and z
00293 
00294     static bool weightsAreOK(const Vector& wts) {
00295         for (int i=0; i<wts.size(); ++i)
00296             if (wts[i] <= 0) return false;
00297         return true;
00298     }
00299 
00300     bool isReasonable() const {
00301         return (    allocationStage==Stage::Topology
00302                  || allocationStage==Stage::Model);
00303     }
00304 };
00305 
00306 //==============================================================================
00307 //                           CONSTRAINT ERR INFO
00308 //==============================================================================
00309 // Used for qerr, uerr, and udoterr.
00310 class ConstraintErrInfo {
00311 public:
00312     ConstraintErrInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
00313 
00314     ConstraintErrInfo(Stage         allocation,
00315                       int           index, // QErr, UErr, or UDotErrIndex
00316                       int           nerr,
00317                       const Vector& varWeights=Vector())
00318     :   allocationStage(allocation), firstIndex(index)
00319     {   assert(isReasonable());
00320         assert(varWeights.size()==0 || varWeights.size()==nerr);
00321         assert(weightsAreOK(varWeights));
00322         if (varWeights.size()) weights=varWeights;
00323         else weights=Vector(nerr, Real(1));
00324     }
00325 
00326     int getFirstIndex() const {return firstIndex;}
00327     int getNumErrs() const {return weights.size();}
00328     const Vector& getWeights() const {return weights;}
00329 
00330     // Default copy constructor, copy assignment, destructor are fine since 
00331     // there is no heap object owned here.
00332 
00333     // These the the "virtual" methods required by template methods elsewhere.
00334     ConstraintErrInfo& deepAssign(const ConstraintErrInfo& src) 
00335     {   return operator=(src); }
00336     void               deepDestruct() {}
00337     const Stage&       getAllocationStage() const {return allocationStage;}
00338 private:
00339     // These are fixed at construction.
00340     Stage     allocationStage;
00341     int       firstIndex;   // a QErrIndex, UErrIndex, or UDotErrIndex
00342     Vector    weights;      // only used for u and z
00343 
00344     static bool weightsAreOK(const Vector& wts) {
00345         for (int i=0; i<wts.size(); ++i)
00346             if (wts[i] <= 0) return false;
00347         return true;
00348     }
00349 
00350     bool isReasonable() const {
00351         return (    allocationStage==Stage::Topology
00352                  || allocationStage==Stage::Model
00353                  || allocationStage==Stage::Instance);
00354     }
00355 };
00356 
00357 
00358 
00359 //==============================================================================
00360 //                           PER SUBSYSTEM INFO
00361 //==============================================================================
00362 // This internal utility class is used to capture all the information needed for
00363 // a single subsystem within the StateImpl.
00364 class PerSubsystemInfo {
00365 public:
00366     PerSubsystemInfo() : currentStage(Stage::Empty)     {initialize();}
00367     PerSubsystemInfo(const String& n, const String& v) 
00368       : name(n), version(v), currentStage(Stage::Empty) {initialize();}
00369 
00370     // Everything will properly clean itself up except for the AbstractValues
00371     // stored in the discrete variable and cache entry arrays. Be sure to
00372     // delete those prior to allowing the arrays themselves to destruct.
00373     ~PerSubsystemInfo() {
00374         clearAllStacks();
00375     }
00376 
00377     // Copy constructor copies all variables but cache only through
00378     // Instance stage. Note that this must be done in conjunction with
00379     // copying the whole state or our global resource indices will
00380     // be nonsense.
00381     PerSubsystemInfo(const PerSubsystemInfo& src) : currentStage(Stage::Empty) {
00382         initialize();
00383         copyFrom(src, Stage::Instance);
00384     }
00385 
00386     PerSubsystemInfo& operator=(const PerSubsystemInfo& src) {
00387         // destination is already initialized; copyFrom() will try
00388         // to reuse space and will properly clean up unused stuff
00389         if (&src != this)
00390             copyFrom(src, Stage::Instance);
00391         return *this;
00392     }
00393 
00394     // Back up to the stage just before g if this subsystem thinks
00395     // it is already at g or beyond. Note that we may be backing up
00396     // over many stages here. Careful: invalidating the stage
00397     // for a subsystem must also invalidate the same stage for all
00398     // the other subsystems and the system as a whole but we don't
00399     // take care of that here. Also, you can't invalidate Stage::Empty.
00400     void invalidateStageJustThisSubsystem(Stage g) {
00401         assert(g > Stage::Empty);
00402         restoreToStage(g.prev());
00403     }
00404 
00405     // Advance from stage g-1 to stage g. This is called at the end
00406     // of realize(g). You can't use this to "advance" to Stage::Empty.
00407     // It is a fatal error if the current stage isn't g-1.
00408     void advanceToStage(Stage g) const {
00409         assert(g > Stage::Empty);
00410         assert(currentStage == g.prev());
00411 
00412         // This validates whatever the current version number is of Stage g.
00413         currentStage = g;
00414     }
00415 
00416 
00417     void clearReferencesToModelStageGlobals() {
00418         qstart.invalidate(); ustart.invalidate(); 
00419         zstart.invalidate();
00420         q.clear(); u.clear(); z.clear();
00421         uWeights.clear(); zWeights.clear();
00422         qdot.clear(); udot.clear(); zdot.clear(); qdotdot.clear();
00423     }
00424 
00425     void clearReferencesToInstanceStageGlobals() {
00426         // These are late-allocated state variables.
00427         qerrWeights.clear(); uerrWeights.clear();
00428 
00429         // These are all mutable cache entries.
00430         qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
00431         qerr.clear();uerr.clear();udoterr.clear();multipliers.clear();
00432 
00433         for (int j=0; j<Stage::NValid; ++j) {
00434             triggerstart[j].invalidate();
00435             triggers[j].clear();
00436         }
00437     }
00438 
00439     QIndex getNextQIndex() const {
00440         if (qInfo.empty()) return QIndex(0);
00441         const ContinuousVarInfo& last = qInfo.back();
00442         return QIndex(last.getFirstIndex()+last.getNumVars());
00443     }
00444     UIndex getNextUIndex() const {
00445         if (uInfo.empty()) return UIndex(0);
00446         const ContinuousVarInfo& last = uInfo.back();
00447         return UIndex(last.getFirstIndex()+last.getNumVars());
00448     }
00449     ZIndex getNextZIndex() const {
00450         if (zInfo.empty()) return ZIndex(0);
00451         const ContinuousVarInfo& last = zInfo.back();
00452         return ZIndex(last.getFirstIndex()+last.getNumVars());
00453     }
00454 
00455     QErrIndex getNextQErrIndex() const {
00456         if (qerrInfo.empty()) return QErrIndex(0);
00457         const ConstraintErrInfo& last = qerrInfo.back();
00458         return QErrIndex(last.getFirstIndex()+last.getNumErrs());
00459     }
00460     UErrIndex getNextUErrIndex() const {
00461         if (uerrInfo.empty()) return UErrIndex(0);
00462         const ConstraintErrInfo& last = uerrInfo.back();
00463         return UErrIndex(last.getFirstIndex()+last.getNumErrs());
00464     }
00465     UDotErrIndex getNextUDotErrIndex() const {
00466         if (udoterrInfo.empty()) return UDotErrIndex(0);
00467         const ConstraintErrInfo& last = udoterrInfo.back();
00468         return UDotErrIndex(last.getFirstIndex()+last.getNumErrs());
00469     }
00470     DiscreteVariableIndex getNextDiscreteVariableIndex() const {
00471         return DiscreteVariableIndex(discreteInfo.size());
00472     }
00473     CacheEntryIndex getNextCacheEntryIndex() const {
00474         return CacheEntryIndex(cacheInfo.size());
00475     }
00476     EventTriggerByStageIndex getNextEventTriggerByStageIndex(Stage g) const {
00477         if (triggerInfo[g].empty()) return EventTriggerByStageIndex(0);
00478         const TriggerInfo& last = triggerInfo[g].back();
00479         return EventTriggerByStageIndex
00480             (last.getFirstIndex()+last.getNumSlots());
00481     }
00482 
00483 
00484     String name;
00485     String version;
00486 
00487         // DEFINITIONS //
00488 
00489     // State variables (continuous or discrete) can be defined (allocated) 
00490     // during realization of Topology or Model stages. Cache entries,
00491     // constraint error slots, and event trigger slots can be defined during 
00492     // realization of Topology, Model, or Instance stages. No further 
00493     // allocations are allowed. Then, when one of these stages is invalidated, 
00494     // all the definitions that occurred during realization of that stage must 
00495     // be forgotten.
00496     // 
00497     // To do that the allocation entries are stored in arrays which are really 
00498     // stacks, with definitions pushed onto the ends as the stage is advanced 
00499     // and popped off the ends as the stage is reduced.
00500 
00501     // Topology and Model stage definitions. 
00502     Array_<ContinuousVarInfo>      qInfo, uInfo, zInfo;
00503     Array_<DiscreteVarInfo>        discreteInfo;
00504 
00505     // Topology, Model, and Instance stage definitions.
00506     mutable Array_<ConstraintErrInfo>   qerrInfo, uerrInfo, udoterrInfo;
00507     mutable Array_<TriggerInfo>         triggerInfo[Stage::NValid];
00508     mutable Array_<CacheEntryInfo>      cacheInfo;
00509    
00510         // GLOBAL RESOURCE ALLOCATIONS //
00511 
00512     // These are our own private views into partitions of the global
00513     // state and cache entries of the same names. The State will assign
00514     // contiguous blocks to this subsystem when the *System* stage is raised
00515     // to Model or Instance stage, and they are invalidated whenever that 
00516     // stage is invalidated. The starting indices are filled in here at 
00517     // the time the views are built.
00518 
00519     // Model stage global resources and views into them.
00520     SystemQIndex    qstart;
00521     SystemUIndex    ustart;
00522     SystemZIndex    zstart;
00523     Vector          q, u, z;
00524     Vector          uWeights, zWeights;
00525 
00526     mutable Vector  qdot, udot, zdot, qdotdot;
00527 
00528     // Instance stage global resources and views into them.
00529     Vector          qerrWeights, uerrWeights;
00530 
00531     // Note that multipliers just use the same indices as udoterr.
00532     mutable SystemQErrIndex                 qerrstart;
00533     mutable SystemUErrIndex                 uerrstart;
00534     mutable SystemUDotErrIndex              udoterrstart;
00535     mutable SystemEventTriggerByStageIndex  triggerstart[Stage::NValid];
00536 
00537     mutable Vector  qerr, uerr;
00538 
00539     mutable Vector  udoterr, multipliers; // same size and partioning
00540     mutable Vector  triggers[Stage::NValid];
00541 
00542     // The currentStage is the highest stage of this subsystem that is valid,
00543     // meaning that it has been realized since the last change to any variable
00544     // that might affect it. All stages less than currentStage are also valid,
00545     // and all higher stages are invalid.
00546     // Each stage has a "stage version" which is like a serial number that is
00547     // bumped every time the stage is invalidated by a variable change. Cache
00548     // entries that are calculated from a particular stage version can record
00549     // the version number to allow a quick check later -- if the current version
00550     // of a cache entry's dependsOn stage is different than the one stored with
00551     // the cache entry, then that cache entry cannot be valid.
00552     mutable Stage        currentStage;
00553     mutable StageVersion stageVersions[Stage::NValid];
00554 
00555 private:
00556     // This is for use in constructors and for resetting an existing State into
00557     // its just-constructed condition.
00558     void initialize() {
00559         clearAllStacks();
00560         qstart.invalidate();ustart.invalidate();zstart.invalidate();
00561         qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
00562         for (int j=0; j<Stage::NValid; ++j) {
00563             triggerstart[j].invalidate();
00564             stageVersions[j] = 1; // never 0
00565         }
00566         currentStage = Stage::Empty;
00567     }
00568 
00569     // Manage allocation stacks.
00570 
00571     void clearContinuousVars();
00572     void clearConstraintErrs();
00573     void clearDiscreteVars();
00574     void clearEventTriggers(int g);
00575     void clearCache();
00576 
00577     void clearAllStacks();
00578 
00579     void popContinuousVarsBackToStage(const Stage& g);
00580     void popDiscreteVarsBackToStage(const Stage& g);
00581     void popConstraintErrsBackToStage(const Stage& g);
00582     void popCacheBackToStage(const Stage& g);
00583     void popEventTriggersBackToStage(const Stage& g);
00584 
00585     void popAllStacksBackToStage(const Stage& g);
00586 
00587     // Call once each for qInfo, uInfo, zInfo.
00588     void copyContinuousVarInfoThroughStage
00589        (const Array_<ContinuousVarInfo>& src, const Stage& g,
00590         Array_<ContinuousVarInfo>& dest);
00591 
00592     void copyDiscreteVarsThroughStage
00593        (const Array_<DiscreteVarInfo>& src, const Stage& g);
00594 
00595     // Call once each for qerrInfo, uerrInfo, udoterrInfo.
00596     void copyConstraintErrInfoThroughStage
00597        (const Array_<ConstraintErrInfo>& src, const Stage& g,
00598         Array_<ConstraintErrInfo>& dest);
00599 
00600     void copyCacheThroughStage
00601        (const Array_<CacheEntryInfo>& src, const Stage& g);
00602 
00603     void copyEventsThroughStage
00604        (const Array_<TriggerInfo>& src, const Stage& g,
00605         Array_<TriggerInfo>& dest);
00606 
00607     void copyAllStacksThroughStage(const PerSubsystemInfo& src, const Stage& g);
00608 
00609     // Restore this subsystem to the way it last was at realize(g) for a given 
00610     // Stage g; that is, invalidate all stages > g. Allocations will be 
00611     // forgotten as Instance, Model, and Topology stages are invalidated.
00612     void restoreToStage(Stage g);
00613 
00614     // Utility which makes "this" a copy of the source subsystem exactly as it
00615     // was after being realized to stage maxStage. If maxStage >= Model then
00616     // all the subsystem-private state variables will be copied, but only
00617     // cached computations up through maxStage come through. We clear
00618     // our references to global variables regardless -- those will have to
00619     // be repaired at the System (State global) level.
00620     void copyFrom(const PerSubsystemInfo& src, Stage maxStage);
00621 };
00622 
00623 
00624 //==============================================================================
00625 //                                 STATE IMPL
00626 //==============================================================================
00627 
00628 class StateImpl {
00629 public:
00630     StateImpl() 
00631     :   t(NaN), currentSystemStage(Stage::Empty) {initializeStageVersions();} 
00632 
00633     // We'll do the copy constructor and assignment explicitly here
00634     // to get tight control over what's allowed.
00635     StateImpl(const StateImpl& src);
00636 
00637     StateImpl& operator=(const StateImpl& src);
00638 
00639     ~StateImpl() {}   // default destructor
00640 
00641     // Copies all the variables but not the cache.
00642     StateImpl* clone() const {return new StateImpl(*this);}
00643 
00644     const Stage& getSystemStage() const {return currentSystemStage;}
00645     Stage&       updSystemStage() const {return currentSystemStage;} // mutable
00646 
00647 
00648     const PerSubsystemInfo& getSubsystem(int subsystem) const {
00649         SimTK_INDEXCHECK(subsystem, (int)subsystems.size(), 
00650                          "StateImpl::getSubsystem()");
00651         return subsystems[subsystem];
00652     }
00653 
00654     PerSubsystemInfo& updSubsystem(int subsystem) {
00655         SimTK_INDEXCHECK(subsystem, (int)subsystems.size(), 
00656                          "StateImpl::updSubsystem()");
00657         return subsystems[subsystem];
00658     }
00659 
00660     const Stage& getSubsystemStage(int subsystem) const {
00661         return subsystems[subsystem].currentStage;
00662     }
00663     Stage& updSubsystemStage(int subsystem) const {
00664         return subsystems[subsystem].currentStage; // mutable
00665     }
00666 
00667     const StageVersion* getSubsystemStageVersions(int subsystem) const {
00668         return subsystems[subsystem].stageVersions;
00669     }
00670 
00671     // Back up the System stage just before stg if it thinks
00672     // it is already at stg or beyond. Note that we may be backing up
00673     // over many stages here. Careful: invalidating the stage
00674     // for the system must also invalidate the same stage for all
00675     // the subsystems (because we trash the shared resource pool
00676     // here if we back up earlier than Stage::Model) but we don't
00677     // take care of that here. Also, you can't invalidate Stage::Empty.
00678     void invalidateJustSystemStage(Stage stg);
00679 
00680     // Advance the System stage from stg-1 to stg. It is a fatal error if
00681     // we're not already at stg-1, and you can't advance to Stage::Empty.
00682     // Also, you can't advance the system to stg unless ALL subsystems have
00683     // already gotten there.
00684     void advanceSystemToStage(Stage stg) const;
00685     
00686     void setNumSubsystems(int i) {
00687         assert(i >= 0);
00688         subsystems.clear();
00689         subsystems.resize(i);
00690     }
00691     
00692     void initializeSubsystem
00693        (SubsystemIndex i, const String& name, const String& version) {
00694         updSubsystem(i).name = name;
00695         updSubsystem(i).version = version;
00696     }
00697       
00698     SubsystemIndex addSubsystem(const String& name, const String& version) {
00699         const SubsystemIndex sx(subsystems.size());
00700         subsystems.push_back(PerSubsystemInfo(name,version));
00701         return sx;
00702     }
00703     
00704     int getNumSubsystems() const {return (int)subsystems.size();}
00705     
00706     const String& getSubsystemName(SubsystemIndex subsys) const {
00707         return subsystems[subsys].name;
00708     }
00709     const String& getSubsystemVersion(SubsystemIndex subsys) const {
00710         return subsystems[subsys].version;
00711     }
00712 
00713     // Make sure the stage is no higher than g-1 for *any* subsystem and
00714     // hence for the system stage also. TODO: this should be more selective.
00715     void invalidateAll(Stage g) {
00716         invalidateJustSystemStage(g);
00717         for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
00718             subsystems[i].invalidateStageJustThisSubsystem(g);
00719     }
00720 
00721     // Make sure the stage is no higher than g-1 for *any* subsystem and
00722     // hence for the system stage also. Same as invalidateAll() except this
00723     // requires only const access and can't be used for g below Instance.
00724     void invalidateAllCacheAtOrAbove(Stage g) const {
00725         SimTK_STAGECHECK_GE_ALWAYS(g, Stage::Instance, 
00726             "StateImpl::invalidateAllCacheAtOrAbove()");
00727 
00728         // We promise not to hurt this State; get non-const access just so
00729         // we can call these methods.
00730         StateImpl* mthis = const_cast<StateImpl*>(this);
00731         mthis->invalidateJustSystemStage(g);
00732         for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
00733             mthis->subsystems[i].invalidateStageJustThisSubsystem(g);
00734     }
00735     
00736     // Move the stage for a particular subsystem from g-1 to g. No other
00737     // subsystems are affected, nor the global system stage.
00738     void advanceSubsystemToStage(SubsystemIndex subsys, Stage g) const {
00739         subsystems[subsys].advanceToStage(g);
00740         // We don't automatically advance the System stage even if this brings
00741         // ALL the subsystems up to stage g.
00742     }
00743      
00744     // We don't expect State entry allocations to be performance critical so
00745     // we'll keep error checking on even in Release mode.
00746     
00747     QIndex allocateQ(SubsystemIndex subsys, const Vector& qInit) {
00748         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model, 
00749                                    "StateImpl::allocateQ()");
00750         // We are currently realizing the next stage.
00751         const Stage allocStage = getSubsystemStage(subsys).next();
00752         PerSubsystemInfo& ss = subsystems[subsys];
00753         const QIndex nxt(ss.getNextQIndex());
00754         ss.qInfo.push_back(ContinuousVarInfo(allocStage,nxt,qInit,Vector())); 
00755         return nxt;
00756     }
00757     
00758     UIndex allocateU(SubsystemIndex subsys, const Vector& uInit) {
00759         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model, 
00760                                    "StateImpl::allocateU()");
00761         const Stage allocStage = getSubsystemStage(subsys).next();
00762         PerSubsystemInfo& ss = subsystems[subsys];
00763         const UIndex nxt(ss.getNextUIndex());
00764         ss.uInfo.push_back(ContinuousVarInfo(allocStage,nxt,uInit,Vector())); 
00765         return nxt;
00766     }
00767     ZIndex allocateZ(SubsystemIndex subsys, const Vector& zInit) {
00768         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model, 
00769                                    "StateImpl::allocateZ()");
00770         const Stage allocStage = getSubsystemStage(subsys).next();
00771         PerSubsystemInfo& ss = subsystems[subsys];
00772         const ZIndex nxt(ss.getNextZIndex());
00773         ss.zInfo.push_back(ContinuousVarInfo(allocStage,nxt,zInit,Vector())); 
00774         return nxt;
00775     }
00776     
00777     QErrIndex allocateQErr(SubsystemIndex subsys, int nqerr) const {
00778         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance, 
00779                                    "StateImpl::allocateQErr()");
00780         const Stage allocStage = getSubsystemStage(subsys).next();
00781         const PerSubsystemInfo& ss = subsystems[subsys];
00782         const QErrIndex nxt(ss.getNextQErrIndex());
00783         ss.qerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nqerr,Vector())); 
00784         return nxt;
00785     }
00786     UErrIndex allocateUErr(SubsystemIndex subsys, int nuerr) const {
00787         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), 
00788                                    Stage::Instance,"StateImpl::allocateUErr()");
00789         const Stage allocStage = getSubsystemStage(subsys).next();
00790         const PerSubsystemInfo& ss = subsystems[subsys];
00791         const UErrIndex nxt(ss.getNextUErrIndex());
00792         ss.uerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nuerr,Vector())); 
00793         return nxt;
00794     }
00795     UDotErrIndex allocateUDotErr(SubsystemIndex subsys, int nudoterr) const {
00796         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
00797                                    "StateImpl::allocateUDotErr()");
00798         const Stage allocStage = getSubsystemStage(subsys).next();
00799         const PerSubsystemInfo& ss = subsystems[subsys];
00800         const UDotErrIndex nxt(ss.getNextUDotErrIndex());
00801         ss.udoterrInfo.push_back
00802            (ConstraintErrInfo(allocStage,nxt,nudoterr,Vector())); 
00803         return nxt;
00804     }
00805     EventTriggerByStageIndex allocateEventTrigger
00806        (SubsystemIndex subsys, Stage g, int nt) const {
00807         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance, 
00808                                    "StateImpl::allocateEventTrigger()");
00809         const Stage allocStage = getSubsystemStage(subsys).next();
00810         const PerSubsystemInfo& ss = subsystems[subsys];
00811         const EventTriggerByStageIndex 
00812             nxt(ss.getNextEventTriggerByStageIndex(g));
00813         ss.triggerInfo[g].push_back(TriggerInfo(allocStage,nxt,nt)); 
00814         return nxt;
00815     }
00816     
00817     // Topology- and Model-stage State variables can only be added during 
00818     // construction; that is, while stage <= Topology. Other entries can be 
00819     // added while stage < Model.
00820     DiscreteVariableIndex allocateDiscreteVariable
00821        (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp) 
00822     {
00823         SimTK_STAGECHECK_RANGE_ALWAYS(Stage(Stage::LowestRuntime).prev(),
00824                                       invalidates, Stage::HighestRuntime, 
00825             "StateImpl::allocateDiscreteVariable()");
00826     
00827         const Stage maxAcceptable = (invalidates <= Stage::Model 
00828                                      ? Stage::Empty : Stage::Topology);
00829         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), 
00830             maxAcceptable.next(), "StateImpl::allocateDiscreteVariable()");
00831 
00832         const Stage allocStage = getSubsystemStage(subsys).next();    
00833         PerSubsystemInfo& ss = subsystems[subsys];
00834         const DiscreteVariableIndex nxt(ss.getNextDiscreteVariableIndex());
00835         ss.discreteInfo.push_back
00836            (DiscreteVarInfo(allocStage,invalidates,vp));
00837         return nxt;
00838     }
00839     
00840     // Cache entries can be allocated while stage < Instance.
00841     CacheEntryIndex allocateCacheEntry
00842        (SubsystemIndex subsys, Stage dependsOn, Stage computedBy,
00843         AbstractValue* vp) const 
00844     {
00845         SimTK_STAGECHECK_RANGE_ALWAYS(Stage(Stage::LowestRuntime).prev(), 
00846                                       dependsOn, Stage::HighestRuntime, 
00847             "StateImpl::allocateCacheEntry()");
00848         SimTK_STAGECHECK_RANGE_ALWAYS(dependsOn, computedBy, Stage::Infinity, 
00849             "StateImpl::allocateCacheEntry()");
00850         SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), 
00851             Stage::Instance, "StateImpl::allocateCacheEntry()");
00852 
00853         const Stage allocStage = getSubsystemStage(subsys).next();    
00854         const PerSubsystemInfo& ss = subsystems[subsys];
00855         const CacheEntryIndex nxt(ss.getNextCacheEntryIndex());
00856         ss.cacheInfo.push_back
00857            (CacheEntryInfo(allocStage, dependsOn, computedBy, vp)); //mutable
00858         return nxt;
00859     }
00860 
00861     // Allocate a discrete variable and a corresponding cache entry for
00862     // updating it, and connect them together.
00863     DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
00864        (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp,
00865         Stage updateDependsOn)
00866     {
00867         const DiscreteVariableIndex dx = 
00868             allocateDiscreteVariable(subsys,invalidates,vp->clone());
00869         const CacheEntryIndex       cx = 
00870             allocateCacheEntry(subsys,updateDependsOn,Stage::Infinity,vp);
00871 
00872         PerSubsystemInfo& ss = subsystems[subsys];
00873         DiscreteVarInfo& dvinfo = ss.discreteInfo[dx];
00874         CacheEntryInfo&  ceinfo = ss.cacheInfo[cx];
00875         dvinfo.setAutoUpdateEntry(cx);
00876         ceinfo.setAssociatedVar(dx);
00877         return dx;
00878     }
00879     
00880         // State dimensions for shared continuous variables.
00881     
00882     int getNY() const {
00883         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00884                             "StateImpl::getNY()");
00885         return y.size();
00886     }
00887     
00888     SystemYIndex getQStart() const {
00889         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00890                             "StateImpl::getQStart()");
00891         return SystemYIndex(0); // q's come first
00892     }
00893     int getNQ() const {
00894         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00895                             "StateImpl::getNQ()");
00896         return q.size();
00897     }
00898     
00899     SystemYIndex getUStart() const {
00900         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00901                             "StateImpl::getUStart()");
00902         return SystemYIndex(q.size()); // u's come right after q's
00903     }
00904     int getNU() const {
00905         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00906                             "StateImpl::getNU()");
00907         return u.size();
00908     }
00909     
00910     SystemYIndex getZStart() const {
00911         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00912                             "StateImpl::getZStart()");
00913         return SystemYIndex(q.size() + u.size()); // q,u, then z
00914     }
00915     int getNZ() const {
00916         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00917                             "StateImpl::getNZ()");
00918         return z.size();
00919     }
00920     
00921     int getNYErr() const {
00922         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00923                             "StateImpl::getNYErr()");
00924         return yerr.size();
00925     }
00926     
00927     SystemYErrIndex getQErrStart() const {
00928         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00929                             "StateImpl::getQErrStart()");
00930         return SystemYErrIndex(0); // qerr's come first
00931     }
00932     int getNQErr() const {
00933         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00934                             "StateImpl::getNQErr()");
00935         return qerr.size();
00936     }
00937     
00938     SystemYErrIndex getUErrStart() const {
00939         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00940                             "StateImpl::getUErrStart()");
00941         return SystemYErrIndex(qerr.size()); // uerr's follow qerrs
00942     }
00943     int getNUErr() const {
00944         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00945                             "StateImpl::getNUErr()");
00946         return uerr.size();
00947     }
00948     
00949     // UDot errors are independent of qerr & uerr.
00950     // This is used for multipliers also.
00951     int getNUDotErr() const {
00952         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00953                             "StateImpl::getNUDotErr()");
00954         return udoterr.size();
00955     }
00956     
00957     int getNEventTriggers() const {
00958         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00959                             "StateImpl::getNEventTriggers()");
00960         return allTriggers.size();
00961     }
00962     
00963     SystemEventTriggerIndex getEventTriggerStartByStage(Stage g) const {
00964         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00965                             "StateImpl::getEventTriggerStartByStage()");
00966         int nxt = 0;
00967         for (int j=0; j<g; ++j)
00968             nxt += triggers[j].size();
00969         return SystemEventTriggerIndex(nxt); // g starts where g-1 leaves off
00970     }
00971     
00972     int getNEventTriggersByStage(Stage g) const {
00973         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
00974                             "StateImpl::getNEventTriggersByStage()");
00975         return triggers[g].size();
00976     }
00977     
00978         // Subsystem dimensions.
00979     
00980     SystemQIndex getQStart(SubsystemIndex subsys) const {
00981         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00982                             "StateImpl::getQStart(subsys)");
00983         return getSubsystem(subsys).qstart;
00984     }
00985     int getNQ(SubsystemIndex subsys) const {
00986         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00987                             "StateImpl::getNQ(subsys)");
00988         return getSubsystem(subsys).q.size();
00989     }
00990     
00991     SystemUIndex getUStart(SubsystemIndex subsys) const {
00992         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00993                             "StateImpl::getUStart(subsys)");
00994         return getSubsystem(subsys).ustart;
00995     }
00996     int getNU(SubsystemIndex subsys) const {
00997         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
00998                             "StateImpl::getNU(subsys)");
00999         return getSubsystem(subsys).u.size();
01000     }
01001     
01002     SystemZIndex getZStart(SubsystemIndex subsys) const {
01003         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01004                             "StateImpl::getZStart(subsys)");
01005         return getSubsystem(subsys).zstart;
01006     }
01007     int getNZ(SubsystemIndex subsys) const {
01008         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01009                             "StateImpl::getNZ(subsys)");
01010         return getSubsystem(subsys).z.size();
01011     }
01012     
01013     SystemQErrIndex getQErrStart(SubsystemIndex subsys) const {
01014         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01015                             "StateImpl::getQErrStart(subsys)");
01016         return getSubsystem(subsys).qerrstart;
01017     }
01018     int getNQErr(SubsystemIndex subsys) const {
01019         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01020                             "StateImpl::getNQErr(subsys)");
01021         return getSubsystem(subsys).qerr.size();
01022     }
01023     
01024     SystemUErrIndex getUErrStart(SubsystemIndex subsys) const {
01025         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01026                             "StateImpl::getUErrStart(subsys)");
01027         return getSubsystem(subsys).uerrstart;
01028     }
01029     int getNUErr(SubsystemIndex subsys) const {
01030         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01031                             "StateImpl::getNUErr(subsys)");
01032         return getSubsystem(subsys).uerr.size();
01033     }
01034     
01035     // These are used for multipliers also.
01036     SystemUDotErrIndex getUDotErrStart(SubsystemIndex subsys) const {
01037         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01038                             "StateImpl::getUDotErrStart(subsys)");
01039         return getSubsystem(subsys).udoterrstart;
01040     }
01041     int getNUDotErr(SubsystemIndex subsys) const {
01042         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01043                             "StateImpl::getNUDotErr(subsys)");
01044         return getSubsystem(subsys).udoterr.size();
01045     }
01046     
01047     SystemEventTriggerByStageIndex getEventTriggerStartByStage
01048        (SubsystemIndex subsys, Stage g) const {
01049         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01050                             "StateImpl::getEventTriggerStartByStage(subsys)");
01051         return getSubsystem(subsys).triggerstart[g];
01052     }
01053     
01054     int getNEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
01055         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01056                             "StateImpl::getNEventTriggersByStage(subsys)");
01057         return getSubsystem(subsys).triggers[g].size();
01058     }
01059     
01060         // Per-subsystem access to the global shared variables.
01061     
01062     const Vector& getQ(SubsystemIndex subsys) const {
01063         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01064                             "StateImpl::getQ(subsys)");
01065         return getSubsystem(subsys).q;
01066     }
01067     const Vector& getU(SubsystemIndex subsys) const {
01068         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01069                             "StateImpl::getU(subsys)");
01070         return getSubsystem(subsys).u;
01071     }
01072     const Vector& getZ(SubsystemIndex subsys) const {
01073         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01074                             "StateImpl::getZ(subsys)");
01075         return getSubsystem(subsys).z;
01076     }
01077 
01078     const Vector& getUWeights(SubsystemIndex subsys) const {
01079         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01080             "StateImpl::getUWeights(subsys)");
01081         return getSubsystem(subsys).uWeights;
01082     }
01083     const Vector& getZWeights(SubsystemIndex subsys) const {
01084         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01085             "StateImpl::getZWeights(subsys)");
01086         return getSubsystem(subsys).zWeights;
01087     }
01088 
01089     const Vector& getQDot(SubsystemIndex subsys) const {
01090         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01091                             "StateImpl::getQDot(subsys)");
01092         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Velocity, 
01093                             "StateImpl::getQDot(subsys)");
01094         return getSubsystem(subsys).qdot;
01095     }
01096     const Vector& getUDot(SubsystemIndex subsys) const {
01097         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01098                             "StateImpl::getUDot(subsys)");
01099         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, 
01100                             "StateImpl::getUDot(subsys)");
01101         return getSubsystem(subsys).udot;
01102     }
01103     const Vector& getZDot(SubsystemIndex subsys) const {
01104         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01105                             "StateImpl::getZDot(subsys)");
01106         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Dynamics, 
01107                             "StateImpl::getZDot(subsys)");
01108         return getSubsystem(subsys).zdot;
01109     }
01110     const Vector& getQDotDot(SubsystemIndex subsys) const {
01111         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01112                             "StateImpl::getQDotDot(subsys)");
01113         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, 
01114                             "StateImpl::getQDotDot(subsys)");
01115         return getSubsystem(subsys).qdotdot;
01116     }
01117     
01118     Vector& updQ(SubsystemIndex subsys) {
01119         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01120                             "StateImpl::updQ(subsys)");
01121         invalidateAll(Stage::Position);
01122         return updSubsystem(subsys).q;
01123     }
01124     Vector& updU(SubsystemIndex subsys) {
01125         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01126                             "StateImpl::updU(subsys)");
01127         invalidateAll(Stage::Velocity);
01128         return updSubsystem(subsys).u;
01129     }
01130     Vector& updZ(SubsystemIndex subsys) {
01131         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01132                             "StateImpl::updZ(subsys)");
01133         invalidateAll(Stage::Dynamics);
01134         return updSubsystem(subsys).z;
01135     }
01136 
01137     Vector& updUWeights(SubsystemIndex subsys) {
01138         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01139             "StateImpl::updUWeights(subsys)");
01140         invalidateAll(Stage::Report);
01141         return updSubsystem(subsys).uWeights;
01142     }
01143     Vector& updZWeights(SubsystemIndex subsys) {
01144         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01145             "StateImpl::updZWeights(subsys)");
01146         invalidateAll(Stage::Report);
01147         return updSubsystem(subsys).zWeights;
01148     }
01149     
01150         // These are mutable so the routines are const.
01151     
01152     Vector& updQDot(SubsystemIndex subsys) const {
01153         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01154                             "StateImpl::updQDot(subsys)");
01155         return getSubsystem(subsys).qdot;
01156     }
01157     Vector& updUDot(SubsystemIndex subsys) const {
01158         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01159                             "StateImpl::updUDot(subsys)");
01160         return getSubsystem(subsys).udot;
01161     }
01162     Vector& updZDot(SubsystemIndex subsys) const {
01163         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01164                             "StateImpl::updZDot(subsys)");
01165         return getSubsystem(subsys).zdot;
01166     }
01167     Vector& updQDotDot(SubsystemIndex subsys) const {
01168         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01169                             "StateImpl::updQDotDot(subsys)");
01170         return getSubsystem(subsys).qdotdot;
01171     }
01172     
01173     
01174     const Vector& getQErr(SubsystemIndex subsys) const {
01175         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01176                             "StateImpl::getQErr(subsys)");
01177         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Position, 
01178                             "StateImpl::getQErr(subsys)");
01179         return getSubsystem(subsys).qerr;
01180     }
01181     const Vector& getUErr(SubsystemIndex subsys) const {
01182         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01183                             "StateImpl::getUErr(subsys)");
01184         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Velocity, 
01185                             "StateImpl::getUErr(subsys)");
01186         return getSubsystem(subsys).uerr;
01187     }
01188 
01189     const Vector& getQErrWeights(SubsystemIndex subsys) const {
01190         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01191             "StateImpl::getQErrWeights(subsys)");
01192         return getSubsystem(subsys).qerrWeights;
01193     }
01194     const Vector& getUErrWeights(SubsystemIndex subsys) const {
01195         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01196             "StateImpl::getUErrWeights(subsys)");
01197         return getSubsystem(subsys).uerrWeights;
01198     }
01199 
01200     const Vector& getUDotErr(SubsystemIndex subsys) const {
01201         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01202                             "StateImpl::getUDotErr(subsys)");
01203         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, 
01204                             "StateImpl::getUDotErr(subsys)");
01205         return getSubsystem(subsys).udoterr;
01206     }
01207     const Vector& getMultipliers(SubsystemIndex subsys) const {
01208         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01209                             "StateImpl::getMultipliers(subsys)");
01210         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration, 
01211                             "StateImpl::getMultipliers(subsys)");
01212         return getSubsystem(subsys).multipliers;
01213     }
01214     
01215     const Vector& getEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
01216         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01217                             "StateImpl::getEventTriggersByStage(subsys)");
01218         SimTK_STAGECHECK_GE(getSubsystemStage(subsys), g, 
01219                             "StateImpl::getEventTriggersByStage(subsys)");
01220         return getSubsystem(subsys).triggers[g];
01221     }
01222     
01223     Vector& updQErr(SubsystemIndex subsys) const {
01224         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01225                             "StateImpl::updQErr(subsys)");
01226         return getSubsystem(subsys).qerr;
01227     }
01228     Vector& updUErr(SubsystemIndex subsys) const {
01229         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01230                             "StateImpl::updUErr(subsys)");
01231         return getSubsystem(subsys).uerr;
01232     }
01233     
01234     Vector& updQErrWeights(SubsystemIndex subsys) {
01235         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01236             "StateImpl::updQErrWeights(subsys)");
01237         invalidateAll(Stage::Position);
01238         return updSubsystem(subsys).qerrWeights;
01239     }
01240     Vector& updUErrWeights(SubsystemIndex subsys) {
01241         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01242             "StateImpl::updUErrWeights(subsys)");
01243         invalidateAll(Stage::Velocity);
01244         return updSubsystem(subsys).uerrWeights;
01245     }
01246 
01247     Vector& updUDotErr(SubsystemIndex subsys) const {
01248         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01249                             "StateImpl::updUDotErr(subsys)");
01250         return getSubsystem(subsys).udoterr;
01251     }
01252     Vector& updMultipliers(SubsystemIndex subsys) const {
01253         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01254                             "StateImpl::updMultipliers(subsys)");
01255         return getSubsystem(subsys).multipliers;
01256     }
01257     Vector& updEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
01258         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01259                             "StateImpl::updEventTriggersByStage(subsys)");
01260         return getSubsystem(subsys).triggers[g];
01261     }
01262     
01263         // Direct access to the global shared state and cache entries.
01264         // Time is allocated in Stage::Topology, State in Stage::Model, and
01265         // Cache in Stage::Instance.
01266     
01267     const Real& getTime() const {
01268         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Topology, 
01269                             "StateImpl::getTime()");
01270         return t;
01271     }
01272     
01273     const Vector& getY() const {
01274         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01275                             "StateImpl::getY()");
01276         return y;
01277     }
01278     
01279     const Vector& getQ() const {
01280         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01281                             "StateImpl::getQ()");
01282         return q;
01283     }
01284     
01285     const Vector& getU() const {
01286         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01287                             "StateImpl::getU()");
01288         return u;
01289     }
01290     
01291     const Vector& getZ() const {
01292         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01293                             "StateImpl::getZ()");
01294         return z;
01295     }
01296         
01297     const Vector& getUWeights() const {
01298         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01299                             "StateImpl::getUWeights()");
01300         return uWeights;
01301     }
01302     
01303     const Vector& getZWeights() const {
01304         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01305                             "StateImpl::getZWeights()");
01306         return zWeights;
01307     }
01308        
01309     // You can call these as long as stage >= allocation stage, but the
01310     // stage will be backed up if necessary to one stage prior to the invalidated stage.
01311     Real& updTime() {  // Back to Stage::Time-1
01312         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Topology, 
01313                             "StateImpl::updTime()");
01314         invalidateAll(Stage::Time);
01315         return t;
01316     }
01317     
01318     Vector& updY() {    // Back to Stage::Position-1
01319         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01320                             "StateImpl::updY()");
01321         invalidateAll(Stage::Position);
01322         return y;
01323     }
01324     
01325     Vector& updQ() {    // Stage::Position-1
01326         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01327                             "StateImpl::updQ()");
01328         invalidateAll(Stage::Position);
01329         return q;
01330     }
01331     
01332     Vector& updU() {     // Stage::Velocity-1
01333         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01334                             "StateImpl::updU()");
01335         invalidateAll(Stage::Velocity);
01336         return u;
01337     }
01338     
01339     Vector& updZ() {     // Stage::Dynamics-1
01340         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01341                             "StateImpl::updZ()");
01342         invalidateAll(Stage::Dynamics);
01343         return z;
01344     }
01345      
01346     Vector& updUWeights() { // Invalidates Report stage only
01347         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01348             "StateImpl::updUWeights()");
01349         invalidateAll(Stage::Report);
01350         return uWeights;
01351     }
01352     
01353     Vector& updZWeights() { // Invalidates Report stage only
01354         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01355             "StateImpl::updZWeights()");
01356         invalidateAll(Stage::Dynamics);
01357         return zWeights;
01358     }   
01359 
01360     // These cache entries you can get at their "computedBy" stages.
01361     const Vector& getYDot() const {
01362         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, 
01363                             "StateImpl::getYDot()");
01364         return ydot;
01365     }
01366     
01367     const Vector& getQDot() const {
01368         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity, 
01369                             "StateImpl::getQDot()");
01370         return qdot;
01371     }
01372     
01373     const Vector& getZDot() const {
01374         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Dynamics, 
01375                             "StateImpl::getZDot()");
01376         return zdot;
01377     }
01378     
01379     const Vector& getUDot() const {
01380         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, 
01381                             "StateImpl::getUDot()");
01382         return udot;
01383     }
01384     
01385     const Vector& getQDotDot() const {
01386         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, 
01387                             "StateImpl::getQDotDot()");
01388         return qdotdot;
01389     }
01390     
01391     // Cache updates are allowed any time after they have been allocated.
01392     Vector& updYDot() const {
01393         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01394                             "StateImpl::updYDot()");
01395         return ydot;
01396     }
01397     
01398     Vector& updQDot() const {
01399         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01400                             "StateImpl::updQDot()");
01401         return qdot;
01402     }
01403     
01404     Vector& updUDot() const {
01405         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01406                             "StateImpl::updUDot()");
01407         return udot;
01408     }
01409     
01410     Vector& updZDot() const {
01411         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01412                             "StateImpl::updZDot()");
01413         return zdot;
01414     }
01415     
01416     Vector& updQDotDot() const {
01417         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model, 
01418                             "StateImpl::updQDotDot()");
01419         return qdotdot;
01420     }
01421     
01422     
01423     const Vector& getYErr() const {
01424         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity, 
01425                             "StateImpl::getYErr()");
01426         return yerr;
01427     }
01428     
01429     const Vector& getQErr() const {
01430         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Position, 
01431                             "StateImpl::getQErr()");
01432         return qerr;
01433     }
01434     const Vector& getUErr() const {
01435         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity, 
01436                             "StateImpl::getUErr()");
01437         return uerr;
01438     }
01439     
01440     const Vector& getQErrWeights() const {
01441         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01442             "StateImpl::getQErrWeights()");
01443         return qerrWeights;
01444     }
01445     const Vector& getUErrWeights() const {
01446         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01447             "StateImpl::getUErrWeights()");
01448         return uerrWeights;
01449     }
01450 
01451     const Vector& getUDotErr() const {
01452         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, 
01453                             "StateImpl::getUDotErr()");
01454         return udoterr;
01455     }
01456     const Vector& getMultipliers() const {
01457         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, 
01458                             "StateImpl::getMultipliers()");
01459         return multipliers;
01460     }
01461     
01462     Vector& updYErr() const {
01463         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01464                             "StateImpl::updYErr()");
01465         return yerr;
01466     }
01467     Vector& updQErr() const{
01468         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01469                             "StateImpl::updQErr()");
01470         return qerr;
01471     }
01472     Vector& updUErr() const{
01473         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01474                             "StateImpl::updUErr()");
01475         return uerr;
01476     }
01477 
01478     Vector& updQErrWeights() {
01479         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01480             "StateImpl::updQErrWeights()");
01481         invalidateAll(Stage::Position);
01482         return qerrWeights;
01483     }
01484     Vector& updUErrWeights() {
01485         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01486             "StateImpl::updUErrWeights()");
01487         invalidateAll(Stage::Velocity);
01488         return uerrWeights;
01489     }
01490 
01491     Vector& updUDotErr() const{
01492         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01493                             "StateImpl::updUDotErr()");
01494         return udoterr;
01495     }
01496     Vector& updMultipliers() const{
01497         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01498                             "StateImpl::updMultipliers()");
01499         return multipliers;
01500     }
01501     
01502     const Vector& getEventTriggers() const {
01503         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration, 
01504                             "StateImpl::getEventTriggers()");
01505         return allTriggers;
01506     }
01507     const Vector& getEventTriggersByStage(Stage g) const {
01508         SimTK_STAGECHECK_GE(getSystemStage(), g, 
01509                             "StateImpl::getEventTriggersByStage()");
01510         return triggers[g];
01511     }
01512     
01513     // These are mutable; hence 'const'.
01514     Vector& updEventTriggers() const {
01515         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01516                             "StateImpl::updEventTriggers()");
01517         return allTriggers;
01518     }
01519     Vector& updEventTriggersByStage(Stage g) const {
01520         SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance, 
01521                             "StateImpl::updEventTriggersByStage()");
01522         return triggers[g];
01523     }
01524 
01525     CacheEntryIndex getDiscreteVarUpdateIndex
01526        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01527         const PerSubsystemInfo& ss = subsystems[subsys];
01528         SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
01529             "StateImpl::getDiscreteVarUpdateIndex()");
01530         const DiscreteVarInfo& dv = ss.discreteInfo[index];
01531         return dv.getAutoUpdateEntry();
01532     } 
01533 
01534     Stage getDiscreteVarAllocationStage
01535        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01536         const PerSubsystemInfo& ss = subsystems[subsys];
01537         SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
01538             "StateImpl::getDiscreteVarAllocationStage()");
01539         const DiscreteVarInfo& dv = ss.discreteInfo[index];
01540         return dv.getAllocationStage();
01541     } 
01542 
01543     Stage getDiscreteVarInvalidatesStage
01544        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01545         const PerSubsystemInfo& ss = subsystems[subsys];
01546         SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
01547             "StateImpl::getDiscreteVarInvalidatesStage()");
01548         const DiscreteVarInfo& dv = ss.discreteInfo[index];
01549         return dv.getInvalidatedStage();
01550     } 
01551 
01552     // You can access at any time a variable that was allocated during 
01553     // realizeTopology(), but don't access others until you have done 
01554     // realizeModel().
01555     const AbstractValue& 
01556     getDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) const {
01557         const PerSubsystemInfo& ss = subsystems[subsys];
01558     
01559         SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
01560                          "StateImpl::getDiscreteVariable()");
01561         const DiscreteVarInfo& dv = ss.discreteInfo[index];
01562     
01563         if (dv.getAllocationStage() > Stage::Topology) {
01564             SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Model, 
01565                                 "StateImpl::getDiscreteVariable()");
01566         }
01567     
01568         return dv.getValue();
01569     }
01570     Real getDiscreteVarLastUpdateTime(SubsystemIndex subsys, DiscreteVariableIndex index) const {
01571         const PerSubsystemInfo& ss = subsystems[subsys];
01572         SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
01573                          "StateImpl::getDiscreteVarLastUpdateTime()");
01574         const DiscreteVarInfo& dv = ss.discreteInfo[index];
01575         return dv.getTimeLastUpdated();
01576     }
01577 
01578     const AbstractValue& getDiscreteVarUpdateValue
01579        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01580         const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
01581         SimTK_ERRCHK2(cx.isValid(), "StateImpl::getDiscreteVarUpdateValue()", 
01582             "Subsystem %d has a discrete variable %d but it does not have an"
01583             " associated update cache variable.", (int)subsys, (int)index);
01584         return getCacheEntry(subsys, cx);
01585     }
01586     AbstractValue& updDiscreteVarUpdateValue
01587        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01588         const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
01589         SimTK_ERRCHK2(cx.isValid(), "StateImpl::updDiscreteVarUpdateValue()", 
01590             "Subsystem %d has a discrete variable %d but it does not have an"
01591             " associated update cache variable.", (int)subsys, (int)index);
01592         return updCacheEntry(subsys, cx);
01593     }
01594     bool isDiscreteVarUpdateValueRealized
01595        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01596         const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
01597         SimTK_ERRCHK2(cx.isValid(), "StateImpl::isDiscreteVarUpdateValueRealized()", 
01598             "Subsystem %d has a discrete variable %d but it does not have an"
01599             " associated update cache variable.", (int)subsys, (int)index);
01600         return isCacheValueRealized(subsys, cx);
01601     }
01602     void markDiscreteVarUpdateValueRealized
01603        (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01604         const CacheEntryIndex cx = getDiscreteVarUpdateIndex(subsys,index);
01605         SimTK_ERRCHK2(cx.isValid(), 
01606             "StateImpl::markDiscreteVarUpdateValueRealized()", 
01607             "Subsystem %d has a discrete variable %d but it does not have an"
01608             " associated update cache variable.", (int)subsys, (int)index);
01609         markCacheValueRealized(subsys, cx);
01610     }
01611 
01612     // You can update at any time a variable that was allocated during 
01613     // realizeTopology(), but later variables must wait until you have done 
01614     // realizeModel(). This always backs the stage up to one earlier than the 
01615     // variable's "invalidates" stage, and if there is an auto-update cache 
01616     // entry it is also invalidated, regardless of its "depends on" stage.
01617     AbstractValue& 
01618     updDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) {
01619         PerSubsystemInfo& ss = subsystems[subsys];
01620     
01621         SimTK_INDEXCHECK(index,(int)ss.discreteInfo.size(),
01622                          "StateImpl::updDiscreteVariable()");
01623         DiscreteVarInfo& dv = ss.discreteInfo[index];
01624 
01625         if (dv.getAllocationStage() > Stage::Topology) {
01626             SimTK_STAGECHECK_GE(getSubsystemStage(subsys), 
01627                 Stage::Model, "StateImpl::updDiscreteVariable()");
01628         }
01629     
01630         invalidateAll(dv.getInvalidatedStage());
01631 
01632         // Invalidate the auto-update entry, if any.
01633         const CacheEntryIndex cx = dv.getAutoUpdateEntry();
01634         if (cx.isValid()) {
01635             CacheEntryInfo& ce = ss.cacheInfo[cx];
01636             ce.invalidate();
01637         }
01638     
01639         // We're now marking this variable as having been updated at the 
01640         // current time.
01641         return dv.updValue(t);
01642     }
01643     
01644     Stage getCacheEntryAllocationStage
01645        (SubsystemIndex subsys, CacheEntryIndex index) const {
01646         const PerSubsystemInfo& ss = subsystems[subsys];
01647         SimTK_INDEXCHECK(index,(int)ss.cacheInfo.size(),
01648             "StateImpl::getCacheEntryAllocationStage()");
01649         const CacheEntryInfo& ce = ss.cacheInfo[index];
01650         return ce.getAllocationStage();
01651     } 
01652 
01653     // Stage >= ce.stage
01654     // This method gets called a lot, so make it fast in Release mode.
01655     const AbstractValue& 
01656     getCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
01657         const PerSubsystemInfo& ss = subsystems[subsys];
01658     
01659         SimTK_INDEXCHECK(index,(int)ss.cacheInfo.size(),
01660                          "StateImpl::getCacheEntry()");
01661         const CacheEntryInfo& ce = ss.cacheInfo[index];
01662 
01663         // These two unconditional checks are somewhat expensive; I'm leaving
01664         // them in though because they catch so many user errors.
01665         // (sherm 20130222).
01666         SimTK_STAGECHECK_GE_ALWAYS(ss.currentStage, 
01667             ce.getDependsOnStage(), "StateImpl::getCacheEntry()");
01668 
01669         if (ss.currentStage < ce.getComputedByStage()) {
01670             const StageVersion currentDependsOnVersion = 
01671                 ss.stageVersions[ce.getDependsOnStage()];
01672             const StageVersion lastCacheVersion = 
01673                 ce.getVersionWhenLastComputed();
01674 
01675             if (lastCacheVersion != currentDependsOnVersion) {
01676                 SimTK_THROW4(Exception::CacheEntryOutOfDate,
01677                     ss.currentStage, ce.getDependsOnStage(), 
01678                     currentDependsOnVersion, lastCacheVersion);
01679             }
01680         }
01681 
01682         // If we get here then we're either past the "computed by" stage, or
01683         // we're past "depends on" with an explicit validation having been made 
01684         // at the "depends on" stage's current version.
01685 
01686         return ce.getValue();
01687     }
01688     
01689     // You can access a cache entry for update any time after it has been
01690     // allocated. This does not affect the stage.
01691     AbstractValue& 
01692     updCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
01693         const PerSubsystemInfo& ss = subsystems[subsys];
01694     
01695         SimTK_INDEXCHECK(index,(int)ss.cacheInfo.size(),
01696                          "StateImpl::updCacheEntry()");
01697         CacheEntryInfo& ce = ss.cacheInfo[index];
01698     
01699         return ce.updValue();
01700     }
01701 
01702     bool isCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
01703         const PerSubsystemInfo& ss = subsystems[subx];
01704         SimTK_INDEXCHECK(cx,(int)ss.cacheInfo.size(),
01705                          "StateImpl::isCacheValueRealized()");
01706         const CacheEntryInfo& ce = ss.cacheInfo[cx];
01707         return ce.isCurrent(getSubsystemStage(subx), 
01708                             getSubsystemStageVersions(subx));
01709     }
01710     void markCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
01711         const PerSubsystemInfo& ss = subsystems[subx];
01712         SimTK_INDEXCHECK(cx,(int)ss.cacheInfo.size(),
01713                          "StateImpl::markCacheValueRealized()");
01714         CacheEntryInfo& ce = ss.cacheInfo[cx];
01715     
01716         // If this cache entry depends on anything, it can't be valid unless 
01717         // we're at least *working* on its depends-on stage, meaning the current
01718         // stage would have to be the one before that. The depends-on stage is 
01719         // required to be at least Stage::Topology, so its prev() stage exists.
01720         SimTK_STAGECHECK_GE(getSubsystemStage(subx), 
01721                             ce.getDependsOnStage().prev(), 
01722                             "StateImpl::markCacheValueRealized()");
01723 
01724         ce.markAsComputed(getSubsystemStageVersions(subx));
01725     }
01726 
01727     void markCacheValueNotRealized
01728        (SubsystemIndex subx, CacheEntryIndex cx) const {
01729         const PerSubsystemInfo& ss = subsystems[subx];
01730         SimTK_INDEXCHECK(cx,(int)ss.cacheInfo.size(),
01731             "StateImpl::markCacheValueNotRealized()");
01732         CacheEntryInfo& ce = ss.cacheInfo[cx];
01733 
01734         ce.invalidate();
01735     }
01736 
01737     StageVersion getSystemTopologyStageVersion() const
01738     {   return systemStageVersions[Stage::Topology]; }
01739 
01740     void setSystemTopologyStageVersion(StageVersion topoVersion)
01741     {   assert(topoVersion>0); 
01742         systemStageVersions[Stage::Topology]=topoVersion; }
01743 
01744     // Capture the stage versions only for currently-realized stages.
01745     void getSystemStageVersions(Array_<StageVersion>& versions) const {
01746         versions.resize(currentSystemStage+1);
01747         for (int i=0; i <= currentSystemStage; ++i)
01748             versions[i] = systemStageVersions[i];
01749     }
01750 
01751     // If the current state is realized at least as high as the previous one, 
01752     // then report Stage::Infinity if all of those stage versions match.
01753     // Otherwise report either the first mismatch or the first now-invalid
01754     // stage if lower stages match.
01755     Stage getLowestSystemStageDifference
01756        (const Array_<StageVersion>& prevVersions) const {
01757         const int nRealizedBefore = (int)prevVersions.size();
01758         const int nRealizedNow    = (int)currentSystemStage+1; // count from 0
01759         const int nRealizedBoth   = std::min(nRealizedBefore,nRealizedNow);
01760 
01761         // First check the stages both had in common.
01762         Stage g=Stage::Topology; 
01763         for (; g < nRealizedBoth; ++g)
01764             if (systemStageVersions[g] != prevVersions[g])
01765                 return g;
01766 
01767         // All stages that were valid before and now have identical versions.
01768         // If that's all there was before then nothing has changed.
01769         return nRealizedNow >= nRealizedBefore ? Stage::Infinity
01770                                                : g; // 1st unrealized stage
01771     }
01772 
01773     void autoUpdateDiscreteVariables();
01774     
01775     String toString() const;    
01776     String cacheToString() const;
01777 
01778 private:
01779     void initializeStageVersions() {
01780         for (int i=0; i < Stage::NValid; ++i)
01781             systemStageVersions[i] = 1; // never 0
01782     }
01783 
01784 private:
01785         // Shared global resource State variables //
01786 
01787     // We consider time t to be a state variable allocated at Topology stage,
01788     // with its "invalidated" stage Stage::Time. The value of t is NaN in an 
01789     // Empty State, and is initialized to zero when the System stage advances
01790     // to Stage::Topology (i.e., when the System is realized to stage Topology).
01791     Real            t;
01792 
01793     // The continuous state variables are allocated at Model stage, and given
01794     // their specified initial values when the System stage advances to
01795     // Stage::Model (i.e., when the System is realized to Model stage).
01796     Vector          y; // All the continuous state variables together {q,u,z}
01797 
01798         // These are views into y.
01799     Vector          q; // Stage::Position continuous variables
01800     Vector          u; // Stage::Velocity continuous variables
01801     Vector          z; // Stage::Dynamics continuous variables
01802 
01803         // These are not views; there are no qWeights.
01804     Vector          uWeights; // scaling for u
01805     Vector          zWeights; // scaling for z
01806 
01807         // These are Instance stage state variables.
01808     Vector          qerrWeights; // Scaling for perrs
01809     Vector          uerrWeights; // Scaling for pdoterrs and verrs
01810 
01811         // Shared global resource Cache entries //
01812 
01813     // This is the System's currently highest-valid Stage.
01814     mutable Stage        currentSystemStage;
01815     // This contains a counter for each system stage which is bumped each
01816     // time that stage is invalidated. This allows detection of a state
01817     // that has been changed even after a subsequent realization. The
01818     // Topology stage entry should match the System's Topology version.
01819     mutable StageVersion systemStageVersions[Stage::NValid];
01820 
01821         // DIFFERENTIAL EQUATIONS
01822 
01823     // All the state derivatives taken together (qdot,udot,zdot)
01824     mutable Vector  ydot; 
01825 
01826     // These are views into ydot.
01827     mutable Vector  qdot;       // Stage::Velocity
01828     mutable Vector  udot;       // Stage::Acceleration
01829     mutable Vector  zdot;       // Stage::Acceleration
01830 
01831     // This is an independent cache entry.
01832     mutable Vector  qdotdot;    // Stage::Acceleration
01833 
01834         // ALGEBRAIC EQUATIONS
01835 
01836     mutable Vector  yerr;        // All constraint errors together (qerr,uerr)
01837     mutable Vector  udoterr;     // Stage::Acceleration (Index 1 constraints)
01838     mutable Vector  multipliers; // Stage::Acceleration (Index 1 algebraic vars)
01839 
01840     // These are views into yerr.
01841     mutable Vector  qerr;       // Stage::Position (Index 3 constraints)
01842     mutable Vector  uerr;       // Stage::Velocity (Index 2 constraints)
01843 
01844         // DISCRETE EQUATIONS
01845 
01846     // All the event triggers together, ordered by stage.
01847     mutable Vector  allTriggers;
01848 
01849     // These are views into allTriggers.
01850     mutable Vector  triggers[Stage::NValid];
01851     
01852 
01853         // Subsystem support //
01854 
01855     // Subsystem 0 (always present) is for the System as a whole. Its name
01856     // and version are the System name and version.
01857     Array_<PerSubsystemInfo> subsystems;
01858 
01859     // Return true only if all subsystems are realized to at least Stage g.
01860     bool allSubsystemsAtLeastAtStage(Stage g) const {
01861         for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
01862             if (subsystems[i].currentStage < g)
01863                 return false;
01864         return true;
01865     }
01866 
01867 };
01868 
01869 
01870 //==============================================================================
01871 //             INLINE IMPLEMENTATIONS OF STATE METHODS
01872 //==============================================================================
01873 // These mostly just forward to the StateImpl object.
01874 
01875 inline void State::setNumSubsystems(int i) {
01876     updImpl().setNumSubsystems(i);
01877 }
01878 inline void State::initializeSubsystem
01879    (SubsystemIndex subsys, const String& name, const String& version) {
01880     updImpl().initializeSubsystem(subsys, name, version);
01881 }
01882 
01883 inline SubsystemIndex State::addSubsystem
01884    (const String& name, const String& version) {
01885     return updImpl().addSubsystem(name, version);
01886 }
01887 inline int State::getNumSubsystems() const {
01888     return getImpl().getNumSubsystems();
01889 }
01890 inline const String& State::getSubsystemName(SubsystemIndex subsys) const {
01891     return getImpl().getSubsystemName(subsys);
01892 }
01893 inline const String& State::getSubsystemVersion(SubsystemIndex subsys) const {
01894     return getImpl().getSubsystemVersion(subsys);
01895 }
01896 inline const Stage& State::getSubsystemStage(SubsystemIndex subsys) const {
01897     return getImpl().getSubsystemStage(subsys);
01898 }
01899 inline const Stage& State::getSystemStage() const {
01900     return getImpl().getSystemStage();
01901 }
01902 inline void State::invalidateAll(Stage stage) {
01903     updImpl().invalidateAll(stage);
01904 }
01905 inline void State::invalidateAllCacheAtOrAbove(Stage stage) const {
01906     getImpl().invalidateAllCacheAtOrAbove(stage);
01907 }
01908 inline void State::advanceSubsystemToStage(SubsystemIndex subsys, Stage stage) const {
01909     getImpl().advanceSubsystemToStage(subsys, stage);
01910 }
01911 inline void State::advanceSystemToStage(Stage stage) const {
01912     getImpl().advanceSystemToStage(stage);
01913 }
01914 
01915 inline StageVersion State::getSystemTopologyStageVersion() const 
01916 {   return getImpl().getSystemTopologyStageVersion(); }
01917 
01918 // Continuous state variables
01919 inline QIndex State::allocateQ(SubsystemIndex subsys, const Vector& qInit) {
01920     return updImpl().allocateQ(subsys, qInit);
01921 }
01922 inline UIndex State::allocateU(SubsystemIndex subsys, const Vector& uInit) {
01923     return updImpl().allocateU(subsys, uInit);
01924 }
01925 inline ZIndex State::allocateZ(SubsystemIndex subsys, const Vector& zInit) {
01926     return updImpl().allocateZ(subsys, zInit);
01927 }
01928 
01929 // Constraint errors and multipliers
01930 inline QErrIndex State::allocateQErr(SubsystemIndex subsys, int nqerr) const {
01931     return getImpl().allocateQErr(subsys, nqerr);
01932 }
01933 inline UErrIndex State::allocateUErr(SubsystemIndex subsys, int nuerr) const {
01934     return getImpl().allocateUErr(subsys, nuerr);
01935 }
01936 inline UDotErrIndex State::
01937 allocateUDotErr(SubsystemIndex subsys, int nudoterr) const {
01938     return getImpl().allocateUDotErr(subsys, nudoterr);
01939 }
01940 
01941 // Event witness functions
01942 inline EventTriggerByStageIndex State::
01943 allocateEventTrigger(SubsystemIndex subsys, Stage stage, int nevent) const {
01944     return getImpl().allocateEventTrigger(subsys, stage, nevent);
01945 }
01946 
01947 // Discrete Variables
01948 inline DiscreteVariableIndex State::
01949 allocateDiscreteVariable(SubsystemIndex subsys, Stage stage, AbstractValue* v) {
01950     return updImpl().allocateDiscreteVariable(subsys, stage, v);
01951 }
01952 inline DiscreteVariableIndex State::
01953 allocateAutoUpdateDiscreteVariable
01954    (SubsystemIndex subsys, Stage invalidates, AbstractValue* v,
01955     Stage updateDependsOn) {
01956     return updImpl().allocateAutoUpdateDiscreteVariable
01957        (subsys, invalidates, v, updateDependsOn); 
01958 }
01959 
01960 inline CacheEntryIndex State::
01961 getDiscreteVarUpdateIndex
01962    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01963     return getImpl().getDiscreteVarUpdateIndex(subsys, index);
01964 }
01965 inline Stage State::
01966 getDiscreteVarAllocationStage
01967    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01968     return getImpl().getDiscreteVarAllocationStage(subsys, index);
01969 }
01970 inline Stage State::
01971 getDiscreteVarInvalidatesStage
01972    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01973     return getImpl().getDiscreteVarInvalidatesStage(subsys, index);
01974 }
01975 
01976 inline const AbstractValue& State::
01977 getDiscreteVariable(SubsystemIndex subsys, DiscreteVariableIndex index) const {
01978     return getImpl().getDiscreteVariable(subsys, index);
01979 }
01980 inline Real State::
01981 getDiscreteVarLastUpdateTime
01982    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01983     return getImpl().getDiscreteVarLastUpdateTime(subsys, index);
01984 }
01985 inline const AbstractValue& State::
01986 getDiscreteVarUpdateValue
01987    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01988     return getImpl().getDiscreteVarUpdateValue(subsys, index);
01989 }
01990 inline AbstractValue& State::
01991 updDiscreteVarUpdateValue
01992    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01993     return getImpl().updDiscreteVarUpdateValue(subsys, index);
01994 }
01995 inline bool State::
01996 isDiscreteVarUpdateValueRealized
01997    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
01998     return getImpl().isDiscreteVarUpdateValueRealized(subsys, index);
01999 }
02000 inline void State::
02001 markDiscreteVarUpdateValueRealized
02002    (SubsystemIndex subsys, DiscreteVariableIndex index) const {
02003     getImpl().markDiscreteVarUpdateValueRealized(subsys, index);
02004 }
02005 
02006 inline AbstractValue& State::
02007 updDiscreteVariable
02008    (SubsystemIndex subsys, DiscreteVariableIndex index) {
02009     return updImpl().updDiscreteVariable(subsys, index);
02010 }
02011 inline void State::
02012 setDiscreteVariable
02013    (SubsystemIndex i, DiscreteVariableIndex index, const AbstractValue& v) {
02014     updDiscreteVariable(i,index) = v;
02015 }
02016 
02017 // Cache Entries
02018 inline CacheEntryIndex State::
02019 allocateCacheEntry(SubsystemIndex subsys, Stage dependsOn, Stage computedBy, 
02020                    AbstractValue* v) const {
02021     return getImpl().allocateCacheEntry(subsys, dependsOn, computedBy, v);
02022 }
02023 
02024 inline Stage State::
02025 getCacheEntryAllocationStage(SubsystemIndex subsys, CacheEntryIndex index) const {
02026     return getImpl().getCacheEntryAllocationStage(subsys, index);
02027 }
02028 inline const AbstractValue& State::
02029 getCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
02030     return getImpl().getCacheEntry(subsys, index);
02031 }
02032 inline AbstractValue& State::
02033 updCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
02034     return getImpl().updCacheEntry(subsys, index);
02035 }
02036 
02037 inline bool State::
02038 isCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
02039     return getImpl().isCacheValueRealized(subx, cx); 
02040 }
02041 inline void State::
02042 markCacheValueRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
02043     getImpl().markCacheValueRealized(subx, cx); 
02044 }
02045 inline void State::
02046 markCacheValueNotRealized(SubsystemIndex subx, CacheEntryIndex cx) const {
02047     getImpl().markCacheValueNotRealized(subx, cx); 
02048 }
02049 
02050 // Global Resource Dimensions
02051 
02052 inline int State::getNY() const {
02053     return getImpl().getNY();
02054 }
02055 inline int State::getNQ() const {
02056     return getImpl().getNQ();
02057 }
02058 inline SystemYIndex State::getQStart() const {
02059     return getImpl().getQStart();
02060 }
02061 inline int State::getNU() const {
02062     return getImpl().getNU();
02063 }
02064 inline SystemYIndex State::getUStart() const {
02065     return getImpl().getUStart();
02066 }
02067 inline int State::getNZ() const {
02068     return getImpl().getNZ();
02069 }
02070 inline SystemYIndex State::getZStart() const {
02071     return getImpl().getZStart();
02072 }
02073 inline int State::getNYErr() const {
02074     return getImpl().getNYErr();
02075 }
02076 inline int State::getNQErr() const {
02077     return getImpl().getNQErr();
02078 }
02079 inline SystemYErrIndex State::getQErrStart() const {
02080     return getImpl().getQErrStart();
02081 }
02082 inline int State::getNUErr() const {
02083     return getImpl().getNUErr();
02084 }
02085 inline SystemYErrIndex State::getUErrStart() const {
02086     return getImpl().getUErrStart();
02087 }
02088 inline int State::getNUDotErr() const {
02089     return getImpl().getNUDotErr();
02090 }
02091 inline int State::getNMultipliers() const {
02092     return getNUDotErr();
02093 }
02094 inline int State::getNEventTriggers() const {
02095     return getImpl().getNEventTriggers();
02096 }
02097 inline int State::getNEventTriggersByStage(Stage stage) const {
02098     return getImpl().getNEventTriggersByStage(stage);
02099 }
02100 
02101 
02102 
02103 // Per-Subsystem Dimensions
02104 inline SystemQIndex State::getQStart(SubsystemIndex subsys) const {
02105     return getImpl().getQStart(subsys);
02106 }
02107 inline int State::getNQ(SubsystemIndex subsys) const {
02108     return getImpl().getNQ(subsys);
02109 }
02110 inline SystemUIndex State::getUStart(SubsystemIndex subsys) const {
02111     return getImpl().getUStart(subsys);
02112 }
02113 inline int State::getNU(SubsystemIndex subsys) const {
02114     return getImpl().getNU(subsys);
02115 }
02116 inline SystemZIndex State::getZStart(SubsystemIndex subsys) const {
02117     return getImpl().getZStart(subsys);
02118 }
02119 inline int State::getNZ(SubsystemIndex subsys) const {
02120     return getImpl().getNZ(subsys);
02121 }
02122 inline SystemQErrIndex State::getQErrStart(SubsystemIndex subsys) const {
02123     return getImpl().getQErrStart(subsys);
02124 }
02125 inline int State::getNQErr(SubsystemIndex subsys) const {
02126     return getImpl().getNQErr(subsys);
02127 }
02128 inline SystemUErrIndex State::getUErrStart(SubsystemIndex subsys) const {
02129     return getImpl().getUErrStart(subsys);
02130 }
02131 inline int State::getNUErr(SubsystemIndex subsys) const {
02132     return getImpl().getNUErr(subsys);
02133 }
02134 inline SystemUDotErrIndex State::getUDotErrStart(SubsystemIndex subsys) const {
02135     return getImpl().getUDotErrStart(subsys);
02136 }
02137 inline int State::getNUDotErr(SubsystemIndex subsys) const {
02138     return getImpl().getNUDotErr(subsys);
02139 }
02140 inline SystemMultiplierIndex State::getMultipliersStart(SubsystemIndex subsys) const {
02141     return SystemMultiplierIndex(getUDotErrStart(subsys));
02142 }
02143 inline int State::getNMultipliers(SubsystemIndex subsys) const {
02144     return getNUDotErr(subsys);
02145 }
02146 inline SystemEventTriggerByStageIndex State::
02147 getEventTriggerStartByStage(SubsystemIndex subsys, Stage stage) const {
02148     return getImpl().getEventTriggerStartByStage(subsys, stage);
02149 }
02150 inline int State::
02151 getNEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
02152     return getImpl().getNEventTriggersByStage(subsys, stage);
02153 }
02154 
02155 
02156 
02157 inline const Vector& State::
02158 getEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
02159     return getImpl().getEventTriggersByStage(subsys, stage);
02160 }
02161 inline Vector& State::
02162 updEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
02163     return getImpl().updEventTriggersByStage(subsys, stage);
02164 }
02165 inline const Vector& State::getQ(SubsystemIndex subsys) const {
02166     return getImpl().getQ(subsys);
02167 }
02168 inline const Vector& State::getU(SubsystemIndex subsys) const {
02169     return getImpl().getU(subsys);
02170 }
02171 inline const Vector& State::getZ(SubsystemIndex subsys) const {
02172     return getImpl().getZ(subsys);
02173 }
02174 inline const Vector& State::getUWeights(SubsystemIndex subsys) const {
02175     return getImpl().getUWeights(subsys);
02176 }
02177 inline const Vector& State::getZWeights(SubsystemIndex subsys) const {
02178     return getImpl().getZWeights(subsys);
02179 }
02180 inline Vector& State::updQ(SubsystemIndex subsys) {
02181     return updImpl().updQ(subsys);
02182 }
02183 inline Vector& State::updU(SubsystemIndex subsys) {
02184     return updImpl().updU(subsys);
02185 }
02186 inline Vector& State::updZ(SubsystemIndex subsys) {
02187     return updImpl().updZ(subsys);
02188 }
02189 inline Vector& State::updUWeights(SubsystemIndex subsys) {
02190     return updImpl().updUWeights(subsys);
02191 }
02192 inline Vector& State::updZWeights(SubsystemIndex subsys) {
02193     return updImpl().updZWeights(subsys);
02194 }
02195 inline const Vector& State::getQDot(SubsystemIndex subsys) const {
02196     return getImpl().getQDot(subsys);
02197 }
02198 inline const Vector& State::getUDot(SubsystemIndex subsys) const {
02199     return getImpl().getUDot(subsys);
02200 }
02201 inline const Vector& State::getZDot(SubsystemIndex subsys) const {
02202     return getImpl().getZDot(subsys);
02203 }
02204 inline const Vector& State::getQDotDot(SubsystemIndex subsys) const {
02205     return getImpl().getQDotDot(subsys);
02206 }
02207 inline Vector& State::updQDot(SubsystemIndex subsys) const {
02208     return getImpl().updQDot(subsys);
02209 }
02210 inline Vector& State::updUDot(SubsystemIndex subsys) const {
02211     return getImpl().updUDot(subsys);
02212 }
02213 inline Vector& State::updZDot(SubsystemIndex subsys) const {
02214     return getImpl().updZDot(subsys);
02215 }
02216 inline Vector& State::updQDotDot(SubsystemIndex subsys) const {
02217     return getImpl().updQDotDot(subsys);
02218 }
02219 inline const Vector& State::getQErr(SubsystemIndex subsys) const {
02220     return getImpl().getQErr(subsys);
02221 }
02222 inline const Vector& State::getUErr(SubsystemIndex subsys) const {
02223     return getImpl().getUErr(subsys);
02224 }
02225 inline const Vector& State::getQErrWeights(SubsystemIndex subsys) const {
02226     return getImpl().getQErrWeights(subsys);
02227 }
02228 inline const Vector& State::getUErrWeights(SubsystemIndex subsys) const {
02229     return getImpl().getUErrWeights(subsys);
02230 }
02231 inline const Vector& State::getUDotErr(SubsystemIndex subsys) const {
02232     return getImpl().getUDotErr(subsys);
02233 }
02234 inline const Vector& State::getMultipliers(SubsystemIndex subsys) const {
02235     return getImpl().getMultipliers(subsys);
02236 }
02237 inline Vector& State::updQErr(SubsystemIndex subsys) const {
02238     return getImpl().updQErr(subsys);
02239 }
02240 inline Vector& State::updUErr(SubsystemIndex subsys) const {
02241     return getImpl().updUErr(subsys);
02242 }
02243 inline Vector& State::updQErrWeights(SubsystemIndex subsys) {
02244     return updImpl().updQErrWeights(subsys);
02245 }
02246 inline Vector& State::updUErrWeights(SubsystemIndex subsys) {
02247     return updImpl().updUErrWeights(subsys);
02248 }
02249 inline Vector& State::updUDotErr(SubsystemIndex subsys) const {
02250     return getImpl().updUDotErr(subsys);
02251 }
02252 inline Vector& State::updMultipliers(SubsystemIndex subsys) const {
02253     return getImpl().updMultipliers(subsys);
02254 }
02255 
02256 inline SystemEventTriggerIndex State::
02257 getEventTriggerStartByStage(Stage stage) const {
02258     return getImpl().getEventTriggerStartByStage(stage);
02259 }
02260 
02261 inline const Vector& State::getEventTriggers() const {
02262     return getImpl().getEventTriggers();
02263 }
02264 inline const Vector& State::getEventTriggersByStage(Stage stage) const {
02265     return getImpl().getEventTriggersByStage(stage);
02266 }
02267 
02268 inline Vector& State::updEventTriggers() const {
02269     return getImpl().updEventTriggers();
02270 }
02271 inline Vector& State::updEventTriggersByStage(Stage stage) const {
02272     return getImpl().updEventTriggersByStage(stage);
02273 }
02274 
02275 inline const Real& State::getTime() const {
02276     return getImpl().getTime();
02277 }
02278 inline const Vector& State::getY() const {
02279     return getImpl().getY();
02280 }
02281 inline const Vector& State::getQ() const {
02282     return getImpl().getQ();
02283 }
02284 inline const Vector& State::getU() const {
02285     return getImpl().getU();
02286 }
02287 inline const Vector& State::getZ() const {
02288     return getImpl().getZ();
02289 }
02290 inline const Vector& State::getUWeights() const {
02291     return getImpl().getUWeights();
02292 }
02293 inline const Vector& State::getZWeights() const {
02294     return getImpl().getZWeights();
02295 }
02296 Real& State::updTime() {
02297     return updImpl().updTime();
02298 }
02299 inline Vector& State::updY() {
02300     return updImpl().updY();
02301 }
02302 inline void State::setTime(Real t) {
02303     updTime() = t;
02304 }
02305 inline void State::setY(const Vector& y) {
02306     updY() = y;
02307 }
02308 inline Vector& State::updQ() {
02309     return updImpl().updQ();
02310 }
02311 inline Vector& State::updU() {
02312     return updImpl().updU();
02313 }
02314 inline Vector& State::updZ() {
02315     return updImpl().updZ();
02316 }
02317 inline Vector& State::updUWeights() {
02318     return updImpl().updUWeights();
02319 }
02320 inline Vector& State::updZWeights() {
02321     return updImpl().updZWeights();
02322 }
02323 inline void State::setQ(const Vector& q) {
02324     updQ() = q;
02325 }
02326 inline void State::setU(const Vector& u) {
02327     updU() = u;
02328 }
02329 inline void State::setZ(const Vector& z) {
02330     updZ() = z;
02331 }
02332 inline const Vector& State::getYDot() const {
02333     return getImpl().getYDot();
02334 }
02335 inline const Vector& State::getQDot() const {
02336     return getImpl().getQDot();
02337 }
02338 inline const Vector& State::getZDot() const {
02339     return getImpl().getZDot();
02340 }
02341 inline const Vector& State::getUDot() const {
02342     return getImpl().getUDot();
02343 }
02344 inline const Vector& State::getQDotDot() const {
02345     return getImpl().getQDotDot();
02346 }
02347 inline Vector& State::updYDot() const {
02348     return getImpl().updYDot();
02349 }
02350 inline Vector& State::updQDot() const {
02351     return getImpl().updQDot();
02352 }
02353 inline Vector& State::updZDot() const {
02354     return getImpl().updZDot();
02355 }
02356 inline Vector& State::updUDot() const {
02357     return getImpl().updUDot();
02358 }
02359 inline Vector& State::updQDotDot() const {
02360     return getImpl().updQDotDot();
02361 }
02362 inline const Vector& State::getYErr() const {
02363     return getImpl().getYErr();
02364 }
02365 inline const Vector& State::getQErr() const {
02366     return getImpl().getQErr();
02367 }
02368 inline const Vector& State::getUErr() const {
02369     return getImpl().getUErr();
02370 }
02371 inline const Vector& State::getQErrWeights() const {
02372     return getImpl().getQErrWeights();
02373 }
02374 inline const Vector& State::getUErrWeights() const {
02375     return getImpl().getUErrWeights();
02376 }
02377 inline const Vector& State::getUDotErr() const {
02378     return getImpl().getUDotErr();
02379 }
02380 inline const Vector& State::getMultipliers() const {
02381     return getImpl().getMultipliers();
02382 }
02383 inline Vector& State::updYErr() const {
02384     return getImpl().updYErr();
02385 }
02386 inline Vector& State::updQErr() const {
02387     return getImpl().updQErr();
02388 }
02389 inline Vector& State::updUErr() const {
02390     return getImpl().updUErr();
02391 }
02392 inline Vector& State::updQErrWeights() {
02393     return updImpl().updQErrWeights();
02394 }
02395 inline Vector& State::updUErrWeights() {
02396     return updImpl().updUErrWeights();
02397 }
02398 inline Vector& State::updUDotErr() const {
02399     return getImpl().updUDotErr();
02400 }
02401 inline Vector& State::updMultipliers() const {
02402     return getImpl().updMultipliers();
02403 }
02404 
02405 inline void State::
02406 setSystemTopologyStageVersion(StageVersion topoVersion)
02407 {   return updImpl().setSystemTopologyStageVersion(topoVersion); }
02408 
02409 inline void State::
02410 getSystemStageVersions(Array_<StageVersion>& versions) const {
02411     return getImpl().getSystemStageVersions(versions); 
02412 }
02413 inline Stage State::
02414 getLowestSystemStageDifference(const Array_<StageVersion>& prev) const {
02415     return getImpl().getLowestSystemStageDifference(prev); 
02416 }
02417 inline void State::autoUpdateDiscreteVariables() {
02418     updImpl().autoUpdateDiscreteVariables(); 
02419 }
02420 
02421 inline String State::toString() const {
02422     return getImpl().toString();
02423 }
02424 inline String State::cacheToString() const {
02425     return getImpl().cacheToString();
02426 }
02427 
02428 
02429 } // namespace SimTK
02430    // End of hiding from Doxygen
02432 
02433 #endif // SimTK_SimTKCOMMON_STATE_IMPL_H_
02434 
02435 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines