Simbody  3.8
StateImpl.h
Go to the documentation of this file.
1 #ifndef SimTK_SimTKCOMMON_STATE_IMPL_H_
2 #define SimTK_SimTKCOMMON_STATE_IMPL_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2005-15 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: Peter Eastman *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26  // Hide from Doxygen
32 
33 // This header is logically an extension of State.h and is intended to be
34 // included from within State.h and nowhere else.
35 
36 namespace SimTK {
37 
38 //==============================================================================
39 // LIST OF DEPENDENTS
40 //==============================================================================
41 /* This class contains a set of downstream cache entries that are dependent
42 on the value of some prerequisite that is the owner of this object.
43 Prerequisites may be
44  - continuous state variables q,u,z
45  - discrete variables, or
46  - upstream cache entries.
47 
48 When the prerequisite's value changes or is explicitly invalidated, the
49 notePrerequisiteChange() method is invoked, causing the invalidate() method of
50 each of the dependent cache entries to be called.
51 
52 CAUTION: upstream cache entries may be invalidated implicitly by their
53 depends-on stage becoming invalid. For this to work, the depends-on stage
54 of a downstream cache entry must be at least as high as the highest depends-on
55 stage of any of its upstream cache prerequisites. That way it is guaranteed to
56 be implicitly invalidated whenever any of its upstream prerequisites are.
57 
58 The dependents are expected to know their prerequisites so that they can
59 remove themselves from any lists of dependents they are on when deleted; and so
60 that they can be registered on the appropriate lists of dependents when copied.
61 */
62 class ListOfDependents {
63  using CacheList = Array_<CacheEntryKey>;
64 public:
65  using size_type = CacheList::size_type;
66  using value_type = CacheEntryKey;
67  using iterator = CacheList::iterator;
68  using const_iterator = CacheList::const_iterator;
69 
70  // default constructors, assignment, destructor
71 
72  void clear() {m_dependents.clear();}
73  bool empty() const {return m_dependents.empty();}
74 
75  size_type size() const {return m_dependents.size();}
76  const_iterator cbegin() const {return m_dependents.cbegin();}
77  const_iterator cend() const {return m_dependents.cend();}
78  iterator begin() {return m_dependents.begin();}
79  iterator end() {return m_dependents.end();}
80  const value_type& front() const {return m_dependents.front();}
81  const value_type& back() const {return m_dependents.back();}
82 
83  // Search the dependents list to see if this cache entry is already on it.
84  // This is a linear search but we expect these lists to be *very* short.
85  // We only expect to use this during set up and tear down, not while
86  // running so we'll leave the asserts in Release builds.
87  bool contains(const CacheEntryKey& ck) const {
88  SimTK_ASSERT2_ALWAYS(isCacheEntryKeyValid(ck),
89  "ListOfDependents::contains(): invalid cache key (%d,%d).",
90  (int)ck.first, (int)ck.second);
91  return std::find(cbegin(), cend(), ck) != cend();
92  }
93 
94  // When a cache entry is allocated or copied it has to add itself to the
95  // dependency lists for its prerequisites. If it is already on the list then
96  // we screwed up the bookkeeping somehow. We don't expect to do this often
97  // so we'll leave the assert in Release builds.
98  void addDependent(const CacheEntryKey& ck) {
99  SimTK_ASSERT2_ALWAYS(!contains(ck),
100  "ListOfDependents::addDependent(): Cache entry (%d,%d) was already "
101  "present in the list of dependents.",
102  (int)ck.first, (int)ck.second);
103  m_dependents.push_back(ck);
104  }
105 
106  // Invalidates dependents (mutable).
107  inline void notePrerequisiteChange(const StateImpl& stateImpl) const;
108 
109  // When a cache entry gets de-allocated, it will call this method to
110  // remove itself from any lists of dependents it is on. This doesn't happen
111  // often so keep the asserts in Release.
112  void removeDependent(const CacheEntryKey& ck) {
113  SimTK_ASSERT2_ALWAYS(isCacheEntryKeyValid(ck),
114  "ListOfDependents::removeDependent(): invalid cache key (%d,%d).",
115  (int)ck.first, (int)ck.second);
116 
117  auto p = std::find(begin(), end(), ck);
118  SimTK_ASSERT2_ALWAYS(p!=m_dependents.end(),
119  "ListOfDependents::removeDependent(): Cache entry (%d,%d) to be "
120  "removed should have been present in the list of dependents.",
121  (int)ck.first, (int)ck.second);
122  m_dependents.erase(p);
123  }
124 
125  static bool isCacheEntryKeyValid(const CacheEntryKey& ck)
126  { return ck.first.isValid() && ck.second.isValid(); }
127 
128 private:
129  CacheList m_dependents;
130 };
131 
132 // These local classes
133 // DiscreteVarInfo
134 // CacheVarInfo
135 // EventInfo
136 // contain the information needed for each discrete variable and cache entry,
137 // including a reference to its current value and everything needed to
138 // understand its dependencies, as well as information for each Event allocated
139 // by a Subsystem.
140 //
141 // These are intended as elements in an allocation stack as described above,
142 // so it is expected that they will get reaallocated, copied, and destructed
143 // during allocation as the Array_ gets resized from time to time. However, the
144 // discrete variable and cache entry *values* must remain in a fixed location in
145 // memory once allocated, because callers are permitted to retain references to
146 // these values once they have been allocated. So pointers to the AbstractValues
147 // are kept in these objects, and only the pointers are copied around. That
148 // means the actual value object will not be deleted by the destructor; be sure
149 // to do that explicitly in the higher-level destructor or you'll have a nasty
150 // leak.
151 
152 //==============================================================================
153 // DISCRETE VAR INFO
154 //==============================================================================
155 class DiscreteVarInfo {
156 public:
157  DiscreteVarInfo() = default;
158 
159  DiscreteVarInfo(Stage allocation, Stage invalidated, AbstractValue* v)
160  : m_allocationStage(allocation), m_invalidatedStage(invalidated),
161  m_value(v)
162  { assert(isReasonable()); }
163 
164  // Default copy constructor, copy assignment, destructor are shallow.
165 
166  // Use this to make this entry contain a *copy* of the source value.
167  // If the destination already has a value, the new value must be
168  // assignment compatible.
169  DiscreteVarInfo& deepAssign(const DiscreteVarInfo& src) {
170  *this = src; // copy assignment forgets dependents
171  return *this;
172  }
173 
174  // For use in the containing class's destructor.
175  void deepDestruct(StateImpl&) {
176  m_value.reset();
177  }
178 
179  const Stage& getAllocationStage() const {return m_allocationStage;}
180 
181  // Exchange value pointers (should be from this dv's update cache entry).
182  void swapValue(Real updTime, ClonePtr<AbstractValue>& other)
183  { m_value.swap(other); m_timeLastUpdated=updTime; }
184 
185  const AbstractValue& getValue() const {assert(m_value); return *m_value;}
186 
187  // Whenever we hand out this variables value for write access we update
188  // the value version, note the update time, and notify any dependents that
189  // they are now invalid with respect to this variable's value.
190  AbstractValue& updValue(const StateImpl& stateImpl, Real updTime) {
191  assert(m_value);
192  ++m_valueVersion;
193  m_timeLastUpdated=updTime;
194  m_dependents.notePrerequisiteChange(stateImpl);
195  return *m_value;
196  }
197  ValueVersion getValueVersion() const {return m_valueVersion;}
198  Real getTimeLastUpdated() const
199  { assert(m_value); return m_timeLastUpdated; }
200 
201  const Stage& getInvalidatedStage() const {return m_invalidatedStage;}
202  CacheEntryIndex getAutoUpdateEntry() const {return m_autoUpdateEntry;}
203  void setAutoUpdateEntry(CacheEntryIndex cx) {m_autoUpdateEntry = cx;}
204 
205  const ListOfDependents& getDependents() const {return m_dependents;}
206  ListOfDependents& updDependents() {return m_dependents;}
207 
208 private:
209  // These are fixed at construction.
210  Stage m_allocationStage;
211  Stage m_invalidatedStage;
212  CacheEntryIndex m_autoUpdateEntry; // if auto update var
213 
214  // This is the list of cache entries that have declared explicitly that
215  // this variable is a prerequisite. This list gets forgotten if we
216  // copy this variable; copied cache entries (if any) have to re-register
217  // themselves.
218  ResetOnCopy<ListOfDependents> m_dependents;
219 
220  // These change at run time.
221  ClonePtr<AbstractValue> m_value;
222  ValueVersion m_valueVersion{1};
223  Real m_timeLastUpdated{NaN};
224 
225  bool isReasonable() const
226  { return (m_allocationStage==Stage::Topology
227  || m_allocationStage==Stage::Model)
228  && (m_invalidatedStage > m_allocationStage)
229  && (m_value != nullptr); }
230 };
231 
232 
233 
234 //==============================================================================
235 // CACHE ENTRY INFO
236 //==============================================================================
237 /* A cache entry holds an AbstractValue that is computed ("realized") from the
238 values of state variables. Its primary purpose is to efficiently and flawlessly
239 keep track of whether that value is up to date with respect to the state
240 variables from which it is realized. For efficiency, there are two
241 mechanisms used: the "computation stage" system provides coarse granularity
242 that is sufficient for most cache entries. When additional finesse is needed,
243 a cache entry may also specify a set of prerequisite state variables or other
244 cache entries that it depends on.
245 
246 A cache entry specifies a "depends-on" stage. If its owning subsystem's stage
247 is below that, the cache entry's value *cannot* be valid. It may optionally
248 specify a "computed-by" stage; if its subsystem's stage has reached that then
249 the cache entry's value is *guaranteed* to be valid. If the stage is in
250 between, then validity is determined as follows:
251  - compare the saved depends-on stage version with the curent version; if they
252  don't match then the cache entry is not valid
253  - then, the cache entry is valid if its up-to-date-with-prerequisites
254  flag is set and invalid otherwise.
255 
256 Cache entries also have an allocation stage (Topology, Model, or Instance)
257 and must be de-allocated if that stage is invalidated.
258 */
259 class SimTK_SimTKCOMMON_EXPORT CacheEntryInfo {
260 public:
261  CacheEntryInfo() {}
262 
263  CacheEntryInfo(const CacheEntryKey& myKey,
264  Stage allocation, Stage dependsOn, Stage computedBy,
265  AbstractValue* value)
266  : m_myKey(myKey),
267  m_allocationStage(allocation), m_dependsOnStage(dependsOn),
268  m_computedByStage(computedBy), m_value(value)
269  { assert(isReasonable()); }
270 
271  CacheEntryInfo& setPrerequisiteQ() {m_qIsPrerequisite = true; return *this;}
272  CacheEntryInfo& setPrerequisiteU() {m_uIsPrerequisite = true; return *this;}
273  CacheEntryInfo& setPrerequisiteZ() {m_zIsPrerequisite = true; return *this;}
274 
275  CacheEntryInfo& setPrerequisite(const DiscreteVarKey& dk) {
276  SimTK_ASSERT2(!isPrerequisite(dk),
277  "CacheEntryInfo::setPrerequisite(): "
278  "Discrete variable (%d,%d) is already on the list.",
279  (int)dk.first, (int)dk.second);
280  m_discreteVarPrerequisites.push_back(dk);
281  return *this;
282  }
283 
284  CacheEntryInfo& setPrerequisite(const CacheEntryKey& ck) {
285  SimTK_ASSERT2(!isPrerequisite(ck),
286  "CacheEntryInfo::setPrerequisite(): "
287  "Cache entry (%d,%d) is already on the list.",
288  (int)ck.first, (int)ck.second);
289  m_cacheEntryPrerequisites.push_back(ck);
290  return *this;
291  }
292 
293  // Add this cache entry to the lists of dependents of its prerequisites in
294  // the containing State. If there are no prerequisites the "is up to date
295  // with prerequisites" flag is set true, otherwise it is set false and
296  // requires an explicit markAsUpToDate() call to be valid.
297  void registerWithPrerequisites(StateImpl&);
298 
299  // Remove this cache entry from the lists of dependents of its prerequisites
300  // in the containing State. This is invoked on destruction. It is possible
301  // that a prerequisite has already been destructed; that's not an error
302  // because we don't attempt to destruct dependents before their
303  // prerequisites when un-allocating things.
304  void unregisterWithPrerequisites(StateImpl&) const;
305 
306  // This method must be *very* fast and inline-able; it is called every time
307  // a cache entry is accessed to look at its value -- and that is a lot!
308  SimTK_FORCE_INLINE bool isUpToDate(const StateImpl&) const;
309 
310  // This must be called when cache entry evaluation is complete to record
311  // the depends-on version and set the "is up to date with prerequisites"
312  // flag. A cache entry with no prerequisites need not call this as long
313  // as it isn't accessed until its computed-by stage.
314  inline void markAsUpToDate(const StateImpl&);
315 
316  // We know this cache entry is illegally out of date from an earlier
317  // call to isUpToDate(). Now we can afford to spend some time making
318  // a nice error message.
319  void throwHelpfulOutOfDateMessage(const StateImpl&,
320  const char* funcName) const;
321 
322 
323  // This affects only the explicit "last computed" flags which do not fully
324  // determine whether the value is current; see isUpToDate() above.
325  // If a cache entry has a computed-by stage, you have to invalidate that
326  // stage in its subsystem also if you want to ensure it is invalid.
327  void invalidate(const StateImpl& stateImpl) {
328  m_dependsOnVersionWhenLastComputed = StageVersion(0);
329  m_isUpToDateWithPrerequisites = false;
330  ++m_valueVersion;
331  m_dependents.notePrerequisiteChange(stateImpl);
332  }
333 
334  // Use this to make this entry contain a *copy* of the source value.
335  CacheEntryInfo& deepAssign(const CacheEntryInfo& src) {
336  *this = src; // copy assignment forgets dependents
337  return *this;
338  }
339 
340  // For use in the containing class's destructor.
341  void deepDestruct(StateImpl& stateImpl) {
342  m_value.reset(); // destruct the AbstractValue
343  unregisterWithPrerequisites(stateImpl);
344  }
345 
346  const Stage& getAllocationStage() const {return m_allocationStage;}
347 
348  // Exchange values with a discrete variable (presumably this
349  // cache entry has been determined to be that variable's update
350  // entry but we're not checking here).
351  void swapValue(Real updTime, DiscreteVarInfo& dv)
352  { dv.swapValue(updTime, m_value); }
353 
354  const AbstractValue& getValue() const {assert(m_value); return *m_value;}
355 
356  // Merely handing out the cache entry's value with write access does not
357  // trigger invalidation of dependents. (Maybe it should, but currently it
358  // gets done often with no intent to modify, esp. by SBStateDigest.)
359  // So be sure that the cache entry gets invalidated first either by an
360  // explicit prerequisite change notification, or because the depends-on
361  // stage got invalidated.
362  AbstractValue& updValue(const StateImpl& stateImpl) {
363  assert(m_value);
364  return *m_value;
365  }
366  ValueVersion getValueVersion() const {return m_valueVersion;}
367 
368  // Recall the stage version number of this CacheEntry's owner Subsystem's
369  // depends-on stage as it was at last realization of this entry.
370  StageVersion getDependsOnVersionWhenLastComputed() const
371  { return m_dependsOnVersionWhenLastComputed; }
372 
373  const Stage& getDependsOnStage() const {return m_dependsOnStage;}
374  const Stage& getComputedByStage() const {return m_computedByStage;}
375  DiscreteVariableIndex getAssociatedVar() const {return m_associatedVar;}
376  void setAssociatedVar(DiscreteVariableIndex dx) {m_associatedVar=dx;}
377 
378  bool isQPrerequisite() const {return m_qIsPrerequisite;}
379  bool isUPrerequisite() const {return m_uIsPrerequisite;}
380  bool isZPrerequisite() const {return m_zIsPrerequisite;}
381  bool isPrerequisite(const DiscreteVarKey& dk) const {
382  return std::find(m_discreteVarPrerequisites.cbegin(),
383  m_discreteVarPrerequisites.cend(), dk)
384  != m_discreteVarPrerequisites.cend();
385  }
386  bool isPrerequisite(const CacheEntryKey& ck) const {
387  return std::find(m_cacheEntryPrerequisites.cbegin(),
388  m_cacheEntryPrerequisites.cend(), ck)
389  != m_cacheEntryPrerequisites.cend();
390  }
391 
392  void recordPrerequisiteVersions(const StateImpl&);
393  void validatePrerequisiteVersions(const StateImpl&) const;
394 
395  const ListOfDependents& getDependents() const {return m_dependents;}
396  ListOfDependents& updDependents() {return m_dependents;}
397 
398 private:
399  // These are fixed at construction.
400  CacheEntryKey m_myKey; // location in State
401  Stage m_allocationStage; // lifetime
402  Stage m_dependsOnStage; // can't be valid until here
403  Stage m_computedByStage; // must be valid after here
404  DiscreteVariableIndex m_associatedVar; // if an auto-update entry
405 
406  // Dependencies in addition to depends-on stage dependence. These are set
407  // during allocation and used during de-allocation and copying.
408  bool m_qIsPrerequisite{false},
409  m_uIsPrerequisite{false},
410  m_zIsPrerequisite{false};
411  Array_<DiscreteVarKey> m_discreteVarPrerequisites;
412  Array_<CacheEntryKey> m_cacheEntryPrerequisites;
413 
414  // This is set when other cache entries dependent on this one are
415  // constructed. Each of these is invalidated whenever this entry is
416  // explicitly invalidated (not when dependsOn stage gets invalidated).
417  // This is not copied if the cache entry is copied; copied dependents
418  // if any have to re-register themselves.
419  ResetOnCopy<ListOfDependents> m_dependents;
420 
421  // These change at run time. Initially assume we don't have any
422  // prerequisites so we are up to date with respect to them. We'll change
423  // the initial value to false in registerWithPrerequisites() if there
424  // are some.
425  ClonePtr<AbstractValue> m_value;
426  ValueVersion m_valueVersion{1};
427  StageVersion m_dependsOnVersionWhenLastComputed{0};
428  bool m_isUpToDateWithPrerequisites{true};
429 
430 
431  // These are just for debugging. At the time this is marked valid version
432  // numbers are recorded for every prerequisite. Then the "is valid" code
433  // can double check that the "is up to date with prerequisites" flag is
434  // set correctly.
435  ValueVersion m_qVersion{0}, m_uVersion{0}, m_zVersion{0};
436  Array_<ValueVersion> m_discreteVarVersions;
437  Array_<ValueVersion> m_cacheEntryVersions;
438 
439  bool isReasonable() const {
440  return ( m_allocationStage==Stage::Topology
441  || m_allocationStage==Stage::Model
442  || m_allocationStage==Stage::Instance)
443  && (m_computedByStage >= m_dependsOnStage)
444  && (m_value != nullptr)
445  && (m_dependsOnVersionWhenLastComputed >= 0)
446  && (ListOfDependents::isCacheEntryKeyValid(m_myKey));
447  }
448 };
449 
450 
451 
452 //==============================================================================
453 // TRIGGER INFO
454 //==============================================================================
455 class TriggerInfo {
456 public:
457  TriggerInfo()
458  : allocationStage(Stage::Empty), firstIndex(-1), nslots(0) {}
459 
460  TriggerInfo(Stage allocation, int index, int n)
461  : allocationStage(allocation), firstIndex(index), nslots(n)
462  { assert(isReasonable()); assert(n>0);}
463 
464  // Default copy constructor, copy assignment, destructor are fine since
465  // there is no heap object owned here.
466 
467  int getFirstIndex() const {return firstIndex;}
468  int getNumSlots() const {return nslots;}
469 
470  // These the the "virtual" methods required by template methods elsewhere.
471  TriggerInfo& deepAssign(const TriggerInfo& src)
472  { return operator=(src); }
473  void deepDestruct(StateImpl&) {}
474  const Stage& getAllocationStage() const {return allocationStage;}
475 private:
476  // These are fixed at construction.
477  Stage allocationStage;
478  int firstIndex;
479  int nslots;
480 
481  bool isReasonable() const {
482  return ( allocationStage==Stage::Topology
483  || allocationStage==Stage::Model
484  || allocationStage==Stage::Instance);
485  }
486 };
487 
488 
489 
490 //==============================================================================
491 // CONTINUOUS VAR INFO
492 //==============================================================================
493 // Used for q, u, and z (in separate stacks).
494 // These accumulate default values for this subsystem's use of shared
495 // global state variables. After the System is advanced to Stage::Model,
496 // the state will allocate those globals and copy the initial
497 // values stored here into them. Some of these are allocated at Topology
498 // stage, and some at Model stage. If Model stage is invalidated, variables
499 // allocated then are forgotten while the ones allocated at Topology stage
500 // remain.
501 class ContinuousVarInfo {
502 public:
503  ContinuousVarInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
504 
505  ContinuousVarInfo(Stage allocation,
506  int index, // QIndex, UIndex, or ZIndex
507  const Vector& initVals,
508  const Vector& varWeights=Vector())
509  : allocationStage(allocation), firstIndex(index), initialValues(initVals)
510  { assert(isReasonable());
511  assert(varWeights.size()==0 || varWeights.size()==initVals.size());
512  assert(weightsAreOK(varWeights));
513  if (varWeights.size()) weights=varWeights;
514  else weights=Vector(initVals.size(), Real(1));
515  }
516 
517  int getFirstIndex() const {return firstIndex;}
518  int getNumVars() const {return initialValues.size();}
519  const Vector& getInitialValues() const {return initialValues;}
520  const Vector& getWeights() const {return weights;}
521 
522  // Default copy constructor, copy assignment, destructor are fine since
523  // there is no heap object owned here.
524 
525  // These the the "virtual" methods required by template methods elsewhere.
526  ContinuousVarInfo& deepAssign(const ContinuousVarInfo& src)
527  { return operator=(src); }
528  void deepDestruct(StateImpl&) {}
529  const Stage& getAllocationStage() const {return allocationStage;}
530 private:
531  // These are fixed at construction.
532  Stage allocationStage;
533  int firstIndex; // a QIndex, UIndex, or ZIndex
534  Vector initialValues;
535  Vector weights; // only used for u and z
536 
537  static bool weightsAreOK(const Vector& wts) {
538  for (int i=0; i<wts.size(); ++i)
539  if (wts[i] <= 0) return false;
540  return true;
541  }
542 
543  bool isReasonable() const {
544  return ( allocationStage==Stage::Topology
545  || allocationStage==Stage::Model);
546  }
547 };
548 
549 //==============================================================================
550 // CONSTRAINT ERR INFO
551 //==============================================================================
552 // Used for qerr, uerr, and udoterr.
553 class ConstraintErrInfo {
554 public:
555  ConstraintErrInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
556 
557  ConstraintErrInfo(Stage allocation,
558  int index, // QErr, UErr, or UDotErrIndex
559  int nerr,
560  const Vector& varWeights=Vector())
561  : allocationStage(allocation), firstIndex(index)
562  { assert(isReasonable());
563  assert(varWeights.size()==0 || varWeights.size()==nerr);
564  assert(weightsAreOK(varWeights));
565  if (varWeights.size()) weights=varWeights;
566  else weights=Vector(nerr, Real(1));
567  }
568 
569  int getFirstIndex() const {return firstIndex;}
570  int getNumErrs() const {return weights.size();}
571  const Vector& getWeights() const {return weights;}
572 
573  // Default copy constructor, copy assignment, destructor are fine since
574  // there is no heap object owned here.
575 
576  // These the the "virtual" methods required by template methods elsewhere.
577  ConstraintErrInfo& deepAssign(const ConstraintErrInfo& src)
578  { return operator=(src); }
579  void deepDestruct(StateImpl&) {}
580  const Stage& getAllocationStage() const {return allocationStage;}
581 private:
582  // These are fixed at construction.
583  Stage allocationStage;
584  int firstIndex; // a QErrIndex, UErrIndex, or UDotErrIndex
585  Vector weights; // only used for u and z
586 
587  static bool weightsAreOK(const Vector& wts) {
588  for (int i=0; i<wts.size(); ++i)
589  if (wts[i] <= 0) return false;
590  return true;
591  }
592 
593  bool isReasonable() const {
594  return ( allocationStage==Stage::Topology
595  || allocationStage==Stage::Model
596  || allocationStage==Stage::Instance);
597  }
598 };
599 
600 
601 
602 //==============================================================================
603 // PER SUBSYSTEM INFO
604 //==============================================================================
605 // This internal utility class is used to capture all the information needed for
606 // a single subsystem within the StateImpl.
607 class SimTK_SimTKCOMMON_EXPORT PerSubsystemInfo {
608 public:
609  explicit PerSubsystemInfo(StateImpl& stateImpl,
610  const String& n="", const String& v="")
611  : m_stateImpl(stateImpl), name(n), version(v)
612  { initialize(); }
613 
614  // Everything will properly clean itself up.
615  ~PerSubsystemInfo() {
616  }
617 
618  // Copy constructor copies all variables but cache only through
619  // Instance stage. Note that this must be done in conjunction with
620  // copying the whole state or our global resource indices will
621  // be nonsense. Also, dependency lists are cleared and cache entries
622  // must re-register in the new State after all subsystems have been copied.
623  // (Dependencies can be cross-subsystem.)
624  // The back reference to the StateImpl is null after this and must be
625  // set to reference the new StateImpl.
626  PerSubsystemInfo(const PerSubsystemInfo& src) {
627  initialize();
628  copyFrom(src, Stage::Instance);
629  }
630 
631  // The back reference to the containing StateImpl remains unchanged.
632  PerSubsystemInfo& operator=(const PerSubsystemInfo& src) {
633  // destination is already initialized; copyFrom() will try
634  // to reuse space and will properly clean up unused stuff
635  if (&src != this)
636  copyFrom(src, Stage::Instance);
637  return *this;
638  }
639 
640  // Back up to the stage just before g if this subsystem thinks
641  // it is already at g or beyond. Note that we may be backing up
642  // over many stages here. Careful: invalidating the stage
643  // for a subsystem must also invalidate the same stage for all
644  // the other subsystems and the system as a whole but we don't
645  // take care of that here. Also, you can't invalidate Stage::Empty.
646  void invalidateStageJustThisSubsystem(Stage g) {
647  assert(g > Stage::Empty);
648  restoreToStage(g.prev());
649  }
650 
651  // Advance from stage g-1 to stage g. This is called at the end
652  // of realize(g). You can't use this to "advance" to Stage::Empty.
653  // It is a fatal error if the current stage isn't g-1.
654  void advanceToStage(Stage g) const {
655  assert(g > Stage::Empty);
656  assert(currentStage == g.prev());
657 
658  // This validates whatever the current version number is of Stage g.
659  currentStage = g;
660  }
661 
662 
663  void clearReferencesToModelStageGlobals() {
664  qstart.invalidate(); ustart.invalidate();
665  zstart.invalidate();
666  q.clear(); u.clear(); z.clear();
667  uWeights.clear(); zWeights.clear();
668  qdot.clear(); udot.clear(); zdot.clear(); qdotdot.clear();
669  }
670 
671  void clearReferencesToInstanceStageGlobals() {
672  // These are late-allocated state variables.
673  qerrWeights.clear(); uerrWeights.clear();
674 
675  // These are all mutable cache entries.
676  qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
677  qerr.clear();uerr.clear();udoterr.clear();multipliers.clear();
678 
679  for (int j=0; j<Stage::NValid; ++j) {
680  triggerstart[j].invalidate();
681  triggers[j].clear();
682  }
683  }
684 
685  QIndex getNextQIndex() const {
686  if (q_info.empty()) return QIndex(0);
687  const ContinuousVarInfo& last = q_info.back();
688  return QIndex(last.getFirstIndex()+last.getNumVars());
689  }
690  UIndex getNextUIndex() const {
691  if (uInfo.empty()) return UIndex(0);
692  const ContinuousVarInfo& last = uInfo.back();
693  return UIndex(last.getFirstIndex()+last.getNumVars());
694  }
695  ZIndex getNextZIndex() const {
696  if (zInfo.empty()) return ZIndex(0);
697  const ContinuousVarInfo& last = zInfo.back();
698  return ZIndex(last.getFirstIndex()+last.getNumVars());
699  }
700 
701  QErrIndex getNextQErrIndex() const {
702  if (qerrInfo.empty()) return QErrIndex(0);
703  const ConstraintErrInfo& last = qerrInfo.back();
704  return QErrIndex(last.getFirstIndex()+last.getNumErrs());
705  }
706  UErrIndex getNextUErrIndex() const {
707  if (uerrInfo.empty()) return UErrIndex(0);
708  const ConstraintErrInfo& last = uerrInfo.back();
709  return UErrIndex(last.getFirstIndex()+last.getNumErrs());
710  }
711  UDotErrIndex getNextUDotErrIndex() const {
712  if (udoterrInfo.empty()) return UDotErrIndex(0);
713  const ConstraintErrInfo& last = udoterrInfo.back();
714  return UDotErrIndex(last.getFirstIndex()+last.getNumErrs());
715  }
716  DiscreteVariableIndex getNextDiscreteVariableIndex() const {
717  return DiscreteVariableIndex(discreteInfo.size());
718  }
719  CacheEntryIndex getNextCacheEntryIndex() const {
720  return CacheEntryIndex(cacheInfo.size());
721  }
722  EventTriggerByStageIndex getNextEventTriggerByStageIndex(Stage g) const {
723  if (triggerInfo[g].empty()) return EventTriggerByStageIndex(0);
724  const TriggerInfo& last = triggerInfo[g].back();
726  (last.getFirstIndex()+last.getNumSlots());
727  }
728 
729  bool hasDiscreteVar(DiscreteVariableIndex index) const {
730  return index < (int)discreteInfo.size();
731  }
732 
733 
734  SimTK_FORCE_INLINE const DiscreteVarInfo&
735  getDiscreteVarInfo(DiscreteVariableIndex index) const {
736  SimTK_INDEXCHECK(index,(int)discreteInfo.size(),
737  "PerSubsystemInfo::getDiscreteVarInfo()");
738  return discreteInfo[index];
739  }
740 
741  SimTK_FORCE_INLINE DiscreteVarInfo&
742  updDiscreteVarInfo(DiscreteVariableIndex index) {
743  SimTK_INDEXCHECK(index,(int)discreteInfo.size(),
744  "PerSubsystemInfo::updDiscreteVarInfo()");
745  return discreteInfo[index];
746  }
747 
748 
749  bool hasCacheEntry(CacheEntryIndex index) const {
750  return index < (int)cacheInfo.size();
751  }
752 
753  SimTK_FORCE_INLINE const CacheEntryInfo&
754  getCacheEntryInfo(CacheEntryIndex index) const {
755  SimTK_INDEXCHECK(index,(int)cacheInfo.size(),
756  "PerSubsystemInfo::getCacheEntryInfo()");
757  return cacheInfo[index];
758  }
759 
760  SimTK_FORCE_INLINE CacheEntryInfo&
761  updCacheEntryInfo(CacheEntryIndex index) const {
762  SimTK_INDEXCHECK(index,(int)cacheInfo.size(),
763  "PerSubsystemInfo::updCacheEntryInfo()");
764  return cacheInfo[index]; // mutable
765  }
766 
767  SimTK_FORCE_INLINE Stage getCurrentStage() const {return currentStage;}
768  SimTK_FORCE_INLINE StageVersion getStageVersion(Stage g) const
769  { return stageVersions[g]; }
770 
771 private:
772 friend class StateImpl;
773  ReferencePtr<StateImpl> m_stateImpl; // container of this subsystem
774 
775  String name;
776  String version;
777 
778  // DEFINITIONS //
779 
780  // State variables (continuous or discrete) can be defined (allocated)
781  // during realization of Topology or Model stages. Cache entries,
782  // constraint error slots, and event trigger slots can be defined during
783  // realization of Topology, Model, or Instance stages. No further
784  // allocations are allowed. Then, when one of these stages is invalidated,
785  // all the definitions that occurred during realization of that stage must
786  // be forgotten.
787  //
788  // To do that the allocation entries are stored in arrays which are really
789  // stacks, with definitions pushed onto the ends as the stage is advanced
790  // and popped off the ends as the stage is reduced.
791 
792  // Topology and Model stage definitions.
793  Array_<ContinuousVarInfo> q_info, uInfo, zInfo;//qInfo -> q_info Qt definition conflict
794  Array_<DiscreteVarInfo> discreteInfo;
795 
796  // Topology, Model, and Instance stage definitions.
797  mutable Array_<ConstraintErrInfo> qerrInfo, uerrInfo, udoterrInfo;
798  mutable Array_<TriggerInfo> triggerInfo[Stage::NValid];
799  mutable Array_<CacheEntryInfo> cacheInfo;
800 
801  // GLOBAL RESOURCE ALLOCATIONS //
802 
803  // These are our own private views into partitions of the global
804  // state and cache entries of the same names. The State will assign
805  // contiguous blocks to this subsystem when the *System* stage is raised
806  // to Model or Instance stage, and they are invalidated whenever that
807  // stage is invalidated. The starting indices are filled in here at
808  // the time the views are built.
809 
810  // Model stage global resources and views into them.
811  SystemQIndex qstart;
812  SystemUIndex ustart;
813  SystemZIndex zstart;
814  Vector q, u, z;
815  Vector uWeights, zWeights;
816 
817  mutable Vector qdot, udot, zdot, qdotdot;
818 
819  // Instance stage global resources and views into them.
820  Vector qerrWeights, uerrWeights;
821 
822  // Note that multipliers just use the same indices as udoterr.
823  mutable SystemQErrIndex qerrstart;
824  mutable SystemUErrIndex uerrstart;
825  mutable SystemUDotErrIndex udoterrstart;
826  mutable SystemEventTriggerByStageIndex triggerstart[Stage::NValid];
827 
828  mutable Vector qerr, uerr;
829 
830  mutable Vector udoterr, multipliers; // same size and partioning
831  mutable Vector triggers[Stage::NValid];
832 
833  // The currentStage is the highest stage of this subsystem that is valid,
834  // meaning that it has been realized since the last change to any variable
835  // that might affect it. All stages less than currentStage are also valid,
836  // and all higher stages are invalid.
837  // Each stage has a "stage version" which is like a serial number that is
838  // bumped every time the stage is invalidated by a variable change. Cache
839  // entries that are calculated from a particular stage version can record
840  // the version number to allow a quick check later -- if the current version
841  // of a cache entry's dependsOn stage is different than the one stored with
842  // the cache entry, then that cache entry cannot be valid.
843  mutable Stage currentStage;
844  mutable StageVersion stageVersions[Stage::NValid];
845 
846 private:
847  // This is for use in constructors and for resetting an existing State into
848  // its just-constructed condition.
849  void initialize() {
850  clearAllStacks();
851  qstart.invalidate();ustart.invalidate();zstart.invalidate();
852  qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
853  for (int j=0; j<Stage::NValid; ++j) {
854  triggerstart[j].invalidate();
855  stageVersions[j] = 1; // never 0
856  }
857  currentStage = Stage::Empty;
858  }
859 
860  // Manage allocation stacks.
861 
862  void clearContinuousVars();
863  void clearConstraintErrs();
864  void clearDiscreteVars();
865  void clearEventTriggers(int g);
866  void clearCache();
867 
868  void clearAllStacks();
869 
870  void popContinuousVarsBackToStage(const Stage& g);
871  void popDiscreteVarsBackToStage(const Stage& g);
872  void popConstraintErrsBackToStage(const Stage& g);
873  void popCacheBackToStage(const Stage& g);
874  void popEventTriggersBackToStage(const Stage& g);
875 
876  void popAllStacksBackToStage(const Stage& g);
877 
878  // Call once each for q_info, uInfo, zInfo.
879  void copyContinuousVarInfoThroughStage
880  (const Array_<ContinuousVarInfo>& src, const Stage& g,
881  Array_<ContinuousVarInfo>& dest);
882 
883  void copyDiscreteVarsThroughStage
884  (const Array_<DiscreteVarInfo>& src, const Stage& g);
885 
886  // Call once each for qerrInfo, uerrInfo, udoterrInfo.
887  void copyConstraintErrInfoThroughStage
888  (const Array_<ConstraintErrInfo>& src, const Stage& g,
889  Array_<ConstraintErrInfo>& dest);
890 
891  void copyCacheThroughStage
892  (const Array_<CacheEntryInfo>& src, const Stage& g);
893 
894  void copyEventsThroughStage
895  (const Array_<TriggerInfo>& src, const Stage& g,
896  Array_<TriggerInfo>& dest);
897 
898  void copyAllStacksThroughStage(const PerSubsystemInfo& src, const Stage& g);
899 
900  // Restore this subsystem to the way it last was at realize(g) for a given
901  // Stage g; that is, invalidate all stages > g. Allocations will be
902  // forgotten as Instance, Model, and Topology stages are invalidated.
903  void restoreToStage(Stage g);
904 
905  // Utility which makes "this" a copy of the source subsystem exactly as it
906  // was after being realized to stage maxStage. If maxStage >= Model then
907  // all the subsystem-private state variables will be copied, but only
908  // cached computations up through maxStage come through. We clear
909  // our references to global variables regardless -- those will have to
910  // be repaired at the System (State global) level.
911  void copyFrom(const PerSubsystemInfo& src, Stage maxStage);
912 
913  // Stack methods; see implementation for explanation.
914  template <class T>
915  void clearAllocationStack(Array_<T>& stack);
916  template <class T>
917  void resizeAllocationStack(Array_<T>& stack, int newSize);
918  template <class T>
919  void popAllocationStackBackToStage(Array_<T>& stack, const Stage&);
920  template <class T>
921  void copyAllocationStackThroughStage(Array_<T>& stack,
922  const Array_<T>& src, const Stage&);
923 };
924 
925 
926 //==============================================================================
927 // STATE IMPL
928 //==============================================================================
929 
930 class SimTK_SimTKCOMMON_EXPORT StateImpl {
931 public:
932  // For stages, the version is always the one that will be there when
933  // the stage is next realized, so the constructor initializes them to
934  // the first valid stage version, 1. Value versions for state variables
935  // and cache entries are initialized to 0 (not valid) and bumped when
936  // marked valid.
937  StateImpl() {
938  for (int i=0; i < Stage::NValid; ++i)
939  systemStageVersions[i] = 1; // 0 is not legal
940  }
941 
942  // We'll do the copy constructor and assignment explicitly here
943  // to get tight control over what's allowed.
944  StateImpl(const StateImpl& src);
945 
946  StateImpl& operator=(const StateImpl& src);
947 
948  ~StateImpl() {}
949 
950  // Copies all the variables but not the cache.
951  StateImpl* clone() const {return new StateImpl(*this);}
952 
953  const Stage& getSystemStage() const {return currentSystemStage;}
954  Stage& updSystemStage() const {return currentSystemStage;} // mutable
955 
956 
957  SimTK_FORCE_INLINE const PerSubsystemInfo&
958  getSubsystem(SubsystemIndex subx) const {
959  SimTK_INDEXCHECK(subx, (int)subsystems.size(),
960  "StateImpl::getSubsystem()");
961  return subsystems[subx];
962  }
963 
964  SimTK_FORCE_INLINE PerSubsystemInfo&
965  updSubsystem(SubsystemIndex subx) {
966  SimTK_INDEXCHECK(subx, (int)subsystems.size(),
967  "StateImpl::updSubsystem()");
968  return subsystems[subx];
969  }
970 
971  const Stage& getSubsystemStage(int subsystem) const {
972  return subsystems[subsystem].currentStage;
973  }
974  Stage& updSubsystemStage(int subsystem) const {
975  return subsystems[subsystem].currentStage; // mutable
976  }
977 
978  const StageVersion* getSubsystemStageVersions(int subsystem) const {
979  return subsystems[subsystem].stageVersions;
980  }
981 
982  // Back up the System stage just before stg if it thinks
983  // it is already at stg or beyond. Note that we may be backing up
984  // over many stages here. Careful: invalidating the stage
985  // for the system must also invalidate the same stage for all
986  // the subsystems (because we trash the shared resource pool
987  // here if we back up earlier than Stage::Model) but we don't
988  // take care of that here. Also, you can't invalidate Stage::Empty.
989  void invalidateJustSystemStage(Stage stg);
990 
991  // Advance the System stage from stg-1 to stg. It is a fatal error if
992  // we're not already at stg-1, and you can't advance to Stage::Empty.
993  // Also, you can't advance the system to stg unless ALL subsystems have
994  // already gotten there.
995  void advanceSystemToStage(Stage stg) const;
996 
997  void setNumSubsystems(int nSubs) {
998  assert(nSubs >= 0);
999  subsystems.clear();
1000  for (int i=0; i < nSubs; ++i)
1001  subsystems.emplace_back(*this); // set backpointer
1002  }
1003 
1004  void initializeSubsystem
1005  (SubsystemIndex i, const String& name, const String& version) {
1006  updSubsystem(i).name = name;
1007  updSubsystem(i).version = version;
1008  }
1009 
1010  SubsystemIndex addSubsystem(const String& name, const String& version) {
1011  const SubsystemIndex sx(subsystems.size());
1012  subsystems.emplace_back(*this, name, version);
1013  return sx;
1014  }
1015 
1016  int getNumSubsystems() const {return (int)subsystems.size();}
1017 
1018  const String& getSubsystemName(SubsystemIndex subsys) const {
1019  return subsystems[subsys].name;
1020  }
1021  const String& getSubsystemVersion(SubsystemIndex subsys) const {
1022  return subsystems[subsys].version;
1023  }
1024 
1025  // Make sure the stage is no higher than g-1 for *any* subsystem and
1026  // hence for the system stage also. TODO: this should be more selective.
1027  void invalidateAll(Stage g) {
1028  invalidateJustSystemStage(g);
1029  for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
1030  subsystems[i].invalidateStageJustThisSubsystem(g);
1031  }
1032 
1033  // Make sure the stage is no higher than g-1 for *any* subsystem and
1034  // hence for the system stage also. Same as invalidateAll() except this
1035  // requires only const access and can't be used for g below Instance.
1036  void invalidateAllCacheAtOrAbove(Stage g) const {
1038  "StateImpl::invalidateAllCacheAtOrAbove()");
1039 
1040  // We promise not to hurt this State; get non-const access just so
1041  // we can call these methods.
1042  StateImpl* mthis = const_cast<StateImpl*>(this);
1043  mthis->invalidateJustSystemStage(g);
1044  for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
1045  mthis->subsystems[i].invalidateStageJustThisSubsystem(g);
1046  }
1047 
1048  // Move the stage for a particular subsystem from g-1 to g. No other
1049  // subsystems are affected, nor the global system stage.
1050  void advanceSubsystemToStage(SubsystemIndex subsys, Stage g) const {
1051  subsystems[subsys].advanceToStage(g);
1052  // We don't automatically advance the System stage even if this brings
1053  // ALL the subsystems up to stage g.
1054  }
1055 
1056  // We don't expect State entry allocations to be performance critical so
1057  // we'll keep error checking on even in Release mode.
1058 
1059  QIndex allocateQ(SubsystemIndex subsys, const Vector& qInit) {
1060  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model,
1061  "StateImpl::allocateQ()");
1062  // We are currently realizing the next stage.
1063  const Stage allocStage = getSubsystemStage(subsys).next();
1064  PerSubsystemInfo& ss = subsystems[subsys];
1065  const QIndex nxt(ss.getNextQIndex());
1066  ss.q_info.push_back(ContinuousVarInfo(allocStage,nxt,qInit,Vector()));
1067  return nxt;
1068  }
1069 
1070  UIndex allocateU(SubsystemIndex subsys, const Vector& uInit) {
1071  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model,
1072  "StateImpl::allocateU()");
1073  const Stage allocStage = getSubsystemStage(subsys).next();
1074  PerSubsystemInfo& ss = subsystems[subsys];
1075  const UIndex nxt(ss.getNextUIndex());
1076  ss.uInfo.push_back(ContinuousVarInfo(allocStage,nxt,uInit,Vector()));
1077  return nxt;
1078  }
1079  ZIndex allocateZ(SubsystemIndex subsys, const Vector& zInit) {
1080  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Model,
1081  "StateImpl::allocateZ()");
1082  const Stage allocStage = getSubsystemStage(subsys).next();
1083  PerSubsystemInfo& ss = subsystems[subsys];
1084  const ZIndex nxt(ss.getNextZIndex());
1085  ss.zInfo.push_back(ContinuousVarInfo(allocStage,nxt,zInit,Vector()));
1086  return nxt;
1087  }
1088 
1089  QErrIndex allocateQErr(SubsystemIndex subsys, int nqerr) const {
1090  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
1091  "StateImpl::allocateQErr()");
1092  const Stage allocStage = getSubsystemStage(subsys).next();
1093  const PerSubsystemInfo& ss = subsystems[subsys];
1094  const QErrIndex nxt(ss.getNextQErrIndex());
1095  ss.qerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nqerr,Vector()));
1096  return nxt;
1097  }
1098  UErrIndex allocateUErr(SubsystemIndex subsys, int nuerr) const {
1099  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys),
1100  Stage::Instance,"StateImpl::allocateUErr()");
1101  const Stage allocStage = getSubsystemStage(subsys).next();
1102  const PerSubsystemInfo& ss = subsystems[subsys];
1103  const UErrIndex nxt(ss.getNextUErrIndex());
1104  ss.uerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nuerr,Vector()));
1105  return nxt;
1106  }
1107  UDotErrIndex allocateUDotErr(SubsystemIndex subsys, int nudoterr) const {
1108  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
1109  "StateImpl::allocateUDotErr()");
1110  const Stage allocStage = getSubsystemStage(subsys).next();
1111  const PerSubsystemInfo& ss = subsystems[subsys];
1112  const UDotErrIndex nxt(ss.getNextUDotErrIndex());
1113  ss.udoterrInfo.push_back
1114  (ConstraintErrInfo(allocStage,nxt,nudoterr,Vector()));
1115  return nxt;
1116  }
1117  EventTriggerByStageIndex allocateEventTrigger
1118  (SubsystemIndex subsys, Stage g, int nt) const {
1119  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys), Stage::Instance,
1120  "StateImpl::allocateEventTrigger()");
1121  const Stage allocStage = getSubsystemStage(subsys).next();
1122  const PerSubsystemInfo& ss = subsystems[subsys];
1124  nxt(ss.getNextEventTriggerByStageIndex(g));
1125  ss.triggerInfo[g].push_back(TriggerInfo(allocStage,nxt,nt));
1126  return nxt;
1127  }
1128 
1129  // Topology- and Model-stage State variables can only be added during
1130  // construction; that is, while stage <= Topology. Other entries can be
1131  // added while stage < Model.
1132  DiscreteVariableIndex allocateDiscreteVariable
1133  (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp)
1134  {
1136  invalidates, Stage::HighestRuntime,
1137  "StateImpl::allocateDiscreteVariable()");
1138 
1139  const Stage maxAcceptable = (invalidates <= Stage::Model
1141  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys),
1142  maxAcceptable.next(), "StateImpl::allocateDiscreteVariable()");
1143 
1144  const Stage allocStage = getSubsystemStage(subsys).next();
1145  PerSubsystemInfo& ss = subsystems[subsys];
1146  const DiscreteVariableIndex nxt(ss.getNextDiscreteVariableIndex());
1147  ss.discreteInfo.push_back
1148  (DiscreteVarInfo(allocStage,invalidates,vp));
1149  return nxt;
1150  }
1151 
1152  // Cache entries can be allocated while stage < Instance.
1153  CacheEntryIndex allocateCacheEntry
1154  (SubsystemIndex subsys, Stage dependsOn, Stage computedBy,
1155  AbstractValue* vp) const
1156  {
1158  dependsOn, Stage::HighestRuntime,
1159  "StateImpl::allocateCacheEntry()");
1160  SimTK_STAGECHECK_RANGE_ALWAYS(dependsOn, computedBy, Stage::Infinity,
1161  "StateImpl::allocateCacheEntry()");
1162  SimTK_STAGECHECK_LT_ALWAYS(getSubsystemStage(subsys),
1163  Stage::Instance, "StateImpl::allocateCacheEntry()");
1164 
1165  const Stage allocStage = getSubsystemStage(subsys).next();
1166  const PerSubsystemInfo& ss = subsystems[subsys];
1167  const CacheEntryIndex nxt(ss.getNextCacheEntryIndex());
1168  ss.cacheInfo.emplace_back(CacheEntryKey(subsys,nxt),
1169  allocStage, dependsOn, computedBy, vp);
1170  return nxt;
1171  }
1172 
1173  CacheEntryIndex allocateCacheEntryWithPrerequisites
1174  (SubsystemIndex subsys, Stage earliest, Stage latest,
1175  bool qPre, bool uPre, bool zPre,
1176  const Array_<DiscreteVarKey>& discreteVars,
1177  const Array_<CacheEntryKey>& cacheEntries,
1178  AbstractValue* value)
1179  {
1180  // First pre-check that no cache entry prerequisite has a later
1181  // depends-on stage than this one does. I'm doing this first rather
1182  // than combining it with the loop below so there won't be side effects
1183  // if this exception gets caught (likely only in testing).
1184  for (const auto& ckey : cacheEntries) {
1185  const CacheEntryInfo& prereq = getCacheEntryInfo(ckey);
1186  SimTK_ERRCHK4_ALWAYS(prereq.getDependsOnStage() <= earliest,
1187  "State::allocateCacheEntryWithPrerequisites()",
1188  "Prerequisite cache entry (%d,%d) has depends-on stage %s "
1189  "but this one would have lower depends-on stage %s. That "
1190  "would mean the prerequisite could get invalidated without "
1191  "invalidating this one; not good.",
1192  (int)ckey.first, (int)ckey.second,
1193  prereq.getDependsOnStage().getName().c_str(),
1194  earliest.getName().c_str());
1195  }
1196 
1197  const CacheEntryIndex cx =
1198  allocateCacheEntry(subsys,earliest,latest,value);
1199  CacheEntryInfo& cinfo = updCacheEntryInfo(CacheEntryKey(subsys,cx));
1200 
1201  if (qPre) cinfo.setPrerequisiteQ();
1202  if (uPre) cinfo.setPrerequisiteU();
1203  if (zPre) cinfo.setPrerequisiteZ();
1204  for (const auto& dk : discreteVars)
1205  cinfo.setPrerequisite(dk);
1206  for (const auto& ckey : cacheEntries)
1207  cinfo.setPrerequisite(ckey); // already validated above
1208 
1209  cinfo.registerWithPrerequisites(*this);
1210  return cx;
1211  }
1212 
1213  // Allocate a discrete variable and a corresponding cache entry for
1214  // updating it, and connect them together.
1215  DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
1216  (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp,
1217  Stage updateDependsOn)
1218  {
1219  const DiscreteVariableIndex dx =
1220  allocateDiscreteVariable(subsys,invalidates,vp->clone());
1221  const CacheEntryIndex cx =
1222  allocateCacheEntry(subsys,updateDependsOn,Stage::Infinity,vp);
1223 
1224  PerSubsystemInfo& ss = subsystems[subsys];
1225  DiscreteVarInfo& dvinfo = ss.discreteInfo[dx];
1226  CacheEntryInfo& ceinfo = ss.cacheInfo[cx];
1227  dvinfo.setAutoUpdateEntry(cx);
1228  ceinfo.setAssociatedVar(dx);
1229  return dx;
1230  }
1231 
1232  // State dimensions for shared continuous variables.
1233 
1234  int getNY() const {
1235  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1236  "StateImpl::getNY()");
1237  return y.size();
1238  }
1239 
1240  SystemYIndex getQStart() const {
1241  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1242  "StateImpl::getQStart()");
1243  return SystemYIndex(0); // q's come first
1244  }
1245  int getNQ() const {
1246  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1247  "StateImpl::getNQ()");
1248  return q.size();
1249  }
1250 
1251  SystemYIndex getUStart() const {
1252  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1253  "StateImpl::getUStart()");
1254  return SystemYIndex(q.size()); // u's come right after q's
1255  }
1256  int getNU() const {
1257  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1258  "StateImpl::getNU()");
1259  return u.size();
1260  }
1261 
1262  SystemYIndex getZStart() const {
1263  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1264  "StateImpl::getZStart()");
1265  return SystemYIndex(q.size() + u.size()); // q,u, then z
1266  }
1267  int getNZ() const {
1268  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1269  "StateImpl::getNZ()");
1270  return z.size();
1271  }
1272 
1273  int getNYErr() const {
1274  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1275  "StateImpl::getNYErr()");
1276  return yerr.size();
1277  }
1278 
1279  SystemYErrIndex getQErrStart() const {
1280  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1281  "StateImpl::getQErrStart()");
1282  return SystemYErrIndex(0); // qerr's come first
1283  }
1284  int getNQErr() const {
1285  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1286  "StateImpl::getNQErr()");
1287  return qerr.size();
1288  }
1289 
1290  SystemYErrIndex getUErrStart() const {
1291  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1292  "StateImpl::getUErrStart()");
1293  return SystemYErrIndex(qerr.size()); // uerr's follow qerrs
1294  }
1295  int getNUErr() const {
1296  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1297  "StateImpl::getNUErr()");
1298  return uerr.size();
1299  }
1300 
1301  // UDot errors are independent of qerr & uerr.
1302  // This is used for multipliers also.
1303  int getNUDotErr() const {
1304  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1305  "StateImpl::getNUDotErr()");
1306  return udoterr.size();
1307  }
1308 
1309  int getNEventTriggers() const {
1310  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1311  "StateImpl::getNEventTriggers()");
1312  return allTriggers.size();
1313  }
1314 
1315  SystemEventTriggerIndex getEventTriggerStartByStage(Stage g) const {
1316  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1317  "StateImpl::getEventTriggerStartByStage()");
1318  int nxt = 0;
1319  for (int j=0; j<g; ++j)
1320  nxt += triggers[j].size();
1321  return SystemEventTriggerIndex(nxt); // g starts where g-1 leaves off
1322  }
1323 
1324  int getNEventTriggersByStage(Stage g) const {
1325  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1326  "StateImpl::getNEventTriggersByStage()");
1327  return triggers[g].size();
1328  }
1329 
1330  std::mutex& getStateLock() const {
1331  return stateLock; // mutable
1332  }
1333 
1334  // Subsystem dimensions.
1335 
1336  SystemQIndex getQStart(SubsystemIndex subsys) const {
1337  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1338  "StateImpl::getQStart(subsys)");
1339  return getSubsystem(subsys).qstart;
1340  }
1341  int getNQ(SubsystemIndex subsys) const {
1342  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1343  "StateImpl::getNQ(subsys)");
1344  return getSubsystem(subsys).q.size();
1345  }
1346 
1347  SystemUIndex getUStart(SubsystemIndex subsys) const {
1348  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1349  "StateImpl::getUStart(subsys)");
1350  return getSubsystem(subsys).ustart;
1351  }
1352  int getNU(SubsystemIndex subsys) const {
1353  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1354  "StateImpl::getNU(subsys)");
1355  return getSubsystem(subsys).u.size();
1356  }
1357 
1358  SystemZIndex getZStart(SubsystemIndex subsys) const {
1359  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1360  "StateImpl::getZStart(subsys)");
1361  return getSubsystem(subsys).zstart;
1362  }
1363  int getNZ(SubsystemIndex subsys) const {
1364  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1365  "StateImpl::getNZ(subsys)");
1366  return getSubsystem(subsys).z.size();
1367  }
1368 
1369  SystemQErrIndex getQErrStart(SubsystemIndex subsys) const {
1370  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1371  "StateImpl::getQErrStart(subsys)");
1372  return getSubsystem(subsys).qerrstart;
1373  }
1374  int getNQErr(SubsystemIndex subsys) const {
1375  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1376  "StateImpl::getNQErr(subsys)");
1377  return getSubsystem(subsys).qerr.size();
1378  }
1379 
1380  SystemUErrIndex getUErrStart(SubsystemIndex subsys) const {
1381  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1382  "StateImpl::getUErrStart(subsys)");
1383  return getSubsystem(subsys).uerrstart;
1384  }
1385  int getNUErr(SubsystemIndex subsys) const {
1386  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1387  "StateImpl::getNUErr(subsys)");
1388  return getSubsystem(subsys).uerr.size();
1389  }
1390 
1391  // These are used for multipliers also.
1392  SystemUDotErrIndex getUDotErrStart(SubsystemIndex subsys) const {
1393  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1394  "StateImpl::getUDotErrStart(subsys)");
1395  return getSubsystem(subsys).udoterrstart;
1396  }
1397  int getNUDotErr(SubsystemIndex subsys) const {
1398  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1399  "StateImpl::getNUDotErr(subsys)");
1400  return getSubsystem(subsys).udoterr.size();
1401  }
1402 
1403  SystemEventTriggerByStageIndex getEventTriggerStartByStage
1404  (SubsystemIndex subsys, Stage g) const {
1405  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1406  "StateImpl::getEventTriggerStartByStage(subsys)");
1407  return getSubsystem(subsys).triggerstart[g];
1408  }
1409 
1410  int getNEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
1411  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1412  "StateImpl::getNEventTriggersByStage(subsys)");
1413  return getSubsystem(subsys).triggers[g].size();
1414  }
1415 
1416  // Per-subsystem access to the global shared variables.
1417 
1418  const Vector& getQ(SubsystemIndex subsys) const {
1419  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1420  "StateImpl::getQ(subsys)");
1421  return getSubsystem(subsys).q;
1422  }
1423  const Vector& getU(SubsystemIndex subsys) const {
1424  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1425  "StateImpl::getU(subsys)");
1426  return getSubsystem(subsys).u;
1427  }
1428  const Vector& getZ(SubsystemIndex subsys) const {
1429  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1430  "StateImpl::getZ(subsys)");
1431  return getSubsystem(subsys).z;
1432  }
1433 
1434  const Vector& getUWeights(SubsystemIndex subsys) const {
1435  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1436  "StateImpl::getUWeights(subsys)");
1437  return getSubsystem(subsys).uWeights;
1438  }
1439  const Vector& getZWeights(SubsystemIndex subsys) const {
1440  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1441  "StateImpl::getZWeights(subsys)");
1442  return getSubsystem(subsys).zWeights;
1443  }
1444 
1445  const Vector& getQDot(SubsystemIndex subsys) const {
1446  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1447  "StateImpl::getQDot(subsys)");
1448  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Velocity,
1449  "StateImpl::getQDot(subsys)");
1450  return getSubsystem(subsys).qdot;
1451  }
1452  const Vector& getUDot(SubsystemIndex subsys) const {
1453  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1454  "StateImpl::getUDot(subsys)");
1455  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration,
1456  "StateImpl::getUDot(subsys)");
1457  return getSubsystem(subsys).udot;
1458  }
1459  const Vector& getZDot(SubsystemIndex subsys) const {
1460  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1461  "StateImpl::getZDot(subsys)");
1462  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Dynamics,
1463  "StateImpl::getZDot(subsys)");
1464  return getSubsystem(subsys).zdot;
1465  }
1466  const Vector& getQDotDot(SubsystemIndex subsys) const {
1467  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1468  "StateImpl::getQDotDot(subsys)");
1469  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration,
1470  "StateImpl::getQDotDot(subsys)");
1471  return getSubsystem(subsys).qdotdot;
1472  }
1473 
1474  Vector& updQ(SubsystemIndex subsys) {
1475  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1476  "StateImpl::updQ(subsys)");
1477  invalidateAll(Stage::Position);
1478  noteQChange();
1479  return updSubsystem(subsys).q;
1480  }
1481  Vector& updU(SubsystemIndex subsys) {
1482  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1483  "StateImpl::updU(subsys)");
1484  invalidateAll(Stage::Velocity);
1485  noteUChange();
1486  return updSubsystem(subsys).u;
1487  }
1488  Vector& updZ(SubsystemIndex subsys) {
1489  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1490  "StateImpl::updZ(subsys)");
1491  invalidateAll(Stage::Dynamics);
1492  noteZChange();
1493  return updSubsystem(subsys).z;
1494  }
1495 
1496  Vector& updUWeights(SubsystemIndex subsys) {
1497  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1498  "StateImpl::updUWeights(subsys)");
1499  invalidateAll(Stage::Report);
1500  return updSubsystem(subsys).uWeights;
1501  }
1502  Vector& updZWeights(SubsystemIndex subsys) {
1503  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1504  "StateImpl::updZWeights(subsys)");
1505  invalidateAll(Stage::Report);
1506  return updSubsystem(subsys).zWeights;
1507  }
1508 
1509  // These are mutable so the routines are const.
1510 
1511  Vector& updQDot(SubsystemIndex subsys) const {
1512  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1513  "StateImpl::updQDot(subsys)");
1514  return getSubsystem(subsys).qdot;
1515  }
1516  Vector& updUDot(SubsystemIndex subsys) const {
1517  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1518  "StateImpl::updUDot(subsys)");
1519  return getSubsystem(subsys).udot;
1520  }
1521  Vector& updZDot(SubsystemIndex subsys) const {
1522  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1523  "StateImpl::updZDot(subsys)");
1524  return getSubsystem(subsys).zdot;
1525  }
1526  Vector& updQDotDot(SubsystemIndex subsys) const {
1527  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1528  "StateImpl::updQDotDot(subsys)");
1529  return getSubsystem(subsys).qdotdot;
1530  }
1531 
1532 
1533  const Vector& getQErr(SubsystemIndex subsys) const {
1534  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1535  "StateImpl::getQErr(subsys)");
1536  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Position,
1537  "StateImpl::getQErr(subsys)");
1538  return getSubsystem(subsys).qerr;
1539  }
1540  const Vector& getUErr(SubsystemIndex subsys) const {
1541  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1542  "StateImpl::getUErr(subsys)");
1543  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Velocity,
1544  "StateImpl::getUErr(subsys)");
1545  return getSubsystem(subsys).uerr;
1546  }
1547 
1548  const Vector& getQErrWeights(SubsystemIndex subsys) const {
1549  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1550  "StateImpl::getQErrWeights(subsys)");
1551  return getSubsystem(subsys).qerrWeights;
1552  }
1553  const Vector& getUErrWeights(SubsystemIndex subsys) const {
1554  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1555  "StateImpl::getUErrWeights(subsys)");
1556  return getSubsystem(subsys).uerrWeights;
1557  }
1558 
1559  const Vector& getUDotErr(SubsystemIndex subsys) const {
1560  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1561  "StateImpl::getUDotErr(subsys)");
1562  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration,
1563  "StateImpl::getUDotErr(subsys)");
1564  return getSubsystem(subsys).udoterr;
1565  }
1566  const Vector& getMultipliers(SubsystemIndex subsys) const {
1567  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1568  "StateImpl::getMultipliers(subsys)");
1569  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), Stage::Acceleration,
1570  "StateImpl::getMultipliers(subsys)");
1571  return getSubsystem(subsys).multipliers;
1572  }
1573 
1574  const Vector& getEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
1575  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1576  "StateImpl::getEventTriggersByStage(subsys)");
1577  SimTK_STAGECHECK_GE(getSubsystemStage(subsys), g,
1578  "StateImpl::getEventTriggersByStage(subsys)");
1579  return getSubsystem(subsys).triggers[g];
1580  }
1581 
1582  Vector& updQErr(SubsystemIndex subsys) const {
1583  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1584  "StateImpl::updQErr(subsys)");
1585  return getSubsystem(subsys).qerr;
1586  }
1587  Vector& updUErr(SubsystemIndex subsys) const {
1588  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1589  "StateImpl::updUErr(subsys)");
1590  return getSubsystem(subsys).uerr;
1591  }
1592 
1593  Vector& updQErrWeights(SubsystemIndex subsys) {
1594  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1595  "StateImpl::updQErrWeights(subsys)");
1596  invalidateAll(Stage::Position);
1597  return updSubsystem(subsys).qerrWeights;
1598  }
1599  Vector& updUErrWeights(SubsystemIndex subsys) {
1600  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1601  "StateImpl::updUErrWeights(subsys)");
1602  invalidateAll(Stage::Velocity);
1603  return updSubsystem(subsys).uerrWeights;
1604  }
1605 
1606  Vector& updUDotErr(SubsystemIndex subsys) const {
1607  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1608  "StateImpl::updUDotErr(subsys)");
1609  return getSubsystem(subsys).udoterr;
1610  }
1611  Vector& updMultipliers(SubsystemIndex subsys) const {
1612  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1613  "StateImpl::updMultipliers(subsys)");
1614  return getSubsystem(subsys).multipliers;
1615  }
1616  Vector& updEventTriggersByStage(SubsystemIndex subsys, Stage g) const {
1617  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1618  "StateImpl::updEventTriggersByStage(subsys)");
1619  return getSubsystem(subsys).triggers[g];
1620  }
1621 
1622  // Direct access to the global shared state and cache entries.
1623  // Time is allocated in Stage::Topology, State in Stage::Model, and
1624  // Cache in Stage::Instance.
1625 
1626  const Real& getTime() const {
1627  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Topology,
1628  "StateImpl::getTime()");
1629  return t;
1630  }
1631 
1632  const Vector& getY() const {
1633  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1634  "StateImpl::getY()");
1635  return y;
1636  }
1637 
1638  const Vector& getQ() const {
1639  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1640  "StateImpl::getQ()");
1641  return q;
1642  }
1643 
1644  const Vector& getU() const {
1645  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1646  "StateImpl::getU()");
1647  return u;
1648  }
1649 
1650  const Vector& getZ() const {
1651  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1652  "StateImpl::getZ()");
1653  return z;
1654  }
1655 
1656  const Vector& getUWeights() const {
1657  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1658  "StateImpl::getUWeights()");
1659  return uWeights;
1660  }
1661 
1662  const Vector& getZWeights() const {
1663  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1664  "StateImpl::getZWeights()");
1665  return zWeights;
1666  }
1667 
1668  // You can call these as long as stage >= allocation stage, but the
1669  // stage will be backed up if necessary to one stage prior to the
1670  // invalidated stage.
1671  Real& updTime() { // Back to Stage::Time-1
1672  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Topology,
1673  "StateImpl::updTime()");
1674  invalidateAll(Stage::Time);
1675  return t;
1676  }
1677 
1678  Vector& updY() { // Back to Stage::Position-1
1679  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1680  "StateImpl::updY()");
1681  invalidateAll(Stage::Position);
1682  noteYChange();
1683  return y;
1684  }
1685 
1686  Vector& updQ() { // Stage::Position-1
1687  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1688  "StateImpl::updQ()");
1689  invalidateAll(Stage::Position);
1690  noteQChange();
1691  return q;
1692  }
1693 
1694  Vector& updU() { // Stage::Velocity-1
1695  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1696  "StateImpl::updU()");
1697  invalidateAll(Stage::Velocity);
1698  noteUChange();
1699  return u;
1700  }
1701 
1702  Vector& updZ() { // Stage::Dynamics-1
1703  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1704  "StateImpl::updZ()");
1705  invalidateAll(Stage::Dynamics);
1706  noteZChange();
1707  return z;
1708  }
1709 
1710  Vector& updUWeights() { // Invalidates Report stage only
1711  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1712  "StateImpl::updUWeights()");
1713  invalidateAll(Stage::Report);
1714  return uWeights;
1715  }
1716 
1717  Vector& updZWeights() { // Invalidates Report stage only
1718  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1719  "StateImpl::updZWeights()");
1720  invalidateAll(Stage::Dynamics);
1721  return zWeights;
1722  }
1723 
1724  // These cache entries you can get at their "computedBy" stages.
1725  const Vector& getYDot() const {
1726  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration,
1727  "StateImpl::getYDot()");
1728  return ydot;
1729  }
1730 
1731  const Vector& getQDot() const {
1732  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity,
1733  "StateImpl::getQDot()");
1734  return qdot;
1735  }
1736 
1737  const Vector& getZDot() const {
1738  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Dynamics,
1739  "StateImpl::getZDot()");
1740  return zdot;
1741  }
1742 
1743  const Vector& getUDot() const {
1744  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration,
1745  "StateImpl::getUDot()");
1746  return udot;
1747  }
1748 
1749  const Vector& getQDotDot() const {
1750  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration,
1751  "StateImpl::getQDotDot()");
1752  return qdotdot;
1753  }
1754 
1755  // Cache updates are allowed any time after they have been allocated.
1756  Vector& updYDot() const {
1757  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1758  "StateImpl::updYDot()");
1759  return ydot;
1760  }
1761 
1762  Vector& updQDot() const {
1763  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1764  "StateImpl::updQDot()");
1765  return qdot;
1766  }
1767 
1768  Vector& updUDot() const {
1769  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1770  "StateImpl::updUDot()");
1771  return udot;
1772  }
1773 
1774  Vector& updZDot() const {
1775  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1776  "StateImpl::updZDot()");
1777  return zdot;
1778  }
1779 
1780  Vector& updQDotDot() const {
1781  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Model,
1782  "StateImpl::updQDotDot()");
1783  return qdotdot;
1784  }
1785 
1786 
1787  const Vector& getYErr() const {
1788  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity,
1789  "StateImpl::getYErr()");
1790  return yerr;
1791  }
1792 
1793  const Vector& getQErr() const {
1794  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Position,
1795  "StateImpl::getQErr()");
1796  return qerr;
1797  }
1798  const Vector& getUErr() const {
1799  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Velocity,
1800  "StateImpl::getUErr()");
1801  return uerr;
1802  }
1803 
1804  const Vector& getQErrWeights() const {
1805  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1806  "StateImpl::getQErrWeights()");
1807  return qerrWeights;
1808  }
1809  const Vector& getUErrWeights() const {
1810  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1811  "StateImpl::getUErrWeights()");
1812  return uerrWeights;
1813  }
1814 
1815  const Vector& getUDotErr() const {
1816  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration,
1817  "StateImpl::getUDotErr()");
1818  return udoterr;
1819  }
1820  const Vector& getMultipliers() const {
1821  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration,
1822  "StateImpl::getMultipliers()");
1823  return multipliers;
1824  }
1825 
1826  Vector& updYErr() const {
1827  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1828  "StateImpl::updYErr()");
1829  return yerr;
1830  }
1831  Vector& updQErr() const{
1832  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1833  "StateImpl::updQErr()");
1834  return qerr;
1835  }
1836  Vector& updUErr() const{
1837  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1838  "StateImpl::updUErr()");
1839  return uerr;
1840  }
1841 
1842  Vector& updQErrWeights() {
1843  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1844  "StateImpl::updQErrWeights()");
1845  invalidateAll(Stage::Position);
1846  return qerrWeights;
1847  }
1848  Vector& updUErrWeights() {
1849  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1850  "StateImpl::updUErrWeights()");
1851  invalidateAll(Stage::Velocity);
1852  return uerrWeights;
1853  }
1854 
1855  Vector& updUDotErr() const{
1856  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1857  "StateImpl::updUDotErr()");
1858  return udoterr;
1859  }
1860  Vector& updMultipliers() const{
1861  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1862  "StateImpl::updMultipliers()");
1863  return multipliers;
1864  }
1865 
1866  const Vector& getEventTriggers() const {
1867  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Acceleration,
1868  "StateImpl::getEventTriggers()");
1869  return allTriggers;
1870  }
1871  const Vector& getEventTriggersByStage(Stage g) const {
1872  SimTK_STAGECHECK_GE(getSystemStage(), g,
1873  "StateImpl::getEventTriggersByStage()");
1874  return triggers[g];
1875  }
1876 
1877  // These are mutable; hence 'const'.
1878  Vector& updEventTriggers() const {
1879  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1880  "StateImpl::updEventTriggers()");
1881  return allTriggers;
1882  }
1883  Vector& updEventTriggersByStage(Stage g) const {
1884  SimTK_STAGECHECK_GE(getSystemStage(), Stage::Instance,
1885  "StateImpl::updEventTriggersByStage()");
1886  return triggers[g];
1887  }
1888 
1889  bool hasDiscreteVar(const DiscreteVarKey& dk) const {
1890  return getSubsystem(dk.first).hasDiscreteVar(dk.second);
1891  }
1892 
1893  const DiscreteVarInfo& getDiscreteVarInfo(const DiscreteVarKey& dk) const {
1894  return getSubsystem(dk.first).getDiscreteVarInfo(dk.second);
1895  }
1896 
1897  DiscreteVarInfo& updDiscreteVarInfo(const DiscreteVarKey& dk) {
1898  return updSubsystem(dk.first).updDiscreteVarInfo(dk.second);
1899  }
1900 
1901  CacheEntryIndex getDiscreteVarUpdateIndex(const DiscreteVarKey& dk) const {
1902  return getDiscreteVarInfo(dk).getAutoUpdateEntry();
1903  }
1904 
1905  Stage getDiscreteVarAllocationStage(const DiscreteVarKey& dk) const {
1906  return getDiscreteVarInfo(dk).getAllocationStage();
1907  }
1908 
1909  Stage getDiscreteVarInvalidatesStage(const DiscreteVarKey& dk) const {
1910  return getDiscreteVarInfo(dk).getInvalidatedStage();
1911  }
1912 
1913  // You can access a variable any time after it has been allocated.
1914  const AbstractValue&
1915  getDiscreteVariable(const DiscreteVarKey& dk) const {
1916  const DiscreteVarInfo& dv = getDiscreteVarInfo(dk);
1917  return dv.getValue();
1918  }
1919 
1920  Real getDiscreteVarLastUpdateTime(const DiscreteVarKey& dk) const {
1921  return getDiscreteVarInfo(dk).getTimeLastUpdated();
1922  }
1923 
1924  const AbstractValue& getDiscreteVarUpdateValue(const DiscreteVarKey& dk) const {
1925  const CacheEntryIndex cx = getDiscreteVarUpdateIndex(dk);
1926  SimTK_ERRCHK2(cx.isValid(), "StateImpl::getDiscreteVarUpdateValue()",
1927  "Subsystem %d has a discrete variable %d but it does not have an"
1928  " associated update cache variable.",
1929  (int)dk.first, (int)dk.second);
1930  return getCacheEntry(CacheEntryKey(dk.first,cx));
1931  }
1932  AbstractValue& updDiscreteVarUpdateValue(const DiscreteVarKey& dk) const {
1933  const CacheEntryIndex cx = getDiscreteVarUpdateIndex(dk);
1934  SimTK_ERRCHK2(cx.isValid(), "StateImpl::updDiscreteVarUpdateValue()",
1935  "Subsystem %d has a discrete variable %d but it does not have an"
1936  " associated update cache variable.",
1937  (int)dk.first, (int)dk.second);
1938  return updCacheEntry(CacheEntryKey(dk.first,cx));
1939  }
1940  bool isDiscreteVarUpdateValueRealized(const DiscreteVarKey& dk) const {
1941  const CacheEntryIndex cx = getDiscreteVarUpdateIndex(dk);
1942  SimTK_ERRCHK2(cx.isValid(),
1943  "StateImpl::isDiscreteVarUpdateValueRealized()",
1944  "Subsystem %d has a discrete variable %d but it does not have an"
1945  " associated update cache variable.",
1946  (int)dk.first, (int)dk.second);
1947  return isCacheValueRealized(CacheEntryKey(dk.first,cx));
1948  }
1949  void markDiscreteVarUpdateValueRealized(const DiscreteVarKey& dk) const {
1950  const CacheEntryIndex cx = getDiscreteVarUpdateIndex(dk);
1951  SimTK_ERRCHK2(cx.isValid(),
1952  "StateImpl::markDiscreteVarUpdateValueRealized()",
1953  "Subsystem %d has a discrete variable %d but it does not have an"
1954  " associated update cache variable.",
1955  (int)dk.first, (int)dk.second);
1956  markCacheValueRealized(CacheEntryKey(dk.first,cx));
1957  }
1958 
1959  // You can update a variable's value any time after it is allocated.
1960  // This always backs the stage up to one earlier than the
1961  // variable's "invalidates" stage, and if there is an auto-update cache
1962  // entry it is also invalidated, regardless of its "depends on" stage.
1963  // Any explicit dependent cache entries are also invalidated.
1964  AbstractValue&
1965  updDiscreteVariable(const DiscreteVarKey& dk) {
1966  DiscreteVarInfo& dv = updDiscreteVarInfo(dk);
1967 
1968  // Invalidate the "invalidates" stage. (All subsystems and the system
1969  // have their stage reduced to no higher than invalidates-1.)
1970  invalidateAll(dv.getInvalidatedStage());
1971 
1972  // Invalidate the auto-update entry, if any.
1973  const CacheEntryIndex cx = dv.getAutoUpdateEntry();
1974  if (cx.isValid()) {
1975  CacheEntryInfo& ce = updCacheEntryInfo(CacheEntryKey(dk.first,cx));
1976  ce.invalidate(*this);
1977  }
1978 
1979  // We're now marking this variable as having been updated at the
1980  // current time. Dependents get invalidated here.
1981  return dv.updValue(*this, t);
1982  }
1983 
1984  bool hasCacheEntry(const CacheEntryKey& ck) const {
1985  return getSubsystem(ck.first).hasCacheEntry(ck.second);
1986  }
1987 
1988  const CacheEntryInfo&
1989  getCacheEntryInfo(const CacheEntryKey& ck) const {
1990  return getSubsystem(ck.first).getCacheEntryInfo(ck.second);
1991  }
1992 
1993  CacheEntryInfo&
1994  updCacheEntryInfo(const CacheEntryKey& ck) const {
1995  return getSubsystem(ck.first).updCacheEntryInfo(ck.second);
1996  }
1997 
1998  Stage getCacheEntryAllocationStage(const CacheEntryKey& ck) const {
1999  return getCacheEntryInfo(ck).getAllocationStage();
2000  }
2001 
2002 
2003  // Stage >= ce.stage
2004  // This method gets called a lot, so make it fast in Release mode. Keep
2005  // it small so it gets inlined.
2006  const AbstractValue&
2007  getCacheEntry(const CacheEntryKey& ck) const {
2008  const CacheEntryInfo& ce = getCacheEntryInfo(ck);
2009 
2010  if (!ce.isUpToDate(*this))
2011  ce.throwHelpfulOutOfDateMessage(*this, __func__);
2012  return ce.getValue();
2013  }
2014 
2015  // You can access a cache entry for update any time after it has been
2016  // allocated. This does not affect the stage.
2017  AbstractValue&
2018  updCacheEntry(const CacheEntryKey& ck) const {
2019  return updCacheEntryInfo(ck).updValue(*this);
2020  }
2021 
2022  bool isCacheValueRealized(const CacheEntryKey& ck) const {
2023  const CacheEntryInfo& ce = getCacheEntryInfo(ck);
2024  return ce.isUpToDate(*this);
2025  }
2026 
2027  void markCacheValueRealized(const CacheEntryKey& ck) const {
2028  CacheEntryInfo& ce = updCacheEntryInfo(ck);
2029 
2030  // This cache entry can't be valid unless
2031  // we're at least *working* on its depends-on stage, meaning the current
2032  // stage would have to be the one before that. The depends-on stage is
2033  // required to be at least Stage::Topology, so its prev() stage exists.
2034  SimTK_STAGECHECK_GE(getSubsystemStage(ck.first),
2035  ce.getDependsOnStage().prev(),
2036  "StateImpl::markCacheValueRealized()");
2037 
2038  ce.markAsUpToDate(*this);
2039  }
2040 
2041  void markCacheValueNotRealized(const CacheEntryKey& ck) const {
2042  CacheEntryInfo& ce = updCacheEntryInfo(ck);
2043  ce.invalidate(*this);
2044  }
2045 
2046  StageVersion getSystemTopologyStageVersion() const
2047  { return systemStageVersions[Stage::Topology]; }
2048 
2049  void setSystemTopologyStageVersion(StageVersion topoVersion)
2050  { assert(topoVersion>0);
2051  systemStageVersions[Stage::Topology]=topoVersion; }
2052 
2053  // Capture the stage versions only for currently-realized stages.
2054  void getSystemStageVersions(Array_<StageVersion>& versions) const {
2055  versions.resize(currentSystemStage+1);
2056  for (int i=0; i <= currentSystemStage; ++i)
2057  versions[i] = systemStageVersions[i];
2058  }
2059 
2060  // If the current state is realized at least as high as the previous one,
2061  // then report Stage::Infinity if all of those stage versions match.
2062  // Otherwise report either the first mismatch or the first now-invalid
2063  // stage if lower stages match.
2064  Stage getLowestSystemStageDifference
2065  (const Array_<StageVersion>& prevVersions) const {
2066  const int nRealizedBefore = (int)prevVersions.size();
2067  const int nRealizedNow = (int)currentSystemStage+1; // count from 0
2068  const int nRealizedBoth = std::min(nRealizedBefore,nRealizedNow);
2069 
2070  // First check the stages both had in common.
2071  Stage g=Stage::Topology;
2072  for (; g < nRealizedBoth; ++g)
2073  if (systemStageVersions[g] != prevVersions[g])
2074  return g;
2075 
2076  // All stages that were valid before and now have identical versions.
2077  // If that's all there was before then nothing has changed.
2078  return nRealizedNow >= nRealizedBefore ? Stage::Infinity
2079  : g; // 1st unrealized stage
2080  }
2081 
2082  ValueVersion getQValueVersion() const {return qVersion;}
2083  ValueVersion getUValueVersion() const {return uVersion;}
2084  ValueVersion getZValueVersion() const {return zVersion;}
2085 
2086  const ListOfDependents& getQDependents() const {return qDependents;}
2087  const ListOfDependents& getUDependents() const {return uDependents;}
2088  const ListOfDependents& getZDependents() const {return zDependents;}
2089 
2090  ListOfDependents& updQDependents() {return qDependents;}
2091  ListOfDependents& updUDependents() {return uDependents;}
2092  ListOfDependents& updZDependents() {return zDependents;}
2093 
2094  void autoUpdateDiscreteVariables();
2095 
2096  String toString() const;
2097  String cacheToString() const;
2098 
2099 private:
2100  // This is the guts of copy construction and copy assignment which have to
2101  // be done carefully to manage what gets copied and whether the resulting
2102  // cache entries are valid.
2103  void copyFrom(const StateImpl& source);
2104 
2105  // Make sure that no cache entry copied from src could accidentally think
2106  // it was up to date, by setting all the version counters higher than
2107  // the ones in the source. (Don't set these to zero because then a
2108  // subsequent bump to 1 could make some cache entries look valid.)
2109  void invalidateCopiedStageVersions(const StateImpl& src) {
2110  for (int i=1; i <= src.currentSystemStage; ++i)
2111  systemStageVersions[i] = src.systemStageVersions[i]+1;
2112 
2113  qVersion = src.qVersion + 1;
2114  uVersion = src.uVersion + 1;
2115  zVersion = src.zVersion + 1;
2116 
2117  qDependents.clear(); // these shouldn't copy
2118  uDependents.clear();
2119  zDependents.clear();
2120  }
2121 
2122  // After we have copied variables and cache entries from another state into
2123  // this one, all variable and cache entry dependency lists are empty. Any
2124  // cache entry with prerequisites must register itself now.
2125  void registerWithPrerequisitesAfterCopy() {
2126  for (auto& subsys : subsystems) {
2127  for (auto& ce : subsys.cacheInfo)
2128  ce.registerWithPrerequisites(*this);
2129  }
2130  }
2131 
2132 private:
2133  // Bump modification version numbers for state variables and notify their
2134  // dependents.
2135  void noteQChange()
2136  { ++qVersion; qDependents.notePrerequisiteChange(*this); }
2137  void noteUChange()
2138  { ++uVersion; uDependents.notePrerequisiteChange(*this); }
2139  void noteZChange()
2140  { ++zVersion; zDependents.notePrerequisiteChange(*this); }
2141 
2142  void noteYChange() {noteQChange();noteUChange();noteZChange();}
2143 
2144  // Return true only if all subsystems are realized to at least Stage g.
2145  bool allSubsystemsAtLeastAtStage(Stage g) const {
2146  for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
2147  if (subsystems[i].currentStage < g)
2148  return false;
2149  return true;
2150  }
2151 
2152 private:
2153  // Subsystem support //
2154 
2155  Array_<PerSubsystemInfo> subsystems;
2156 
2157  // Shared global resource State variables //
2158 
2159  // We consider time t to be a state variable allocated at Topology stage,
2160  // with its "invalidated" stage Stage::Time. The value of t is NaN in an
2161  // Empty State, and is initialized to zero when the System stage advances
2162  // to Stage::Topology (i.e., when the System is realized to stage Topology).
2163  Real t{NaN}; // no value until Topology stage
2164 
2165  // The continuous state variables are allocated at Model stage, and given
2166  // their specified initial values when the System stage advances to
2167  // Stage::Model (i.e., when the System is realized to Model stage).
2168  Vector y; // All the continuous state variables together {q,u,z}
2169 
2170  // These are views into y.
2171  Vector q; // Stage::Position continuous variables
2172  Vector u; // Stage::Velocity continuous variables
2173  Vector z; // Stage::Dynamics continuous variables
2174 
2175  // These version numbers are incremented whenever the corresponding variable
2176  // is handed out with write access. updY() increments all three versions.
2177  ValueVersion qVersion{1};
2178  ValueVersion uVersion{1};
2179  ValueVersion zVersion{1};
2180 
2181  // These lists are notified whenever the corresponding variable is requested
2182  // for write access. updY() notifies all three lists. These do not
2183  // get copied when state copying is done; cache entries must re-register
2184  // with their prerequisites after a copy.
2185  ListOfDependents qDependents;
2186  ListOfDependents uDependents;
2187  ListOfDependents zDependents;
2188 
2189  // These are not views; there are no qWeights (because qdot=N*u).
2190  Vector uWeights; // scaling for u
2191  Vector zWeights; // scaling for z
2192 
2193  // These are Instance stage state variables.
2194  Vector qerrWeights; // Scaling for perrs
2195  Vector uerrWeights; // Scaling for pdoterrs and verrs
2196 
2197  // Shared global resource Cache entries //
2198 
2199  // This is the System's currently highest-valid Stage.
2200  mutable Stage currentSystemStage{Stage::Empty};
2201 
2202  // This contains a counter for each system stage which is bumped each
2203  // time that stage is invalidated. This allows detection of a state
2204  // that has been changed even after a subsequent realization. The
2205  // Topology stage entry should match the System's Topology version.
2206  // These are initialized to version 1.
2207  mutable StageVersion systemStageVersions[Stage::NValid];
2208 
2209  // DIFFERENTIAL EQUATIONS
2210 
2211  // All the state derivatives taken together (qdot,udot,zdot)
2212  mutable Vector ydot;
2213 
2214  // These are views into ydot.
2215  mutable Vector qdot; // Stage::Velocity
2216  mutable Vector udot; // Stage::Acceleration
2217  mutable Vector zdot; // Stage::Acceleration
2218 
2219  // This is an independent cache entry.
2220  mutable Vector qdotdot; // Stage::Acceleration
2221 
2222  // ALGEBRAIC EQUATIONS
2223 
2224  mutable Vector yerr; // All constraint errors together (qerr,uerr)
2225  mutable Vector udoterr; // Stage::Acceleration (Index 1 constraints)
2226  mutable Vector multipliers; // Stage::Acceleration (Index 1 algebraic vars)
2227 
2228  // These are views into yerr.
2229  mutable Vector qerr; // Stage::Position (Index 3 constraints)
2230  mutable Vector uerr; // Stage::Velocity (Index 2 constraints)
2231 
2232  // DISCRETE EQUATIONS
2233 
2234  // All the event triggers together, ordered by stage.
2235  mutable Vector allTriggers;
2236 
2237  // These are views into allTriggers.
2238  mutable Vector triggers[Stage::NValid];
2239 
2240  // State specific mutex that should be locked by external methods whenever
2241  // they are performing non-thread-safe operations involving the state.
2242  // This is not copied when a State is copied or assigned; each State object
2243  // has its own mutex.
2244  mutable std::mutex stateLock;
2245 
2246 };
2247 
2248 //==============================================================================
2249 // LIST OF DEPENDENTS :: IMPLEMENTATIONS
2250 //==============================================================================
2251 inline void ListOfDependents::
2252 notePrerequisiteChange(const StateImpl& stateImpl) const {
2253  for (auto ckey : m_dependents) {
2254  // Cache entries are mutable.
2255  CacheEntryInfo& ce = stateImpl.updCacheEntryInfo(ckey);
2256  ce.invalidate(stateImpl);
2257  }
2258 }
2259 
2260 //==============================================================================
2261 // CACHE ENTRY INFO :: INLINE IMPLEMENTATIONS
2262 //==============================================================================
2263 SimTK_FORCE_INLINE bool CacheEntryInfo::
2264 isUpToDate(const StateImpl& stateImpl) const {
2265  const PerSubsystemInfo& subsys = stateImpl.getSubsystem(m_myKey.first);
2266  assert(&subsys.getCacheEntryInfo(m_myKey.second) == this);
2267  if (subsys.getCurrentStage() >= m_computedByStage)
2268  return true; // guaranteed to have been computed by now
2269  if (subsys.getCurrentStage() < m_dependsOnStage)
2270  return false; // can't have been computed yet
2271 
2272  // Stage is OK; is valid if the depends-on stage version hasn't
2273  // changed and if no prerequisite invalidated this.
2274  const StageVersion version = subsys.getStageVersion(m_dependsOnStage);
2275  assert(version >= 1);
2276  if (!( version == m_dependsOnVersionWhenLastComputed
2277  && m_isUpToDateWithPrerequisites))
2278  return false;
2279 
2280  // This entry is allegedly up to date. In Debug we'll double check.
2281  #ifndef NDEBUG
2282  validatePrerequisiteVersions(stateImpl);
2283  #endif
2284  return true;
2285 }
2286 
2287 inline void CacheEntryInfo::
2288 markAsUpToDate(const StateImpl& stateImpl) {
2289  const PerSubsystemInfo& subsys = stateImpl.getSubsystem(m_myKey.first);
2290  assert(&subsys.getCacheEntryInfo(m_myKey.second) == this);
2291  const StageVersion version = subsys.getStageVersion(m_dependsOnStage);
2292  assert(version >= 1);
2293  m_dependsOnVersionWhenLastComputed = version;
2294  m_isUpToDateWithPrerequisites = true;
2295 
2296  // In Debug we'll record versions for all the prerequisites so we
2297  // can double check later in isUpToDate().
2298  #ifndef NDEBUG
2299  recordPrerequisiteVersions(stateImpl);
2300  #endif
2301 }
2302 
2303 //==============================================================================
2304 // INLINE IMPLEMENTATIONS OF STATE METHODS
2305 //==============================================================================
2306 // These mostly just forward to the StateImpl object.
2307 
2308 inline void State::setNumSubsystems(int i) {
2309  updImpl().setNumSubsystems(i);
2310 }
2311 inline void State::initializeSubsystem
2312  (SubsystemIndex subsys, const String& name, const String& version) {
2313  updImpl().initializeSubsystem(subsys, name, version);
2314 }
2315 
2317  (const String& name, const String& version) {
2318  return updImpl().addSubsystem(name, version);
2319 }
2320 inline int State::getNumSubsystems() const {
2321  return getImpl().getNumSubsystems();
2322 }
2323 inline const String& State::getSubsystemName(SubsystemIndex subsys) const {
2324  return getImpl().getSubsystemName(subsys);
2325 }
2326 inline const String& State::getSubsystemVersion(SubsystemIndex subsys) const {
2327  return getImpl().getSubsystemVersion(subsys);
2328 }
2329 inline const Stage& State::getSubsystemStage(SubsystemIndex subsys) const {
2330  return getImpl().getSubsystemStage(subsys);
2331 }
2332 inline const Stage& State::getSystemStage() const {
2333  return getImpl().getSystemStage();
2334 }
2335 inline void State::invalidateAll(Stage stage) {
2336  updImpl().invalidateAll(stage);
2337 }
2338 inline void State::invalidateAllCacheAtOrAbove(Stage stage) const {
2339  getImpl().invalidateAllCacheAtOrAbove(stage);
2340 }
2341 inline void State::advanceSubsystemToStage(SubsystemIndex subsys, Stage stage) const {
2342  getImpl().advanceSubsystemToStage(subsys, stage);
2343 }
2344 inline void State::advanceSystemToStage(Stage stage) const {
2345  getImpl().advanceSystemToStage(stage);
2346 }
2347 
2349 { return getImpl().getSystemTopologyStageVersion(); }
2350 
2351 // Continuous state variables
2352 inline QIndex State::allocateQ(SubsystemIndex subsys, const Vector& qInit) {
2353  return updImpl().allocateQ(subsys, qInit);
2354 }
2355 inline UIndex State::allocateU(SubsystemIndex subsys, const Vector& uInit) {
2356  return updImpl().allocateU(subsys, uInit);
2357 }
2358 inline ZIndex State::allocateZ(SubsystemIndex subsys, const Vector& zInit) {
2359  return updImpl().allocateZ(subsys, zInit);
2360 }
2361 
2362 // Constraint errors and multipliers
2363 inline QErrIndex State::allocateQErr(SubsystemIndex subsys, int nqerr) const {
2364  return getImpl().allocateQErr(subsys, nqerr);
2365 }
2366 inline UErrIndex State::allocateUErr(SubsystemIndex subsys, int nuerr) const {
2367  return getImpl().allocateUErr(subsys, nuerr);
2368 }
2369 inline UDotErrIndex State::
2370 allocateUDotErr(SubsystemIndex subsys, int nudoterr) const {
2371  return getImpl().allocateUDotErr(subsys, nudoterr);
2372 }
2373 
2374 // Event witness functions
2376 allocateEventTrigger(SubsystemIndex subsys, Stage stage, int nevent) const {
2377  return getImpl().allocateEventTrigger(subsys, stage, nevent);
2378 }
2379 
2380 // Discrete Variables
2382 allocateDiscreteVariable(SubsystemIndex subsys, Stage stage, AbstractValue* v) {
2383  return updImpl().allocateDiscreteVariable(subsys, stage, v);
2384 }
2387  (SubsystemIndex subsys, Stage invalidates, AbstractValue* v,
2388  Stage updateDependsOn) {
2389  return updImpl().allocateAutoUpdateDiscreteVariable
2390  (subsys, invalidates, v, updateDependsOn);
2391 }
2392 
2393 inline CacheEntryIndex State::
2395  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2396  return getImpl().getDiscreteVarUpdateIndex(DiscreteVarKey(subsys,index));
2397 }
2398 inline Stage State::
2400  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2401  return getImpl().getDiscreteVarAllocationStage(DiscreteVarKey(subsys,index));
2402 }
2403 inline Stage State::
2405  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2406  return getImpl().getDiscreteVarInvalidatesStage(DiscreteVarKey(subsys,index));
2407 }
2408 
2409 inline const AbstractValue& State::
2411  return getImpl().getDiscreteVariable(DiscreteVarKey(subsys,index));
2412 }
2413 inline Real State::
2415  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2416  return getImpl().getDiscreteVarLastUpdateTime(DiscreteVarKey(subsys,index));
2417 }
2418 inline const AbstractValue& State::
2420  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2421  return getImpl().getDiscreteVarUpdateValue(DiscreteVarKey(subsys,index));
2422 }
2423 inline AbstractValue& State::
2425  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2426  return getImpl().updDiscreteVarUpdateValue(DiscreteVarKey(subsys,index));
2427 }
2428 inline bool State::
2430  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2431  return getImpl().isDiscreteVarUpdateValueRealized
2432  (DiscreteVarKey(subsys,index));
2433 }
2434 inline void State::
2436  (SubsystemIndex subsys, DiscreteVariableIndex index) const {
2437  getImpl().markDiscreteVarUpdateValueRealized(DiscreteVarKey(subsys,index));
2438 }
2439 
2440 inline AbstractValue& State::
2442  (SubsystemIndex subsys, DiscreteVariableIndex index) {
2443  return updImpl().updDiscreteVariable(DiscreteVarKey(subsys,index));
2444 }
2445 inline void State::
2447  (SubsystemIndex i, DiscreteVariableIndex index, const AbstractValue& v) {
2448  updDiscreteVariable(i,index) = v;
2449 }
2450 
2451 // Cache Entries
2452 inline CacheEntryIndex State::
2453 allocateCacheEntry(SubsystemIndex subsys, Stage dependsOn, Stage computedBy,
2454  AbstractValue* value) const {
2455  return getImpl().allocateCacheEntry(subsys, dependsOn, computedBy, value);
2456 }
2457 
2458 inline CacheEntryIndex State::
2460  (SubsystemIndex subsys, Stage earliest, Stage latest,
2461  bool q, bool u, bool z,
2462  const Array_<DiscreteVarKey>& discreteVars,
2463  const Array_<CacheEntryKey>& cacheEntries,
2464  AbstractValue* value) {
2465  return updImpl().allocateCacheEntryWithPrerequisites
2466  (subsys, earliest, latest, q, u, z, discreteVars, cacheEntries, value);
2467 }
2468 
2469 inline Stage State::
2471  return getImpl().getCacheEntryAllocationStage(CacheEntryKey(subsys,index));
2472 }
2473 inline const AbstractValue& State::
2474 getCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
2475  return getImpl().getCacheEntry(CacheEntryKey(subsys,index));
2476 }
2477 inline AbstractValue& State::
2478 updCacheEntry(SubsystemIndex subsys, CacheEntryIndex index) const {
2479  return getImpl().updCacheEntry(CacheEntryKey(subsys,index));
2480 }
2481 
2482 inline bool State::
2484  return getImpl().isCacheValueRealized(CacheEntryKey(subx,cx));
2485 }
2486 
2487 inline void State::
2489  getImpl().markCacheValueRealized(CacheEntryKey(subx,cx));
2490 }
2491 
2492 inline void State::
2494  getImpl().markCacheValueNotRealized(CacheEntryKey(subx,cx));
2495 }
2496 
2497 inline std::mutex& State::getStateLock() const {
2498  return getImpl().getStateLock();
2499 }
2500 // Global Resource Dimensions
2501 
2502 inline int State::getNY() const {
2503  return getImpl().getNY();
2504 }
2505 inline int State::getNQ() const {
2506  return getImpl().getNQ();
2507 }
2508 inline SystemYIndex State::getQStart() const {
2509  return getImpl().getQStart();
2510 }
2511 inline int State::getNU() const {
2512  return getImpl().getNU();
2513 }
2514 inline SystemYIndex State::getUStart() const {
2515  return getImpl().getUStart();
2516 }
2517 inline int State::getNZ() const {
2518  return getImpl().getNZ();
2519 }
2520 inline SystemYIndex State::getZStart() const {
2521  return getImpl().getZStart();
2522 }
2523 inline int State::getNYErr() const {
2524  return getImpl().getNYErr();
2525 }
2526 inline int State::getNQErr() const {
2527  return getImpl().getNQErr();
2528 }
2529 inline SystemYErrIndex State::getQErrStart() const {
2530  return getImpl().getQErrStart();
2531 }
2532 inline int State::getNUErr() const {
2533  return getImpl().getNUErr();
2534 }
2535 inline SystemYErrIndex State::getUErrStart() const {
2536  return getImpl().getUErrStart();
2537 }
2538 inline int State::getNUDotErr() const {
2539  return getImpl().getNUDotErr();
2540 }
2541 inline int State::getNMultipliers() const {
2542  return getNUDotErr();
2543 }
2544 inline int State::getNEventTriggers() const {
2545  return getImpl().getNEventTriggers();
2546 }
2547 inline int State::getNEventTriggersByStage(Stage stage) const {
2548  return getImpl().getNEventTriggersByStage(stage);
2549 }
2550 
2551 
2552 
2553 // Per-Subsystem Dimensions
2554 inline SystemQIndex State::getQStart(SubsystemIndex subsys) const {
2555  return getImpl().getQStart(subsys);
2556 }
2557 inline int State::getNQ(SubsystemIndex subsys) const {
2558  return getImpl().getNQ(subsys);
2559 }
2560 inline SystemUIndex State::getUStart(SubsystemIndex subsys) const {
2561  return getImpl().getUStart(subsys);
2562 }
2563 inline int State::getNU(SubsystemIndex subsys) const {
2564  return getImpl().getNU(subsys);
2565 }
2566 inline SystemZIndex State::getZStart(SubsystemIndex subsys) const {
2567  return getImpl().getZStart(subsys);
2568 }
2569 inline int State::getNZ(SubsystemIndex subsys) const {
2570  return getImpl().getNZ(subsys);
2571 }
2572 inline SystemQErrIndex State::getQErrStart(SubsystemIndex subsys) const {
2573  return getImpl().getQErrStart(subsys);
2574 }
2575 inline int State::getNQErr(SubsystemIndex subsys) const {
2576  return getImpl().getNQErr(subsys);
2577 }
2578 inline SystemUErrIndex State::getUErrStart(SubsystemIndex subsys) const {
2579  return getImpl().getUErrStart(subsys);
2580 }
2581 inline int State::getNUErr(SubsystemIndex subsys) const {
2582  return getImpl().getNUErr(subsys);
2583 }
2585  return getImpl().getUDotErrStart(subsys);
2586 }
2587 inline int State::getNUDotErr(SubsystemIndex subsys) const {
2588  return getImpl().getNUDotErr(subsys);
2589 }
2591  return SystemMultiplierIndex(getUDotErrStart(subsys));
2592 }
2593 inline int State::getNMultipliers(SubsystemIndex subsys) const {
2594  return getNUDotErr(subsys);
2595 }
2597 getEventTriggerStartByStage(SubsystemIndex subsys, Stage stage) const {
2598  return getImpl().getEventTriggerStartByStage(subsys, stage);
2599 }
2600 inline int State::
2601 getNEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
2602  return getImpl().getNEventTriggersByStage(subsys, stage);
2603 }
2604 
2605 
2606 
2607 inline const Vector& State::
2608 getEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
2609  return getImpl().getEventTriggersByStage(subsys, stage);
2610 }
2611 inline Vector& State::
2612 updEventTriggersByStage(SubsystemIndex subsys, Stage stage) const {
2613  return getImpl().updEventTriggersByStage(subsys, stage);
2614 }
2615 inline const Vector& State::getQ(SubsystemIndex subsys) const {
2616  return getImpl().getQ(subsys);
2617 }
2618 inline const Vector& State::getU(SubsystemIndex subsys) const {
2619  return getImpl().getU(subsys);
2620 }
2621 inline const Vector& State::getZ(SubsystemIndex subsys) const {
2622  return getImpl().getZ(subsys);
2623 }
2624 inline const Vector& State::getUWeights(SubsystemIndex subsys) const {
2625  return getImpl().getUWeights(subsys);
2626 }
2627 inline const Vector& State::getZWeights(SubsystemIndex subsys) const {
2628  return getImpl().getZWeights(subsys);
2629 }
2630 inline Vector& State::updQ(SubsystemIndex subsys) {
2631  return updImpl().updQ(subsys);
2632 }
2633 inline Vector& State::updU(SubsystemIndex subsys) {
2634  return updImpl().updU(subsys);
2635 }
2636 inline Vector& State::updZ(SubsystemIndex subsys) {
2637  return updImpl().updZ(subsys);
2638 }
2639 inline Vector& State::updUWeights(SubsystemIndex subsys) {
2640  return updImpl().updUWeights(subsys);
2641 }
2642 inline Vector& State::updZWeights(SubsystemIndex subsys) {
2643  return updImpl().updZWeights(subsys);
2644 }
2645 inline const Vector& State::getQDot(SubsystemIndex subsys) const {
2646  return getImpl().getQDot(subsys);
2647 }
2648 inline const Vector& State::getUDot(SubsystemIndex subsys) const {
2649  return getImpl().getUDot(subsys);
2650 }
2651 inline const Vector& State::getZDot(SubsystemIndex subsys) const {
2652  return getImpl().getZDot(subsys);
2653 }
2654 inline const Vector& State::getQDotDot(SubsystemIndex subsys) const {
2655  return getImpl().getQDotDot(subsys);
2656 }
2657 inline Vector& State::updQDot(SubsystemIndex subsys) const {
2658  return getImpl().updQDot(subsys);
2659 }
2660 inline Vector& State::updUDot(SubsystemIndex subsys) const {
2661  return getImpl().updUDot(subsys);
2662 }
2663 inline Vector& State::updZDot(SubsystemIndex subsys) const {
2664  return getImpl().updZDot(subsys);
2665 }
2666 inline Vector& State::updQDotDot(SubsystemIndex subsys) const {
2667  return getImpl().updQDotDot(subsys);
2668 }
2669 inline const Vector& State::getQErr(SubsystemIndex subsys) const {
2670  return getImpl().getQErr(subsys);
2671 }
2672 inline const Vector& State::getUErr(SubsystemIndex subsys) const {
2673  return getImpl().getUErr(subsys);
2674 }
2675 inline const Vector& State::getQErrWeights(SubsystemIndex subsys) const {
2676  return getImpl().getQErrWeights(subsys);
2677 }
2678 inline const Vector& State::getUErrWeights(SubsystemIndex subsys) const {
2679  return getImpl().getUErrWeights(subsys);
2680 }
2681 inline const Vector& State::getUDotErr(SubsystemIndex subsys) const {
2682  return getImpl().getUDotErr(subsys);
2683 }
2684 inline const Vector& State::getMultipliers(SubsystemIndex subsys) const {
2685  return getImpl().getMultipliers(subsys);
2686 }
2687 inline Vector& State::updQErr(SubsystemIndex subsys) const {
2688  return getImpl().updQErr(subsys);
2689 }
2690 inline Vector& State::updUErr(SubsystemIndex subsys) const {
2691  return getImpl().updUErr(subsys);
2692 }
2694  return updImpl().updQErrWeights(subsys);
2695 }
2697  return updImpl().updUErrWeights(subsys);
2698 }
2699 inline Vector& State::updUDotErr(SubsystemIndex subsys) const {
2700  return getImpl().updUDotErr(subsys);
2701 }
2702 inline Vector& State::updMultipliers(SubsystemIndex subsys) const {
2703  return getImpl().updMultipliers(subsys);
2704 }
2705 
2707 getEventTriggerStartByStage(Stage stage) const {
2708  return getImpl().getEventTriggerStartByStage(stage);
2709 }
2710 
2711 inline const Vector& State::getEventTriggers() const {
2712  return getImpl().getEventTriggers();
2713 }
2714 inline const Vector& State::getEventTriggersByStage(Stage stage) const {
2715  return getImpl().getEventTriggersByStage(stage);
2716 }
2717 
2718 inline Vector& State::updEventTriggers() const {
2719  return getImpl().updEventTriggers();
2720 }
2721 inline Vector& State::updEventTriggersByStage(Stage stage) const {
2722  return getImpl().updEventTriggersByStage(stage);
2723 }
2724 
2725 inline const Real& State::getTime() const {
2726  return getImpl().getTime();
2727 }
2728 inline const Vector& State::getY() const {
2729  return getImpl().getY();
2730 }
2731 inline const Vector& State::getQ() const {
2732  return getImpl().getQ();
2733 }
2734 inline const Vector& State::getU() const {
2735  return getImpl().getU();
2736 }
2737 inline const Vector& State::getZ() const {
2738  return getImpl().getZ();
2739 }
2740 inline const Vector& State::getUWeights() const {
2741  return getImpl().getUWeights();
2742 }
2743 inline const Vector& State::getZWeights() const {
2744  return getImpl().getZWeights();
2745 }
2746 Real& State::updTime() {
2747  return updImpl().updTime();
2748 }
2749 inline Vector& State::updY() {
2750  return updImpl().updY();
2751 }
2752 inline void State::setTime(Real t) {
2753  updTime() = t;
2754 }
2755 inline void State::setY(const Vector& y) {
2756  updY() = y;
2757 }
2758 inline Vector& State::updQ() {
2759  return updImpl().updQ();
2760 }
2761 inline Vector& State::updU() {
2762  return updImpl().updU();
2763 }
2764 inline Vector& State::updZ() {
2765  return updImpl().updZ();
2766 }
2767 inline Vector& State::updUWeights() {
2768  return updImpl().updUWeights();
2769 }
2770 inline Vector& State::updZWeights() {
2771  return updImpl().updZWeights();
2772 }
2773 inline void State::setQ(const Vector& q) {
2774  updQ() = q;
2775 }
2776 inline void State::setU(const Vector& u) {
2777  updU() = u;
2778 }
2779 inline void State::setZ(const Vector& z) {
2780  updZ() = z;
2781 }
2782 inline const Vector& State::getYDot() const {
2783  return getImpl().getYDot();
2784 }
2785 inline const Vector& State::getQDot() const {
2786  return getImpl().getQDot();
2787 }
2788 inline const Vector& State::getZDot() const {
2789  return getImpl().getZDot();
2790 }
2791 inline const Vector& State::getUDot() const {
2792  return getImpl().getUDot();
2793 }
2794 inline const Vector& State::getQDotDot() const {
2795  return getImpl().getQDotDot();
2796 }
2797 inline Vector& State::updYDot() const {
2798  return getImpl().updYDot();
2799 }
2800 inline Vector& State::updQDot() const {
2801  return getImpl().updQDot();
2802 }
2803 inline Vector& State::updZDot() const {
2804  return getImpl().updZDot();
2805 }
2806 inline Vector& State::updUDot() const {
2807  return getImpl().updUDot();
2808 }
2809 inline Vector& State::updQDotDot() const {
2810  return getImpl().updQDotDot();
2811 }
2812 inline const Vector& State::getYErr() const {
2813  return getImpl().getYErr();
2814 }
2815 inline const Vector& State::getQErr() const {
2816  return getImpl().getQErr();
2817 }
2818 inline const Vector& State::getUErr() const {
2819  return getImpl().getUErr();
2820 }
2821 inline const Vector& State::getQErrWeights() const {
2822  return getImpl().getQErrWeights();
2823 }
2824 inline const Vector& State::getUErrWeights() const {
2825  return getImpl().getUErrWeights();
2826 }
2827 inline const Vector& State::getUDotErr() const {
2828  return getImpl().getUDotErr();
2829 }
2830 inline const Vector& State::getMultipliers() const {
2831  return getImpl().getMultipliers();
2832 }
2833 inline Vector& State::updYErr() const {
2834  return getImpl().updYErr();
2835 }
2836 inline Vector& State::updQErr() const {
2837  return getImpl().updQErr();
2838 }
2839 inline Vector& State::updUErr() const {
2840  return getImpl().updUErr();
2841 }
2842 inline Vector& State::updQErrWeights() {
2843  return updImpl().updQErrWeights();
2844 }
2845 inline Vector& State::updUErrWeights() {
2846  return updImpl().updUErrWeights();
2847 }
2848 inline Vector& State::updUDotErr() const {
2849  return getImpl().updUDotErr();
2850 }
2851 inline Vector& State::updMultipliers() const {
2852  return getImpl().updMultipliers();
2853 }
2854 
2855 inline void State::
2857 { return updImpl().setSystemTopologyStageVersion(topoVersion); }
2858 
2859 inline void State::
2860 getSystemStageVersions(Array_<StageVersion>& versions) const {
2861  return getImpl().getSystemStageVersions(versions);
2862 }
2863 inline Stage State::
2864 getLowestSystemStageDifference(const Array_<StageVersion>& prev) const {
2865  return getImpl().getLowestSystemStageDifference(prev);
2866 }
2867 
2868 inline ValueVersion State::getQValueVersion() const {
2869  return getImpl().getQValueVersion();
2870 }
2871 
2872 inline ValueVersion State::getUValueVersion() const {
2873  return getImpl().getUValueVersion();
2874 }
2875 
2876 inline ValueVersion State::getZValueVersion() const {
2877  return getImpl().getZValueVersion();
2878 }
2879 
2880 inline const ListOfDependents& State::getQDependents() const {
2881  return getImpl().getQDependents();
2882 }
2883 
2884 inline const ListOfDependents& State::getUDependents() const {
2885  return getImpl().getUDependents();
2886 }
2887 
2888 inline const ListOfDependents& State::getZDependents() const {
2889  return getImpl().getZDependents();
2890 }
2891 
2892 inline bool State::hasCacheEntry(const CacheEntryKey& cacheEntry) const {
2893  return getImpl().hasCacheEntry(cacheEntry);
2894 }
2895 
2896 inline const CacheEntryInfo& State::
2897 getCacheEntryInfo(const CacheEntryKey& cacheEntry) const {
2898  return getImpl().getCacheEntryInfo(cacheEntry);
2899 }
2900 
2901 inline CacheEntryInfo& State::
2902 updCacheEntryInfo(const CacheEntryKey& cacheEntry) {
2903  return updImpl().updCacheEntryInfo(cacheEntry);
2904 }
2905 
2906 inline bool State::hasDiscreteVar(const DiscreteVarKey& discreteVar) const {
2907  return getImpl().hasDiscreteVar(discreteVar);
2908 }
2909 
2910 inline const DiscreteVarInfo& State::
2911 getDiscreteVarInfo(const DiscreteVarKey& discreteVar) const {
2912  return getImpl().getDiscreteVarInfo(discreteVar);
2913 }
2914 
2915 inline const PerSubsystemInfo& State::
2916 getPerSubsystemInfo(SubsystemIndex subx) const {
2917  return getImpl().getSubsystem(subx);
2918 }
2919 
2920 
2921 inline void State::autoUpdateDiscreteVariables() {
2922  updImpl().autoUpdateDiscreteVariables();
2923 }
2924 
2925 inline String State::toString() const {
2926  return getImpl().toString();
2927 }
2928 inline String State::cacheToString() const {
2929  return getImpl().cacheToString();
2930 }
2931 
2932 
2933 } // namespace SimTK
2934  // End of hiding from Doxygen
2936 
2937 #endif // SimTK_SimTKCOMMON_STATE_IMPL_H_
2938 
2939 
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
#define SimTK_ASSERT2(cond, msg, a1, a2)
Definition: ExceptionMacros.h:375
#define SimTK_STAGECHECK_RANGE_ALWAYS(lower, current, upper, methodNm)
Definition: ExceptionMacros.h:186
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
#define SimTK_STAGECHECK_GE_ALWAYS(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:180
#define SimTK_ASSERT2_ALWAYS(cond, msg, a1, a2)
Definition: ExceptionMacros.h:353
#define SimTK_ERRCHK4_ALWAYS(cond, whereChecked, fmt, a1, a2, a3, a4)
Definition: ExceptionMacros.h:297
#define SimTK_STAGECHECK_GE(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:208
#define SimTK_STAGECHECK_LT_ALWAYS(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:183
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
#define SimTK_FORCE_INLINE
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:269
This unique integer type is for selecting non-shared cache entries.
This unique integer type is for selecting discrete variables.
Unique integer type for Subsystem-local, per-stage event indexing.
Unique integer type for Subsystem-local qErr indexing.
Unique integer type for Subsystem-local q indexing.
@ NValid
Definition: Stage.h:88
@ Time
A new time has been realized.
Definition: Stage.h:73
@ LowestRuntime
For iterating over meaningful stage values.
Definition: Stage.h:83
@ Topology
System topology realized.
Definition: Stage.h:70
@ Empty
Lower than any legitimate Stage.
Definition: Stage.h:69
@ HighestRuntime
Definition: Stage.h:84
@ Acceleration
Accelerations and multipliers calculated.
Definition: Stage.h:77
@ Position
Spatial configuration available.
Definition: Stage.h:74
@ Dynamics
Forces calculated.
Definition: Stage.h:76
@ Velocity
Spatial velocities available.
Definition: Stage.h:75
@ Model
Modeling choices made.
Definition: Stage.h:71
@ Infinity
Higher than any legitimate Stage.
Definition: Stage.h:79
@ Instance
Physical parameters set.
Definition: Stage.h:72
@ Report
Report-only quantities evaluated.
Definition: Stage.h:78
Vector & updQ()
These are just views into Y.
SystemEventTriggerIndex getEventTriggerStartByStage(Stage) const
Return the index within the global event trigger array at which the first of the event triggers assoc...
int getNYErr() const
Get the total number nyerr=nqerr+nuerr of shared cache entries for position-level and velocity-level ...
void invalidateAll(Stage)
If any subsystem or the system stage is currently at or higher than the passed-in one,...
void setDiscreteVariable(SubsystemIndex, DiscreteVariableIndex, const AbstractValue &)
Alternate interface to updDiscreteVariable.
int getNQErr() const
Return the total number nqerr=mp+nQuaternions of cache entries for position-level constraint errors.
int getNEventTriggers() const
Return the total number of event trigger function slots in the cache.
int getNUDotErr() const
Return the total number nudotErr=mp+mv+ma of cache entries for acceleration-level constraint errors (...
void setU(const Vector &u)
void setY(const Vector &y)
const ListOfDependents & getUDependents() const
(Advanced) Return the list of cache entries for which u was specified as an explicit prerequisite.
const ListOfDependents & getQDependents() const
(Advanced) Return the list of cache entries for which q was specified as an explicit prerequisite.
void setSystemTopologyStageVersion(StageVersion topoVersion)
(Advanced) This explicitly modifies the Topology stage version; don't use this method unless you know...
SystemYErrIndex getUErrStart() const
Returns the yErr index at which the uErr's begin. Callable at Instance stage.
Vector & updZ()
Vector & updYDot() const
These are mutable.
AbstractValue & updCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a writable reference to the value contained in a particular cache entry.
const Vector & getYErr() const
Return the current constraint errors for all constraints.
const Vector & getUDot() const
void markCacheValueNotRealized(SubsystemIndex, CacheEntryIndex) const
(Advanced) Normally cache entries are invalidated automatically, however this method allows manual in...
void autoUpdateDiscreteVariables()
(Advanced) This is called at the beginning of every integration step to set the values of auto-update...
Vector & updU()
QIndex allocateQ(SubsystemIndex, const Vector &qInit)
Allocate generalized coordinates q, which are second order continuous state variables.
int getNQ() const
Get total number of shared q's (generalized coordinates; second order state variables).
bool hasDiscreteVar(const DiscreteVarKey &discreteVar) const
(Advanced) Check whether this State has a particular discrete state variable.
Vector & updMultipliers() const
const Vector & getQDotDot() const
This has its own space, not a view.
String cacheToString() const
(Debugging) Not suitable for serialization.
const Stage & getSystemStage() const
This returns the global stage for this State.
bool hasCacheEntry(const CacheEntryKey &cacheEntry) const
(Advanced) Check whether this State has a particular cache entry.
CacheEntryInfo & updCacheEntryInfo(const CacheEntryKey &cacheEntry)
(Advanced) Return a writable reference to the cache entry information for a particular cache entry.
ValueVersion getQValueVersion() const
(Advanced) Return a ValueVersion for q, meaning an integer that is incremented whenever any q is chan...
const Real & getTime() const
You can call these as long as system stage >= Model.
bool isDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Check whether the update value for this auto-update discrete variable has already been computed since...
const DiscreteVarInfo & getDiscreteVarInfo(const DiscreteVarKey &discreteVar) const
(Advanced) Return a reference to the discrete variable information for a particular discrete variable...
Vector & updQDot() const
void setZ(const Vector &z)
const String & getSubsystemVersion(SubsystemIndex) const
QErrIndex allocateQErr(SubsystemIndex, int nqerr) const
Allocate nqerr cache slots to hold the current error for position-level (holonomic) constraint equati...
Vector & updUErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraints.
const AbstractValue & getDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the current value of its associated update cache entry...
ValueVersion getZValueVersion() const
(Advanced) Return a ValueVersion for z, meaning an integer that is incremented whenever any z is chan...
void setNumSubsystems(int i)
Set the number of subsystems in this state.
Stage getDiscreteVarInvalidatesStage(SubsystemIndex, DiscreteVariableIndex) const
What is the earliest stage that is invalidated when this discrete variable is modified?...
SystemYIndex getUStart() const
Returns the y index at which the u's begin. Callable at Model stage.
const Vector & getY() const
SystemYErrIndex getQErrStart() const
Returns the yErr index at which the qErr's begin. Callable at Instance stage.
const Vector & getU() const
Vector & updYErr() const
These are mutable.
AbstractValue & updDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return a writable reference to the value of its associated up...
int getNumSubsystems() const
Return the number of Subsystems known to this State.
CacheEntryIndex allocateCacheEntryWithPrerequisites(SubsystemIndex, Stage earliest, Stage latest, bool q, bool u, bool z, const Array_< DiscreteVarKey > &discreteVars, const Array_< CacheEntryKey > &cacheEntries, AbstractValue *value)
(Advanced) Allocate a cache entry with prerequisites other than just reaching a particular computatio...
const Vector & getUErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraint equation...
int getNU() const
Get total number of shared u's (generalized speeds; mobilities).
const Vector & getYDot() const
const Vector & getMultipliers() const
Vector & updQErr() const
const Vector & getZDot() const
const String & getSubsystemName(SubsystemIndex) const
Stage getDiscreteVarAllocationStage(SubsystemIndex, DiscreteVariableIndex) const
At what stage was this State when this discrete variable was allocated? The answer must be Stage::Emp...
SystemYIndex getQStart() const
Returns the y index at which the q's begin. Callable at Model stage.
const Vector & getUDotErr() const
These have their own space, they are not views.
AbstractValue & updDiscreteVariable(SubsystemIndex, DiscreteVariableIndex)
Get a writable reference to the value stored in the indicated discrete state variable dv,...
SystemMultiplierIndex getMultipliersStart(SubsystemIndex) const
Real getDiscreteVarLastUpdateTime(SubsystemIndex, DiscreteVariableIndex) const
Return the time of last update for this discrete variable.
Vector & updEventTriggersByStage(Stage) const
Vector & updUDot() const
StageVersion getSystemTopologyStageVersion() const
The Topology stage version number (an integer) stored in this State must match the topology cache ver...
void advanceSubsystemToStage(SubsystemIndex, Stage) const
Advance a particular Subsystem's current stage by one to the indicated stage.
const Vector & getQErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mquat position inline constraints equations.
void advanceSystemToStage(Stage) const
Advance the System-level current stage by one to the indicated stage.
void setQ(const Vector &q)
Alternate interface.
const Vector & getZ() const
void markCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Mark the value of a particular cache entry as up to date after it has been recalculated.
UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const
Allocate nudoterr cache slots to hold the current error for acceleration-level (acceleration-only,...
Vector & updUErr() const
Vector & updQDotDot() const
This is a separate shared cache entry, not part of YDot.
const ListOfDependents & getZDependents() const
(Advanced) Return the list of cache entries for which z was specified as an explicit prerequisite.
SystemYIndex getZStart() const
Returns the y index at which the z's begin. Callable at Model stage.
const AbstractValue & getCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a const reference to the value contained in a particular cache entry.
std::mutex & getStateLock() const
Returns a mutex that should be used to lock the state whenever multiple threads are asynchronously wr...
CacheEntryIndex getDiscreteVarUpdateIndex(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the CacheEntryIndex for its associated update cache en...
Vector & updUWeights()
Set u weights (and q weights indirectly).
UErrIndex allocateUErr(SubsystemIndex, int nuerr) const
Allocate nuerr cache slots to hold the current error for velocity-level (nonholonomic and holonomic f...
Stage getCacheEntryAllocationStage(SubsystemIndex, CacheEntryIndex) const
At what stage was this State when this cache entry was allocated? The answer must be Stage::Empty,...
ValueVersion getUValueVersion() const
(Advanced) Return a ValueVersion for u, meaning an integer that is incremented whenever any u is chan...
int getNZ() const
Get total number of shared z's (auxiliary state variables).
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *, Stage updateDependsOn)
This method allocates a DiscreteVariable whose value should be updated automatically after each time ...
int getNY() const
Get the total number ny=nq+nu+nz of shared continuous state variables.
void invalidateAllCacheAtOrAbove(Stage) const
If any subsystem or the system stage is currently at or higher than the passed-in one,...
const Vector & getZWeights() const
Get a unit weighting (1/unit change) for each z that can be used to weight a vector dz so that the di...
int getNUErr() const
Return the total number nuerr=mp+mv of cache entries for velocity-level constraint errors (including ...
SubsystemIndex addSubsystem(const String &name, const String &version)
Register a new subsystem as a client of this State.
const Stage & getSubsystemStage(SubsystemIndex) const
int getNMultipliers() const
Return the total number of constraint multipliers; necessarily the same as the number of acceleration...
const Vector & getUWeights() const
Get a unit weighting (1/unit change) for each u that can be used to weight a vector du so that the di...
const Vector & getUErr() const
const AbstractValue & getDiscreteVariable(SubsystemIndex, DiscreteVariableIndex) const
Get the current value of the indicated discrete variable.
Stage getLowestSystemStageDifference(const Array_< StageVersion > &prevVersions) const
(Advanced) Given a list of per-stage version numbers extracted by an earlier call to getSystemStageVe...
SystemUDotErrIndex getUDotErrStart(SubsystemIndex) const
const Vector & getQErr() const
These are just views into YErr.
String toString() const
(Debugging) Not suitable for serialization.
Real & updTime()
You can call these as long as System stage >= Model, but the stage will be backed up if necessary to ...
Vector & updZWeights()
Set z weights.
CacheEntryIndex allocateCacheEntry(SubsystemIndex, Stage earliest, Stage latest, AbstractValue *value) const
There are two Stages supplied explicitly as arguments to this method: earliest and latest.
Vector & updEventTriggers() const
Vector & updZDot() const
const Vector & getEventTriggersByStage(Stage) const
const CacheEntryInfo & getCacheEntryInfo(const CacheEntryKey &cacheEntry) const
(Advanced) Return a const reference to the cache entry information for a particular cache entry.
Vector & updUDotErr() const
void markDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Mark the update value for this auto-update discrete variable as up-to-date with respect to the state ...
Vector & updY()
UIndex allocateU(SubsystemIndex, const Vector &uInit)
Allocate generalized speeds u, which are first order continuous state variables related to the deriva...
int getNEventTriggersByStage(Stage) const
Return the size of the partition of event trigger functions which are evaluated at a given Stage.
void initializeSubsystem(SubsystemIndex, const String &name, const String &version)
Set the name and version for a given subsystem, which must already have a slot allocated.
DiscreteVariableIndex allocateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *)
The Stage supplied here in the call is the earliest subsystem stage which is invalidated by a change ...
const Vector & getEventTriggers() const
EventTriggerByStageIndex allocateEventTrigger(SubsystemIndex, Stage stage, int nevent) const
Allocate room for nevent witness function values that will be available at the indicated stage.
ZIndex allocateZ(SubsystemIndex, const Vector &zInit)
Allocate auxiliary first order continuous state variables z.
Vector & updQErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mquat position inline constraint equations.
void setTime(Real t)
An alternate syntax equivalent to updTime() and updY().
bool isCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Check whether the value in a particular cache entry has been recalculated since the last change to th...
const PerSubsystemInfo & getPerSubsystemInfo(SubsystemIndex) const
(Advanced) Return a reference to the per-subsystem information in the state.
void getSystemStageVersions(Array_< StageVersion > &versions) const
(Advanced) Record the current version numbers of each valid System-level stage.
const Vector & getQ() const
These are just views into Y.
const Vector & getQDot() const
These are just views into YDot.
Provide a unique integer type for identifying Subsystems.
This unique integer type is for identifying a triggered event within a particular Stage of the full S...
This unique integer type is for identifying a triggered event in the full System-level view of the St...
This unique integer type is for indexing global "multiplier-like" arrays, that is,...
This unique integer type is for indexing global "qErr-like" arrays, that is, arrays that inherently h...
This unique integer type is for indexing global "q-like" arrays, that is, arrays that inherently have...
This unique integer type is for indexing global "uDotErr-like" arrays, that is, arrays that inherentl...
This unique integer type is for indexing global "uErr-like" arrays, that is, arrays that inherently h...
This unique integer type is for indexing global "u-like" arrays, that is, arrays that inherently have...
This unique integer type is for indexing the global, System-level "yErr-like" arrays,...
This unique integer type is for indexing the global, System-level "y-like" arrays,...
This unique integer type is for indexing global "z-like" arrays, that is, arrays that inherently have...
Unique integer type for Subsystem-local uDotErr indexing.
Unique integer type for Subsystem-local uErr indexing.
Unique integer type for Subsystem-local u indexing.
Unique integer type for Subsystem-local z indexing.
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1477
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
std::pair< SubsystemIndex, DiscreteVariableIndex > DiscreteVarKey
Definition: State.h:165
long long StageVersion
This is the type to use for Stage version numbers that get incremented whenever a state variable chan...
Definition: Stage.h:44
long long ValueVersion
This is the type to use for state variable version numbers that get incremented whenever a state valu...
Definition: Stage.h:52
std::pair< SubsystemIndex, CacheEntryIndex > CacheEntryKey
Definition: State.h:164
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607