Simbody
3.5
|
00001 #ifndef SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_ 00002 #define SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm): SimTKcommon * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK biosimulation toolkit originating from * 00008 * Simbios, the NIH National Center for Physics-Based Simulation of * 00009 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00010 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2006-14 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00027 #include "SimTKcommon/basics.h" 00028 #include "SimTKcommon/Simmatrix.h" 00029 #include "SimTKcommon/internal/State.h" 00030 00031 #include <cassert> 00032 00033 namespace SimTK { 00034 00035 class System; 00036 class DecorativeGeometry; 00037 00038 //============================================================================== 00039 // SUBSYSTEM :: GUTS 00040 //============================================================================== 00047 class SimTK_SimTKCOMMON_EXPORT Subsystem::Guts { 00048 public: 00049 00054 explicit Guts(const String& name="<NONAME>", const String& version="0.0.0"); 00055 00057 virtual ~Guts(); 00058 00061 Guts(const Guts&); 00062 00065 const String& getName() const {return m_subsystemName;} 00068 const String& getVersion() const {return m_subsystemVersion;} 00069 00083 QIndex allocateQ(State& s, const Vector& qInit) const 00084 { return s.allocateQ(getMySubsystemIndex(), qInit); } 00085 UIndex allocateU(State& s, const Vector& uInit) const 00086 { return s.allocateU(getMySubsystemIndex(), uInit); } 00087 ZIndex allocateZ(State& s, const Vector& zInit) const 00088 { return s.allocateZ(getMySubsystemIndex(), zInit); } 00089 00090 DiscreteVariableIndex 00091 allocateDiscreteVariable(State& s, Stage g, AbstractValue* v) const 00092 { return s.allocateDiscreteVariable(getMySubsystemIndex(), g, v); } 00093 DiscreteVariableIndex allocateAutoUpdateDiscreteVariable 00094 (State& s, Stage invalidates, AbstractValue* v, Stage updateDependsOn) const 00095 { return s.allocateAutoUpdateDiscreteVariable 00096 (getMySubsystemIndex(),invalidates,v,updateDependsOn); } 00097 CacheEntryIndex allocateCacheEntry 00098 (const State& s, Stage dependsOn, Stage computedBy, AbstractValue* v) const 00099 { return s.allocateCacheEntry 00100 (getMySubsystemIndex(), dependsOn, computedBy, v); } 00101 00102 CacheEntryIndex allocateCacheEntry 00103 (const State& state, Stage g, AbstractValue* v) const 00104 { return allocateCacheEntry(state, g, g, v); } 00105 CacheEntryIndex allocateLazyCacheEntry 00106 (const State& state, Stage earliest, AbstractValue* v) const 00107 { return allocateCacheEntry(state, earliest, Stage::Infinity, v); } 00108 00109 QErrIndex allocateQErr(const State& s, int nqerr) const 00110 { return s.allocateQErr(getMySubsystemIndex(), nqerr); } 00111 UErrIndex allocateUErr(const State& s, int nuerr) const 00112 { return s.allocateUErr(getMySubsystemIndex(), nuerr); } 00113 UDotErrIndex allocateUDotErr(const State& s, int nudoterr) const 00114 { return s.allocateUDotErr(getMySubsystemIndex(), nudoterr); } 00115 EventTriggerByStageIndex 00116 allocateEventTriggersByStage(const State& s, Stage g, int ntriggers) const 00117 { return s.allocateEventTrigger(getMySubsystemIndex(),g,ntriggers); } 00118 00119 const Vector& getQ(const State& s) const 00120 { return s.getQ(getMySubsystemIndex()); } 00121 const Vector& getU(const State& s) const 00122 { return s.getU(getMySubsystemIndex()); } 00123 const Vector& getZ(const State& s) const 00124 { return s.getZ(getMySubsystemIndex()); } 00125 const Vector& getUWeights(const State& s) const 00126 { return s.getUWeights(getMySubsystemIndex()); } 00127 const Vector& getZWeights(const State& s) const 00128 { return s.getZWeights(getMySubsystemIndex()); } 00129 00130 Vector& updQ(State& s) const {return s.updQ(getMySubsystemIndex());} 00131 Vector& updU(State& s) const {return s.updU(getMySubsystemIndex());} 00132 Vector& updZ(State& s) const {return s.updZ(getMySubsystemIndex());} 00133 00134 const Vector& getQDot (const State& s) const 00135 { return s.getQDot(getMySubsystemIndex()); } 00136 const Vector& getUDot (const State& s) const 00137 { return s.getUDot(getMySubsystemIndex()); } 00138 const Vector& getZDot (const State& s) const 00139 { return s.getZDot(getMySubsystemIndex()); } 00140 const Vector& getQDotDot(const State& s) const 00141 { return s.getQDotDot(getMySubsystemIndex()); } 00142 00143 Vector& updQDot (const State& s) const 00144 { return s.updQDot(getMySubsystemIndex()); } 00145 Vector& updUDot (const State& s) const 00146 { return s.updUDot(getMySubsystemIndex()); } 00147 Vector& updZDot (const State& s) const 00148 { return s.updZDot(getMySubsystemIndex()); } 00149 Vector& updQDotDot(const State& s) const 00150 { return s.updQDotDot(getMySubsystemIndex()); } 00151 00152 const Vector& getQErr(const State& s) const 00153 { return s.getQErr(getMySubsystemIndex()); } 00154 const Vector& getUErr(const State& s) const 00155 { return s.getUErr(getMySubsystemIndex()); } 00156 const Vector& getQErrWeights(const State& s) const 00157 { return s.getQErrWeights(getMySubsystemIndex()); } 00158 const Vector& getUErrWeights(const State& s) const 00159 { return s.getUErrWeights(getMySubsystemIndex()); } 00160 00161 const Vector& getUDotErr(const State& s) const 00162 { return s.getUDotErr(getMySubsystemIndex()); } 00163 const Vector& getMultipliers(const State& s) const 00164 { return s.getMultipliers(getMySubsystemIndex()); } 00165 const Vector& getEventTriggersByStage(const State& s, Stage g) const 00166 { return s.getEventTriggersByStage(getMySubsystemIndex(),g); } 00167 00168 Vector& updQErr(const State& s) const 00169 { return s.updQErr(getMySubsystemIndex()); } 00170 Vector& updUErr(const State& s) const 00171 { return s.updUErr(getMySubsystemIndex()); } 00172 Vector& updUDotErr(const State& s) const 00173 { return s.updUDotErr(getMySubsystemIndex()); } 00174 Vector& updMultipliers(const State& s) const 00175 { return s.updMultipliers(getMySubsystemIndex()); } 00176 Vector& updEventTriggersByStage(const State& s, Stage g) const 00177 { return s.updEventTriggersByStage(getMySubsystemIndex(),g); } 00178 00179 SystemQIndex getQStart(const State& s) const 00180 { return s.getQStart(getMySubsystemIndex()); } 00181 int getNQ(const State& s) const 00182 { return s.getNQ(getMySubsystemIndex()); } 00183 00184 SystemUIndex getUStart(const State& s) const 00185 { return s.getUStart(getMySubsystemIndex()); } 00186 int getNU(const State& s) const 00187 { return s.getNU(getMySubsystemIndex()); } 00188 00189 SystemZIndex getZStart(const State& s) const 00190 { return s.getZStart(getMySubsystemIndex()); } 00191 int getNZ(const State& s) const 00192 { return s.getNZ(getMySubsystemIndex()); } 00193 00194 SystemQErrIndex getQErrStart(const State& s) const 00195 { return s.getQErrStart(getMySubsystemIndex()); } 00196 int getNQErr(const State& s) const 00197 { return s.getNQErr(getMySubsystemIndex()); } 00198 00199 SystemUErrIndex getUErrStart(const State& s) const 00200 { return s.getUErrStart(getMySubsystemIndex()); } 00201 int getNUErr(const State& s) const 00202 { return s.getNUErr(getMySubsystemIndex()); } 00203 00204 SystemUDotErrIndex getUDotErrStart(const State& s) const 00205 { return s.getUDotErrStart(getMySubsystemIndex()); } 00206 int getNUDotErr(const State& s) const 00207 { return s.getNUDotErr(getMySubsystemIndex()); } 00208 00209 SystemMultiplierIndex getMultipliersStart(const State& s) const 00210 { return s.getMultipliersStart(getMySubsystemIndex()); } 00211 int getNMultipliers(const State& s) const 00212 { return s.getNMultipliers(getMySubsystemIndex()); } 00213 00214 SystemEventTriggerByStageIndex getEventTriggerStartByStage(const State& s, Stage g) const 00215 { return s.getEventTriggerStartByStage(getMySubsystemIndex(),g); } 00216 int getNEventTriggersByStage (const State& s, Stage g) const 00217 { return s.getNEventTriggersByStage(getMySubsystemIndex(),g); } 00218 00219 00220 // For convenience. 00221 void setQ(State& s, const Vector& q) const { 00222 SimTK_ASSERT(q.size() == getNQ(s), "Subsystem::Guts::setQ()"); 00223 updQ(s) = q; 00224 } 00225 void setU(State& s, const Vector& u) const { 00226 SimTK_ASSERT(u.size() == getNU(s), "Subsystem::Guts::setU()"); 00227 updU(s) = u; 00228 } 00229 void setZ(State& s, const Vector& z) const { 00230 SimTK_ASSERT(z.size() == getNZ(s), "Subsystem::Guts::setZ()"); 00231 updZ(s) = z; 00232 } 00233 00234 Stage getStage(const State& s) const 00235 { return s.getSubsystemStage(getMySubsystemIndex()); } 00236 void advanceToStage(const State& s, Stage g) const 00237 { s.advanceSubsystemToStage(getMySubsystemIndex(), g); } 00238 00239 const AbstractValue& 00240 getDiscreteVariable(const State& s, DiscreteVariableIndex index) const 00241 { return s.getDiscreteVariable(getMySubsystemIndex(), index); } 00242 AbstractValue& updDiscreteVariable(State& s, DiscreteVariableIndex index) const 00243 { return s.updDiscreteVariable(getMySubsystemIndex(), index); } 00244 const AbstractValue& getCacheEntry(const State& s, CacheEntryIndex index) const 00245 { return s.getCacheEntry(getMySubsystemIndex(), index); } 00246 AbstractValue& updCacheEntry(const State& s, CacheEntryIndex index) const 00247 { return s.updCacheEntry(getMySubsystemIndex(), index); } 00248 Real getDiscreteVarLastUpdateTime(const State& s, DiscreteVariableIndex dx) const 00249 { return s.getDiscreteVarLastUpdateTime(getMySubsystemIndex(),dx); } 00250 CacheEntryIndex 00251 getDiscreteVarUpdateIndex(const State& s, DiscreteVariableIndex dx) const 00252 { return s.getDiscreteVarUpdateIndex(getMySubsystemIndex(),dx); } 00253 const AbstractValue& 00254 getDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const 00255 { return s.getDiscreteVarUpdateValue(getMySubsystemIndex(),dx); } 00256 AbstractValue& 00257 updDiscreteVarUpdateValue(const State& s, DiscreteVariableIndex dx) const 00258 { return s.updDiscreteVarUpdateValue(getMySubsystemIndex(),dx); } 00259 bool isDiscreteVarUpdateValueRealized 00260 (const State& s, DiscreteVariableIndex dx) const 00261 { return s.isDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); } 00262 void markDiscreteVarUpdateValueRealized 00263 (const State& s, DiscreteVariableIndex dx) const 00264 { return s.markDiscreteVarUpdateValueRealized(getMySubsystemIndex(),dx); } 00265 00266 bool isCacheValueRealized(const State& s, CacheEntryIndex cx) const 00267 { return s.isCacheValueRealized(getMySubsystemIndex(), cx); } 00268 void markCacheValueRealized(const State& s, CacheEntryIndex cx) const 00269 { s.markCacheValueRealized(getMySubsystemIndex(), cx); } 00270 void markCacheValueNotRealized(const State& s, CacheEntryIndex cx) const 00271 { s.markCacheValueNotRealized(getMySubsystemIndex(), cx); } 00276 MeasureIndex adoptMeasure(AbstractMeasure& m); 00277 00284 template <class T> Measure_<T> getMeasure_(MeasureIndex mx) const 00285 { return Measure_<T>::getAs(getMeasure(mx)); } 00286 00291 AbstractMeasure getMeasure(MeasureIndex mx) const { 00292 SimTK_ASSERT(0 <= mx && mx < m_measures.size(), 00293 "Subsystem::Guts::getMeasure()"); 00294 return AbstractMeasure(m_measures[mx]); 00295 } 00296 00297 bool isInSystem() const {return m_mySystem != 0;} 00298 bool isInSameSystem(const Subsystem& otherSubsystem) const; 00299 00300 const System& getSystem() const { 00301 SimTK_ASSERT(isInSystem(), "Subsystem::getSystem()"); 00302 return *m_mySystem; 00303 } 00304 System& updSystem() { 00305 SimTK_ASSERT(isInSystem(), "Subsystem::updSystem()"); 00306 return *m_mySystem; 00307 } 00308 void setSystem(System& sys, SubsystemIndex id) { 00309 SimTK_ASSERT(!isInSystem(), "Subsystem::setSystem()"); 00310 SimTK_ASSERT(id.isValid(), "Subsystem::setSystem()"); 00311 m_mySystem = &sys; 00312 m_mySubsystemIndex = id; 00313 } 00314 SubsystemIndex getMySubsystemIndex() const { 00315 SimTK_ASSERT(isInSystem(), "Subsystem::getMySubsystemIndex()"); 00316 return m_mySubsystemIndex; 00317 } 00318 00319 00323 bool subsystemTopologyHasBeenRealized() const 00324 { return m_subsystemTopologyRealized; } 00325 00334 void invalidateSubsystemTopologyCache() const; 00335 00336 // These are wrappers for the virtual methods defined below. They 00337 // are used to ensure good behavior. Most of them deal automatically with 00338 // the Subsystem's Measures, as well as invoking the corresponding virtual 00339 // for the Subsystem's own processing. 00340 00341 Subsystem::Guts* clone() const; 00342 00343 // Realize this subsystem's part of the State from Stage-1 to Stage 00344 // for the indicated stage. After doing some checking, these routines 00345 // call the concrete subsystem's corresponding virtual method, and 00346 // on return they make sure the stage has been properly updated. 00347 // Note that these will do nothing if the Subsystem stage is already 00348 // at or greater than the indicated stage. 00349 void realizeSubsystemTopology (State&) const; 00350 void realizeSubsystemModel (State&) const; 00351 void realizeSubsystemInstance (const State&) const; 00352 void realizeSubsystemTime (const State&) const; 00353 void realizeSubsystemPosition (const State&) const; 00354 void realizeSubsystemVelocity (const State&) const; 00355 void realizeSubsystemDynamics (const State&) const; 00356 void realizeSubsystemAcceleration(const State&) const; 00357 void realizeSubsystemReport (const State&) const; 00358 00359 // Generate decorative geometry computable at a specific stage. This will 00360 // throw an exception if this subsystem's state hasn't already been realized 00361 // to that stage. Note that the list is not inclusive -- you have to 00362 // request geometry from each stage to get all of it. 00363 // The generated geometry will be *appended* to the supplied output vector. 00364 void calcDecorativeGeometryAndAppend 00365 (const State&, Stage, Array_<DecorativeGeometry>&) const; 00366 00367 void createScheduledEvent(const State& state, EventId& eventId) const; 00368 void createTriggeredEvent(const State& state, EventId& eventId, 00369 EventTriggerByStageIndex& triggerFunctionIndex, 00370 Stage stage) const; 00371 00372 // These methods are called by the corresponding methods of System. 00373 // Each subsystem is responsible for defining its own events, and 00374 // System then combines the information from them, and dispatches events 00375 // to the appropriate subsystems for handling when they occur. 00376 void calcEventTriggerInfo 00377 (const State&, Array_<EventTriggerInfo>&) const; 00378 void calcTimeOfNextScheduledEvent 00379 (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 00380 bool includeCurrentTime) const; 00381 void calcTimeOfNextScheduledReport 00382 (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 00383 bool includeCurrentTime) const; 00384 void handleEvents 00385 (State&, Event::Cause, const Array_<EventId>& eventIds, 00386 const HandleEventsOptions& options, HandleEventsResults& results) const; 00387 void reportEvents 00388 (const State&, Event::Cause, const Array_<EventId>& eventIds) const; 00389 00390 protected: 00391 // These virtual methods should be overridden in concrete Subsystems as 00392 // necessary. They should never be called directly; instead call the 00393 // wrapper routines above, which have the same name but without the "Impl" 00394 // (implementation) at the end. 00395 00396 // The "realize..." wrappers will call the "realize...Impl" methods below 00397 // only when the current stage for the Subsystem is the one just prior 00398 // to the stage being realized. For example, realizeSubsystemVelocityImpl() 00399 // is called by realizeSubsystemVelocity() only when the passed-in State 00400 // shows this subsystem's stage to be exactly Stage::Position. 00401 // 00402 // The default implementations provided here do nothing. That means the 00403 // wrappers will simply check that the current stage is correct and 00404 // advance it if necessary. 00405 00406 // The destructor is already virtual; see above. 00407 00408 virtual Subsystem::Guts* cloneImpl() const = 0; 00409 00410 virtual int realizeSubsystemTopologyImpl(State& s) const {return 0;} 00411 virtual int realizeSubsystemModelImpl (State& s) const {return 0;} 00412 virtual int realizeSubsystemInstanceImpl(const State& s) const {return 0;} 00413 virtual int realizeSubsystemTimeImpl (const State& s) const {return 0;} 00414 virtual int realizeSubsystemPositionImpl(const State& s) const {return 0;} 00415 virtual int realizeSubsystemVelocityImpl(const State& s) const {return 0;} 00416 virtual int realizeSubsystemDynamicsImpl(const State& s) const {return 0;} 00417 virtual int realizeSubsystemAccelerationImpl(const State& s)const{return 0;} 00418 virtual int realizeSubsystemReportImpl (const State& s) const {return 0;} 00419 00420 virtual int calcDecorativeGeometryAndAppendImpl 00421 (const State&, Stage, Array_<DecorativeGeometry>&) const {return 0;} 00422 00423 virtual void calcEventTriggerInfoImpl 00424 (const State&, Array_<EventTriggerInfo>&) const {} 00425 virtual void calcTimeOfNextScheduledEventImpl 00426 (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 00427 bool includeCurrentTime) const {} 00428 virtual void calcTimeOfNextScheduledReportImpl 00429 (const State&, Real& tNextEvent, Array_<EventId>& eventIds, 00430 bool includeCurrentTime) const {} 00431 virtual void handleEventsImpl 00432 (State&, Event::Cause, const Array_<EventId>& eventIds, 00433 const HandleEventsOptions& options, 00434 HandleEventsResults& results) const {} 00435 virtual void reportEventsImpl 00436 (const State&, Event::Cause, const Array_<EventId>& eventIds) const {} 00437 00438 00439 public: 00442 const Subsystem& getOwnerSubsystemHandle() const { 00443 SimTK_ASSERT(m_myHandle, "Subsystem::getOwnerSubsystemHandle()"); 00444 return *m_myHandle; 00445 } 00448 Subsystem& updOwnerSubsystemHandle() { 00449 SimTK_ASSERT(m_myHandle, "Subsystem::getOwnerSubsystemHandle()"); 00450 return *m_myHandle; 00451 } 00452 00457 void setOwnerSubsystemHandle(Subsystem& subsys) {m_myHandle=&subsys;} 00458 00461 bool hasOwnerSubsystemHandle() const {return m_myHandle != 0;} 00462 00463 private: 00464 // Suppressed. 00465 Guts& operator=(const Guts&); 00466 00467 //------------------------------------------------------------------------------ 00468 private: 00469 00470 // TOPOLOGY STATE INFORMATION 00471 String m_subsystemName; 00472 String m_subsystemVersion; 00473 System* m_mySystem; // the System to which this Subsystem belongs 00474 SubsystemIndex m_mySubsystemIndex; // Subsystem # within System 00475 00476 friend class Subsystem; 00477 Subsystem* m_myHandle; // the owner handle of this Guts object 00478 00479 // This is the list of Measures belonging to this Subsystem. 00480 Array_<AbstractMeasure::Implementation*> 00481 m_measures; 00482 00483 // TOPOLOGY CACHE INFORMATION 00484 mutable bool m_subsystemTopologyRealized; 00485 }; 00486 00487 00488 //============================================================================== 00489 // SUBSYSTEM INLINES 00490 //============================================================================== 00491 // These had to wait for Subsystem::Guts to be defined. 00492 00493 inline SubsystemIndex Subsystem::getMySubsystemIndex() const 00494 { return getSubsystemGuts().getMySubsystemIndex(); } 00495 00496 inline const String& Subsystem::getName() const {return getSubsystemGuts().getName();} 00497 inline const String& Subsystem::getVersion() const {return getSubsystemGuts().getVersion();} 00498 00499 inline bool Subsystem::subsystemTopologyHasBeenRealized() const { 00500 return getSubsystemGuts().subsystemTopologyHasBeenRealized(); 00501 } 00502 00503 inline void Subsystem::invalidateSubsystemTopologyCache() const { 00504 getSubsystemGuts().invalidateSubsystemTopologyCache(); // mutable 00505 } 00506 00507 inline MeasureIndex Subsystem::adoptMeasure(AbstractMeasure& m) 00508 { return updSubsystemGuts().adoptMeasure(m); } 00509 inline AbstractMeasure Subsystem::getMeasure(MeasureIndex mx) const 00510 { return getSubsystemGuts().getMeasure(mx); } 00511 00512 00513 inline bool Subsystem::isInSystem() const 00514 { return getSubsystemGuts().isInSystem(); } 00515 inline bool Subsystem::isInSameSystem(const Subsystem& otherSubsystem) const 00516 { return getSubsystemGuts().isInSameSystem(otherSubsystem); } 00517 00518 inline const System& Subsystem::getSystem() const 00519 { return getSubsystemGuts().getSystem(); } 00520 inline System& Subsystem::updSystem() 00521 { return updSubsystemGuts().updSystem(); } 00522 inline void Subsystem::setSystem(System& sys, SubsystemIndex id) 00523 { updSubsystemGuts().setSystem(sys,id); } 00524 00525 inline bool Subsystem::isOwnerHandle() const 00526 { return guts==0 || &guts->getOwnerSubsystemHandle()==this; } 00527 00528 } // namespace SimTK 00529 00530 #endif // SimTK_SimTKCOMMON_SUBSYSTEM_GUTS_H_