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