MeasureImplementation.h
Go to the documentation of this file.00001 #ifndef SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
00002 #define SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "SimTKcommon/basics.h"
00036 #include "SimTKcommon/Simmatrix.h"
00037 #include "SimTKcommon/internal/State.h"
00038 #include "SimTKcommon/internal/Measure.h"
00039 #include "SimTKcommon/internal/Subsystem.h"
00040 #include "SimTKcommon/internal/System.h"
00041 #include "SimTKcommon/internal/SubsystemGuts.h"
00042
00043 namespace SimTK {
00044
00046
00048
00052 class SimTK_SimTKCOMMON_EXPORT Measure::Implementation {
00053 protected:
00054
00055
00056 explicit Implementation(const std::string& name="<NONAME>")
00057 : measureName(name), refCount(0), mySubsystem(0) {}
00058
00059 Implementation(const Implementation& src)
00060 : measureName(src.measureName), refCount(0), mySubsystem(0) {}
00061
00062 Implementation& operator=(const Implementation& src)
00063 { if (&src != this) {measureName=src.measureName; refCount=0; mySubsystem=0;}
00064 return *this; }
00065
00066
00067
00068
00069 int incrRefCount() const {return ++refCount;}
00070
00071
00072 int decrRefCount() const {return --refCount;}
00073
00074 const std::string& getName() const {return measureName;}
00075 Implementation* clone() const {return cloneImpl();}
00076
00077 void realizeTopology (State& s) const {realizeMeasureTopologyImpl(s);}
00078 void realizeModel (State& s) const {realizeMeasureModelImpl(s);}
00079 void realizeInstance (const State& s) const {realizeMeasureInstanceImpl(s);}
00080 void realizeTime (const State& s) const {realizeMeasureTimeImpl(s);}
00081 void realizePosition (const State& s) const {realizeMeasurePositionImpl(s);}
00082 void realizeVelocity (const State& s) const {realizeMeasureVelocityImpl(s);}
00083 void realizeDynamics (const State& s) const {realizeMeasureDynamicsImpl(s);}
00084 void realizeAcceleration(const State& s) const {realizeMeasureAccelerationImpl(s);}
00085 void realizeReport (const State& s) const {realizeMeasureReportImpl(s);}
00086
00087 void initialize(State& s) const {initializeImpl(s);}
00088 Stage getValueDependence(const State& s) const {return getValueDependenceImpl(s);}
00089
00090 void setSubsystem(Subsystem& sub, MeasureIndex mx)
00091 { assert(!mySubsystem && mx.isValid());
00092 mySubsystem = ⊂ myIndex = mx; }
00093
00094 bool isInSubsystem() const {return mySubsystem != 0;}
00095 const Subsystem& getSubsystem() const {assert(mySubsystem); return *mySubsystem;}
00096 Subsystem& updSubsystem() {assert(mySubsystem); return *mySubsystem;}
00097 MeasureIndex getSubsystemMeasureIndex() const {assert(mySubsystem); return myIndex;}
00098
00099 void invalidateTopologyCache() const
00100 { if (isInSubsystem()) getSubsystem().invalidateSubsystemTopologyCache(); }
00101
00102
00103
00104
00105
00106 virtual ~Implementation() {}
00107 virtual Implementation* cloneImpl() const = 0;
00108
00109 virtual void realizeMeasureTopologyImpl(State&) const {}
00110 virtual void realizeMeasureModelImpl(State&) const {}
00111 virtual void realizeMeasureInstanceImpl(const State&) const {}
00112 virtual void realizeMeasureTimeImpl(const State&) const {}
00113 virtual void realizeMeasurePositionImpl(const State&) const {}
00114 virtual void realizeMeasureVelocityImpl(const State&) const {}
00115 virtual void realizeMeasureDynamicsImpl(const State&) const {}
00116 virtual void realizeMeasureAccelerationImpl(const State&) const {}
00117 virtual void realizeMeasureReportImpl(const State&) const {}
00118
00119 virtual void initializeImpl(State&) const {}
00120 virtual Stage getValueDependenceImpl(const State&) const = 0;
00121
00122 private:
00123 std::string measureName;
00124
00125
00126 Subsystem* mySubsystem;
00127 MeasureIndex myIndex;
00128
00129
00130
00131 mutable int refCount;
00132
00133 friend class Measure;
00134 friend class Subsystem::Guts;
00135 friend class Subsystem::Guts::GutsRep;
00136 };
00137
00139
00141
00142 inline Measure::Measure(Implementation* g) : impl(g)
00143 { if (impl) impl->incrRefCount();}
00144 inline Measure::Measure(Subsystem& sub, Implementation* g, const SetHandle&) : impl(g) {
00145 SimTK_ASSERT_ALWAYS(impl, "An empty Measure handle can't be put in a Subsystem.");
00146 impl->incrRefCount();
00147 sub.adoptMeasure(*this);
00148 }
00149 inline Measure::Measure(const Measure& src) : impl(0) {
00150 if (src.impl) {
00151 impl = src.impl->mySubsystem ? src.impl : src.impl->clone();
00152 impl->incrRefCount();
00153 }
00154 }
00155 inline Measure& Measure::operator=(const Measure& src) {
00156 if (&src != this) {
00157 if (impl && impl->decrRefCount()==0) delete impl;
00158 impl = src.impl->mySubsystem ? src.impl : src.impl->clone();
00159 impl->incrRefCount();
00160 }
00161 return *this;
00162 }
00163 inline Measure::~Measure()
00164 { if (impl && impl->decrRefCount()==0) delete impl;}
00165 inline bool Measure::isInSubsystem() const
00166 { return impl && impl->isInSubsystem();}
00167 inline const Subsystem& Measure::getSubsystem() const
00168 { assert(impl); return impl->getSubsystem();}
00169 inline MeasureIndex Measure::getSubsystemMeasureIndex() const
00170 { assert(impl); return impl->getSubsystemMeasureIndex();}
00171
00172 inline Stage Measure::getValueDependence(const State& s) const
00173 { assert(impl); return impl->getValueDependence(s);}
00174
00176
00178
00184 template <class T>
00185 class Measure_<T>::Implementation : public Measure::Implementation {
00186 public:
00187 const T& getValue(const State& s) const {return getValueImpl(s);}
00188
00189 virtual const T& getValueImpl(const State& s) const = 0;
00190 };
00191
00193
00195
00196 template <class T>
00197 class Measure::Constant_<T>::Implementation : public Measure_<T>::Implementation {
00198 public:
00199 Implementation(const T& value) : value(value) {}
00200
00201 void setValue(const T& v) {
00202 value = v;
00203 this->invalidateTopologyCache();
00204 }
00205
00206
00207 Implementation* cloneImpl() const {return new Implementation(*this);}
00208 const T& getValueImpl(const State&) const {return value;}
00209 Stage getValueDependenceImpl(const State&) const {return Stage::Topology;}
00210
00211 private:
00212 T value;
00213 };
00214
00215
00217
00219
00220 template <class T>
00221 class Measure::Sinusoid_<T>::Implementation : public Measure_<T>::Implementation {
00222 public:
00223 Implementation(const T& amplitude,
00224 const T& frequency,
00225 const T& phase=T(0))
00226 : a(amplitude), w(frequency), p(phase) {}
00227
00228
00229 Implementation* cloneImpl() const {return new Implementation(*this);}
00230 const T& getValueImpl(const State& s) const
00231 { return getValueEntry(s);}
00232
00233 Stage getValueDependenceImpl(const State&) const {return Stage::Time;}
00234
00235 void realizeMeasureTopologyImpl(State& s) const {
00236 cacheEntryIndex = this->getSubsystem().allocateCacheEntry
00237 (s, Stage::Time, new Value<T>(CNT<T>::getNaN()));
00238 }
00239 void realizeMeasureTimeImpl(const State& s) const {
00240 assert(cacheEntryIndex >= 0);
00241 updValueEntry(s) = a*std::sin(w*s.getTime() + p);
00242 }
00243
00244 private:
00245 const T& getValueEntry(const State& s) const
00246 { assert(cacheEntryIndex >= 0);
00247 return Value<T>::downcast(this->getSubsystem().getCacheEntry(s, cacheEntryIndex));}
00248 T& updValueEntry(const State& s) const
00249 { assert(cacheEntryIndex >= 0);
00250 return Value<T>::downcast(this->getSubsystem().updCacheEntry(s, cacheEntryIndex));}
00251
00252
00253 T a, w, p;
00254
00255
00256 mutable CacheEntryIndex cacheEntryIndex;
00257 };
00258
00260
00262
00263 template <class T>
00264 class Measure::Integrate_<T>::Implementation : public Measure_<T>::Implementation {
00265 public:
00266 Implementation() : derivMeasure(0), icMeasure(0) {}
00267 Implementation(const Measure_<T>& deriv, const Measure_<T>& ic)
00268 : derivMeasure(&deriv), icMeasure(&ic) {}
00269
00270 void setValue(State& s, const T& value) const
00271 { assert(zIndex >= 0); this->getSubsystem().updZ(s)[zIndex] = value; }
00272
00273 const Measure_<T>& getDerivativeMeasure() const
00274 { assert(derivMeasure); return *derivMeasure; }
00275 const Measure_<T>& getInitialConditionMeasure() const
00276 { assert(icMeasure); return *icMeasure; }
00277
00278 void setDerivativeMeasure(const Measure_<T>& d)
00279 { derivMeasure = &d; this->invalidateTopologyCache(); }
00280 void setInitialConditionMeasure(const Measure_<T>& ic)
00281 { icMeasure = ⁣ this->invalidateTopologyCache(); }
00282
00283
00284 Implementation* cloneImpl() const {return new Implementation(*this);}
00285 const T& getValueImpl(const State& s) const
00286 { assert(zIndex.isValid()); return this->getSubsystem().getZ(s)[zIndex]; }
00287 Stage getValueDependenceImpl(const State&) const {return Stage::Time;}
00288
00289 void initializeImpl(State& s) const {
00290 assert(zIndex >= 0);
00291 Real& z = this->getSubsystem().updZ(s)[zIndex];
00292 if (icMeasure) z = icMeasure->getValue(s);
00293 else z = 0;
00294 }
00295
00296 void realizeMeasureTopologyImpl(State& s) const {
00297 static const Vector zero(1, Real(0));
00298 zIndex = this->getSubsystem().allocateZ(s, zero);
00299 }
00300
00301 void realizeMeasureAccelerationImpl(const State& s) const {
00302 assert(zIndex.isValid());
00303 Real& zdot = this->getSubsystem().updZDot(s)[zIndex];
00304 if (derivMeasure) zdot = derivMeasure->getValue(s);
00305 else zdot = 0;
00306 }
00307
00308 private:
00309
00310 const Measure_<T>* derivMeasure;
00311 const Measure_<T>* icMeasure;
00312
00313
00314 mutable ZIndex zIndex;
00315 };
00316
00317
00318 }
00319
00320 #endif // SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_