1 #ifndef SimTK_SimTKCOMMON_STATE_IMPL_H_ 2 #define SimTK_SimTKCOMMON_STATE_IMPL_H_ 62 class ListOfDependents {
63 using CacheList = Array_<CacheEntryKey>;
65 using size_type = CacheList::size_type;
67 using iterator = CacheList::iterator;
68 using const_iterator = CacheList::const_iterator;
72 void clear() {m_dependents.clear();}
73 bool empty()
const {
return m_dependents.empty();}
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();}
89 "ListOfDependents::contains(): invalid cache key (%d,%d).",
90 (
int)ck.first, (
int)ck.second);
91 return std::find(cbegin(), cend(), ck) != cend();
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);
107 inline void notePrerequisiteChange(
const StateImpl& stateImpl)
const;
114 "ListOfDependents::removeDependent(): invalid cache key (%d,%d).",
115 (
int)ck.first, (
int)ck.second);
117 auto p = std::find(begin(), end(), ck);
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);
126 {
return ck.first.isValid() && ck.second.isValid(); }
129 CacheList m_dependents;
155 class DiscreteVarInfo {
157 DiscreteVarInfo() =
default;
159 DiscreteVarInfo(Stage allocation, Stage invalidated, AbstractValue* v)
160 : m_allocationStage(allocation), m_invalidatedStage(invalidated),
162 { assert(isReasonable()); }
169 DiscreteVarInfo& deepAssign(
const DiscreteVarInfo& src) {
175 void deepDestruct(StateImpl&) {
179 const Stage& getAllocationStage()
const {
return m_allocationStage;}
182 void swapValue(
Real updTime, ClonePtr<AbstractValue>& other)
183 { m_value.swap(other); m_timeLastUpdated=updTime; }
185 const AbstractValue& getValue()
const {assert(m_value);
return *m_value;}
190 AbstractValue& updValue(
const StateImpl& stateImpl,
Real updTime) {
193 m_timeLastUpdated=updTime;
194 m_dependents.notePrerequisiteChange(stateImpl);
197 ValueVersion getValueVersion()
const {
return m_valueVersion;}
198 Real getTimeLastUpdated()
const 199 { assert(m_value);
return m_timeLastUpdated; }
201 const Stage& getInvalidatedStage()
const {
return m_invalidatedStage;}
202 CacheEntryIndex getAutoUpdateEntry()
const {
return m_autoUpdateEntry;}
203 void setAutoUpdateEntry(CacheEntryIndex cx) {m_autoUpdateEntry = cx;}
205 const ListOfDependents& getDependents()
const {
return m_dependents;}
206 ListOfDependents& updDependents() {
return m_dependents;}
210 Stage m_allocationStage;
211 Stage m_invalidatedStage;
212 CacheEntryIndex m_autoUpdateEntry;
218 ResetOnCopy<ListOfDependents> m_dependents;
221 ClonePtr<AbstractValue> m_value;
225 bool isReasonable()
const 228 && (m_invalidatedStage > m_allocationStage)
229 && (m_value !=
nullptr); }
264 Stage allocation, Stage dependsOn, Stage computedBy,
265 AbstractValue* value)
267 m_allocationStage(allocation), m_dependsOnStage(dependsOn),
268 m_computedByStage(computedBy), m_value(value)
269 { assert(isReasonable()); }
271 CacheEntryInfo& setPrerequisiteQ() {m_qIsPrerequisite =
true;
return *
this;}
272 CacheEntryInfo& setPrerequisiteU() {m_uIsPrerequisite =
true;
return *
this;}
273 CacheEntryInfo& setPrerequisiteZ() {m_zIsPrerequisite =
true;
return *
this;}
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);
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);
297 void registerWithPrerequisites(StateImpl&);
304 void unregisterWithPrerequisites(StateImpl&)
const;
314 inline void markAsUpToDate(
const StateImpl&);
319 void throwHelpfulOutOfDateMessage(
const StateImpl&,
320 const char* funcName)
const;
327 void invalidate(
const StateImpl& stateImpl) {
329 m_isUpToDateWithPrerequisites =
false;
331 m_dependents.notePrerequisiteChange(stateImpl);
335 CacheEntryInfo& deepAssign(
const CacheEntryInfo& src) {
341 void deepDestruct(StateImpl& stateImpl) {
343 unregisterWithPrerequisites(stateImpl);
346 const Stage& getAllocationStage()
const {
return m_allocationStage;}
351 void swapValue(
Real updTime, DiscreteVarInfo& dv)
352 { dv.swapValue(updTime, m_value); }
354 const AbstractValue& getValue()
const {assert(m_value);
return *m_value;}
362 AbstractValue& updValue(
const StateImpl& stateImpl) {
366 ValueVersion getValueVersion()
const {
return m_valueVersion;}
370 StageVersion getDependsOnVersionWhenLastComputed()
const 371 {
return m_dependsOnVersionWhenLastComputed; }
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;}
378 bool isQPrerequisite()
const {
return m_qIsPrerequisite;}
379 bool isUPrerequisite()
const {
return m_uIsPrerequisite;}
380 bool isZPrerequisite()
const {
return m_zIsPrerequisite;}
382 return std::find(m_discreteVarPrerequisites.cbegin(),
383 m_discreteVarPrerequisites.cend(), dk)
384 != m_discreteVarPrerequisites.cend();
387 return std::find(m_cacheEntryPrerequisites.cbegin(),
388 m_cacheEntryPrerequisites.cend(), ck)
389 != m_cacheEntryPrerequisites.cend();
393 void recordPrerequisiteVersions(
const StateImpl&);
394 void validatePrerequisiteVersions(
const StateImpl&)
const;
397 const ListOfDependents& getDependents()
const {
return m_dependents;}
398 ListOfDependents& updDependents() {
return m_dependents;}
403 Stage m_allocationStage;
404 Stage m_dependsOnStage;
405 Stage m_computedByStage;
406 DiscreteVariableIndex m_associatedVar;
410 bool m_qIsPrerequisite{
false},
411 m_uIsPrerequisite{
false},
412 m_zIsPrerequisite{
false};
413 Array_<DiscreteVarKey> m_discreteVarPrerequisites;
414 Array_<CacheEntryKey> m_cacheEntryPrerequisites;
421 ResetOnCopy<ListOfDependents> m_dependents;
427 ClonePtr<AbstractValue> m_value;
430 bool m_isUpToDateWithPrerequisites{
true};
438 ValueVersion m_qVersion{0}, m_uVersion{0}, m_zVersion{0};
439 Array_<ValueVersion> m_discreteVarVersions;
440 Array_<ValueVersion> m_cacheEntryVersions;
443 bool isReasonable()
const {
447 && (m_computedByStage >= m_dependsOnStage)
448 && (m_value !=
nullptr)
449 && (m_dependsOnVersionWhenLastComputed >= 0)
450 && (ListOfDependents::isCacheEntryKeyValid(m_myKey));
462 : allocationStage(Stage::Empty), firstIndex(-1), nslots(0) {}
464 TriggerInfo(Stage allocation,
int index,
int n)
465 : allocationStage(allocation), firstIndex(index), nslots(n)
466 { assert(isReasonable()); assert(n>0);}
471 int getFirstIndex()
const {
return firstIndex;}
472 int getNumSlots()
const {
return nslots;}
475 TriggerInfo& deepAssign(
const TriggerInfo& src)
476 {
return operator=(src); }
477 void deepDestruct(StateImpl&) {}
478 const Stage& getAllocationStage()
const {
return allocationStage;}
481 Stage allocationStage;
485 bool isReasonable()
const {
505 class ContinuousVarInfo {
507 ContinuousVarInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
509 ContinuousVarInfo(Stage allocation,
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));
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;}
530 ContinuousVarInfo& deepAssign(
const ContinuousVarInfo& src)
531 {
return operator=(src); }
532 void deepDestruct(StateImpl&) {}
533 const Stage& getAllocationStage()
const {
return allocationStage;}
536 Stage allocationStage;
541 static bool weightsAreOK(
const Vector& wts) {
542 for (
int i=0; i<wts.size(); ++i)
543 if (wts[i] <= 0)
return false;
547 bool isReasonable()
const {
557 class ConstraintErrInfo {
559 ConstraintErrInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
561 ConstraintErrInfo(Stage allocation,
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;
573 int getFirstIndex()
const {
return firstIndex;}
574 int getNumErrs()
const {
return weights.size();}
575 const Vector& getWeights()
const {
return weights;}
581 ConstraintErrInfo& deepAssign(
const ConstraintErrInfo& src)
582 {
return operator=(src); }
583 void deepDestruct(StateImpl&) {}
584 const Stage& getAllocationStage()
const {
return allocationStage;}
587 Stage allocationStage;
591 static bool weightsAreOK(
const Vector& wts) {
592 for (
int i=0; i<wts.size(); ++i)
593 if (wts[i] <= 0)
return false;
597 bool isReasonable()
const {
613 explicit PerSubsystemInfo(StateImpl& stateImpl,
614 const String& n=
"",
const String& v=
"")
615 : m_stateImpl(stateImpl), name(n), version(v)
619 ~PerSubsystemInfo() {
630 PerSubsystemInfo(
const PerSubsystemInfo& src) {
636 PerSubsystemInfo& operator=(
const PerSubsystemInfo& src) {
650 void invalidateStageJustThisSubsystem(Stage g) {
652 restoreToStage(g.prev());
658 void advanceToStage(Stage g)
const {
660 assert(currentStage == g.prev());
667 void clearReferencesToModelStageGlobals() {
668 qstart.invalidate(); ustart.invalidate();
670 q.clear(); u.clear(); z.clear();
671 uWeights.clear(); zWeights.clear();
672 qdot.clear(); udot.clear(); zdot.clear(); qdotdot.clear();
675 void clearReferencesToInstanceStageGlobals() {
677 qerrWeights.clear(); uerrWeights.clear();
680 qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
681 qerr.clear();uerr.clear();udoterr.clear();multipliers.clear();
684 triggerstart[j].invalidate();
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());
694 UIndex getNextUIndex()
const {
695 if (uInfo.empty())
return UIndex(0);
696 const ContinuousVarInfo& last = uInfo.back();
697 return UIndex(last.getFirstIndex()+last.getNumVars());
699 ZIndex getNextZIndex()
const {
700 if (zInfo.empty())
return ZIndex(0);
701 const ContinuousVarInfo& last = zInfo.back();
702 return ZIndex(last.getFirstIndex()+last.getNumVars());
705 QErrIndex getNextQErrIndex()
const {
706 if (qerrInfo.empty())
return QErrIndex(0);
707 const ConstraintErrInfo& last = qerrInfo.back();
708 return QErrIndex(last.getFirstIndex()+last.getNumErrs());
710 UErrIndex getNextUErrIndex()
const {
711 if (uerrInfo.empty())
return UErrIndex(0);
712 const ConstraintErrInfo& last = uerrInfo.back();
713 return UErrIndex(last.getFirstIndex()+last.getNumErrs());
715 UDotErrIndex getNextUDotErrIndex()
const {
716 if (udoterrInfo.empty())
return UDotErrIndex(0);
717 const ConstraintErrInfo& last = udoterrInfo.back();
718 return UDotErrIndex(last.getFirstIndex()+last.getNumErrs());
720 DiscreteVariableIndex getNextDiscreteVariableIndex()
const {
721 return DiscreteVariableIndex(discreteInfo.size());
723 CacheEntryIndex getNextCacheEntryIndex()
const {
724 return CacheEntryIndex(cacheInfo.size());
726 EventTriggerByStageIndex getNextEventTriggerByStageIndex(Stage g)
const {
727 if (triggerInfo[g].empty())
return EventTriggerByStageIndex(0);
728 const TriggerInfo& last = triggerInfo[g].back();
729 return EventTriggerByStageIndex
730 (last.getFirstIndex()+last.getNumSlots());
733 bool hasDiscreteVar(DiscreteVariableIndex index)
const {
734 return index < (int)discreteInfo.size();
739 getDiscreteVarInfo(DiscreteVariableIndex index)
const {
741 "PerSubsystemInfo::getDiscreteVarInfo()");
742 return discreteInfo[index];
746 updDiscreteVarInfo(DiscreteVariableIndex index) {
748 "PerSubsystemInfo::updDiscreteVarInfo()");
749 return discreteInfo[index];
753 bool hasCacheEntry(CacheEntryIndex index)
const {
754 return index < (int)cacheInfo.size();
758 getCacheEntryInfo(CacheEntryIndex index)
const {
760 "PerSubsystemInfo::getCacheEntryInfo()");
761 return cacheInfo[index];
765 updCacheEntryInfo(CacheEntryIndex index)
const {
767 "PerSubsystemInfo::updCacheEntryInfo()");
768 return cacheInfo[index];
773 {
return stageVersions[g]; }
776 friend class StateImpl;
777 ReferencePtr<StateImpl> m_stateImpl;
797 Array_<ContinuousVarInfo> q_info, uInfo, zInfo;
798 Array_<DiscreteVarInfo> discreteInfo;
801 mutable Array_<ConstraintErrInfo> qerrInfo, uerrInfo, udoterrInfo;
803 mutable Array_<CacheEntryInfo> cacheInfo;
819 Vector uWeights, zWeights;
821 mutable Vector qdot, udot, zdot, qdotdot;
824 Vector qerrWeights, uerrWeights;
827 mutable SystemQErrIndex qerrstart;
828 mutable SystemUErrIndex uerrstart;
829 mutable SystemUDotErrIndex udoterrstart;
830 mutable SystemEventTriggerByStageIndex triggerstart[
Stage::NValid];
832 mutable Vector qerr, uerr;
834 mutable Vector udoterr, multipliers;
847 mutable Stage currentStage;
855 qstart.invalidate();ustart.invalidate();zstart.invalidate();
856 qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
858 triggerstart[j].invalidate();
859 stageVersions[j] = 1;
866 void clearContinuousVars();
867 void clearConstraintErrs();
868 void clearDiscreteVars();
869 void clearEventTriggers(
int g);
872 void clearAllStacks();
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);
880 void popAllStacksBackToStage(
const Stage& g);
883 void copyContinuousVarInfoThroughStage
884 (
const Array_<ContinuousVarInfo>& src,
const Stage& g,
885 Array_<ContinuousVarInfo>& dest);
887 void copyDiscreteVarsThroughStage
888 (
const Array_<DiscreteVarInfo>& src,
const Stage& g);
891 void copyConstraintErrInfoThroughStage
892 (
const Array_<ConstraintErrInfo>& src,
const Stage& g,
893 Array_<ConstraintErrInfo>& dest);
895 void copyCacheThroughStage
896 (
const Array_<CacheEntryInfo>& src,
const Stage& g);
898 void copyEventsThroughStage
899 (
const Array_<TriggerInfo>& src,
const Stage& g,
900 Array_<TriggerInfo>& dest);
902 void copyAllStacksThroughStage(
const PerSubsystemInfo& src,
const Stage& g);
907 void restoreToStage(Stage g);
915 void copyFrom(
const PerSubsystemInfo& src, Stage maxStage);
919 void clearAllocationStack(Array_<T>& stack);
921 void resizeAllocationStack(Array_<T>& stack,
int newSize);
923 void popAllocationStackBackToStage(Array_<T>& stack,
const Stage&);
925 void copyAllocationStackThroughStage(Array_<T>& stack,
926 const Array_<T>& src,
const Stage&);
943 systemStageVersions[i] = 1;
948 StateImpl(
const StateImpl& src);
950 StateImpl& operator=(
const StateImpl& src);
955 StateImpl* clone()
const {
return new StateImpl(*
this);}
957 const Stage& getSystemStage()
const {
return currentSystemStage;}
958 Stage& updSystemStage()
const {
return currentSystemStage;}
962 getSubsystem(SubsystemIndex subx)
const {
964 "StateImpl::getSubsystem()");
965 return subsystems[subx];
969 updSubsystem(SubsystemIndex subx) {
971 "StateImpl::updSubsystem()");
972 return subsystems[subx];
975 const Stage& getSubsystemStage(
int subsystem)
const {
976 return subsystems[subsystem].currentStage;
978 Stage& updSubsystemStage(
int subsystem)
const {
979 return subsystems[subsystem].currentStage;
982 const StageVersion* getSubsystemStageVersions(
int subsystem)
const {
983 return subsystems[subsystem].stageVersions;
993 void invalidateJustSystemStage(Stage stg);
999 void advanceSystemToStage(Stage stg)
const;
1001 void setNumSubsystems(
int nSubs) {
1004 for (
int i=0; i < nSubs; ++i)
1005 subsystems.emplace_back(*
this);
1008 void initializeSubsystem
1009 (SubsystemIndex i,
const String& name,
const String& version) {
1010 updSubsystem(i).name = name;
1011 updSubsystem(i).version = version;
1014 SubsystemIndex addSubsystem(
const String& name,
const String& version) {
1015 const SubsystemIndex sx(subsystems.size());
1016 subsystems.emplace_back(*
this, name, version);
1020 int getNumSubsystems()
const {
return (
int)subsystems.size();}
1022 const String& getSubsystemName(SubsystemIndex subsys)
const {
1023 return subsystems[subsys].name;
1025 const String& getSubsystemVersion(SubsystemIndex subsys)
const {
1026 return subsystems[subsys].version;
1031 void invalidateAll(Stage g) {
1032 invalidateJustSystemStage(g);
1033 for (SubsystemIndex i(0); i<(int)subsystems.size(); ++i)
1034 subsystems[i].invalidateStageJustThisSubsystem(g);
1040 void invalidateAllCacheAtOrAbove(Stage g)
const {
1042 "StateImpl::invalidateAllCacheAtOrAbove()");
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);
1054 void advanceSubsystemToStage(SubsystemIndex subsys, Stage g)
const {
1055 subsystems[subsys].advanceToStage(g);
1063 QIndex allocateQ(SubsystemIndex subsys,
const Vector& qInit) {
1065 "StateImpl::allocateQ()");
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()));
1074 UIndex allocateU(SubsystemIndex subsys,
const Vector& uInit) {
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()));
1083 ZIndex allocateZ(SubsystemIndex subsys,
const Vector& zInit) {
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()));
1093 QErrIndex allocateQErr(SubsystemIndex subsys,
int nqerr)
const {
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()));
1102 UErrIndex allocateUErr(SubsystemIndex subsys,
int nuerr)
const {
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()));
1111 UDotErrIndex allocateUDotErr(SubsystemIndex subsys,
int nudoterr)
const {
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()));
1121 EventTriggerByStageIndex allocateEventTrigger
1122 (SubsystemIndex subsys, Stage g,
int nt)
const {
1124 "StateImpl::allocateEventTrigger()");
1125 const Stage allocStage = getSubsystemStage(subsys).next();
1126 const PerSubsystemInfo& ss = subsystems[subsys];
1127 const EventTriggerByStageIndex
1128 nxt(ss.getNextEventTriggerByStageIndex(g));
1129 ss.triggerInfo[g].push_back(TriggerInfo(allocStage,nxt,nt));
1136 DiscreteVariableIndex allocateDiscreteVariable
1137 (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp)
1141 "StateImpl::allocateDiscreteVariable()");
1143 const Stage maxAcceptable = (invalidates <=
Stage::Model 1146 maxAcceptable.next(),
"StateImpl::allocateDiscreteVariable()");
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));
1157 CacheEntryIndex allocateCacheEntry
1158 (SubsystemIndex subsys, Stage dependsOn, Stage computedBy,
1159 AbstractValue* vp)
const 1163 "StateImpl::allocateCacheEntry()");
1165 "StateImpl::allocateCacheEntry()");
1169 const Stage allocStage = getSubsystemStage(subsys).next();
1170 const PerSubsystemInfo& ss = subsystems[subsys];
1171 const CacheEntryIndex nxt(ss.getNextCacheEntryIndex());
1173 allocStage, dependsOn, computedBy, vp);
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)
1188 for (
const auto& ckey : cacheEntries) {
1189 const CacheEntryInfo& prereq = getCacheEntryInfo(ckey);
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());
1201 const CacheEntryIndex cx =
1202 allocateCacheEntry(subsys,earliest,latest,value);
1203 CacheEntryInfo& cinfo = updCacheEntryInfo(
CacheEntryKey(subsys,cx));
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);
1213 cinfo.registerWithPrerequisites(*
this);
1219 DiscreteVariableIndex allocateAutoUpdateDiscreteVariable
1220 (SubsystemIndex subsys, Stage invalidates, AbstractValue* vp,
1221 Stage updateDependsOn)
1223 const DiscreteVariableIndex dx =
1224 allocateDiscreteVariable(subsys,invalidates,vp->clone());
1225 const CacheEntryIndex cx =
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);
1240 "StateImpl::getNY()");
1244 SystemYIndex getQStart()
const {
1246 "StateImpl::getQStart()");
1247 return SystemYIndex(0);
1251 "StateImpl::getNQ()");
1255 SystemYIndex getUStart()
const {
1257 "StateImpl::getUStart()");
1258 return SystemYIndex(q.size());
1262 "StateImpl::getNU()");
1266 SystemYIndex getZStart()
const {
1268 "StateImpl::getZStart()");
1269 return SystemYIndex(q.size() + u.size());
1273 "StateImpl::getNZ()");
1277 int getNYErr()
const {
1279 "StateImpl::getNYErr()");
1283 SystemYErrIndex getQErrStart()
const {
1285 "StateImpl::getQErrStart()");
1286 return SystemYErrIndex(0);
1288 int getNQErr()
const {
1290 "StateImpl::getNQErr()");
1294 SystemYErrIndex getUErrStart()
const {
1296 "StateImpl::getUErrStart()");
1297 return SystemYErrIndex(qerr.size());
1299 int getNUErr()
const {
1301 "StateImpl::getNUErr()");
1307 int getNUDotErr()
const {
1309 "StateImpl::getNUDotErr()");
1310 return udoterr.size();
1313 int getNEventTriggers()
const {
1315 "StateImpl::getNEventTriggers()");
1316 return allTriggers.size();
1319 SystemEventTriggerIndex getEventTriggerStartByStage(Stage g)
const {
1321 "StateImpl::getEventTriggerStartByStage()");
1323 for (
int j=0; j<g; ++j)
1324 nxt += triggers[j].size();
1325 return SystemEventTriggerIndex(nxt);
1328 int getNEventTriggersByStage(Stage g)
const {
1330 "StateImpl::getNEventTriggersByStage()");
1331 return triggers[g].size();
1334 std::mutex& getStateLock()
const {
1340 SystemQIndex getQStart(SubsystemIndex subsys)
const {
1342 "StateImpl::getQStart(subsys)");
1343 return getSubsystem(subsys).qstart;
1345 int getNQ(SubsystemIndex subsys)
const {
1347 "StateImpl::getNQ(subsys)");
1348 return getSubsystem(subsys).q.size();
1351 SystemUIndex getUStart(SubsystemIndex subsys)
const {
1353 "StateImpl::getUStart(subsys)");
1354 return getSubsystem(subsys).ustart;
1356 int getNU(SubsystemIndex subsys)
const {
1358 "StateImpl::getNU(subsys)");
1359 return getSubsystem(subsys).u.size();
1362 SystemZIndex getZStart(SubsystemIndex subsys)
const {
1364 "StateImpl::getZStart(subsys)");
1365 return getSubsystem(subsys).zstart;
1367 int getNZ(SubsystemIndex subsys)
const {
1369 "StateImpl::getNZ(subsys)");
1370 return getSubsystem(subsys).z.size();
1373 SystemQErrIndex getQErrStart(SubsystemIndex subsys)
const {
1375 "StateImpl::getQErrStart(subsys)");
1376 return getSubsystem(subsys).qerrstart;
1378 int getNQErr(SubsystemIndex subsys)
const {
1380 "StateImpl::getNQErr(subsys)");
1381 return getSubsystem(subsys).qerr.size();
1384 SystemUErrIndex getUErrStart(SubsystemIndex subsys)
const {
1386 "StateImpl::getUErrStart(subsys)");
1387 return getSubsystem(subsys).uerrstart;
1389 int getNUErr(SubsystemIndex subsys)
const {
1391 "StateImpl::getNUErr(subsys)");
1392 return getSubsystem(subsys).uerr.size();
1396 SystemUDotErrIndex getUDotErrStart(SubsystemIndex subsys)
const {
1398 "StateImpl::getUDotErrStart(subsys)");
1399 return getSubsystem(subsys).udoterrstart;
1401 int getNUDotErr(SubsystemIndex subsys)
const {
1403 "StateImpl::getNUDotErr(subsys)");
1404 return getSubsystem(subsys).udoterr.size();
1407 SystemEventTriggerByStageIndex getEventTriggerStartByStage
1408 (SubsystemIndex subsys, Stage g)
const {
1410 "StateImpl::getEventTriggerStartByStage(subsys)");
1411 return getSubsystem(subsys).triggerstart[g];
1414 int getNEventTriggersByStage(SubsystemIndex subsys, Stage g)
const {
1416 "StateImpl::getNEventTriggersByStage(subsys)");
1417 return getSubsystem(subsys).triggers[g].size();
1422 const Vector& getQ(SubsystemIndex subsys)
const {
1424 "StateImpl::getQ(subsys)");
1425 return getSubsystem(subsys).q;
1427 const Vector& getU(SubsystemIndex subsys)
const {
1429 "StateImpl::getU(subsys)");
1430 return getSubsystem(subsys).u;
1432 const Vector& getZ(SubsystemIndex subsys)
const {
1434 "StateImpl::getZ(subsys)");
1435 return getSubsystem(subsys).z;
1438 const Vector& getUWeights(SubsystemIndex subsys)
const {
1440 "StateImpl::getUWeights(subsys)");
1441 return getSubsystem(subsys).uWeights;
1443 const Vector& getZWeights(SubsystemIndex subsys)
const {
1445 "StateImpl::getZWeights(subsys)");
1446 return getSubsystem(subsys).zWeights;
1449 const Vector& getQDot(SubsystemIndex subsys)
const {
1451 "StateImpl::getQDot(subsys)");
1453 "StateImpl::getQDot(subsys)");
1454 return getSubsystem(subsys).qdot;
1456 const Vector& getUDot(SubsystemIndex subsys)
const {
1458 "StateImpl::getUDot(subsys)");
1460 "StateImpl::getUDot(subsys)");
1461 return getSubsystem(subsys).udot;
1463 const Vector& getZDot(SubsystemIndex subsys)
const {
1465 "StateImpl::getZDot(subsys)");
1467 "StateImpl::getZDot(subsys)");
1468 return getSubsystem(subsys).zdot;
1470 const Vector& getQDotDot(SubsystemIndex subsys)
const {
1472 "StateImpl::getQDotDot(subsys)");
1474 "StateImpl::getQDotDot(subsys)");
1475 return getSubsystem(subsys).qdotdot;
1478 Vector& updQ(SubsystemIndex subsys) {
1480 "StateImpl::updQ(subsys)");
1483 return updSubsystem(subsys).q;
1485 Vector& updU(SubsystemIndex subsys) {
1487 "StateImpl::updU(subsys)");
1490 return updSubsystem(subsys).u;
1492 Vector& updZ(SubsystemIndex subsys) {
1494 "StateImpl::updZ(subsys)");
1497 return updSubsystem(subsys).z;
1500 Vector& updUWeights(SubsystemIndex subsys) {
1502 "StateImpl::updUWeights(subsys)");
1504 return updSubsystem(subsys).uWeights;
1506 Vector& updZWeights(SubsystemIndex subsys) {
1508 "StateImpl::updZWeights(subsys)");
1510 return updSubsystem(subsys).zWeights;
1515 Vector& updQDot(SubsystemIndex subsys)
const {
1517 "StateImpl::updQDot(subsys)");
1518 return getSubsystem(subsys).qdot;
1520 Vector& updUDot(SubsystemIndex subsys)
const {
1522 "StateImpl::updUDot(subsys)");
1523 return getSubsystem(subsys).udot;
1525 Vector& updZDot(SubsystemIndex subsys)
const {
1527 "StateImpl::updZDot(subsys)");
1528 return getSubsystem(subsys).zdot;
1530 Vector& updQDotDot(SubsystemIndex subsys)
const {
1532 "StateImpl::updQDotDot(subsys)");
1533 return getSubsystem(subsys).qdotdot;
1537 const Vector& getQErr(SubsystemIndex subsys)
const {
1539 "StateImpl::getQErr(subsys)");
1541 "StateImpl::getQErr(subsys)");
1542 return getSubsystem(subsys).qerr;
1544 const Vector& getUErr(SubsystemIndex subsys)
const {
1546 "StateImpl::getUErr(subsys)");
1548 "StateImpl::getUErr(subsys)");
1549 return getSubsystem(subsys).uerr;
1552 const Vector& getQErrWeights(SubsystemIndex subsys)
const {
1554 "StateImpl::getQErrWeights(subsys)");
1555 return getSubsystem(subsys).qerrWeights;
1557 const Vector& getUErrWeights(SubsystemIndex subsys)
const {
1559 "StateImpl::getUErrWeights(subsys)");
1560 return getSubsystem(subsys).uerrWeights;
1563 const Vector& getUDotErr(SubsystemIndex subsys)
const {
1565 "StateImpl::getUDotErr(subsys)");
1567 "StateImpl::getUDotErr(subsys)");
1568 return getSubsystem(subsys).udoterr;
1570 const Vector& getMultipliers(SubsystemIndex subsys)
const {
1572 "StateImpl::getMultipliers(subsys)");
1574 "StateImpl::getMultipliers(subsys)");
1575 return getSubsystem(subsys).multipliers;
1578 const Vector& getEventTriggersByStage(SubsystemIndex subsys, Stage g)
const {
1580 "StateImpl::getEventTriggersByStage(subsys)");
1582 "StateImpl::getEventTriggersByStage(subsys)");
1583 return getSubsystem(subsys).triggers[g];
1586 Vector& updQErr(SubsystemIndex subsys)
const {
1588 "StateImpl::updQErr(subsys)");
1589 return getSubsystem(subsys).qerr;
1591 Vector& updUErr(SubsystemIndex subsys)
const {
1593 "StateImpl::updUErr(subsys)");
1594 return getSubsystem(subsys).uerr;
1597 Vector& updQErrWeights(SubsystemIndex subsys) {
1599 "StateImpl::updQErrWeights(subsys)");
1601 return updSubsystem(subsys).qerrWeights;
1603 Vector& updUErrWeights(SubsystemIndex subsys) {
1605 "StateImpl::updUErrWeights(subsys)");
1607 return updSubsystem(subsys).uerrWeights;
1610 Vector& updUDotErr(SubsystemIndex subsys)
const {
1612 "StateImpl::updUDotErr(subsys)");
1613 return getSubsystem(subsys).udoterr;
1615 Vector& updMultipliers(SubsystemIndex subsys)
const {
1617 "StateImpl::updMultipliers(subsys)");
1618 return getSubsystem(subsys).multipliers;
1620 Vector& updEventTriggersByStage(SubsystemIndex subsys, Stage g)
const {
1622 "StateImpl::updEventTriggersByStage(subsys)");
1623 return getSubsystem(subsys).triggers[g];
1630 const Real& getTime()
const {
1632 "StateImpl::getTime()");
1636 const Vector& getY()
const {
1638 "StateImpl::getY()");
1642 const Vector& getQ()
const {
1644 "StateImpl::getQ()");
1648 const Vector& getU()
const {
1650 "StateImpl::getU()");
1654 const Vector& getZ()
const {
1656 "StateImpl::getZ()");
1660 const Vector& getUWeights()
const {
1662 "StateImpl::getUWeights()");
1666 const Vector& getZWeights()
const {
1668 "StateImpl::getZWeights()");
1677 "StateImpl::updTime()");
1684 "StateImpl::updY()");
1692 "StateImpl::updQ()");
1700 "StateImpl::updU()");
1708 "StateImpl::updZ()");
1716 "StateImpl::updUWeights()");
1723 "StateImpl::updZWeights()");
1729 const Vector& getYDot()
const {
1731 "StateImpl::getYDot()");
1735 const Vector& getQDot()
const {
1737 "StateImpl::getQDot()");
1741 const Vector& getZDot()
const {
1743 "StateImpl::getZDot()");
1747 const Vector& getUDot()
const {
1749 "StateImpl::getUDot()");
1753 const Vector& getQDotDot()
const {
1755 "StateImpl::getQDotDot()");
1760 Vector& updYDot()
const {
1762 "StateImpl::updYDot()");
1766 Vector& updQDot()
const {
1768 "StateImpl::updQDot()");
1772 Vector& updUDot()
const {
1774 "StateImpl::updUDot()");
1778 Vector& updZDot()
const {
1780 "StateImpl::updZDot()");
1784 Vector& updQDotDot()
const {
1786 "StateImpl::updQDotDot()");
1791 const Vector& getYErr()
const {
1793 "StateImpl::getYErr()");
1797 const Vector& getQErr()
const {
1799 "StateImpl::getQErr()");
1802 const Vector& getUErr()
const {
1804 "StateImpl::getUErr()");
1808 const Vector& getQErrWeights()
const {
1810 "StateImpl::getQErrWeights()");
1813 const Vector& getUErrWeights()
const {
1815 "StateImpl::getUErrWeights()");
1819 const Vector& getUDotErr()
const {
1821 "StateImpl::getUDotErr()");
1824 const Vector& getMultipliers()
const {
1826 "StateImpl::getMultipliers()");
1830 Vector& updYErr()
const {
1832 "StateImpl::updYErr()");
1837 "StateImpl::updQErr()");
1842 "StateImpl::updUErr()");
1846 Vector& updQErrWeights() {
1848 "StateImpl::updQErrWeights()");
1852 Vector& updUErrWeights() {
1854 "StateImpl::updUErrWeights()");
1859 Vector& updUDotErr()
const{
1861 "StateImpl::updUDotErr()");
1864 Vector& updMultipliers()
const{
1866 "StateImpl::updMultipliers()");
1870 const Vector& getEventTriggers()
const {
1872 "StateImpl::getEventTriggers()");
1875 const Vector& getEventTriggersByStage(Stage g)
const {
1877 "StateImpl::getEventTriggersByStage()");
1882 Vector& updEventTriggers()
const {
1884 "StateImpl::updEventTriggers()");
1887 Vector& updEventTriggersByStage(Stage g)
const {
1889 "StateImpl::updEventTriggersByStage()");
1894 return getSubsystem(dk.first).hasDiscreteVar(dk.second);
1897 const DiscreteVarInfo& getDiscreteVarInfo(
const DiscreteVarKey& dk)
const {
1898 return getSubsystem(dk.first).getDiscreteVarInfo(dk.second);
1902 return updSubsystem(dk.first).updDiscreteVarInfo(dk.second);
1905 CacheEntryIndex getDiscreteVarUpdateIndex(
const DiscreteVarKey& dk)
const {
1906 return getDiscreteVarInfo(dk).getAutoUpdateEntry();
1909 Stage getDiscreteVarAllocationStage(
const DiscreteVarKey& dk)
const {
1910 return getDiscreteVarInfo(dk).getAllocationStage();
1913 Stage getDiscreteVarInvalidatesStage(
const DiscreteVarKey& dk)
const {
1914 return getDiscreteVarInfo(dk).getInvalidatedStage();
1918 const AbstractValue&
1920 const DiscreteVarInfo& dv = getDiscreteVarInfo(dk);
1921 return dv.getValue();
1925 return getDiscreteVarInfo(dk).getTimeLastUpdated();
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);
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);
1944 bool isDiscreteVarUpdateValueRealized(
const DiscreteVarKey& dk)
const {
1945 const CacheEntryIndex cx = getDiscreteVarUpdateIndex(dk);
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);
1953 void markDiscreteVarUpdateValueRealized(
const DiscreteVarKey& dk)
const {
1954 const CacheEntryIndex cx = getDiscreteVarUpdateIndex(dk);
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);
1970 DiscreteVarInfo& dv = updDiscreteVarInfo(dk);
1974 invalidateAll(dv.getInvalidatedStage());
1977 const CacheEntryIndex cx = dv.getAutoUpdateEntry();
1979 CacheEntryInfo& ce = updCacheEntryInfo(
CacheEntryKey(dk.first,cx));
1980 ce.invalidate(*
this);
1985 return dv.updValue(*
this, t);
1989 return getSubsystem(ck.first).hasCacheEntry(ck.second);
1992 const CacheEntryInfo&
1994 return getSubsystem(ck.first).getCacheEntryInfo(ck.second);
1999 return getSubsystem(ck.first).updCacheEntryInfo(ck.second);
2002 Stage getCacheEntryAllocationStage(
const CacheEntryKey& ck)
const {
2003 return getCacheEntryInfo(ck).getAllocationStage();
2010 const AbstractValue&
2012 const CacheEntryInfo& ce = getCacheEntryInfo(ck);
2014 if (!ce.isUpToDate(*
this))
2015 ce.throwHelpfulOutOfDateMessage(*
this, __func__);
2016 return ce.getValue();
2023 return updCacheEntryInfo(ck).updValue(*
this);
2027 const CacheEntryInfo& ce = getCacheEntryInfo(ck);
2028 return ce.isUpToDate(*
this);
2031 void markCacheValueRealized(
const CacheEntryKey& ck)
const {
2032 CacheEntryInfo& ce = updCacheEntryInfo(ck);
2039 ce.getDependsOnStage().prev(),
2040 "StateImpl::markCacheValueRealized()");
2042 ce.markAsUpToDate(*
this);
2045 void markCacheValueNotRealized(
const CacheEntryKey& ck)
const {
2046 CacheEntryInfo& ce = updCacheEntryInfo(ck);
2047 ce.invalidate(*
this);
2053 void setSystemTopologyStageVersion(
StageVersion topoVersion)
2054 { assert(topoVersion>0);
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];
2068 Stage getLowestSystemStageDifference
2069 (
const Array_<StageVersion>& prevVersions)
const {
2070 const int nRealizedBefore = (int)prevVersions.size();
2071 const int nRealizedNow = (int)currentSystemStage+1;
2072 const int nRealizedBoth =
std::min(nRealizedBefore,nRealizedNow);
2076 for (; g < nRealizedBoth; ++g)
2077 if (systemStageVersions[g] != prevVersions[g])
2086 ValueVersion getQValueVersion()
const {
return qVersion;}
2087 ValueVersion getUValueVersion()
const {
return uVersion;}
2088 ValueVersion getZValueVersion()
const {
return zVersion;}
2090 const ListOfDependents& getQDependents()
const {
return qDependents;}
2091 const ListOfDependents& getUDependents()
const {
return uDependents;}
2092 const ListOfDependents& getZDependents()
const {
return zDependents;}
2094 ListOfDependents& updQDependents() {
return qDependents;}
2095 ListOfDependents& updUDependents() {
return uDependents;}
2096 ListOfDependents& updZDependents() {
return zDependents;}
2098 void autoUpdateDiscreteVariables();
2100 String toString()
const;
2101 String cacheToString()
const;
2107 void copyFrom(
const StateImpl& source);
2113 void invalidateCopiedStageVersions(
const StateImpl& src) {
2114 for (
int i=1; i <= src.currentSystemStage; ++i)
2115 systemStageVersions[i] = src.systemStageVersions[i]+1;
2117 qVersion = src.qVersion + 1;
2118 uVersion = src.uVersion + 1;
2119 zVersion = src.zVersion + 1;
2121 qDependents.clear();
2122 uDependents.clear();
2123 zDependents.clear();
2129 void registerWithPrerequisitesAfterCopy() {
2130 for (
auto& subsys : subsystems) {
2131 for (
auto& ce : subsys.cacheInfo)
2132 ce.registerWithPrerequisites(*
this);
2140 { ++qVersion; qDependents.notePrerequisiteChange(*
this); }
2142 { ++uVersion; uDependents.notePrerequisiteChange(*
this); }
2144 { ++zVersion; zDependents.notePrerequisiteChange(*
this); }
2146 void noteYChange() {noteQChange();noteUChange();noteZChange();}
2149 bool allSubsystemsAtLeastAtStage(Stage g)
const {
2150 for (SubsystemIndex i(0); i < (int)subsystems.size(); ++i)
2151 if (subsystems[i].currentStage < g)
2159 Array_<PerSubsystemInfo> subsystems;
2189 ListOfDependents qDependents;
2190 ListOfDependents uDependents;
2191 ListOfDependents zDependents;
2230 mutable Vector multipliers;
2239 mutable Vector allTriggers;
2248 mutable std::mutex stateLock;
2255 inline void ListOfDependents::
2256 notePrerequisiteChange(
const StateImpl& stateImpl)
const {
2257 for (
auto ckey : m_dependents) {
2259 CacheEntryInfo& ce = stateImpl.updCacheEntryInfo(ckey);
2260 ce.invalidate(stateImpl);
2268 isUpToDate(
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)
2273 if (subsys.getCurrentStage() < m_dependsOnStage)
2278 const StageVersion version = subsys.getStageVersion(m_dependsOnStage);
2279 assert(version >= 1);
2280 if (!( version == m_dependsOnVersionWhenLastComputed
2281 && m_isUpToDateWithPrerequisites))
2286 validatePrerequisiteVersions(stateImpl);
2291 inline void CacheEntryInfo::
2292 markAsUpToDate(
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;
2303 recordPrerequisiteVersions(stateImpl);
2313 updImpl().setNumSubsystems(i);
2316 (SubsystemIndex subsys,
const String& name,
const String& version) {
2317 updImpl().initializeSubsystem(subsys, name, version);
2321 (
const String& name,
const String& version) {
2322 return updImpl().addSubsystem(name, version);
2325 return getImpl().getNumSubsystems();
2328 return getImpl().getSubsystemName(subsys);
2331 return getImpl().getSubsystemVersion(subsys);
2334 return getImpl().getSubsystemStage(subsys);
2337 return getImpl().getSystemStage();
2340 updImpl().invalidateAll(stage);
2343 getImpl().invalidateAllCacheAtOrAbove(stage);
2346 getImpl().advanceSubsystemToStage(subsys, stage);
2349 getImpl().advanceSystemToStage(stage);
2353 {
return getImpl().getSystemTopologyStageVersion(); }
2357 return updImpl().allocateQ(subsys, qInit);
2360 return updImpl().allocateU(subsys, uInit);
2363 return updImpl().allocateZ(subsys, zInit);
2368 return getImpl().allocateQErr(subsys, nqerr);
2371 return getImpl().allocateUErr(subsys, nuerr);
2375 return getImpl().allocateUDotErr(subsys, nudoterr);
2379 inline EventTriggerByStageIndex
State:: 2381 return getImpl().allocateEventTrigger(subsys, stage, nevent);
2385 inline DiscreteVariableIndex
State:: 2387 return updImpl().allocateDiscreteVariable(subsys, stage, v);
2389 inline DiscreteVariableIndex
State:: 2391 (SubsystemIndex subsys, Stage invalidates, AbstractValue* v,
2392 Stage updateDependsOn) {
2393 return updImpl().allocateAutoUpdateDiscreteVariable
2394 (subsys, invalidates, v, updateDependsOn);
2397 inline CacheEntryIndex
State:: 2399 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2400 return getImpl().getDiscreteVarUpdateIndex(
DiscreteVarKey(subsys,index));
2404 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2405 return getImpl().getDiscreteVarAllocationStage(
DiscreteVarKey(subsys,index));
2409 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2410 return getImpl().getDiscreteVarInvalidatesStage(
DiscreteVarKey(subsys,index));
2413 inline const AbstractValue&
State:: 2415 return getImpl().getDiscreteVariable(
DiscreteVarKey(subsys,index));
2419 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2420 return getImpl().getDiscreteVarLastUpdateTime(
DiscreteVarKey(subsys,index));
2422 inline const AbstractValue&
State:: 2424 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2425 return getImpl().getDiscreteVarUpdateValue(
DiscreteVarKey(subsys,index));
2429 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2430 return getImpl().updDiscreteVarUpdateValue(
DiscreteVarKey(subsys,index));
2434 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2435 return getImpl().isDiscreteVarUpdateValueRealized
2440 (SubsystemIndex subsys, DiscreteVariableIndex index)
const {
2441 getImpl().markDiscreteVarUpdateValueRealized(
DiscreteVarKey(subsys,index));
2446 (SubsystemIndex subsys, DiscreteVariableIndex index) {
2447 return updImpl().updDiscreteVariable(
DiscreteVarKey(subsys,index));
2451 (SubsystemIndex i, DiscreteVariableIndex index,
const AbstractValue& v) {
2452 updDiscreteVariable(i,index) = v;
2456 inline CacheEntryIndex
State:: 2458 AbstractValue* value)
const {
2459 return getImpl().allocateCacheEntry(subsys, dependsOn, computedBy, value);
2462 inline CacheEntryIndex
State:: 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);
2475 return getImpl().getCacheEntryAllocationStage(
CacheEntryKey(subsys,index));
2477 inline const AbstractValue&
State:: 2478 getCacheEntry(SubsystemIndex subsys, CacheEntryIndex index)
const {
2479 return getImpl().getCacheEntry(
CacheEntryKey(subsys,index));
2482 updCacheEntry(SubsystemIndex subsys, CacheEntryIndex index)
const {
2483 return getImpl().updCacheEntry(
CacheEntryKey(subsys,index));
2488 return getImpl().isCacheValueRealized(
CacheEntryKey(subx,cx));
2498 getImpl().markCacheValueNotRealized(
CacheEntryKey(subx,cx));
2502 return getImpl().getStateLock();
2507 return getImpl().getNY();
2510 return getImpl().getNQ();
2513 return getImpl().getQStart();
2516 return getImpl().getNU();
2519 return getImpl().getUStart();
2522 return getImpl().getNZ();
2525 return getImpl().getZStart();
2528 return getImpl().getNYErr();
2531 return getImpl().getNQErr();
2534 return getImpl().getQErrStart();
2537 return getImpl().getNUErr();
2540 return getImpl().getUErrStart();
2543 return getImpl().getNUDotErr();
2546 return getNUDotErr();
2549 return getImpl().getNEventTriggers();
2552 return getImpl().getNEventTriggersByStage(stage);
2559 return getImpl().getQStart(subsys);
2562 return getImpl().getNQ(subsys);
2565 return getImpl().getUStart(subsys);
2568 return getImpl().getNU(subsys);
2571 return getImpl().getZStart(subsys);
2574 return getImpl().getNZ(subsys);
2577 return getImpl().getQErrStart(subsys);
2580 return getImpl().getNQErr(subsys);
2583 return getImpl().getUErrStart(subsys);
2586 return getImpl().getNUErr(subsys);
2589 return getImpl().getUDotErrStart(subsys);
2592 return getImpl().getNUDotErr(subsys);
2595 return SystemMultiplierIndex(getUDotErrStart(subsys));
2598 return getNUDotErr(subsys);
2600 inline SystemEventTriggerByStageIndex
State:: 2602 return getImpl().getEventTriggerStartByStage(subsys, stage);
2606 return getImpl().getNEventTriggersByStage(subsys, stage);
2613 return getImpl().getEventTriggersByStage(subsys, stage);
2617 return getImpl().updEventTriggersByStage(subsys, stage);
2620 return getImpl().getQ(subsys);
2623 return getImpl().getU(subsys);
2626 return getImpl().getZ(subsys);
2629 return getImpl().getUWeights(subsys);
2632 return getImpl().getZWeights(subsys);
2635 return updImpl().updQ(subsys);
2638 return updImpl().updU(subsys);
2641 return updImpl().updZ(subsys);
2644 return updImpl().updUWeights(subsys);
2647 return updImpl().updZWeights(subsys);
2650 return getImpl().getQDot(subsys);
2653 return getImpl().getUDot(subsys);
2656 return getImpl().getZDot(subsys);
2659 return getImpl().getQDotDot(subsys);
2662 return getImpl().updQDot(subsys);
2665 return getImpl().updUDot(subsys);
2668 return getImpl().updZDot(subsys);
2671 return getImpl().updQDotDot(subsys);
2674 return getImpl().getQErr(subsys);
2677 return getImpl().getUErr(subsys);
2680 return getImpl().getQErrWeights(subsys);
2683 return getImpl().getUErrWeights(subsys);
2686 return getImpl().getUDotErr(subsys);
2689 return getImpl().getMultipliers(subsys);
2692 return getImpl().updQErr(subsys);
2695 return getImpl().updUErr(subsys);
2698 return updImpl().updQErrWeights(subsys);
2701 return updImpl().updUErrWeights(subsys);
2704 return getImpl().updUDotErr(subsys);
2707 return getImpl().updMultipliers(subsys);
2710 inline SystemEventTriggerIndex
State:: 2712 return getImpl().getEventTriggerStartByStage(stage);
2716 return getImpl().getEventTriggers();
2719 return getImpl().getEventTriggersByStage(stage);
2723 return getImpl().updEventTriggers();
2726 return getImpl().updEventTriggersByStage(stage);
2730 return getImpl().getTime();
2733 return getImpl().getY();
2736 return getImpl().getQ();
2739 return getImpl().getU();
2742 return getImpl().getZ();
2745 return getImpl().getUWeights();
2748 return getImpl().getZWeights();
2751 return updImpl().updTime();
2754 return updImpl().updY();
2763 return updImpl().updQ();
2766 return updImpl().updU();
2769 return updImpl().updZ();
2772 return updImpl().updUWeights();
2775 return updImpl().updZWeights();
2787 return getImpl().getYDot();
2790 return getImpl().getQDot();
2793 return getImpl().getZDot();
2796 return getImpl().getUDot();
2799 return getImpl().getQDotDot();
2802 return getImpl().updYDot();
2805 return getImpl().updQDot();
2808 return getImpl().updZDot();
2811 return getImpl().updUDot();
2814 return getImpl().updQDotDot();
2817 return getImpl().getYErr();
2820 return getImpl().getQErr();
2823 return getImpl().getUErr();
2826 return getImpl().getQErrWeights();
2829 return getImpl().getUErrWeights();
2832 return getImpl().getUDotErr();
2835 return getImpl().getMultipliers();
2838 return getImpl().updYErr();
2841 return getImpl().updQErr();
2844 return getImpl().updUErr();
2847 return updImpl().updQErrWeights();
2850 return updImpl().updUErrWeights();
2853 return getImpl().updUDotErr();
2856 return getImpl().updMultipliers();
2861 {
return updImpl().setSystemTopologyStageVersion(topoVersion); }
2865 return getImpl().getSystemStageVersions(versions);
2869 return getImpl().getLowestSystemStageDifference(prev);
2873 return getImpl().getQValueVersion();
2877 return getImpl().getUValueVersion();
2881 return getImpl().getZValueVersion();
2885 return getImpl().getQDependents();
2889 return getImpl().getUDependents();
2893 return getImpl().getZDependents();
2897 return getImpl().hasCacheEntry(cacheEntry);
2900 inline const CacheEntryInfo&
State:: 2902 return getImpl().getCacheEntryInfo(cacheEntry);
2905 inline CacheEntryInfo&
State:: 2907 return updImpl().updCacheEntryInfo(cacheEntry);
2911 return getImpl().hasDiscreteVar(discreteVar);
2914 inline const DiscreteVarInfo&
State:: 2916 return getImpl().getDiscreteVarInfo(discreteVar);
2919 inline const PerSubsystemInfo&
State:: 2921 return getImpl().getSubsystem(subx);
2926 updImpl().autoUpdateDiscreteVariables();
2930 return getImpl().toString();
2933 return getImpl().cacheToString();
2941 #endif // SimTK_SimTKCOMMON_STATE_IMPL_H_ const Vector & getYErr() const
Return the current constraint errors for all constraints.
SystemYIndex getZStart() const
Returns the y index at which the z's begin. Callable at Model stage.
Vector & updUErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraints...
const Vector & getZDot() const
void getSystemStageVersions(Array_< StageVersion > &versions) const
(Advanced) Record the current version numbers of each valid System-level stage.
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
SystemMultiplierIndex getMultipliersStart(SubsystemIndex) const
bool hasDiscreteVar(const DiscreteVarKey &discreteVar) const
(Advanced) Check whether this State has a particular discrete state variable.
Physical parameters set.
Definition: Stage.h:72
void setZ(const Vector &z)
Vector & updQErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mquat position inline constraint equations...
const AbstractValue & getDiscreteVariable(SubsystemIndex, DiscreteVariableIndex) const
Get the current value of the indicated discrete variable.
#define SimTK_STAGECHECK_LT_ALWAYS(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:183
const Vector & getU() const
Modeling choices made.
Definition: Stage.h:71
void setTime(Real t)
An alternate syntax equivalent to updTime() and updY().
const Real & getTime() const
You can call these as long as system stage >= Model.
void advanceSubsystemToStage(SubsystemIndex, Stage) const
Advance a particular Subsystem's current stage by one to the indicated stage.
Vector & updYDot() const
These are mutable.
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
#define SimTK_STAGECHECK_GE_ALWAYS(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:180
const Vector & getQ() const
These are just views into Y.
Spatial velocities available.
Definition: Stage.h:75
int getNY() const
Get the total number ny=nq+nu+nz of shared continuous state variables.
Lower than any legitimate Stage.
Definition: Stage.h:69
const ListOfDependents & getUDependents() const
(Advanced) Return the list of cache entries for which u was specified as an explicit prerequisite...
const Vector & getYDot() const
void setY(const Vector &y)
Stage getDiscreteVarInvalidatesStage(SubsystemIndex, DiscreteVariableIndex) const
What is the earliest stage that is invalidated when this discrete variable is modified? All later stages are also invalidated.
const PerSubsystemInfo & getPerSubsystemInfo(SubsystemIndex) const
(Advanced) Return a reference to the per-subsystem information in the state.
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
StageVersion getSystemTopologyStageVersion() const
The Topology stage version number (an integer) stored in this State must match the topology cache ver...
const Vector & getUDot() const
const Vector & getQDotDot() const
This has its own space, not a view.
void autoUpdateDiscreteVariables()
(Advanced) This is called at the beginning of every integration step to set the values of auto-update...
Vector & updYErr() const
These are mutable.
#define SimTK_ERRCHK4_ALWAYS(cond, whereChecked, fmt, a1, a2, a3, a4)
Definition: ExceptionMacros.h:297
const Vector & getUDotErr() const
These have their own space, they are not views.
int getNUDotErr() const
Return the total number nudotErr=mp+mv+ma of cache entries for acceleration-level constraint errors (...
CacheEntryInfo & updCacheEntryInfo(const CacheEntryKey &cacheEntry)
(Advanced) Return a writable reference to the cache entry information for a particular cache entry...
CacheEntryIndex getDiscreteVarUpdateIndex(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the CacheEntryIndex for its associated update cache en...
#define SimTK_ASSERT2_ALWAYS(cond, msg, a1, a2)
Definition: ExceptionMacros.h:353
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
int getNZ() const
Get total number of shared z's (auxiliary state variables).
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
Vector & updQ()
These are just views into Y.
SystemYErrIndex getQErrStart() const
Returns the yErr index at which the qErr's begin. Callable at Instance stage.
const AbstractValue & getDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the current value of its associated update cache entry...
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
#define SimTK_STAGECHECK_RANGE_ALWAYS(lower, current, upper, methodNm)
Definition: ExceptionMacros.h:186
void setDiscreteVariable(SubsystemIndex, DiscreteVariableIndex, const AbstractValue &)
Alternate interface to updDiscreteVariable.
Vector & updEventTriggers() const
Vector & updQDotDot() const
This is a separate shared cache entry, not part of YDot.
SystemYIndex getUStart() const
Returns the y index at which the u's begin. Callable at Model stage.
ValueVersion getQValueVersion() const
(Advanced) Return a ValueVersion for q, meaning an integer that is incremented whenever any q is chan...
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
const Stage & getSystemStage() const
This returns the global stage for this State.
bool isCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Check whether the value in a particular cache entry has been recalculated since the last change to th...
AbstractValue & updDiscreteVariable(SubsystemIndex, DiscreteVariableIndex)
Get a writable reference to the value stored in the indicated discrete state variable dv...
int getNEventTriggers() const
Return the total number of event trigger function slots in the cache.
void setU(const Vector &u)
bool isDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Check whether the update value for this auto-update discrete variable has already been computed since...
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...
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *, Stage updateDependsOn)
This method allocates a DiscreteVariable whose value should be updated automatically after each time ...
ValueVersion getUValueVersion() const
(Advanced) Return a ValueVersion for u, meaning an integer that is incremented whenever any u is chan...
Report-only quantities evaluated.
Definition: Stage.h:78
void setSystemTopologyStageVersion(StageVersion topoVersion)
(Advanced) This explicitly modifies the Topology stage version; don't use this method unless you know...
Stage getCacheEntryAllocationStage(SubsystemIndex, CacheEntryIndex) const
At what stage was this State when this cache entry was allocated? The answer must be Stage::Empty...
SubsystemIndex addSubsystem(const String &name, const String &version)
Register a new subsystem as a client of this State.
int getNQErr() const
Return the total number nqerr=mp+nQuaternions of cache entries for position-level constraint errors...
bool hasCacheEntry(const CacheEntryKey &cacheEntry) const
(Advanced) Check whether this State has a particular cache entry.
int getNMultipliers() const
Return the total number of constraint multipliers; necessarily the same as the number of acceleration...
System topology realized.
Definition: Stage.h:70
Forces calculated.
Definition: Stage.h:76
SystemYIndex getQStart() const
Returns the y index at which the q's begin. Callable at Model stage.
void markCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Mark the value of a particular cache entry as up to date after it has been recalculated.
SystemEventTriggerIndex getEventTriggerStartByStage(Stage) const
Return the index within the global event trigger array at which the first of the event triggers assoc...
Spatial configuration available.
Definition: Stage.h:74
const DiscreteVarInfo & getDiscreteVarInfo(const DiscreteVarKey &discreteVar) const
(Advanced) Return a reference to the discrete variable information for a particular discrete variable...
const Vector & getUErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraint equation...
const AbstractValue & getCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a const reference to the value contained in a particular cache entry.
const Vector & getY() const
const Vector & getMultipliers() const
Real & updTime()
You can call these as long as System stage >= Model, but the stage will be backed up if necessary to ...
DiscreteVariableIndex allocateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *)
The Stage supplied here in the call is the earliest subsystem stage which is invalidated by a change ...
std::pair< SubsystemIndex, DiscreteVariableIndex > DiscreteVarKey
Definition: State.h:165
Higher than any legitimate Stage.
Definition: Stage.h:79
const Vector & getZ() const
#define SimTK_FORCE_INLINE
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:269
AbstractValue & updCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a writable reference to the value contained in a particular cache entry. ...
String toString() const
(Debugging) Not suitable for serialization.
const String & getSubsystemVersion(SubsystemIndex) const
void setQ(const Vector &q)
Alternate interface.
void markCacheValueNotRealized(SubsystemIndex, CacheEntryIndex) const
(Advanced) Normally cache entries are invalidated automatically, however this method allows manual in...
const ListOfDependents & getQDependents() const
(Advanced) Return the list of cache entries for which q was specified as an explicit prerequisite...
AbstractValue & updDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return a writable reference to the value of its associated up...
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
String cacheToString() const
(Debugging) Not suitable for serialization.
Real getDiscreteVarLastUpdateTime(SubsystemIndex, DiscreteVariableIndex) const
Return the time of last update for this discrete variable.
const Vector & getQErr() const
These are just views into YErr.
SystemYErrIndex getUErrStart() const
Returns the yErr index at which the uErr's begin. Callable at Instance stage.
int getNQ() const
Get total number of shared q's (generalized coordinates; second order state variables).
void invalidateAllCacheAtOrAbove(Stage) const
If any subsystem or the system stage is currently at or higher than the passed-in one...
QErrIndex allocateQErr(SubsystemIndex, int nqerr) const
Allocate nqerr cache slots to hold the current error for position-level (holonomic) constraint equati...
void setNumSubsystems(int i)
Set the number of subsystems in this state.
const CacheEntryInfo & getCacheEntryInfo(const CacheEntryKey &cacheEntry) const
(Advanced) Return a const reference to the cache entry information for a particular cache entry...
#define SimTK_ASSERT2(cond, msg, a1, a2)
Definition: ExceptionMacros.h:375
const Vector & getEventTriggersByStage(Stage) const
Vector & updMultipliers() const
int getNYErr() const
Get the total number nyerr=nqerr+nuerr of shared cache entries for position-level and velocity-level ...
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...
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1473
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
Vector & updUDotErr() const
QIndex allocateQ(SubsystemIndex, const Vector &qInit)
Allocate generalized coordinates q, which are second order continuous state variables.
int size() const
Definition: VectorBase.h:396
Vector & updUWeights()
Set u weights (and q weights indirectly).
UIndex allocateU(SubsystemIndex, const Vector &uInit)
Allocate generalized speeds u, which are first order continuous state variables related to the deriva...
SystemUDotErrIndex getUDotErrStart(SubsystemIndex) const
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.
Vector & updZWeights()
Set z weights.
Stage getDiscreteVarAllocationStage(SubsystemIndex, DiscreteVariableIndex) const
At what stage was this State when this discrete variable was allocated? The answer must be Stage::Emp...
int getNU() const
Get total number of shared u's (generalized speeds; mobilities).
Vector & updEventTriggersByStage(Stage) const
void advanceSystemToStage(Stage) const
Advance the System-level current stage by one to the indicated stage.
For iterating over meaningful stage values.
Definition: Stage.h:83
std::pair< SubsystemIndex, CacheEntryIndex > CacheEntryKey
Definition: State.h:164
void markDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Mark the update value for this auto-update discrete variable as up-to-date with respect to the state ...
ZIndex allocateZ(SubsystemIndex, const Vector &zInit)
Allocate auxiliary first order continuous state variables z.
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...
Stage getLowestSystemStageDifference(const Array_< StageVersion > &prevVersions) const
(Advanced) Given a list of per-stage version numbers extracted by an earlier call to getSystemStageVe...
int getNumSubsystems() const
Return the number of Subsystems known to this State.
A new time has been realized.
Definition: Stage.h:73
std::mutex & getStateLock() const
Returns a mutex that should be used to lock the state whenever multiple threads are asynchronously wr...
ValueVersion getZValueVersion() const
(Advanced) Return a ValueVersion for z, meaning an integer that is incremented whenever any z is chan...
EventTriggerByStageIndex allocateEventTrigger(SubsystemIndex, Stage stage, int nevent) const
Allocate room for nevent witness function values that will be available at the indicated stage...
const String & getSubsystemName(SubsystemIndex) const
Accelerations and multipliers calculated.
Definition: Stage.h:77
void invalidateAll(Stage)
If any subsystem or the system stage is currently at or higher than the passed-in one...
const Stage & getSubsystemStage(SubsystemIndex) const
#define SimTK_STAGECHECK_GE(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:208
const Vector & getEventTriggers() const
UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const
Allocate nudoterr cache slots to hold the current error for acceleration-level (acceleration-only, nonholonomic first derivative, and holonomic second derivative) constraint equations.
const Vector & getQErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mquat position inline constraints equations...
const ListOfDependents & getZDependents() const
(Advanced) Return the list of cache entries for which z was specified as an explicit prerequisite...
const Vector & getUErr() const
UErrIndex allocateUErr(SubsystemIndex, int nuerr) const
Allocate nuerr cache slots to hold the current error for velocity-level (nonholonomic and holonomic f...
const Vector & getQDot() const
These are just views into YDot.
int getNUErr() const
Return the total number nuerr=mp+mv of cache entries for velocity-level constraint errors (including ...
CacheEntryIndex allocateCacheEntry(SubsystemIndex, Stage earliest, Stage latest, AbstractValue *value) const
There are two Stages supplied explicitly as arguments to this method: earliest and latest...
int getNEventTriggersByStage(Stage) const
Return the size of the partition of event trigger functions which are evaluated at a given Stage...