Simbody
3.5
|
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