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;}
205 const ListOfDependents& getDependents()
const {
return m_dependents;}
206 ListOfDependents& updDependents() {
return m_dependents;}
210 Stage m_allocationStage;
211 Stage m_invalidatedStage;
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;}
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();
392 void recordPrerequisiteVersions(
const StateImpl&);
393 void validatePrerequisiteVersions(
const StateImpl&)
const;
395 const ListOfDependents& getDependents()
const {
return m_dependents;}
396 ListOfDependents& updDependents() {
return m_dependents;}
401 Stage m_allocationStage;
402 Stage m_dependsOnStage;
403 Stage m_computedByStage;
408 bool m_qIsPrerequisite{
false},
409 m_uIsPrerequisite{
false},
410 m_zIsPrerequisite{
false};
411 Array_<DiscreteVarKey> m_discreteVarPrerequisites;
412 Array_<CacheEntryKey> m_cacheEntryPrerequisites;
419 ResetOnCopy<ListOfDependents> m_dependents;
425 ClonePtr<AbstractValue> m_value;
428 bool m_isUpToDateWithPrerequisites{
true};
435 ValueVersion m_qVersion{0}, m_uVersion{0}, m_zVersion{0};
436 Array_<ValueVersion> m_discreteVarVersions;
437 Array_<ValueVersion> m_cacheEntryVersions;
439 bool isReasonable()
const {
443 && (m_computedByStage >= m_dependsOnStage)
444 && (m_value !=
nullptr)
445 && (m_dependsOnVersionWhenLastComputed >= 0)
446 && (ListOfDependents::isCacheEntryKeyValid(m_myKey));
458 : allocationStage(Stage::Empty), firstIndex(-1), nslots(0) {}
460 TriggerInfo(Stage allocation,
int index,
int n)
461 : allocationStage(allocation), firstIndex(index), nslots(n)
462 { assert(isReasonable()); assert(n>0);}
467 int getFirstIndex()
const {
return firstIndex;}
468 int getNumSlots()
const {
return nslots;}
471 TriggerInfo& deepAssign(
const TriggerInfo& src)
472 {
return operator=(src); }
473 void deepDestruct(StateImpl&) {}
474 const Stage& getAllocationStage()
const {
return allocationStage;}
477 Stage allocationStage;
481 bool isReasonable()
const {
501 class ContinuousVarInfo {
503 ContinuousVarInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
505 ContinuousVarInfo(Stage allocation,
509 : allocationStage(allocation), firstIndex(index), initialValues(initVals)
510 { assert(isReasonable());
511 assert(varWeights.size()==0 || varWeights.size()==initVals.size());
512 assert(weightsAreOK(varWeights));
513 if (varWeights.size()) weights=varWeights;
514 else weights=
Vector(initVals.size(),
Real(1));
517 int getFirstIndex()
const {
return firstIndex;}
518 int getNumVars()
const {
return initialValues.size();}
519 const Vector& getInitialValues()
const {
return initialValues;}
520 const Vector& getWeights()
const {
return weights;}
526 ContinuousVarInfo& deepAssign(
const ContinuousVarInfo& src)
527 {
return operator=(src); }
528 void deepDestruct(StateImpl&) {}
529 const Stage& getAllocationStage()
const {
return allocationStage;}
532 Stage allocationStage;
537 static bool weightsAreOK(
const Vector& wts) {
538 for (
int i=0; i<wts.size(); ++i)
539 if (wts[i] <= 0)
return false;
543 bool isReasonable()
const {
553 class ConstraintErrInfo {
555 ConstraintErrInfo() : allocationStage(Stage::Empty), firstIndex(-1) {}
557 ConstraintErrInfo(Stage allocation,
561 : allocationStage(allocation), firstIndex(index)
562 { assert(isReasonable());
563 assert(varWeights.size()==0 || varWeights.size()==nerr);
564 assert(weightsAreOK(varWeights));
565 if (varWeights.size()) weights=varWeights;
569 int getFirstIndex()
const {
return firstIndex;}
570 int getNumErrs()
const {
return weights.size();}
571 const Vector& getWeights()
const {
return weights;}
577 ConstraintErrInfo& deepAssign(
const ConstraintErrInfo& src)
578 {
return operator=(src); }
579 void deepDestruct(StateImpl&) {}
580 const Stage& getAllocationStage()
const {
return allocationStage;}
583 Stage allocationStage;
587 static bool weightsAreOK(
const Vector& wts) {
588 for (
int i=0; i<wts.size(); ++i)
589 if (wts[i] <= 0)
return false;
593 bool isReasonable()
const {
609 explicit PerSubsystemInfo(StateImpl& stateImpl,
610 const String& n=
"",
const String& v=
"")
611 : m_stateImpl(stateImpl), name(n), version(v)
615 ~PerSubsystemInfo() {
626 PerSubsystemInfo(
const PerSubsystemInfo& src) {
632 PerSubsystemInfo& operator=(
const PerSubsystemInfo& src) {
646 void invalidateStageJustThisSubsystem(Stage g) {
648 restoreToStage(g.prev());
654 void advanceToStage(Stage g)
const {
656 assert(currentStage == g.prev());
663 void clearReferencesToModelStageGlobals() {
664 qstart.invalidate(); ustart.invalidate();
666 q.clear(); u.clear(); z.clear();
667 uWeights.clear(); zWeights.clear();
668 qdot.clear(); udot.clear(); zdot.clear(); qdotdot.clear();
671 void clearReferencesToInstanceStageGlobals() {
673 qerrWeights.clear(); uerrWeights.clear();
676 qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
677 qerr.clear();uerr.clear();udoterr.clear();multipliers.clear();
680 triggerstart[j].invalidate();
685 QIndex getNextQIndex()
const {
686 if (q_info.empty())
return QIndex(0);
687 const ContinuousVarInfo& last = q_info.back();
688 return QIndex(last.getFirstIndex()+last.getNumVars());
690 UIndex getNextUIndex()
const {
691 if (uInfo.empty())
return UIndex(0);
692 const ContinuousVarInfo& last = uInfo.back();
693 return UIndex(last.getFirstIndex()+last.getNumVars());
695 ZIndex getNextZIndex()
const {
696 if (zInfo.empty())
return ZIndex(0);
697 const ContinuousVarInfo& last = zInfo.back();
698 return ZIndex(last.getFirstIndex()+last.getNumVars());
702 if (qerrInfo.empty())
return QErrIndex(0);
703 const ConstraintErrInfo& last = qerrInfo.back();
704 return QErrIndex(last.getFirstIndex()+last.getNumErrs());
707 if (uerrInfo.empty())
return UErrIndex(0);
708 const ConstraintErrInfo& last = uerrInfo.back();
709 return UErrIndex(last.getFirstIndex()+last.getNumErrs());
713 const ConstraintErrInfo& last = udoterrInfo.back();
714 return UDotErrIndex(last.getFirstIndex()+last.getNumErrs());
724 const TriggerInfo& last = triggerInfo[g].back();
726 (last.getFirstIndex()+last.getNumSlots());
730 return index < (int)discreteInfo.size();
737 "PerSubsystemInfo::getDiscreteVarInfo()");
738 return discreteInfo[index];
744 "PerSubsystemInfo::updDiscreteVarInfo()");
745 return discreteInfo[index];
750 return index < (int)cacheInfo.size();
756 "PerSubsystemInfo::getCacheEntryInfo()");
757 return cacheInfo[index];
763 "PerSubsystemInfo::updCacheEntryInfo()");
764 return cacheInfo[index];
769 {
return stageVersions[g]; }
772 friend class StateImpl;
773 ReferencePtr<StateImpl> m_stateImpl;
793 Array_<ContinuousVarInfo> q_info, uInfo, zInfo;
794 Array_<DiscreteVarInfo> discreteInfo;
797 mutable Array_<ConstraintErrInfo> qerrInfo, uerrInfo, udoterrInfo;
799 mutable Array_<CacheEntryInfo> cacheInfo;
815 Vector uWeights, zWeights;
817 mutable Vector qdot, udot, zdot, qdotdot;
820 Vector qerrWeights, uerrWeights;
828 mutable Vector qerr, uerr;
830 mutable Vector udoterr, multipliers;
843 mutable Stage currentStage;
851 qstart.invalidate();ustart.invalidate();zstart.invalidate();
852 qerrstart.invalidate();uerrstart.invalidate();udoterrstart.invalidate();
854 triggerstart[j].invalidate();
855 stageVersions[j] = 1;
862 void clearContinuousVars();
863 void clearConstraintErrs();
864 void clearDiscreteVars();
865 void clearEventTriggers(
int g);
868 void clearAllStacks();
870 void popContinuousVarsBackToStage(
const Stage& g);
871 void popDiscreteVarsBackToStage(
const Stage& g);
872 void popConstraintErrsBackToStage(
const Stage& g);
873 void popCacheBackToStage(
const Stage& g);
874 void popEventTriggersBackToStage(
const Stage& g);
876 void popAllStacksBackToStage(
const Stage& g);
879 void copyContinuousVarInfoThroughStage
880 (
const Array_<ContinuousVarInfo>& src,
const Stage& g,
881 Array_<ContinuousVarInfo>& dest);
883 void copyDiscreteVarsThroughStage
884 (
const Array_<DiscreteVarInfo>& src,
const Stage& g);
887 void copyConstraintErrInfoThroughStage
888 (
const Array_<ConstraintErrInfo>& src,
const Stage& g,
889 Array_<ConstraintErrInfo>& dest);
891 void copyCacheThroughStage
892 (
const Array_<CacheEntryInfo>& src,
const Stage& g);
894 void copyEventsThroughStage
895 (
const Array_<TriggerInfo>& src,
const Stage& g,
896 Array_<TriggerInfo>& dest);
898 void copyAllStacksThroughStage(
const PerSubsystemInfo& src,
const Stage& g);
903 void restoreToStage(Stage g);
911 void copyFrom(
const PerSubsystemInfo& src, Stage maxStage);
915 void clearAllocationStack(Array_<T>& stack);
917 void resizeAllocationStack(Array_<T>& stack,
int newSize);
919 void popAllocationStackBackToStage(Array_<T>& stack,
const Stage&);
921 void copyAllocationStackThroughStage(Array_<T>& stack,
922 const Array_<T>& src,
const Stage&);
939 systemStageVersions[i] = 1;
944 StateImpl(
const StateImpl& src);
946 StateImpl& operator=(
const StateImpl& src);
951 StateImpl* clone()
const {
return new StateImpl(*
this);}
953 const Stage& getSystemStage()
const {
return currentSystemStage;}
954 Stage& updSystemStage()
const {
return currentSystemStage;}
960 "StateImpl::getSubsystem()");
961 return subsystems[subx];
967 "StateImpl::updSubsystem()");
968 return subsystems[subx];
971 const Stage& getSubsystemStage(
int subsystem)
const {
972 return subsystems[subsystem].currentStage;
974 Stage& updSubsystemStage(
int subsystem)
const {
975 return subsystems[subsystem].currentStage;
978 const StageVersion* getSubsystemStageVersions(
int subsystem)
const {
979 return subsystems[subsystem].stageVersions;
989 void invalidateJustSystemStage(Stage stg);
995 void advanceSystemToStage(Stage stg)
const;
997 void setNumSubsystems(
int nSubs) {
1000 for (
int i=0; i < nSubs; ++i)
1001 subsystems.emplace_back(*
this);
1004 void initializeSubsystem
1006 updSubsystem(i).name = name;
1007 updSubsystem(i).version = version;
1010 SubsystemIndex addSubsystem(
const String& name,
const String& version) {
1012 subsystems.emplace_back(*
this, name, version);
1016 int getNumSubsystems()
const {
return (
int)subsystems.size();}
1019 return subsystems[subsys].name;
1022 return subsystems[subsys].version;
1027 void invalidateAll(Stage g) {
1028 invalidateJustSystemStage(g);
1030 subsystems[i].invalidateStageJustThisSubsystem(g);
1036 void invalidateAllCacheAtOrAbove(Stage g)
const {
1038 "StateImpl::invalidateAllCacheAtOrAbove()");
1042 StateImpl* mthis =
const_cast<StateImpl*
>(
this);
1043 mthis->invalidateJustSystemStage(g);
1045 mthis->subsystems[i].invalidateStageJustThisSubsystem(g);
1050 void advanceSubsystemToStage(
SubsystemIndex subsys, Stage g)
const {
1051 subsystems[subsys].advanceToStage(g);
1061 "StateImpl::allocateQ()");
1063 const Stage allocStage = getSubsystemStage(subsys).next();
1064 PerSubsystemInfo& ss = subsystems[subsys];
1065 const QIndex nxt(ss.getNextQIndex());
1066 ss.q_info.push_back(ContinuousVarInfo(allocStage,nxt,qInit,
Vector()));
1072 "StateImpl::allocateU()");
1073 const Stage allocStage = getSubsystemStage(subsys).next();
1074 PerSubsystemInfo& ss = subsystems[subsys];
1075 const UIndex nxt(ss.getNextUIndex());
1076 ss.uInfo.push_back(ContinuousVarInfo(allocStage,nxt,uInit,
Vector()));
1081 "StateImpl::allocateZ()");
1082 const Stage allocStage = getSubsystemStage(subsys).next();
1083 PerSubsystemInfo& ss = subsystems[subsys];
1084 const ZIndex nxt(ss.getNextZIndex());
1085 ss.zInfo.push_back(ContinuousVarInfo(allocStage,nxt,zInit,
Vector()));
1091 "StateImpl::allocateQErr()");
1092 const Stage allocStage = getSubsystemStage(subsys).next();
1093 const PerSubsystemInfo& ss = subsystems[subsys];
1094 const QErrIndex nxt(ss.getNextQErrIndex());
1095 ss.qerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nqerr,
Vector()));
1101 const Stage allocStage = getSubsystemStage(subsys).next();
1102 const PerSubsystemInfo& ss = subsystems[subsys];
1103 const UErrIndex nxt(ss.getNextUErrIndex());
1104 ss.uerrInfo.push_back(ConstraintErrInfo(allocStage,nxt,nuerr,
Vector()));
1109 "StateImpl::allocateUDotErr()");
1110 const Stage allocStage = getSubsystemStage(subsys).next();
1111 const PerSubsystemInfo& ss = subsystems[subsys];
1113 ss.udoterrInfo.push_back
1114 (ConstraintErrInfo(allocStage,nxt,nudoterr,
Vector()));
1120 "StateImpl::allocateEventTrigger()");
1121 const Stage allocStage = getSubsystemStage(subsys).next();
1122 const PerSubsystemInfo& ss = subsystems[subsys];
1124 nxt(ss.getNextEventTriggerByStageIndex(g));
1125 ss.triggerInfo[g].push_back(TriggerInfo(allocStage,nxt,nt));
1137 "StateImpl::allocateDiscreteVariable()");
1139 const Stage maxAcceptable = (invalidates <=
Stage::Model
1142 maxAcceptable.next(),
"StateImpl::allocateDiscreteVariable()");
1144 const Stage allocStage = getSubsystemStage(subsys).next();
1145 PerSubsystemInfo& ss = subsystems[subsys];
1147 ss.discreteInfo.push_back
1148 (DiscreteVarInfo(allocStage,invalidates,vp));
1155 AbstractValue* vp)
const
1159 "StateImpl::allocateCacheEntry()");
1161 "StateImpl::allocateCacheEntry()");
1165 const Stage allocStage = getSubsystemStage(subsys).next();
1166 const PerSubsystemInfo& ss = subsystems[subsys];
1169 allocStage, dependsOn, computedBy, vp);
1175 bool qPre,
bool uPre,
bool zPre,
1176 const Array_<DiscreteVarKey>& discreteVars,
1177 const Array_<CacheEntryKey>& cacheEntries,
1178 AbstractValue* value)
1184 for (
const auto& ckey : cacheEntries) {
1185 const CacheEntryInfo& prereq = getCacheEntryInfo(ckey);
1187 "State::allocateCacheEntryWithPrerequisites()",
1188 "Prerequisite cache entry (%d,%d) has depends-on stage %s "
1189 "but this one would have lower depends-on stage %s. That "
1190 "would mean the prerequisite could get invalidated without "
1191 "invalidating this one; not good.",
1192 (
int)ckey.first, (
int)ckey.second,
1193 prereq.getDependsOnStage().getName().c_str(),
1194 earliest.getName().c_str());
1198 allocateCacheEntry(subsys,earliest,latest,value);
1199 CacheEntryInfo& cinfo = updCacheEntryInfo(
CacheEntryKey(subsys,cx));
1201 if (qPre) cinfo.setPrerequisiteQ();
1202 if (uPre) cinfo.setPrerequisiteU();
1203 if (zPre) cinfo.setPrerequisiteZ();
1204 for (
const auto& dk : discreteVars)
1205 cinfo.setPrerequisite(dk);
1206 for (
const auto& ckey : cacheEntries)
1207 cinfo.setPrerequisite(ckey);
1209 cinfo.registerWithPrerequisites(*
this);
1217 Stage updateDependsOn)
1220 allocateDiscreteVariable(subsys,invalidates,vp->clone());
1224 PerSubsystemInfo& ss = subsystems[subsys];
1225 DiscreteVarInfo& dvinfo = ss.discreteInfo[dx];
1226 CacheEntryInfo& ceinfo = ss.cacheInfo[cx];
1227 dvinfo.setAutoUpdateEntry(cx);
1228 ceinfo.setAssociatedVar(dx);
1236 "StateImpl::getNY()");
1242 "StateImpl::getQStart()");
1247 "StateImpl::getNQ()");
1253 "StateImpl::getUStart()");
1258 "StateImpl::getNU()");
1264 "StateImpl::getZStart()");
1269 "StateImpl::getNZ()");
1273 int getNYErr()
const {
1275 "StateImpl::getNYErr()");
1281 "StateImpl::getQErrStart()");
1284 int getNQErr()
const {
1286 "StateImpl::getNQErr()");
1292 "StateImpl::getUErrStart()");
1295 int getNUErr()
const {
1297 "StateImpl::getNUErr()");
1303 int getNUDotErr()
const {
1305 "StateImpl::getNUDotErr()");
1306 return udoterr.size();
1309 int getNEventTriggers()
const {
1311 "StateImpl::getNEventTriggers()");
1312 return allTriggers.size();
1317 "StateImpl::getEventTriggerStartByStage()");
1319 for (
int j=0; j<g; ++j)
1320 nxt += triggers[j].size();
1324 int getNEventTriggersByStage(Stage g)
const {
1326 "StateImpl::getNEventTriggersByStage()");
1327 return triggers[g].size();
1330 std::mutex& getStateLock()
const {
1338 "StateImpl::getQStart(subsys)");
1339 return getSubsystem(subsys).qstart;
1343 "StateImpl::getNQ(subsys)");
1344 return getSubsystem(subsys).q.size();
1349 "StateImpl::getUStart(subsys)");
1350 return getSubsystem(subsys).ustart;
1354 "StateImpl::getNU(subsys)");
1355 return getSubsystem(subsys).u.size();
1360 "StateImpl::getZStart(subsys)");
1361 return getSubsystem(subsys).zstart;
1365 "StateImpl::getNZ(subsys)");
1366 return getSubsystem(subsys).z.size();
1371 "StateImpl::getQErrStart(subsys)");
1372 return getSubsystem(subsys).qerrstart;
1376 "StateImpl::getNQErr(subsys)");
1377 return getSubsystem(subsys).qerr.size();
1382 "StateImpl::getUErrStart(subsys)");
1383 return getSubsystem(subsys).uerrstart;
1387 "StateImpl::getNUErr(subsys)");
1388 return getSubsystem(subsys).uerr.size();
1394 "StateImpl::getUDotErrStart(subsys)");
1395 return getSubsystem(subsys).udoterrstart;
1399 "StateImpl::getNUDotErr(subsys)");
1400 return getSubsystem(subsys).udoterr.size();
1406 "StateImpl::getEventTriggerStartByStage(subsys)");
1407 return getSubsystem(subsys).triggerstart[g];
1410 int getNEventTriggersByStage(
SubsystemIndex subsys, Stage g)
const {
1412 "StateImpl::getNEventTriggersByStage(subsys)");
1413 return getSubsystem(subsys).triggers[g].size();
1420 "StateImpl::getQ(subsys)");
1421 return getSubsystem(subsys).q;
1425 "StateImpl::getU(subsys)");
1426 return getSubsystem(subsys).u;
1430 "StateImpl::getZ(subsys)");
1431 return getSubsystem(subsys).z;
1436 "StateImpl::getUWeights(subsys)");
1437 return getSubsystem(subsys).uWeights;
1441 "StateImpl::getZWeights(subsys)");
1442 return getSubsystem(subsys).zWeights;
1447 "StateImpl::getQDot(subsys)");
1449 "StateImpl::getQDot(subsys)");
1450 return getSubsystem(subsys).qdot;
1454 "StateImpl::getUDot(subsys)");
1456 "StateImpl::getUDot(subsys)");
1457 return getSubsystem(subsys).udot;
1461 "StateImpl::getZDot(subsys)");
1463 "StateImpl::getZDot(subsys)");
1464 return getSubsystem(subsys).zdot;
1468 "StateImpl::getQDotDot(subsys)");
1470 "StateImpl::getQDotDot(subsys)");
1471 return getSubsystem(subsys).qdotdot;
1476 "StateImpl::updQ(subsys)");
1479 return updSubsystem(subsys).q;
1483 "StateImpl::updU(subsys)");
1486 return updSubsystem(subsys).u;
1490 "StateImpl::updZ(subsys)");
1493 return updSubsystem(subsys).z;
1498 "StateImpl::updUWeights(subsys)");
1500 return updSubsystem(subsys).uWeights;
1504 "StateImpl::updZWeights(subsys)");
1506 return updSubsystem(subsys).zWeights;
1513 "StateImpl::updQDot(subsys)");
1514 return getSubsystem(subsys).qdot;
1518 "StateImpl::updUDot(subsys)");
1519 return getSubsystem(subsys).udot;
1523 "StateImpl::updZDot(subsys)");
1524 return getSubsystem(subsys).zdot;
1528 "StateImpl::updQDotDot(subsys)");
1529 return getSubsystem(subsys).qdotdot;
1535 "StateImpl::getQErr(subsys)");
1537 "StateImpl::getQErr(subsys)");
1538 return getSubsystem(subsys).qerr;
1542 "StateImpl::getUErr(subsys)");
1544 "StateImpl::getUErr(subsys)");
1545 return getSubsystem(subsys).uerr;
1550 "StateImpl::getQErrWeights(subsys)");
1551 return getSubsystem(subsys).qerrWeights;
1555 "StateImpl::getUErrWeights(subsys)");
1556 return getSubsystem(subsys).uerrWeights;
1561 "StateImpl::getUDotErr(subsys)");
1563 "StateImpl::getUDotErr(subsys)");
1564 return getSubsystem(subsys).udoterr;
1568 "StateImpl::getMultipliers(subsys)");
1570 "StateImpl::getMultipliers(subsys)");
1571 return getSubsystem(subsys).multipliers;
1576 "StateImpl::getEventTriggersByStage(subsys)");
1578 "StateImpl::getEventTriggersByStage(subsys)");
1579 return getSubsystem(subsys).triggers[g];
1584 "StateImpl::updQErr(subsys)");
1585 return getSubsystem(subsys).qerr;
1589 "StateImpl::updUErr(subsys)");
1590 return getSubsystem(subsys).uerr;
1595 "StateImpl::updQErrWeights(subsys)");
1597 return updSubsystem(subsys).qerrWeights;
1601 "StateImpl::updUErrWeights(subsys)");
1603 return updSubsystem(subsys).uerrWeights;
1608 "StateImpl::updUDotErr(subsys)");
1609 return getSubsystem(subsys).udoterr;
1613 "StateImpl::updMultipliers(subsys)");
1614 return getSubsystem(subsys).multipliers;
1618 "StateImpl::updEventTriggersByStage(subsys)");
1619 return getSubsystem(subsys).triggers[g];
1626 const Real& getTime()
const {
1628 "StateImpl::getTime()");
1632 const Vector& getY()
const {
1634 "StateImpl::getY()");
1638 const Vector& getQ()
const {
1640 "StateImpl::getQ()");
1644 const Vector& getU()
const {
1646 "StateImpl::getU()");
1650 const Vector& getZ()
const {
1652 "StateImpl::getZ()");
1656 const Vector& getUWeights()
const {
1658 "StateImpl::getUWeights()");
1662 const Vector& getZWeights()
const {
1664 "StateImpl::getZWeights()");
1673 "StateImpl::updTime()");
1680 "StateImpl::updY()");
1688 "StateImpl::updQ()");
1696 "StateImpl::updU()");
1704 "StateImpl::updZ()");
1712 "StateImpl::updUWeights()");
1719 "StateImpl::updZWeights()");
1725 const Vector& getYDot()
const {
1727 "StateImpl::getYDot()");
1731 const Vector& getQDot()
const {
1733 "StateImpl::getQDot()");
1737 const Vector& getZDot()
const {
1739 "StateImpl::getZDot()");
1743 const Vector& getUDot()
const {
1745 "StateImpl::getUDot()");
1749 const Vector& getQDotDot()
const {
1751 "StateImpl::getQDotDot()");
1756 Vector& updYDot()
const {
1758 "StateImpl::updYDot()");
1762 Vector& updQDot()
const {
1764 "StateImpl::updQDot()");
1768 Vector& updUDot()
const {
1770 "StateImpl::updUDot()");
1774 Vector& updZDot()
const {
1776 "StateImpl::updZDot()");
1780 Vector& updQDotDot()
const {
1782 "StateImpl::updQDotDot()");
1787 const Vector& getYErr()
const {
1789 "StateImpl::getYErr()");
1793 const Vector& getQErr()
const {
1795 "StateImpl::getQErr()");
1798 const Vector& getUErr()
const {
1800 "StateImpl::getUErr()");
1804 const Vector& getQErrWeights()
const {
1806 "StateImpl::getQErrWeights()");
1809 const Vector& getUErrWeights()
const {
1811 "StateImpl::getUErrWeights()");
1815 const Vector& getUDotErr()
const {
1817 "StateImpl::getUDotErr()");
1820 const Vector& getMultipliers()
const {
1822 "StateImpl::getMultipliers()");
1826 Vector& updYErr()
const {
1828 "StateImpl::updYErr()");
1833 "StateImpl::updQErr()");
1838 "StateImpl::updUErr()");
1842 Vector& updQErrWeights() {
1844 "StateImpl::updQErrWeights()");
1848 Vector& updUErrWeights() {
1850 "StateImpl::updUErrWeights()");
1855 Vector& updUDotErr()
const{
1857 "StateImpl::updUDotErr()");
1860 Vector& updMultipliers()
const{
1862 "StateImpl::updMultipliers()");
1866 const Vector& getEventTriggers()
const {
1868 "StateImpl::getEventTriggers()");
1871 const Vector& getEventTriggersByStage(Stage g)
const {
1873 "StateImpl::getEventTriggersByStage()");
1878 Vector& updEventTriggers()
const {
1880 "StateImpl::updEventTriggers()");
1883 Vector& updEventTriggersByStage(Stage g)
const {
1885 "StateImpl::updEventTriggersByStage()");
1890 return getSubsystem(dk.first).hasDiscreteVar(dk.second);
1893 const DiscreteVarInfo& getDiscreteVarInfo(
const DiscreteVarKey& dk)
const {
1894 return getSubsystem(dk.first).getDiscreteVarInfo(dk.second);
1898 return updSubsystem(dk.first).updDiscreteVarInfo(dk.second);
1902 return getDiscreteVarInfo(dk).getAutoUpdateEntry();
1905 Stage getDiscreteVarAllocationStage(
const DiscreteVarKey& dk)
const {
1906 return getDiscreteVarInfo(dk).getAllocationStage();
1909 Stage getDiscreteVarInvalidatesStage(
const DiscreteVarKey& dk)
const {
1910 return getDiscreteVarInfo(dk).getInvalidatedStage();
1914 const AbstractValue&
1916 const DiscreteVarInfo& dv = getDiscreteVarInfo(dk);
1917 return dv.getValue();
1921 return getDiscreteVarInfo(dk).getTimeLastUpdated();
1924 const AbstractValue& getDiscreteVarUpdateValue(
const DiscreteVarKey& dk)
const {
1926 SimTK_ERRCHK2(cx.isValid(),
"StateImpl::getDiscreteVarUpdateValue()",
1927 "Subsystem %d has a discrete variable %d but it does not have an"
1928 " associated update cache variable.",
1929 (
int)dk.first, (
int)dk.second);
1932 AbstractValue& updDiscreteVarUpdateValue(
const DiscreteVarKey& dk)
const {
1934 SimTK_ERRCHK2(cx.isValid(),
"StateImpl::updDiscreteVarUpdateValue()",
1935 "Subsystem %d has a discrete variable %d but it does not have an"
1936 " associated update cache variable.",
1937 (
int)dk.first, (
int)dk.second);
1940 bool isDiscreteVarUpdateValueRealized(
const DiscreteVarKey& dk)
const {
1943 "StateImpl::isDiscreteVarUpdateValueRealized()",
1944 "Subsystem %d has a discrete variable %d but it does not have an"
1945 " associated update cache variable.",
1946 (
int)dk.first, (
int)dk.second);
1949 void markDiscreteVarUpdateValueRealized(
const DiscreteVarKey& dk)
const {
1952 "StateImpl::markDiscreteVarUpdateValueRealized()",
1953 "Subsystem %d has a discrete variable %d but it does not have an"
1954 " associated update cache variable.",
1955 (
int)dk.first, (
int)dk.second);
1966 DiscreteVarInfo& dv = updDiscreteVarInfo(dk);
1970 invalidateAll(dv.getInvalidatedStage());
1975 CacheEntryInfo& ce = updCacheEntryInfo(
CacheEntryKey(dk.first,cx));
1976 ce.invalidate(*
this);
1981 return dv.updValue(*
this, t);
1985 return getSubsystem(ck.first).hasCacheEntry(ck.second);
1988 const CacheEntryInfo&
1990 return getSubsystem(ck.first).getCacheEntryInfo(ck.second);
1995 return getSubsystem(ck.first).updCacheEntryInfo(ck.second);
1998 Stage getCacheEntryAllocationStage(
const CacheEntryKey& ck)
const {
1999 return getCacheEntryInfo(ck).getAllocationStage();
2006 const AbstractValue&
2008 const CacheEntryInfo& ce = getCacheEntryInfo(ck);
2010 if (!ce.isUpToDate(*
this))
2011 ce.throwHelpfulOutOfDateMessage(*
this, __func__);
2012 return ce.getValue();
2019 return updCacheEntryInfo(ck).updValue(*
this);
2023 const CacheEntryInfo& ce = getCacheEntryInfo(ck);
2024 return ce.isUpToDate(*
this);
2027 void markCacheValueRealized(
const CacheEntryKey& ck)
const {
2028 CacheEntryInfo& ce = updCacheEntryInfo(ck);
2035 ce.getDependsOnStage().prev(),
2036 "StateImpl::markCacheValueRealized()");
2038 ce.markAsUpToDate(*
this);
2041 void markCacheValueNotRealized(
const CacheEntryKey& ck)
const {
2042 CacheEntryInfo& ce = updCacheEntryInfo(ck);
2043 ce.invalidate(*
this);
2049 void setSystemTopologyStageVersion(
StageVersion topoVersion)
2050 { assert(topoVersion>0);
2054 void getSystemStageVersions(Array_<StageVersion>& versions)
const {
2055 versions.resize(currentSystemStage+1);
2056 for (
int i=0; i <= currentSystemStage; ++i)
2057 versions[i] = systemStageVersions[i];
2064 Stage getLowestSystemStageDifference
2065 (
const Array_<StageVersion>& prevVersions)
const {
2066 const int nRealizedBefore = (int)prevVersions.size();
2067 const int nRealizedNow = (int)currentSystemStage+1;
2068 const int nRealizedBoth =
std::min(nRealizedBefore,nRealizedNow);
2072 for (; g < nRealizedBoth; ++g)
2073 if (systemStageVersions[g] != prevVersions[g])
2082 ValueVersion getQValueVersion()
const {
return qVersion;}
2083 ValueVersion getUValueVersion()
const {
return uVersion;}
2084 ValueVersion getZValueVersion()
const {
return zVersion;}
2086 const ListOfDependents& getQDependents()
const {
return qDependents;}
2087 const ListOfDependents& getUDependents()
const {
return uDependents;}
2088 const ListOfDependents& getZDependents()
const {
return zDependents;}
2090 ListOfDependents& updQDependents() {
return qDependents;}
2091 ListOfDependents& updUDependents() {
return uDependents;}
2092 ListOfDependents& updZDependents() {
return zDependents;}
2094 void autoUpdateDiscreteVariables();
2096 String toString()
const;
2097 String cacheToString()
const;
2103 void copyFrom(
const StateImpl& source);
2109 void invalidateCopiedStageVersions(
const StateImpl& src) {
2110 for (
int i=1; i <= src.currentSystemStage; ++i)
2111 systemStageVersions[i] = src.systemStageVersions[i]+1;
2113 qVersion = src.qVersion + 1;
2114 uVersion = src.uVersion + 1;
2115 zVersion = src.zVersion + 1;
2117 qDependents.clear();
2118 uDependents.clear();
2119 zDependents.clear();
2125 void registerWithPrerequisitesAfterCopy() {
2126 for (
auto& subsys : subsystems) {
2127 for (
auto& ce : subsys.cacheInfo)
2128 ce.registerWithPrerequisites(*
this);
2136 { ++qVersion; qDependents.notePrerequisiteChange(*
this); }
2138 { ++uVersion; uDependents.notePrerequisiteChange(*
this); }
2140 { ++zVersion; zDependents.notePrerequisiteChange(*
this); }
2142 void noteYChange() {noteQChange();noteUChange();noteZChange();}
2145 bool allSubsystemsAtLeastAtStage(Stage g)
const {
2147 if (subsystems[i].currentStage < g)
2155 Array_<PerSubsystemInfo> subsystems;
2185 ListOfDependents qDependents;
2186 ListOfDependents uDependents;
2187 ListOfDependents zDependents;
2226 mutable Vector multipliers;
2235 mutable Vector allTriggers;
2244 mutable std::mutex stateLock;
2251 inline void ListOfDependents::
2252 notePrerequisiteChange(
const StateImpl& stateImpl)
const {
2253 for (
auto ckey : m_dependents) {
2255 CacheEntryInfo& ce = stateImpl.updCacheEntryInfo(ckey);
2256 ce.invalidate(stateImpl);
2264 isUpToDate(
const StateImpl& stateImpl)
const {
2265 const PerSubsystemInfo& subsys = stateImpl.getSubsystem(m_myKey.first);
2266 assert(&subsys.getCacheEntryInfo(m_myKey.second) ==
this);
2267 if (subsys.getCurrentStage() >= m_computedByStage)
2269 if (subsys.getCurrentStage() < m_dependsOnStage)
2274 const StageVersion version = subsys.getStageVersion(m_dependsOnStage);
2275 assert(version >= 1);
2276 if (!( version == m_dependsOnVersionWhenLastComputed
2277 && m_isUpToDateWithPrerequisites))
2282 validatePrerequisiteVersions(stateImpl);
2287 inline void CacheEntryInfo::
2288 markAsUpToDate(
const StateImpl& stateImpl) {
2289 const PerSubsystemInfo& subsys = stateImpl.getSubsystem(m_myKey.first);
2290 assert(&subsys.getCacheEntryInfo(m_myKey.second) ==
this);
2291 const StageVersion version = subsys.getStageVersion(m_dependsOnStage);
2292 assert(version >= 1);
2293 m_dependsOnVersionWhenLastComputed = version;
2294 m_isUpToDateWithPrerequisites =
true;
2299 recordPrerequisiteVersions(stateImpl);
2309 updImpl().setNumSubsystems(i);
2312 (
SubsystemIndex subsys,
const String& name,
const String& version) {
2313 updImpl().initializeSubsystem(subsys, name, version);
2317 (
const String& name,
const String& version) {
2318 return updImpl().addSubsystem(name, version);
2321 return getImpl().getNumSubsystems();
2324 return getImpl().getSubsystemName(subsys);
2327 return getImpl().getSubsystemVersion(subsys);
2330 return getImpl().getSubsystemStage(subsys);
2333 return getImpl().getSystemStage();
2336 updImpl().invalidateAll(stage);
2339 getImpl().invalidateAllCacheAtOrAbove(stage);
2342 getImpl().advanceSubsystemToStage(subsys, stage);
2345 getImpl().advanceSystemToStage(stage);
2349 {
return getImpl().getSystemTopologyStageVersion(); }
2353 return updImpl().allocateQ(subsys, qInit);
2356 return updImpl().allocateU(subsys, uInit);
2359 return updImpl().allocateZ(subsys, zInit);
2364 return getImpl().allocateQErr(subsys, nqerr);
2367 return getImpl().allocateUErr(subsys, nuerr);
2371 return getImpl().allocateUDotErr(subsys, nudoterr);
2377 return getImpl().allocateEventTrigger(subsys, stage, nevent);
2383 return updImpl().allocateDiscreteVariable(subsys, stage, v);
2388 Stage updateDependsOn) {
2389 return updImpl().allocateAutoUpdateDiscreteVariable
2390 (subsys, invalidates, v, updateDependsOn);
2396 return getImpl().getDiscreteVarUpdateIndex(
DiscreteVarKey(subsys,index));
2401 return getImpl().getDiscreteVarAllocationStage(
DiscreteVarKey(subsys,index));
2406 return getImpl().getDiscreteVarInvalidatesStage(
DiscreteVarKey(subsys,index));
2409 inline const AbstractValue&
State::
2411 return getImpl().getDiscreteVariable(
DiscreteVarKey(subsys,index));
2416 return getImpl().getDiscreteVarLastUpdateTime(
DiscreteVarKey(subsys,index));
2418 inline const AbstractValue&
State::
2421 return getImpl().getDiscreteVarUpdateValue(
DiscreteVarKey(subsys,index));
2426 return getImpl().updDiscreteVarUpdateValue(
DiscreteVarKey(subsys,index));
2431 return getImpl().isDiscreteVarUpdateValueRealized
2437 getImpl().markDiscreteVarUpdateValueRealized(
DiscreteVarKey(subsys,index));
2443 return updImpl().updDiscreteVariable(
DiscreteVarKey(subsys,index));
2454 AbstractValue* value)
const {
2455 return getImpl().allocateCacheEntry(subsys, dependsOn, computedBy, value);
2461 bool q,
bool u,
bool z,
2462 const Array_<DiscreteVarKey>& discreteVars,
2463 const Array_<CacheEntryKey>& cacheEntries,
2464 AbstractValue* value) {
2465 return updImpl().allocateCacheEntryWithPrerequisites
2466 (subsys, earliest, latest, q, u, z, discreteVars, cacheEntries, value);
2471 return getImpl().getCacheEntryAllocationStage(
CacheEntryKey(subsys,index));
2473 inline const AbstractValue&
State::
2475 return getImpl().getCacheEntry(
CacheEntryKey(subsys,index));
2479 return getImpl().updCacheEntry(
CacheEntryKey(subsys,index));
2484 return getImpl().isCacheValueRealized(
CacheEntryKey(subx,cx));
2494 getImpl().markCacheValueNotRealized(
CacheEntryKey(subx,cx));
2498 return getImpl().getStateLock();
2503 return getImpl().getNY();
2506 return getImpl().getNQ();
2509 return getImpl().getQStart();
2512 return getImpl().getNU();
2515 return getImpl().getUStart();
2518 return getImpl().getNZ();
2521 return getImpl().getZStart();
2524 return getImpl().getNYErr();
2527 return getImpl().getNQErr();
2530 return getImpl().getQErrStart();
2533 return getImpl().getNUErr();
2536 return getImpl().getUErrStart();
2539 return getImpl().getNUDotErr();
2545 return getImpl().getNEventTriggers();
2548 return getImpl().getNEventTriggersByStage(stage);
2555 return getImpl().getQStart(subsys);
2558 return getImpl().getNQ(subsys);
2561 return getImpl().getUStart(subsys);
2564 return getImpl().getNU(subsys);
2567 return getImpl().getZStart(subsys);
2570 return getImpl().getNZ(subsys);
2573 return getImpl().getQErrStart(subsys);
2576 return getImpl().getNQErr(subsys);
2579 return getImpl().getUErrStart(subsys);
2582 return getImpl().getNUErr(subsys);
2585 return getImpl().getUDotErrStart(subsys);
2588 return getImpl().getNUDotErr(subsys);
2598 return getImpl().getEventTriggerStartByStage(subsys, stage);
2602 return getImpl().getNEventTriggersByStage(subsys, stage);
2609 return getImpl().getEventTriggersByStage(subsys, stage);
2613 return getImpl().updEventTriggersByStage(subsys, stage);
2616 return getImpl().getQ(subsys);
2619 return getImpl().getU(subsys);
2622 return getImpl().getZ(subsys);
2625 return getImpl().getUWeights(subsys);
2628 return getImpl().getZWeights(subsys);
2631 return updImpl().updQ(subsys);
2634 return updImpl().updU(subsys);
2637 return updImpl().updZ(subsys);
2640 return updImpl().updUWeights(subsys);
2643 return updImpl().updZWeights(subsys);
2646 return getImpl().getQDot(subsys);
2649 return getImpl().getUDot(subsys);
2652 return getImpl().getZDot(subsys);
2655 return getImpl().getQDotDot(subsys);
2658 return getImpl().updQDot(subsys);
2661 return getImpl().updUDot(subsys);
2664 return getImpl().updZDot(subsys);
2667 return getImpl().updQDotDot(subsys);
2670 return getImpl().getQErr(subsys);
2673 return getImpl().getUErr(subsys);
2676 return getImpl().getQErrWeights(subsys);
2679 return getImpl().getUErrWeights(subsys);
2682 return getImpl().getUDotErr(subsys);
2685 return getImpl().getMultipliers(subsys);
2688 return getImpl().updQErr(subsys);
2691 return getImpl().updUErr(subsys);
2694 return updImpl().updQErrWeights(subsys);
2697 return updImpl().updUErrWeights(subsys);
2700 return getImpl().updUDotErr(subsys);
2703 return getImpl().updMultipliers(subsys);
2708 return getImpl().getEventTriggerStartByStage(stage);
2712 return getImpl().getEventTriggers();
2715 return getImpl().getEventTriggersByStage(stage);
2719 return getImpl().updEventTriggers();
2722 return getImpl().updEventTriggersByStage(stage);
2726 return getImpl().getTime();
2729 return getImpl().getY();
2732 return getImpl().getQ();
2735 return getImpl().getU();
2738 return getImpl().getZ();
2741 return getImpl().getUWeights();
2744 return getImpl().getZWeights();
2747 return updImpl().updTime();
2750 return updImpl().updY();
2759 return updImpl().updQ();
2762 return updImpl().updU();
2765 return updImpl().updZ();
2768 return updImpl().updUWeights();
2771 return updImpl().updZWeights();
2783 return getImpl().getYDot();
2786 return getImpl().getQDot();
2789 return getImpl().getZDot();
2792 return getImpl().getUDot();
2795 return getImpl().getQDotDot();
2798 return getImpl().updYDot();
2801 return getImpl().updQDot();
2804 return getImpl().updZDot();
2807 return getImpl().updUDot();
2810 return getImpl().updQDotDot();
2813 return getImpl().getYErr();
2816 return getImpl().getQErr();
2819 return getImpl().getUErr();
2822 return getImpl().getQErrWeights();
2825 return getImpl().getUErrWeights();
2828 return getImpl().getUDotErr();
2831 return getImpl().getMultipliers();
2834 return getImpl().updYErr();
2837 return getImpl().updQErr();
2840 return getImpl().updUErr();
2843 return updImpl().updQErrWeights();
2846 return updImpl().updUErrWeights();
2849 return getImpl().updUDotErr();
2852 return getImpl().updMultipliers();
2857 {
return updImpl().setSystemTopologyStageVersion(topoVersion); }
2861 return getImpl().getSystemStageVersions(versions);
2865 return getImpl().getLowestSystemStageDifference(prev);
2869 return getImpl().getQValueVersion();
2873 return getImpl().getUValueVersion();
2877 return getImpl().getZValueVersion();
2881 return getImpl().getQDependents();
2885 return getImpl().getUDependents();
2889 return getImpl().getZDependents();
2893 return getImpl().hasCacheEntry(cacheEntry);
2896 inline const CacheEntryInfo&
State::
2898 return getImpl().getCacheEntryInfo(cacheEntry);
2901 inline CacheEntryInfo&
State::
2903 return updImpl().updCacheEntryInfo(cacheEntry);
2907 return getImpl().hasDiscreteVar(discreteVar);
2910 inline const DiscreteVarInfo&
State::
2912 return getImpl().getDiscreteVarInfo(discreteVar);
2915 inline const PerSubsystemInfo&
State::
2917 return getImpl().getSubsystem(subx);
2922 updImpl().autoUpdateDiscreteVariables();
2926 return getImpl().toString();
2929 return getImpl().cacheToString();
#define SimTK_INDEXCHECK(ix, ub, where)
Definition: ExceptionMacros.h:145
#define SimTK_ASSERT2(cond, msg, a1, a2)
Definition: ExceptionMacros.h:375
#define SimTK_STAGECHECK_RANGE_ALWAYS(lower, current, upper, methodNm)
Definition: ExceptionMacros.h:186
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
#define SimTK_STAGECHECK_GE_ALWAYS(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:180
#define SimTK_ASSERT2_ALWAYS(cond, msg, a1, a2)
Definition: ExceptionMacros.h:353
#define SimTK_ERRCHK4_ALWAYS(cond, whereChecked, fmt, a1, a2, a3, a4)
Definition: ExceptionMacros.h:297
#define SimTK_STAGECHECK_GE(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:208
#define SimTK_STAGECHECK_LT_ALWAYS(currentStage, targetStage, methodNm)
Definition: ExceptionMacros.h:183
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
#define SimTK_FORCE_INLINE
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:269
This unique integer type is for selecting non-shared cache entries.
This unique integer type is for selecting discrete variables.
Unique integer type for Subsystem-local, per-stage event indexing.
Unique integer type for Subsystem-local qErr indexing.
Unique integer type for Subsystem-local q indexing.
@ NValid
Definition: Stage.h:88
@ Time
A new time has been realized.
Definition: Stage.h:73
@ LowestRuntime
For iterating over meaningful stage values.
Definition: Stage.h:83
@ Topology
System topology realized.
Definition: Stage.h:70
@ Empty
Lower than any legitimate Stage.
Definition: Stage.h:69
@ HighestRuntime
Definition: Stage.h:84
@ Acceleration
Accelerations and multipliers calculated.
Definition: Stage.h:77
@ Position
Spatial configuration available.
Definition: Stage.h:74
@ Dynamics
Forces calculated.
Definition: Stage.h:76
@ Velocity
Spatial velocities available.
Definition: Stage.h:75
@ Model
Modeling choices made.
Definition: Stage.h:71
@ Infinity
Higher than any legitimate Stage.
Definition: Stage.h:79
@ Instance
Physical parameters set.
Definition: Stage.h:72
@ Report
Report-only quantities evaluated.
Definition: Stage.h:78
Vector & updQ()
These are just views into Y.
SystemEventTriggerIndex getEventTriggerStartByStage(Stage) const
Return the index within the global event trigger array at which the first of the event triggers assoc...
int getNYErr() const
Get the total number nyerr=nqerr+nuerr of shared cache entries for position-level and velocity-level ...
void invalidateAll(Stage)
If any subsystem or the system stage is currently at or higher than the passed-in one,...
void setDiscreteVariable(SubsystemIndex, DiscreteVariableIndex, const AbstractValue &)
Alternate interface to updDiscreteVariable.
int getNQErr() const
Return the total number nqerr=mp+nQuaternions of cache entries for position-level constraint errors.
int getNEventTriggers() const
Return the total number of event trigger function slots in the cache.
int getNUDotErr() const
Return the total number nudotErr=mp+mv+ma of cache entries for acceleration-level constraint errors (...
void setU(const Vector &u)
void setY(const Vector &y)
const ListOfDependents & getUDependents() const
(Advanced) Return the list of cache entries for which u was specified as an explicit prerequisite.
const ListOfDependents & getQDependents() const
(Advanced) Return the list of cache entries for which q was specified as an explicit prerequisite.
void setSystemTopologyStageVersion(StageVersion topoVersion)
(Advanced) This explicitly modifies the Topology stage version; don't use this method unless you know...
SystemYErrIndex getUErrStart() const
Returns the yErr index at which the uErr's begin. Callable at Instance stage.
Vector & updYDot() const
These are mutable.
AbstractValue & updCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a writable reference to the value contained in a particular cache entry.
const Vector & getYErr() const
Return the current constraint errors for all constraints.
const Vector & getUDot() const
void markCacheValueNotRealized(SubsystemIndex, CacheEntryIndex) const
(Advanced) Normally cache entries are invalidated automatically, however this method allows manual in...
void autoUpdateDiscreteVariables()
(Advanced) This is called at the beginning of every integration step to set the values of auto-update...
QIndex allocateQ(SubsystemIndex, const Vector &qInit)
Allocate generalized coordinates q, which are second order continuous state variables.
int getNQ() const
Get total number of shared q's (generalized coordinates; second order state variables).
bool hasDiscreteVar(const DiscreteVarKey &discreteVar) const
(Advanced) Check whether this State has a particular discrete state variable.
Vector & updMultipliers() const
const Vector & getQDotDot() const
This has its own space, not a view.
String cacheToString() const
(Debugging) Not suitable for serialization.
const Stage & getSystemStage() const
This returns the global stage for this State.
bool hasCacheEntry(const CacheEntryKey &cacheEntry) const
(Advanced) Check whether this State has a particular cache entry.
CacheEntryInfo & updCacheEntryInfo(const CacheEntryKey &cacheEntry)
(Advanced) Return a writable reference to the cache entry information for a particular cache entry.
ValueVersion getQValueVersion() const
(Advanced) Return a ValueVersion for q, meaning an integer that is incremented whenever any q is chan...
const Real & getTime() const
You can call these as long as system stage >= Model.
bool isDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Check whether the update value for this auto-update discrete variable has already been computed since...
const DiscreteVarInfo & getDiscreteVarInfo(const DiscreteVarKey &discreteVar) const
(Advanced) Return a reference to the discrete variable information for a particular discrete variable...
void setZ(const Vector &z)
const String & getSubsystemVersion(SubsystemIndex) const
QErrIndex allocateQErr(SubsystemIndex, int nqerr) const
Allocate nqerr cache slots to hold the current error for position-level (holonomic) constraint equati...
Vector & updUErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraints.
const AbstractValue & getDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the current value of its associated update cache entry...
ValueVersion getZValueVersion() const
(Advanced) Return a ValueVersion for z, meaning an integer that is incremented whenever any z is chan...
void setNumSubsystems(int i)
Set the number of subsystems in this state.
Stage getDiscreteVarInvalidatesStage(SubsystemIndex, DiscreteVariableIndex) const
What is the earliest stage that is invalidated when this discrete variable is modified?...
SystemYIndex getUStart() const
Returns the y index at which the u's begin. Callable at Model stage.
const Vector & getY() const
SystemYErrIndex getQErrStart() const
Returns the yErr index at which the qErr's begin. Callable at Instance stage.
const Vector & getU() const
Vector & updYErr() const
These are mutable.
AbstractValue & updDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return a writable reference to the value of its associated up...
int getNumSubsystems() const
Return the number of Subsystems known to this State.
CacheEntryIndex allocateCacheEntryWithPrerequisites(SubsystemIndex, Stage earliest, Stage latest, bool q, bool u, bool z, const Array_< DiscreteVarKey > &discreteVars, const Array_< CacheEntryKey > &cacheEntries, AbstractValue *value)
(Advanced) Allocate a cache entry with prerequisites other than just reaching a particular computatio...
const Vector & getUErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraint equation...
int getNU() const
Get total number of shared u's (generalized speeds; mobilities).
const Vector & getYDot() const
const Vector & getMultipliers() const
const Vector & getZDot() const
const String & getSubsystemName(SubsystemIndex) const
Stage getDiscreteVarAllocationStage(SubsystemIndex, DiscreteVariableIndex) const
At what stage was this State when this discrete variable was allocated? The answer must be Stage::Emp...
SystemYIndex getQStart() const
Returns the y index at which the q's begin. Callable at Model stage.
const Vector & getUDotErr() const
These have their own space, they are not views.
AbstractValue & updDiscreteVariable(SubsystemIndex, DiscreteVariableIndex)
Get a writable reference to the value stored in the indicated discrete state variable dv,...
SystemMultiplierIndex getMultipliersStart(SubsystemIndex) const
Real getDiscreteVarLastUpdateTime(SubsystemIndex, DiscreteVariableIndex) const
Return the time of last update for this discrete variable.
Vector & updEventTriggersByStage(Stage) const
StageVersion getSystemTopologyStageVersion() const
The Topology stage version number (an integer) stored in this State must match the topology cache ver...
void advanceSubsystemToStage(SubsystemIndex, Stage) const
Advance a particular Subsystem's current stage by one to the indicated stage.
const Vector & getQErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mquat position inline constraints equations.
void advanceSystemToStage(Stage) const
Advance the System-level current stage by one to the indicated stage.
void setQ(const Vector &q)
Alternate interface.
const Vector & getZ() const
void markCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Mark the value of a particular cache entry as up to date after it has been recalculated.
UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const
Allocate nudoterr cache slots to hold the current error for acceleration-level (acceleration-only,...
Vector & updQDotDot() const
This is a separate shared cache entry, not part of YDot.
const ListOfDependents & getZDependents() const
(Advanced) Return the list of cache entries for which z was specified as an explicit prerequisite.
SystemYIndex getZStart() const
Returns the y index at which the z's begin. Callable at Model stage.
const AbstractValue & getCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a const reference to the value contained in a particular cache entry.
std::mutex & getStateLock() const
Returns a mutex that should be used to lock the state whenever multiple threads are asynchronously wr...
CacheEntryIndex getDiscreteVarUpdateIndex(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the CacheEntryIndex for its associated update cache en...
Vector & updUWeights()
Set u weights (and q weights indirectly).
UErrIndex allocateUErr(SubsystemIndex, int nuerr) const
Allocate nuerr cache slots to hold the current error for velocity-level (nonholonomic and holonomic f...
Stage getCacheEntryAllocationStage(SubsystemIndex, CacheEntryIndex) const
At what stage was this State when this cache entry was allocated? The answer must be Stage::Empty,...
ValueVersion getUValueVersion() const
(Advanced) Return a ValueVersion for u, meaning an integer that is incremented whenever any u is chan...
int getNZ() const
Get total number of shared z's (auxiliary state variables).
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *, Stage updateDependsOn)
This method allocates a DiscreteVariable whose value should be updated automatically after each time ...
int getNY() const
Get the total number ny=nq+nu+nz of shared continuous state variables.
void invalidateAllCacheAtOrAbove(Stage) const
If any subsystem or the system stage is currently at or higher than the passed-in one,...
const Vector & getZWeights() const
Get a unit weighting (1/unit change) for each z that can be used to weight a vector dz so that the di...
int getNUErr() const
Return the total number nuerr=mp+mv of cache entries for velocity-level constraint errors (including ...
SubsystemIndex addSubsystem(const String &name, const String &version)
Register a new subsystem as a client of this State.
const Stage & getSubsystemStage(SubsystemIndex) const
int getNMultipliers() const
Return the total number of constraint multipliers; necessarily the same as the number of acceleration...
const Vector & getUWeights() const
Get a unit weighting (1/unit change) for each u that can be used to weight a vector du so that the di...
const Vector & getUErr() const
const AbstractValue & getDiscreteVariable(SubsystemIndex, DiscreteVariableIndex) const
Get the current value of the indicated discrete variable.
Stage getLowestSystemStageDifference(const Array_< StageVersion > &prevVersions) const
(Advanced) Given a list of per-stage version numbers extracted by an earlier call to getSystemStageVe...
SystemUDotErrIndex getUDotErrStart(SubsystemIndex) const
const Vector & getQErr() const
These are just views into YErr.
String toString() const
(Debugging) Not suitable for serialization.
Real & updTime()
You can call these as long as System stage >= Model, but the stage will be backed up if necessary to ...
Vector & updZWeights()
Set z weights.
CacheEntryIndex allocateCacheEntry(SubsystemIndex, Stage earliest, Stage latest, AbstractValue *value) const
There are two Stages supplied explicitly as arguments to this method: earliest and latest.
Vector & updEventTriggers() const
const Vector & getEventTriggersByStage(Stage) const
const CacheEntryInfo & getCacheEntryInfo(const CacheEntryKey &cacheEntry) const
(Advanced) Return a const reference to the cache entry information for a particular cache entry.
Vector & updUDotErr() const
void markDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Mark the update value for this auto-update discrete variable as up-to-date with respect to the state ...
UIndex allocateU(SubsystemIndex, const Vector &uInit)
Allocate generalized speeds u, which are first order continuous state variables related to the deriva...
int getNEventTriggersByStage(Stage) const
Return the size of the partition of event trigger functions which are evaluated at a given Stage.
void initializeSubsystem(SubsystemIndex, const String &name, const String &version)
Set the name and version for a given subsystem, which must already have a slot allocated.
DiscreteVariableIndex allocateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *)
The Stage supplied here in the call is the earliest subsystem stage which is invalidated by a change ...
const Vector & getEventTriggers() const
EventTriggerByStageIndex allocateEventTrigger(SubsystemIndex, Stage stage, int nevent) const
Allocate room for nevent witness function values that will be available at the indicated stage.
ZIndex allocateZ(SubsystemIndex, const Vector &zInit)
Allocate auxiliary first order continuous state variables z.
Vector & updQErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mquat position inline constraint equations.
void setTime(Real t)
An alternate syntax equivalent to updTime() and updY().
bool isCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Check whether the value in a particular cache entry has been recalculated since the last change to th...
const PerSubsystemInfo & getPerSubsystemInfo(SubsystemIndex) const
(Advanced) Return a reference to the per-subsystem information in the state.
void getSystemStageVersions(Array_< StageVersion > &versions) const
(Advanced) Record the current version numbers of each valid System-level stage.
const Vector & getQ() const
These are just views into Y.
const Vector & getQDot() const
These are just views into YDot.
Provide a unique integer type for identifying Subsystems.
This unique integer type is for identifying a triggered event within a particular Stage of the full S...
This unique integer type is for identifying a triggered event in the full System-level view of the St...
This unique integer type is for indexing global "multiplier-like" arrays, that is,...
This unique integer type is for indexing global "qErr-like" arrays, that is, arrays that inherently h...
This unique integer type is for indexing global "q-like" arrays, that is, arrays that inherently have...
This unique integer type is for indexing global "uDotErr-like" arrays, that is, arrays that inherentl...
This unique integer type is for indexing global "uErr-like" arrays, that is, arrays that inherently h...
This unique integer type is for indexing global "u-like" arrays, that is, arrays that inherently have...
This unique integer type is for indexing the global, System-level "yErr-like" arrays,...
This unique integer type is for indexing the global, System-level "y-like" arrays,...
This unique integer type is for indexing global "z-like" arrays, that is, arrays that inherently have...
Unique integer type for Subsystem-local uDotErr indexing.
Unique integer type for Subsystem-local uErr indexing.
Unique integer type for Subsystem-local u indexing.
Unique integer type for Subsystem-local z indexing.
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1477
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
std::pair< SubsystemIndex, DiscreteVariableIndex > DiscreteVarKey
Definition: State.h:165
long long StageVersion
This is the type to use for Stage version numbers that get incremented whenever a state variable chan...
Definition: Stage.h:44
long long ValueVersion
This is the type to use for state variable version numbers that get incremented whenever a state valu...
Definition: Stage.h:52
std::pair< SubsystemIndex, CacheEntryIndex > CacheEntryKey
Definition: State.h:164
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607