1#ifndef SimTK_SimTKCOMMON_STATE_IMPL_H_
2#define SimTK_SimTKCOMMON_STATE_IMPL_H_
62class 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;
155class 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();
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;
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 {
505class 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 {
557class 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());
706 if (qerrInfo.empty())
return QErrIndex(0);
707 const ConstraintErrInfo& last = qerrInfo.back();
708 return QErrIndex(last.getFirstIndex()+last.getNumErrs());
711 if (uerrInfo.empty())
return UErrIndex(0);
712 const ConstraintErrInfo& last = uerrInfo.back();
713 return UErrIndex(last.getFirstIndex()+last.getNumErrs());
717 const ConstraintErrInfo& last = udoterrInfo.back();
718 return UDotErrIndex(last.getFirstIndex()+last.getNumErrs());
728 const TriggerInfo& last = triggerInfo[g].back();
730 (last.getFirstIndex()+last.getNumSlots());
734 return index < (int)discreteInfo.size();
741 "PerSubsystemInfo::getDiscreteVarInfo()");
742 return discreteInfo[index];
748 "PerSubsystemInfo::updDiscreteVarInfo()");
749 return discreteInfo[index];
754 return index < (int)cacheInfo.size();
760 "PerSubsystemInfo::getCacheEntryInfo()");
761 return cacheInfo[index];
767 "PerSubsystemInfo::updCacheEntryInfo()");
768 return cacheInfo[index];
773 {
return stageVersions[g]; }
776friend 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;
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;}
964 "StateImpl::getSubsystem()");
965 return subsystems[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
1010 updSubsystem(i).name = name;
1011 updSubsystem(i).version = version;
1014 SubsystemIndex addSubsystem(
const String& name,
const String& version) {
1016 subsystems.emplace_back(*
this, name, version);
1020 int getNumSubsystems()
const {
return (
int)subsystems.size();}
1023 return subsystems[subsys].name;
1026 return subsystems[subsys].version;
1031 void invalidateAll(Stage g) {
1032 invalidateJustSystemStage(g);
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);
1049 mthis->subsystems[i].invalidateStageJustThisSubsystem(g);
1054 void advanceSubsystemToStage(
SubsystemIndex subsys, Stage g)
const {
1055 subsystems[subsys].advanceToStage(g);
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()));
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()));
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()));
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()));
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()));
1113 "StateImpl::allocateUDotErr()");
1114 const Stage allocStage = getSubsystemStage(subsys).next();
1115 const PerSubsystemInfo& ss = subsystems[subsys];
1117 ss.udoterrInfo.push_back
1118 (ConstraintErrInfo(allocStage,nxt,nudoterr,
Vector()));
1124 "StateImpl::allocateEventTrigger()");
1125 const Stage allocStage = getSubsystemStage(subsys).next();
1126 const PerSubsystemInfo& ss = subsystems[subsys];
1128 nxt(ss.getNextEventTriggerByStageIndex(g));
1129 ss.triggerInfo[g].push_back(TriggerInfo(allocStage,nxt,nt));
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];
1151 ss.discreteInfo.push_back
1152 (DiscreteVarInfo(allocStage,invalidates,vp));
1159 AbstractValue* vp)
const
1163 "StateImpl::allocateCacheEntry()");
1165 "StateImpl::allocateCacheEntry()");
1169 const Stage allocStage = getSubsystemStage(subsys).next();
1170 const PerSubsystemInfo& ss = subsystems[subsys];
1173 allocStage, dependsOn, computedBy, vp);
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());
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);
1221 Stage updateDependsOn)
1224 allocateDiscreteVariable(subsys,invalidates,vp->clone());
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()");
1246 "StateImpl::getQStart()");
1251 "StateImpl::getNQ()");
1257 "StateImpl::getUStart()");
1262 "StateImpl::getNU()");
1268 "StateImpl::getZStart()");
1273 "StateImpl::getNZ()");
1277 int getNYErr()
const {
1279 "StateImpl::getNYErr()");
1285 "StateImpl::getQErrStart()");
1288 int getNQErr()
const {
1290 "StateImpl::getNQErr()");
1296 "StateImpl::getUErrStart()");
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();
1321 "StateImpl::getEventTriggerStartByStage()");
1323 for (
int j=0; j<g; ++j)
1324 nxt += triggers[j].size();
1328 int getNEventTriggersByStage(Stage g)
const {
1330 "StateImpl::getNEventTriggersByStage()");
1331 return triggers[g].size();
1334 std::mutex& getStateLock()
const {
1342 "StateImpl::getQStart(subsys)");
1343 return getSubsystem(subsys).qstart;
1347 "StateImpl::getNQ(subsys)");
1348 return getSubsystem(subsys).q.size();
1353 "StateImpl::getUStart(subsys)");
1354 return getSubsystem(subsys).ustart;
1358 "StateImpl::getNU(subsys)");
1359 return getSubsystem(subsys).u.size();
1364 "StateImpl::getZStart(subsys)");
1365 return getSubsystem(subsys).zstart;
1369 "StateImpl::getNZ(subsys)");
1370 return getSubsystem(subsys).z.size();
1375 "StateImpl::getQErrStart(subsys)");
1376 return getSubsystem(subsys).qerrstart;
1380 "StateImpl::getNQErr(subsys)");
1381 return getSubsystem(subsys).qerr.size();
1386 "StateImpl::getUErrStart(subsys)");
1387 return getSubsystem(subsys).uerrstart;
1391 "StateImpl::getNUErr(subsys)");
1392 return getSubsystem(subsys).uerr.size();
1398 "StateImpl::getUDotErrStart(subsys)");
1399 return getSubsystem(subsys).udoterrstart;
1403 "StateImpl::getNUDotErr(subsys)");
1404 return getSubsystem(subsys).udoterr.size();
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();
1424 "StateImpl::getQ(subsys)");
1425 return getSubsystem(subsys).q;
1429 "StateImpl::getU(subsys)");
1430 return getSubsystem(subsys).u;
1434 "StateImpl::getZ(subsys)");
1435 return getSubsystem(subsys).z;
1440 "StateImpl::getUWeights(subsys)");
1441 return getSubsystem(subsys).uWeights;
1445 "StateImpl::getZWeights(subsys)");
1446 return getSubsystem(subsys).zWeights;
1451 "StateImpl::getQDot(subsys)");
1453 "StateImpl::getQDot(subsys)");
1454 return getSubsystem(subsys).qdot;
1458 "StateImpl::getUDot(subsys)");
1460 "StateImpl::getUDot(subsys)");
1461 return getSubsystem(subsys).udot;
1465 "StateImpl::getZDot(subsys)");
1467 "StateImpl::getZDot(subsys)");
1468 return getSubsystem(subsys).zdot;
1472 "StateImpl::getQDotDot(subsys)");
1474 "StateImpl::getQDotDot(subsys)");
1475 return getSubsystem(subsys).qdotdot;
1480 "StateImpl::updQ(subsys)");
1483 return updSubsystem(subsys).q;
1487 "StateImpl::updU(subsys)");
1490 return updSubsystem(subsys).u;
1494 "StateImpl::updZ(subsys)");
1497 return updSubsystem(subsys).z;
1502 "StateImpl::updUWeights(subsys)");
1504 return updSubsystem(subsys).uWeights;
1508 "StateImpl::updZWeights(subsys)");
1510 return updSubsystem(subsys).zWeights;
1517 "StateImpl::updQDot(subsys)");
1518 return getSubsystem(subsys).qdot;
1522 "StateImpl::updUDot(subsys)");
1523 return getSubsystem(subsys).udot;
1527 "StateImpl::updZDot(subsys)");
1528 return getSubsystem(subsys).zdot;
1532 "StateImpl::updQDotDot(subsys)");
1533 return getSubsystem(subsys).qdotdot;
1539 "StateImpl::getQErr(subsys)");
1541 "StateImpl::getQErr(subsys)");
1542 return getSubsystem(subsys).qerr;
1546 "StateImpl::getUErr(subsys)");
1548 "StateImpl::getUErr(subsys)");
1549 return getSubsystem(subsys).uerr;
1554 "StateImpl::getQErrWeights(subsys)");
1555 return getSubsystem(subsys).qerrWeights;
1559 "StateImpl::getUErrWeights(subsys)");
1560 return getSubsystem(subsys).uerrWeights;
1565 "StateImpl::getUDotErr(subsys)");
1567 "StateImpl::getUDotErr(subsys)");
1568 return getSubsystem(subsys).udoterr;
1572 "StateImpl::getMultipliers(subsys)");
1574 "StateImpl::getMultipliers(subsys)");
1575 return getSubsystem(subsys).multipliers;
1580 "StateImpl::getEventTriggersByStage(subsys)");
1582 "StateImpl::getEventTriggersByStage(subsys)");
1583 return getSubsystem(subsys).triggers[g];
1588 "StateImpl::updQErr(subsys)");
1589 return getSubsystem(subsys).qerr;
1593 "StateImpl::updUErr(subsys)");
1594 return getSubsystem(subsys).uerr;
1599 "StateImpl::updQErrWeights(subsys)");
1601 return updSubsystem(subsys).qerrWeights;
1605 "StateImpl::updUErrWeights(subsys)");
1607 return updSubsystem(subsys).uerrWeights;
1612 "StateImpl::updUDotErr(subsys)");
1613 return getSubsystem(subsys).udoterr;
1617 "StateImpl::updMultipliers(subsys)");
1618 return getSubsystem(subsys).multipliers;
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);
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 {
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 {
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 {
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 {
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());
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 {
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;
2255inline void ListOfDependents::
2256notePrerequisiteChange(
const StateImpl& stateImpl)
const {
2257 for (
auto ckey : m_dependents) {
2259 CacheEntryInfo& ce = stateImpl.updCacheEntryInfo(ckey);
2260 ce.invalidate(stateImpl);
2268isUpToDate(
const StateImpl& stateImpl)
const {
2269 const PerSubsystemInfo& subsys = stateImpl.getSubsystem(m_myKey.first);
2270 assert(&subsys.getCacheEntryInfo(m_myKey.second) ==
this);
2271 if (subsys.getCurrentStage() >= m_computedByStage)
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);
2291inline void CacheEntryInfo::
2292markAsUpToDate(
const StateImpl& stateImpl) {
2293 const PerSubsystemInfo& subsys = stateImpl.getSubsystem(m_myKey.first);
2294 assert(&subsys.getCacheEntryInfo(m_myKey.second) ==
this);
2295 const StageVersion version = subsys.getStageVersion(m_dependsOnStage);
2296 assert(version >= 1);
2297 m_dependsOnVersionWhenLastComputed = version;
2298 m_isUpToDateWithPrerequisites =
true;
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);
2381 return getImpl().allocateEventTrigger(subsys, stage, nevent);
2387 return updImpl().allocateDiscreteVariable(subsys, stage, v);
2392 Stage updateDependsOn) {
2393 return updImpl().allocateAutoUpdateDiscreteVariable
2394 (subsys, invalidates, v, updateDependsOn);
2400 return getImpl().getDiscreteVarUpdateIndex(
DiscreteVarKey(subsys,index));
2405 return getImpl().getDiscreteVarAllocationStage(
DiscreteVarKey(subsys,index));
2410 return getImpl().getDiscreteVarInvalidatesStage(
DiscreteVarKey(subsys,index));
2413inline const AbstractValue&
State::
2415 return getImpl().getDiscreteVariable(
DiscreteVarKey(subsys,index));
2420 return getImpl().getDiscreteVarLastUpdateTime(
DiscreteVarKey(subsys,index));
2422inline const AbstractValue&
State::
2425 return getImpl().getDiscreteVarUpdateValue(
DiscreteVarKey(subsys,index));
2430 return getImpl().updDiscreteVarUpdateValue(
DiscreteVarKey(subsys,index));
2435 return getImpl().isDiscreteVarUpdateValueRealized
2441 getImpl().markDiscreteVarUpdateValueRealized(
DiscreteVarKey(subsys,index));
2447 return updImpl().updDiscreteVariable(
DiscreteVarKey(subsys,index));
2458 AbstractValue* value)
const {
2459 return getImpl().allocateCacheEntry(subsys, dependsOn, computedBy, value);
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));
2477inline const AbstractValue&
State::
2479 return getImpl().getCacheEntry(
CacheEntryKey(subsys,index));
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();
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);
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);
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);
2900inline const CacheEntryInfo&
State::
2902 return getImpl().getCacheEntryInfo(cacheEntry);
2907 return updImpl().updCacheEntryInfo(cacheEntry);
2911 return getImpl().hasDiscreteVar(discreteVar);
2914inline const DiscreteVarInfo&
State::
2916 return getImpl().getDiscreteVarInfo(discreteVar);
2919inline const PerSubsystemInfo&
State::
2921 return getImpl().getSubsystem(subx);
2926 updImpl().autoUpdateDiscreteVariables();
2930 return getImpl().toString();
2933 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
SystemEventTriggerIndex getEventTriggerStartByStage(Stage) const
Return the index within the global event trigger array at which the first of the event triggers assoc...
int getNYErr() const
Get the total number nyerr=nqerr+nuerr of shared cache entries for position-level and velocity-level ...
Vector & updQErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mquat position inline constraint equations.
void invalidateAll(Stage)
If any subsystem or the system stage is currently at or higher than the passed-in one,...
void setDiscreteVariable(SubsystemIndex, DiscreteVariableIndex, const AbstractValue &)
Alternate interface to updDiscreteVariable.
const Vector & getEventTriggers() const
int getNQErr() const
Return the total number nqerr=mp+nQuaternions of cache entries for position-level constraint errors.
int getNEventTriggers() const
Return the total number of event trigger function slots in the cache.
int getNUDotErr() const
Return the total number nudotErr=mp+mv+ma of cache entries for acceleration-level constraint errors (...
void setU(const Vector &u)
const PerSubsystemInfo & getPerSubsystemInfo(SubsystemIndex) const
(Advanced) Return a reference to the per-subsystem information in the state.
const Vector & getUErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraint equation...
void setY(const Vector &y)
const Stage & getSubsystemStage(SubsystemIndex) const
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.
const ListOfDependents & getUDependents() const
(Advanced) Return the list of cache entries for which u was specified as an explicit prerequisite.
const DiscreteVarInfo & getDiscreteVarInfo(const DiscreteVarKey &discreteVar) const
(Advanced) Return a reference to the discrete variable information for a particular discrete variable...
void markCacheValueNotRealized(SubsystemIndex, CacheEntryIndex) const
(Advanced) Normally cache entries are invalidated automatically, however this method allows manual in...
CacheEntryInfo & updCacheEntryInfo(const CacheEntryKey &cacheEntry)
(Advanced) Return a writable reference to the cache entry information for a particular cache entry.
void autoUpdateDiscreteVariables()
(Advanced) This is called at the beginning of every integration step to set the values of auto-update...
QIndex allocateQ(SubsystemIndex, const Vector &qInit)
Allocate generalized coordinates q, which are second order continuous state variables.
int getNQ() const
Get total number of shared q's (generalized coordinates; second order state variables).
bool hasDiscreteVar(const DiscreteVarKey &discreteVar) const
(Advanced) Check whether this State has a particular discrete state variable.
const AbstractValue & getDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the current value of its associated update cache entry...
const Vector & getEventTriggersByStage(Stage) const
String cacheToString() const
(Debugging) Not suitable for serialization.
const Vector & getZ() const
bool hasCacheEntry(const CacheEntryKey &cacheEntry) const
(Advanced) Check whether this State has a particular cache entry.
ValueVersion getQValueVersion() const
(Advanced) Return a ValueVersion for q, meaning an integer that is incremented whenever any q is chan...
const Vector & getQErrWeights() const
Get the unit weighting (1/unit error) for each of the mp+mquat position inline constraints equations.
bool isDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Check whether the update value for this auto-update discrete variable has already been computed since...
void setZ(const Vector &z)
const Vector & getU() const
QErrIndex allocateQErr(SubsystemIndex, int nqerr) const
Allocate nqerr cache slots to hold the current error for position-level (holonomic) constraint equati...
Vector & updYErr() const
These are mutable.
ValueVersion getZValueVersion() const
(Advanced) Return a ValueVersion for z, meaning an integer that is incremented whenever any z is chan...
const AbstractValue & getDiscreteVariable(SubsystemIndex, DiscreteVariableIndex) const
Get the current value of the indicated discrete variable.
void setNumSubsystems(int i)
Set the number of subsystems in this state.
const Stage & getSystemStage() const
This returns the global stage for this State.
Stage getDiscreteVarInvalidatesStage(SubsystemIndex, DiscreteVariableIndex) const
What is the earliest stage that is invalidated when this discrete variable is modified?...
SystemYIndex getUStart() const
Returns the y index at which the u's begin. Callable at Model stage.
SystemYErrIndex getQErrStart() const
Returns the yErr index at which the qErr's begin. Callable at Instance stage.
int getNumSubsystems() const
Return the number of Subsystems known to this State.
Vector & updYDot() const
These are mutable.
const String & getSubsystemName(SubsystemIndex) const
CacheEntryIndex allocateCacheEntryWithPrerequisites(SubsystemIndex, Stage earliest, Stage latest, bool q, bool u, bool z, const Array_< DiscreteVarKey > &discreteVars, const Array_< CacheEntryKey > &cacheEntries, AbstractValue *value)
(Advanced) Allocate a cache entry with prerequisites other than just reaching a particular computatio...
int getNU() const
Get total number of shared u's (generalized speeds; mobilities).
Vector & updZWeights()
Set z weights.
const ListOfDependents & getZDependents() const
(Advanced) Return the list of cache entries for which z was specified as an explicit prerequisite.
AbstractValue & updCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a writable reference to the value contained in a particular cache entry.
const AbstractValue & getCacheEntry(SubsystemIndex, CacheEntryIndex) const
Retrieve a const reference to the value contained in a particular cache entry.
Vector & updUErrWeights()
Set the unit weighting (1/unit error) for each of the mp+mv velocity-level inline constraints.
const Vector & getUDotErr() const
These have their own space, they are not views.
Stage getDiscreteVarAllocationStage(SubsystemIndex, DiscreteVariableIndex) const
At what stage was this State when this discrete variable was allocated? The answer must be Stage::Emp...
const Vector & getUErr() const
const Real & getTime() const
You can call these as long as system stage >= Model.
SystemYIndex getQStart() const
Returns the y index at which the q's begin. Callable at Model stage.
const String & getSubsystemVersion(SubsystemIndex) const
Vector & updQDotDot() const
This is a separate shared cache entry, not part of YDot.
Vector & updMultipliers() const
SystemMultiplierIndex getMultipliersStart(SubsystemIndex) const
Real getDiscreteVarLastUpdateTime(SubsystemIndex, DiscreteVariableIndex) const
Return the time of last update for this discrete variable.
const Vector & getQ() const
These are just views into Y.
StageVersion getSystemTopologyStageVersion() const
The Topology stage version number (an integer) stored in this State must match the topology cache ver...
void advanceSubsystemToStage(SubsystemIndex, Stage) const
Advance a particular Subsystem's current stage by one to the indicated stage.
AbstractValue & updDiscreteVarUpdateValue(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return a writable reference to the value of its associated up...
std::mutex & getStateLock() const
Returns a mutex that should be used to lock the state whenever multiple threads are asynchronously wr...
void advanceSystemToStage(Stage) const
Advance the System-level current stage by one to the indicated stage.
void setQ(const Vector &q)
Alternate interface.
void markCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Mark the value of a particular cache entry as up to date after it has been recalculated.
const Vector & getUDot() const
UDotErrIndex allocateUDotErr(SubsystemIndex, int nudoterr) const
Allocate nudoterr cache slots to hold the current error for acceleration-level (acceleration-only,...
SystemYIndex getZStart() const
Returns the y index at which the z's begin. Callable at Model stage.
const Vector & getQDot() const
These are just views into YDot.
CacheEntryIndex getDiscreteVarUpdateIndex(SubsystemIndex, DiscreteVariableIndex) const
For an auto-updating discrete variable, return the CacheEntryIndex for its associated update cache en...
UErrIndex allocateUErr(SubsystemIndex, int nuerr) const
Allocate nuerr cache slots to hold the current error for velocity-level (nonholonomic and holonomic f...
Vector & updQ()
These are just views into Y.
Stage getCacheEntryAllocationStage(SubsystemIndex, CacheEntryIndex) const
At what stage was this State when this cache entry was allocated? The answer must be Stage::Empty,...
ValueVersion getUValueVersion() const
(Advanced) Return a ValueVersion for u, meaning an integer that is incremented whenever any u is chan...
int getNZ() const
Get total number of shared z's (auxiliary state variables).
const Vector & getYDot() const
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *, Stage updateDependsOn)
This method allocates a DiscreteVariable whose value should be updated automatically after each time ...
int getNY() const
Get the total number ny=nq+nu+nz of shared continuous state variables.
void invalidateAllCacheAtOrAbove(Stage) const
If any subsystem or the system stage is currently at or higher than the passed-in one,...
int getNUErr() const
Return the total number nuerr=mp+mv of cache entries for velocity-level constraint errors (including ...
const Vector & getZDot() const
Vector & updUWeights()
Set u weights (and q weights indirectly).
SubsystemIndex addSubsystem(const String &name, const String &version)
Register a new subsystem as a client of this State.
const Vector & getMultipliers() const
Vector & updUDotErr() const
int getNMultipliers() const
Return the total number of constraint multipliers; necessarily the same as the number of acceleration...
Stage getLowestSystemStageDifference(const Array_< StageVersion > &prevVersions) const
(Advanced) Given a list of per-stage version numbers extracted by an earlier call to getSystemStageVe...
const Vector & getYErr() const
Return the current constraint errors for all constraints.
SystemUDotErrIndex getUDotErrStart(SubsystemIndex) const
Vector & updEventTriggersByStage(Stage) const
const Vector & getQDotDot() const
This has its own space, not a view.
AbstractValue & updDiscreteVariable(SubsystemIndex, DiscreteVariableIndex)
Get a writable reference to the value stored in the indicated discrete state variable dv,...
String toString() const
(Debugging) Not suitable for serialization.
CacheEntryIndex allocateCacheEntry(SubsystemIndex, Stage earliest, Stage latest, AbstractValue *value) const
There are two Stages supplied explicitly as arguments to this method: earliest and latest.
const ListOfDependents & getQDependents() const
(Advanced) Return the list of cache entries for which q was specified as an explicit prerequisite.
Real & updTime()
You can call these as long as System stage >= Model, but the stage will be backed up if necessary to ...
void markDiscreteVarUpdateValueRealized(SubsystemIndex, DiscreteVariableIndex) const
Mark the update value for this auto-update discrete variable as up-to-date with respect to the state ...
const Vector & getQErr() const
These are just views into YErr.
Vector & updEventTriggers() const
UIndex allocateU(SubsystemIndex, const Vector &uInit)
Allocate generalized speeds u, which are first order continuous state variables related to the deriva...
int getNEventTriggersByStage(Stage) const
Return the size of the partition of event trigger functions which are evaluated at a given Stage.
void initializeSubsystem(SubsystemIndex, const String &name, const String &version)
Set the name and version for a given subsystem, which must already have a slot allocated.
DiscreteVariableIndex allocateDiscreteVariable(SubsystemIndex, Stage invalidates, AbstractValue *)
The Stage supplied here in the call is the earliest subsystem stage which is invalidated by a change ...
const Vector & getY() const
const CacheEntryInfo & getCacheEntryInfo(const CacheEntryKey &cacheEntry) const
(Advanced) Return a const reference to the cache entry information for a particular cache entry.
EventTriggerByStageIndex allocateEventTrigger(SubsystemIndex, Stage stage, int nevent) const
Allocate room for nevent witness function values that will be available at the indicated stage.
ZIndex allocateZ(SubsystemIndex, const Vector &zInit)
Allocate auxiliary first order continuous state variables z.
void setTime(Real t)
An alternate syntax equivalent to updTime() and updY().
bool isCacheValueRealized(SubsystemIndex, CacheEntryIndex) const
Check whether the value in a particular cache entry has been recalculated since the last change to th...
void getSystemStageVersions(Array_< StageVersion > &versions) const
(Advanced) Record the current version numbers of each valid System-level stage.
const Vector & getZWeights() const
Get a unit weighting (1/unit change) for each z that can be used to weight a vector dz so that the di...
const Vector & getUWeights() const
Get a unit weighting (1/unit change) for each u that can be used to weight a vector du so that the di...
Provide a unique integer type for identifying Subsystems.
This unique integer type is for identifying a triggered event within a particular Stage of the full S...
This unique integer type is for identifying a triggered event in the full System-level view of the St...
This unique integer type is for indexing global "multiplier-like" arrays, that is,...
This unique integer type is for indexing global "qErr-like" arrays, that is, arrays that inherently h...
This unique integer type is for indexing global "q-like" arrays, that is, arrays that inherently have...
This unique integer type is for indexing global "uDotErr-like" arrays, that is, arrays that inherentl...
This unique integer type is for indexing global "uErr-like" arrays, that is, arrays that inherently h...
This unique integer type is for indexing global "u-like" arrays, that is, arrays that inherently have...
This unique integer type is for indexing the global, System-level "yErr-like" arrays,...
This unique integer type is for indexing the global, System-level "y-like" arrays,...
This unique integer type is for indexing global "z-like" arrays, that is, arrays that inherently have...
Unique integer type for Subsystem-local uDotErr indexing.
Unique integer type for Subsystem-local uErr indexing.
Unique integer type for Subsystem-local u indexing.
Unique integer type for Subsystem-local z indexing.
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1473
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
std::pair< SubsystemIndex, DiscreteVariableIndex > DiscreteVarKey
Definition: State.h:165
long long StageVersion
This is the type to use for Stage version numbers that get incremented whenever a state variable chan...
Definition: Stage.h:44
long long ValueVersion
This is the type to use for state variable version numbers that get incremented whenever a state valu...
Definition: Stage.h:52
std::pair< SubsystemIndex, CacheEntryIndex > CacheEntryKey
Definition: State.h:164
ELEM min(const VectorBase< ELEM > &v)
Definition: VectorMath.h:178
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606