Simbody
3.5
|
00001 #ifndef SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_ 00002 #define SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_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) 2008-13 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 #include "SimTKcommon/internal/Measure.h" 00031 #include "SimTKcommon/internal/Subsystem.h" 00032 #include "SimTKcommon/internal/System.h" 00033 #include "SimTKcommon/internal/SubsystemGuts.h" 00034 00035 #include <cmath> 00036 00037 00038 namespace SimTK { 00039 00040 00041 //============================================================================== 00042 // ABSTRACT MEASURE :: IMPLEMENTATION 00043 //============================================================================== 00044 00048 class SimTK_SimTKCOMMON_EXPORT AbstractMeasure::Implementation { 00049 protected: 00052 Implementation() : copyNumber(0), mySubsystem(0), refCount(0) {} 00053 00057 Implementation(const Implementation& src) 00058 : copyNumber(src.copyNumber+1), mySubsystem(0), refCount(0) {} 00059 00063 Implementation& operator=(const Implementation& src) { 00064 if (&src != this) 00065 { copyNumber=src.copyNumber+1; 00066 refCount=0; mySubsystem=0; } 00067 return *this; 00068 } 00069 00070 // destructor is virtual; below 00071 00072 // Increment the reference count and return its new value. 00073 int incrRefCount() const {return ++refCount;} 00074 00075 // Decrement the reference count and return its new value. 00076 int decrRefCount() const {return --refCount;} 00077 00078 // Get the current value of the reference counter. 00079 int getRefCount() const {return refCount;} 00080 00081 int getCopyNumber() const {return copyNumber;} 00082 00086 Implementation* clone() const {return cloneVirtual();} 00087 00088 // realizeTopology() is pure virtual below for Measure_<T> to supply. 00089 void realizeModel (State& s) const {realizeMeasureModelVirtual(s);} 00090 void realizeInstance (const State& s) const {realizeMeasureInstanceVirtual(s);} 00091 void realizeTime (const State& s) const {realizeMeasureTimeVirtual(s);} 00092 void realizePosition (const State& s) const {realizeMeasurePositionVirtual(s);} 00093 void realizeVelocity (const State& s) const {realizeMeasureVelocityVirtual(s);} 00094 void realizeDynamics (const State& s) const {realizeMeasureDynamicsVirtual(s);} 00095 void realizeAcceleration(const State& s) const {realizeMeasureAccelerationVirtual(s);} 00096 void realizeReport (const State& s) const {realizeMeasureReportVirtual(s);} 00097 00101 void initialize(State& s) const {initializeVirtual(s);} 00102 00103 int getNumTimeDerivatives() const {return getNumTimeDerivativesVirtual();} 00104 00105 Stage getDependsOnStage(int derivOrder) const { 00106 SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(), 00107 "Measure::getDependsOnStage()", 00108 "derivOrder %d was out of range; this Measure allows 0-%d.", 00109 derivOrder, getNumTimeDerivatives()); 00110 return getDependsOnStageVirtual(derivOrder); 00111 } 00112 00113 00114 void setSubsystem(Subsystem& sub, MeasureIndex mx) 00115 { assert(!mySubsystem && mx.isValid()); 00116 mySubsystem = ⊂ myIndex = mx; } 00117 00118 bool isInSubsystem() const {return mySubsystem != 0;} 00119 const Subsystem& getSubsystem() const {assert(mySubsystem); return *mySubsystem;} 00120 Subsystem& updSubsystem() {assert(mySubsystem); return *mySubsystem;} 00121 MeasureIndex getSubsystemMeasureIndex() const {assert(mySubsystem); return myIndex;} 00122 SubsystemIndex getSubsystemIndex() const 00123 { return getSubsystem().getMySubsystemIndex(); } 00124 00125 void invalidateTopologyCache() const 00126 { if (isInSubsystem()) getSubsystem().invalidateSubsystemTopologyCache(); } 00127 00128 Stage getStage(const State& s) const {return getSubsystem().getStage(s);} 00129 00130 // VIRTUALS // 00131 // Ordinals must retain the same meaning from release to release 00132 // to preserve binary compatibility. 00133 00134 /* 0*/virtual ~Implementation() {} 00135 /* 1*/virtual Implementation* cloneVirtual() const = 0; 00136 00137 /* 2*/virtual void realizeTopology(State&)const = 0; 00138 00139 /* 3*/virtual void realizeMeasureModelVirtual(State&) const {} 00140 /* 4*/virtual void realizeMeasureInstanceVirtual(const State&) const {} 00141 /* 5*/virtual void realizeMeasureTimeVirtual(const State&) const {} 00142 /* 6*/virtual void realizeMeasurePositionVirtual(const State&) const {} 00143 /* 7*/virtual void realizeMeasureVelocityVirtual(const State&) const {} 00144 /* 8*/virtual void realizeMeasureDynamicsVirtual(const State&) const {} 00145 /* 9*/virtual void realizeMeasureAccelerationVirtual(const State&) const {} 00146 /*10*/virtual void realizeMeasureReportVirtual(const State&) const {} 00147 00148 /*11*/virtual void initializeVirtual(State&) const {} 00149 /*12*/virtual int 00150 getNumTimeDerivativesVirtual() const {return 0;} 00151 /*13*/virtual Stage 00152 getDependsOnStageVirtual(int order) const = 0; 00153 00154 private: 00155 int copyNumber; // bumped each time we do a deep copy 00156 00157 // These are set when this Measure is adopted by a Subsystem. 00158 Subsystem* mySubsystem; 00159 MeasureIndex myIndex; 00160 00161 // Measures have shallow copy semantics so they share the Implementation 00162 // objects, which are only deleted when the refCount goes to zero. 00163 mutable int refCount; 00164 00165 friend class AbstractMeasure; 00166 friend class Subsystem::Guts; 00167 }; 00168 00169 //============================================================================== 00170 // ABSTRACT MEASURE DEFINITIONS 00171 //============================================================================== 00172 // These had to wait for AbstractMeasure::Implementation to be defined. 00173 00174 inline AbstractMeasure:: 00175 AbstractMeasure(Implementation* g) 00176 : impl(g) 00177 { if (impl) impl->incrRefCount(); } 00178 00179 inline AbstractMeasure:: 00180 AbstractMeasure(Subsystem& sub, Implementation* g, const SetHandle&) 00181 : impl(g) { 00182 SimTK_ERRCHK(hasImpl(), "AbstractMeasure::AbstractMeasure()", 00183 "An empty Measure handle can't be put in a Subsystem."); 00184 impl->incrRefCount(); 00185 sub.adoptMeasure(*this); 00186 } 00187 00188 // Shallow copy constructor. 00189 inline AbstractMeasure::AbstractMeasure(const AbstractMeasure& src) 00190 : impl(0) { 00191 if (src.impl) { 00192 impl = src.impl; 00193 impl->incrRefCount(); 00194 } 00195 } 00196 00197 // Shallow assignment. 00198 inline AbstractMeasure& AbstractMeasure:: 00199 shallowAssign(const AbstractMeasure& src) { 00200 if (impl != src.impl) { 00201 if (impl && impl->decrRefCount()==0) delete impl; 00202 impl = src.impl; 00203 impl->incrRefCount(); 00204 } 00205 return *this; 00206 } 00207 00208 // Note that even if the source and destination are currently pointing 00209 // to the same Implementation, we still have to make a new copy so that 00210 // afterwards the destination has its own, refcount==1 copy. 00211 inline AbstractMeasure& AbstractMeasure:: 00212 deepAssign(const AbstractMeasure& src) { 00213 if (&src != this) { 00214 if (impl && impl->decrRefCount()==0) delete impl; 00215 if (src.impl) { 00216 impl = src.impl->clone(); 00217 impl->incrRefCount(); 00218 } else 00219 impl = 0; 00220 } 00221 return *this; 00222 } 00223 00224 inline AbstractMeasure:: 00225 ~AbstractMeasure() 00226 { if (impl && impl->decrRefCount()==0) delete impl;} 00227 00228 inline bool AbstractMeasure:: 00229 isInSubsystem() const 00230 { return hasImpl() && getImpl().isInSubsystem(); } 00231 00232 inline const Subsystem& AbstractMeasure:: 00233 getSubsystem() const 00234 { return getImpl().getSubsystem(); } 00235 00236 inline MeasureIndex AbstractMeasure:: 00237 getSubsystemMeasureIndex() const 00238 { return getImpl().getSubsystemMeasureIndex();} 00239 00240 inline int AbstractMeasure:: 00241 getNumTimeDerivatives() const 00242 { return getImpl().getNumTimeDerivatives(); } 00243 00244 inline Stage AbstractMeasure:: 00245 getDependsOnStage(int derivOrder) const 00246 { return getImpl().getDependsOnStage(derivOrder); } 00247 00248 inline int AbstractMeasure:: 00249 getRefCount() const 00250 { return getImpl().getRefCount(); } 00251 00252 // Hide from Doxygen. 00254 // This is a helper class that makes it possible to treat Real, Vec, and 00255 // Vector objects uniformly. 00256 template <class T> class Measure_Num { 00257 }; 00258 00259 template <> class Measure_Num<float> { 00260 public: 00261 typedef float Element; 00262 static int size(const float&) {return 1;} 00263 static const float& get(const float& v, int i) {assert(i==0); return v;} 00264 static float& upd(float& v, int i) {assert(i==0); return v;} 00265 static void makeNaNLike(const float&, float& nanValue) 00266 { nanValue = CNT<float>::getNaN();} 00267 static void makeZeroLike(const float&, float& zeroValue) {zeroValue=0.f;} 00268 }; 00269 00270 template <> class Measure_Num<double> { 00271 public: 00272 typedef double Element; 00273 static int size(const double&) {return 1;} 00274 static const double& get(const double& v, int i) {assert(i==0); return v;} 00275 static double& upd(double& v, int i) {assert(i==0); return v;} 00276 static void makeNaNLike(const double&, double& nanValue) 00277 { nanValue = CNT<double>::getNaN(); } 00278 static void makeZeroLike(const double&, double& zeroValue) {zeroValue=0.;} 00279 }; 00280 00281 // We only support stride 1 (densely packed) Vec types. 00282 template <int M, class E> 00283 class Measure_Num< Vec<M,E,1> > { 00284 typedef Vec<M,E,1> T; 00285 public: 00286 typedef E Element; 00287 static int size(const T&) {return M;} 00288 static const E& get(const T& v, int i) {return v[i];} 00289 static E& upd(T& v, int i) {return v[i];} 00290 static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();} 00291 static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();} 00292 }; 00293 00294 // We only support column major (densely packed) Mat types. 00295 template <int M, int N, class E> 00296 class Measure_Num< Mat<M,N,E> > { 00297 typedef Mat<M,N,E> T; 00298 public: 00299 typedef E Element; 00300 static int size(const T&) {return N;} // number of columns 00301 static const typename T::TCol& get(const T& m, int j) {return m.col(j);} 00302 static typename T::TCol& upd(T& m, int j) {return m.col(j);} 00303 static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();} 00304 static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();} 00305 }; 00306 00307 00308 template <class E> 00309 class Measure_Num< Vector_<E> > { 00310 typedef Vector_<E> T; 00311 public: 00312 typedef E Element; 00313 static int size(const T& v) {return v.size();} 00314 static const E& get(const T& v, int i) {return v[i];} 00315 static E& upd(T& v, int i) {return v[i];} 00316 static void makeNaNLike(const T& v, T& nanValue) 00317 { nanValue.resize(v.size()); nanValue.setToNaN(); } 00318 static void makeZeroLike(const T& v, T& zeroValue) 00319 { zeroValue.resize(v.size()); zeroValue.setToZero(); } 00320 }; 00321 00322 00323 template <class E> 00324 class Measure_Num< Rotation_<E> > { 00325 typedef Rotation_<E> T; 00326 public: 00327 typedef T Element; 00328 static int size(const T&) {return 1;} 00329 static const T& get(const T& v, int i) {assert(i==0); return v;} 00330 static T& upd(T& v, int i) {assert(i==0); return v;} 00331 static void makeNaNLike(const T&, T& nanValue) 00332 { nanValue.setRotationToNaN(); } 00333 static void makeZeroLike(const T&, T& zeroValue) 00334 { zeroValue.setRotationToIdentityMatrix(); } 00335 }; 00336 00337 template <class E> 00338 class Measure_Num< Transform_<E> > { 00339 typedef Transform_<E> T; 00340 public: 00341 typedef T Element; 00342 static int size(const T&) {return 1;} 00343 static const T& get(const T& v, int i) {assert(i==0); return v;} 00344 static T& upd(T& v, int i) {assert(i==0); return v;} 00345 static void makeNaNLike(const T&, T& nanValue) 00346 { nanValue.setToNaN(); } 00347 static void makeZeroLike(const T&, T& zeroValue) 00348 { zeroValue.setToZero(); } 00349 }; 00350 00353 //============================================================================== 00354 // MEASURE_<T> :: IMPLEMENTATION 00355 //============================================================================== 00362 template <class T> 00363 class Measure_<T>::Implementation : public AbstractMeasure::Implementation { 00364 public: 00365 const T& getValue(const State& s, int derivOrder) const { 00366 SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(), 00367 "Measure_<T>::getValue()", 00368 "derivOrder %d was out of range; this Measure allows 0-%d.", 00369 derivOrder, getNumTimeDerivatives()); 00370 00371 // We require the stage to have been advanced to at least the one 00372 // before this measure's depends-on stage since this will get called 00373 // towards the end of the depends-on stage realization. 00374 if (getDependsOnStage(derivOrder) != Stage::Empty) { 00375 #ifndef NDEBUG 00376 Stage prevStage = getDependsOnStage(derivOrder).prev(); 00377 #endif 00378 00379 SimTK_ERRCHK2 00380 ( ( isInSubsystem() && getStage(s)>=prevStage) 00381 || (!isInSubsystem() && s.getSystemStage()>=prevStage), 00382 "Measure_<T>::getValue()", 00383 "Expected State to have been realized to at least stage " 00384 "%s but stage was %s.", 00385 prevStage.getName().c_str(), 00386 (isInSubsystem() ? getStage(s) : s.getSystemStage()) 00387 .getName().c_str()); 00388 } 00389 00390 if (derivOrder < getNumCacheEntries()) { 00391 if (!isCacheValueRealized(s,derivOrder)) { 00392 T& value = updCacheEntry(s,derivOrder); 00393 calcCachedValueVirtual(s, derivOrder, value); 00394 markCacheValueRealized(s,derivOrder); 00395 return value; 00396 } 00397 return getCacheEntry(s,derivOrder); 00398 } 00399 00400 // We can't handle it here -- punt to the concrete Measure 00401 // for higher order derivatives. 00402 return getUncachedValueVirtual(s,derivOrder); 00403 } 00404 00407 void setDefaultValue(const T& defaultValue) { 00408 this->defaultValue = defaultValue; 00409 Measure_Num<T>::makeZeroLike(defaultValue, zeroValue); 00410 this->invalidateTopologyCache(); 00411 } 00412 00416 const T& getDefaultValue() const {return defaultValue;} 00417 00418 void setIsPresumedValidAtDependsOnStage(bool presume) 00419 { presumeValidAtDependsOnStage = presume; 00420 this->invalidateTopologyCache(); } 00421 00422 bool getIsPresumedValidAtDependsOnStage() const 00423 { return presumeValidAtDependsOnStage; } 00424 00425 protected: 00426 explicit Implementation(const T& defaultValue, int numCacheEntries=1) 00427 : presumeValidAtDependsOnStage(false), 00428 defaultValue(defaultValue), 00429 derivIx(numCacheEntries) 00430 { 00431 Measure_Num<T>::makeZeroLike(defaultValue, zeroValue); 00432 } 00433 00437 explicit Implementation(int numCacheEntries=1) 00438 : presumeValidAtDependsOnStage(false), 00439 defaultValue(), 00440 derivIx(numCacheEntries) 00441 { 00442 Measure_Num<T>::makeZeroLike(defaultValue, zeroValue); 00443 } 00444 00448 Implementation(const Implementation& source) 00449 : presumeValidAtDependsOnStage(source.presumeValidAtDependsOnStage), 00450 defaultValue(source.defaultValue), 00451 derivIx(source.derivIx.size()) 00452 { 00453 Measure_Num<T>::makeZeroLike(defaultValue, zeroValue); 00454 } 00455 00456 00459 int size() const {return Measure_Num<T>::size(defaultValue);} 00460 00463 int getNumCacheEntries() const {return (int)derivIx.size();} 00464 00468 const T& getCacheEntry(const State& s, int derivOrder) const { 00469 SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(), 00470 "Measure_<T>::Implementation::getCacheEntry()", 00471 "Derivative order %d is out of range; only %d cache entries" 00472 " were allocated.", derivOrder, getNumCacheEntries()); 00473 00474 return Value<T>::downcast( 00475 this->getSubsystem().getCacheEntry(s, derivIx[derivOrder])); 00476 } 00477 00481 T& updCacheEntry(const State& s, int derivOrder) const { 00482 SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(), 00483 "Measure_<T>::Implementation::updCacheEntry()", 00484 "Derivative order %d is out of range; only %d cache entries" 00485 " were allocated.", derivOrder, getNumCacheEntries()); 00486 00487 return Value<T>::updDowncast( 00488 this->getSubsystem().updCacheEntry(s, derivIx[derivOrder])); 00489 } 00490 00493 bool isCacheValueRealized(const State& s, int derivOrder) const { 00494 SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(), 00495 "Measure_<T>::Implementation::isCacheValueRealized()", 00496 "Derivative order %d is out of range; only %d cache entries" 00497 " were allocated.", derivOrder, getNumCacheEntries()); 00498 00499 return this->getSubsystem().isCacheValueRealized(s, derivIx[derivOrder]); 00500 } 00501 00505 void markCacheValueRealized(const State& s, int derivOrder) const { 00506 SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(), 00507 "Measure_<T>::Implementation::markCacheValueRealized()", 00508 "Derivative order %d is out of range; only %d cache entries" 00509 " were allocated.", derivOrder, getNumCacheEntries()); 00510 00511 this->getSubsystem().markCacheValueRealized(s, derivIx[derivOrder]); 00512 } 00513 00518 void markCacheValueNotRealized(const State& s, int derivOrder) const { 00519 SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(), 00520 "Measure_<T>::Implementation::markCacheValueNotRealized()", 00521 "Derivative order %d is out of range; only %d cache entries" 00522 " were allocated.", derivOrder, getNumCacheEntries()); 00523 00524 this->getSubsystem().markCacheValueNotRealized(s, derivIx[derivOrder]); 00525 } 00526 00527 // VIRTUALS // 00528 // Ordinals must retain the same meaning from release to release 00529 // to preserve binary compatibility. 00530 00533 /* 0*/virtual void realizeMeasureTopologyVirtual(State&) const 00534 {} 00535 00538 /* 1*/virtual void 00539 calcCachedValueVirtual(const State&, int derivOrder, T& value) const 00540 { SimTK_ERRCHK1_ALWAYS(!"implemented", 00541 "Measure_<T>::Implementation::calcCachedValueVirtual()", 00542 "This method should have been overridden by the derived" 00543 " Measure but was not. It is needed to calculate the" 00544 " cached value for derivOrder=%d.", derivOrder); } 00545 00551 /* 2*/virtual const T& 00552 getUncachedValueVirtual(const State&, int derivOrder) const 00553 { SimTK_ERRCHK1_ALWAYS(!"implemented", 00554 "Measure_<T>::Implementation::getUncachedValueVirtual()", 00555 "This method should have been overridden by the derived" 00556 " Measure but was not. It is needed to return the uncached" 00557 " value at derivOrder=%d.", derivOrder); 00558 return *reinterpret_cast<T*>(0); 00559 } 00560 00563 const T& getValueZero() const {return zeroValue;} 00564 00565 private: 00566 // Satisfy the realizeTopology() pure virtual here now that we know the 00567 // data type T. Allocate lazy- or auto-validated- cache entries depending 00568 // on the setting of presumeValidAtDependsOnStage. 00569 void realizeTopology(State& s) const FINAL_11 { 00570 // Allocate cache entries. Initialize the value cache entry to 00571 // the given defaultValue; all the derivative cache entries should be 00572 // initialized to a NaN of the same size. 00573 if (getNumCacheEntries()) { 00574 derivIx[0] = presumeValidAtDependsOnStage 00575 ? this->getSubsystem().allocateCacheEntry 00576 (s, getDependsOnStage(0), new Value<T>(defaultValue)) 00577 : this->getSubsystem().allocateLazyCacheEntry 00578 (s, getDependsOnStage(0), new Value<T>(defaultValue)); 00579 00580 if (getNumCacheEntries() > 1) { 00581 T nanValue; Measure_Num<T>::makeNaNLike(defaultValue, nanValue); 00582 for (int i=1; i < getNumCacheEntries(); ++i) { 00583 derivIx[i] = presumeValidAtDependsOnStage 00584 ? this->getSubsystem().allocateCacheEntry 00585 (s, getDependsOnStage(i), new Value<T>(nanValue)) 00586 : this->getSubsystem().allocateLazyCacheEntry 00587 (s, getDependsOnStage(i), new Value<T>(nanValue)); 00588 } 00589 } 00590 } 00591 00592 // Call the concrete class virtual if any. 00593 realizeMeasureTopologyVirtual(s); 00594 } 00595 00596 //------------------------------------------------------------------------------ 00597 private: 00598 // TOPOLOGY STATE 00599 bool presumeValidAtDependsOnStage; 00600 T defaultValue; 00601 T zeroValue; 00602 00603 // TOPOLOGY CACHE 00604 mutable Array_<CacheEntryIndex> derivIx; 00605 }; 00606 00607 00608 00609 //============================================================================== 00610 // CONSTANT :: IMPLEMENTATION 00611 //============================================================================== 00612 template <class T> 00613 class Measure_<T>::Constant::Implementation 00614 : public Measure_<T>::Implementation 00615 { 00616 public: 00617 // We don't want the base class to allocate *any* cache entries. 00618 Implementation() : Measure_<T>::Implementation(0) {} 00619 explicit Implementation(const T& value) 00620 : Measure_<T>::Implementation(value,0) {} 00621 00624 void setValue(const T& v) {this->setDefaultValue(v);} 00625 00626 // Implementations of virtual methods. 00627 // Measure_<T> virtuals: 00628 // No cached values. 00629 00630 const T& getUncachedValueVirtual(const State&, int derivOrder) const 00631 OVERRIDE_11 00632 { return derivOrder>0 ? this->getValueZero() : this->getDefaultValue(); } 00633 00634 // AbstractMeasure virtuals: 00635 Implementation* cloneVirtual() const OVERRIDE_11 00636 { return new Implementation(*this); } 00637 Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11 00638 { return derivOrder>0 ? Stage::Empty : Stage::Topology; } 00639 int getNumTimeDerivativesVirtual() const OVERRIDE_11 00640 { return std::numeric_limits<int>::max(); } 00641 }; 00642 00643 00644 00645 //============================================================================== 00646 // MEASURE ZERO and ONE 00647 //============================================================================== 00648 // These had to wait for Constant::Implementation to be declared. 00649 00650 template <class T> inline 00651 Measure_<T>::Zero::Zero() : Constant(T(0)) {} 00652 template <class T> inline 00653 Measure_<T>::Zero::Zero(Subsystem& sub) : Constant(sub, T(0)) {} 00654 00655 inline Measure_< Vector >::Zero::Zero(int size) 00656 : Constant(Vector(size, Real(0))) {} 00657 inline Measure_< Vector >::Zero::Zero(Subsystem& sub, int size) 00658 : Constant(sub, Vector(size, Real(0))) {} 00659 00660 template <class T> inline 00661 Measure_<T>::One::One() : Constant(T(1)) {} 00662 template <class T> inline 00663 Measure_<T>::One::One(Subsystem& sub) : Constant(sub, T(1)) {} 00664 00665 inline Measure_< Vector >::One::One(int size) 00666 : Constant(Vector(size, Real(1))) {} 00667 inline Measure_< Vector >::One::One(Subsystem& sub, int size) 00668 : Constant(sub, Vector(size, Real(1))) {} 00669 00670 00671 00672 //============================================================================== 00673 // TIME :: IMPLEMENTATION 00674 //============================================================================== 00675 template <class T> 00676 class Measure_<T>::Time::Implementation {}; 00677 00678 template <> 00679 class Measure_<Real>::Time::Implementation 00680 : public Measure_<Real>::Implementation 00681 { 00682 public: 00683 // We don't want the base class to allocate *any* cache entries. 00684 Implementation() : Measure_<Real>::Implementation(0) {} 00685 00686 // Implementations of virtual methods. 00687 // Measure_<Real> virtuals: 00688 // No cached values. 00689 00690 const Real& getUncachedValueVirtual(const State& s, int derivOrder) const 00691 OVERRIDE_11 00692 { return derivOrder==0 ? s.getTime() 00693 : (derivOrder==1 ? SimTK::One 00694 : SimTK::Zero); } 00695 00696 // AbstractMeasure virtuals: 00697 Implementation* cloneVirtual() const OVERRIDE_11 00698 { return new Implementation(*this); } 00699 Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11 00700 { return derivOrder>0 ? Stage::Empty : Stage::Time; } 00701 00702 // Value is t, 1st derivative is 1, the rest are 0. 00703 int getNumTimeDerivativesVirtual() const OVERRIDE_11 00704 { return std::numeric_limits<int>::max(); } 00705 }; 00706 00707 00708 00709 //============================================================================== 00710 // VARIABLE :: IMPLEMENTATION 00711 //============================================================================== 00712 template <class T> 00713 class Measure_<T>::Variable::Implementation 00714 : public Measure_<T>::Implementation 00715 { 00716 public: 00717 // We don't want the base class to allocate *any* cache entries; 00718 // we'll use the variable as its own value and zeroes for all 00719 // the derivatives. 00720 Implementation() 00721 : Measure_<T>::Implementation(0), 00722 invalidatedStage(Stage::Empty) {} 00723 00724 Implementation(Stage invalidated, const T& defaultValue) 00725 : Measure_<T>::Implementation(defaultValue, 0), 00726 invalidatedStage(invalidated) {} 00727 00728 // Copy constructor should not copy the variable. 00729 Implementation(const Implementation& source) 00730 : Measure_<T>::Implementation(source.getDefaultValue(), 0), 00731 invalidatedStage(source.invalidatedStage) {} 00732 00733 void setInvalidatedStage(Stage invalidates) { 00734 invalidatedStage = invalidates; 00735 this->invalidateTopologyCache(); 00736 } 00737 00738 Stage getInvalidatedStage() const {return invalidatedStage;} 00739 00743 void setValue(State& state, const T& value) const 00744 { updVarValue(state) = value; } 00745 00746 // Implementations of virtual methods. 00747 Implementation* cloneVirtual() const OVERRIDE_11 00748 { return new Implementation(*this); } 00749 00750 int getNumTimeDerivativesVirtual() const OVERRIDE_11 00751 { return std::numeric_limits<int>::max(); } 00752 00753 // Discrete variable is available after Model stage; but all its 00754 // derivatives are zero so are always available. 00755 Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11 00756 { return derivOrder>0 ? Stage::Empty : Stage::Model;} 00757 00758 const T& getUncachedValueVirtual(const State& s, int derivOrder) const 00759 OVERRIDE_11 00760 { return derivOrder>0 ? this->getValueZero() : getVarValue(s); } 00761 00762 // No cached values. 00763 00764 void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 { 00765 discreteVarIndex = this->getSubsystem().allocateDiscreteVariable 00766 (s, invalidatedStage, new Value<T>(this->getDefaultValue())); 00767 } 00768 private: 00769 const T& getVarValue(const State& s) const { 00770 assert(discreteVarIndex.isValid()); 00771 return Value<T>::downcast( 00772 this->getSubsystem().getDiscreteVariable(s, discreteVarIndex)); 00773 } 00774 T& updVarValue(State& s) const { 00775 assert(discreteVarIndex.isValid()); 00776 return Value<T>::downcast( 00777 this->getSubsystem().updDiscreteVariable(s, discreteVarIndex)); 00778 } 00779 00780 // TOPOLOGY STATE 00781 Stage invalidatedStage; // TODO this shouldn't be needed 00782 00783 // TOPOLOGY CACHE 00784 mutable DiscreteVariableIndex discreteVarIndex; 00785 }; 00786 00787 00788 00789 //============================================================================== 00790 // RESULT :: IMPLEMENTATION 00791 //============================================================================== 00792 template <class T> 00793 class Measure_<T>::Result::Implementation 00794 : public Measure_<T>::Implementation 00795 { 00796 public: 00797 // We want the base class to allocate a single cache entry of type T. 00798 Implementation() 00799 : Measure_<T>::Implementation(1), 00800 dependsOnStage(Stage::Topology), invalidatedStage(Stage::Infinity) {} 00801 00802 Implementation(Stage dependsOn, Stage invalidated) 00803 : Measure_<T>::Implementation(1), 00804 dependsOnStage(dependsOn==Stage::Empty ? Stage::Topology : dependsOn), 00805 invalidatedStage(invalidated) 00806 { SimTK_ERRCHK2_ALWAYS(invalidated > dependsOn,"Measure::Result::ctor()", 00807 "Got invalidated stage %s and dependsOn stage %s which is illegal " 00808 "because the invalidated stage must be later than dependsOn.", 00809 invalidated.getName().c_str(), dependsOn.getName().c_str()); 00810 } 00811 00812 // Copy constructor will not copy the cache entry index. 00813 Implementation(const Implementation& source) 00814 : Measure_<T>::Implementation(source), 00815 dependsOnStage(source.dependsOnStage), 00816 invalidatedStage(source.invalidatedStage) {} 00817 00818 void setDependsOnStage(Stage dependsOn) { 00819 if (dependsOn == Stage::Empty) dependsOn = Stage::Topology; 00820 SimTK_ERRCHK2_ALWAYS(dependsOn < getInvalidatedStage(), 00821 "Measure::Result::setDependsOnStage()", 00822 "The provided dependsOn stage %s is illegal because it is not " 00823 "less than the current invalidated stage %s. Change the " 00824 "invalidated stage first with setInvalidatedStage().", 00825 dependsOn.getName().c_str(), 00826 getInvalidatedStage().getName().c_str()); 00827 00828 dependsOnStage = dependsOn; 00829 this->invalidateTopologyCache(); 00830 } 00831 00832 void setInvalidatedStage(Stage invalidated) { 00833 SimTK_ERRCHK2_ALWAYS(invalidated > getDependsOnStage(), 00834 "Measure::Result::setInvalidatedStage()", 00835 "The provided invalidated stage %s is illegal because it is not " 00836 "greater than the current dependsOn stage %s. Change the " 00837 "dependsOn stage first with setDependsOnStage().", 00838 invalidated.getName().c_str(), 00839 getDependsOnStage().getName().c_str()); 00840 00841 invalidatedStage = invalidated; 00842 this->invalidateTopologyCache(); 00843 } 00844 00845 00846 Stage getDependsOnStage() const {return dependsOnStage;} 00847 Stage getInvalidatedStage() const {return invalidatedStage;} 00848 00849 00850 void markAsValid(const State& state) const 00851 { const Stage subsystemStage = this->getSubsystem().getStage(state); 00852 SimTK_ERRCHK3_ALWAYS(subsystemStage >= getDependsOnStage().prev(), 00853 "Measure::Result::markAsValid()", 00854 "This Result Measure cannot be marked valid in a State where this " 00855 "measure's Subsystem has been realized only to stage %s, because " 00856 "its value was declared to depend on stage %s. To mark it valid, " 00857 "we require that the State have been realized at least to the " 00858 "previous stage (%s in this case); that is, you must at least be " 00859 "*working on* the dependsOn stage in order to claim this result is " 00860 "available.", 00861 subsystemStage.getName().c_str(), 00862 getDependsOnStage().getName().c_str(), 00863 getDependsOnStage().prev().getName().c_str()); 00864 this->markCacheValueRealized(state, 0); } 00865 00866 bool isValid(const State& state) const 00867 { return this->isCacheValueRealized(state, 0); } 00868 00869 void markAsNotValid(const State& state) const 00870 { this->markCacheValueNotRealized(state, 0); 00871 state.invalidateAllCacheAtOrAbove(invalidatedStage); } 00872 00873 T& updValue(const State& state) const 00874 { markAsNotValid(state); return this->updCacheEntry(state, 0); } 00875 00876 00877 // Implementations of virtual methods. 00878 Implementation* cloneVirtual() const OVERRIDE_11 00879 { return new Implementation(*this); } 00880 00881 int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;} 00882 00885 Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11 00886 { return derivOrder>0 ? Stage::Empty : dependsOnStage;} 00887 00888 void calcCachedValueVirtual(const State&, int derivOrder, T& value) const 00889 OVERRIDE_11 00890 { SimTK_ERRCHK_ALWAYS(!"calcCachedValueVirtual() implemented", 00891 "Measure_<T>::Result::getValue()", 00892 "Measure_<T>::Result::getValue() was called when the value was not " 00893 "yet valid. For most Measure types, this would have initiated " 00894 "computation of the value, but Result measures must have their values " 00895 "calculated and set externally, and then marked valid."); } 00896 00897 private: 00898 // TOPOLOGY STATE 00899 Stage dependsOnStage; 00900 Stage invalidatedStage; 00901 }; 00902 00903 00904 00905 //============================================================================== 00906 // SINUSOID :: IMPLEMENTATION 00907 //============================================================================== 00908 template <class T> 00909 class Measure_<T>::Sinusoid::Implementation 00910 : public Measure_<T>::Implementation 00911 { 00912 static const int NumDerivs = 3; 00913 public: 00914 Implementation() 00915 : Measure_<T>::Implementation(NumDerivs+1), 00916 a(CNT<T>::getNaN()), w(CNT<T>::getNaN()), p(CNT<T>::getNaN()) {} 00917 00918 Implementation(const T& amplitude, 00919 const T& frequency, 00920 const T& phase=T(0)) 00921 : Measure_<T>::Implementation(NumDerivs+1), 00922 a(amplitude), w(frequency), p(phase) {} 00923 00924 // Default copy constructor is fine. 00925 00926 // Implementations of virtual methods. 00927 Implementation* cloneVirtual() const OVERRIDE_11 00928 { return new Implementation(*this); } 00929 00930 int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return NumDerivs;} 00931 00932 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 00933 { return Stage::Time; } 00934 00935 void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const 00936 OVERRIDE_11 00937 { 00938 // We need to allow the compiler to select std::sin or SimTK::sin 00939 // based on the argument type. 00940 using std::sin; using std::cos; 00941 00942 assert(NumDerivs == 3); 00943 const Real t = s.getTime(); 00944 const T arg = w*t + p; 00945 00946 switch (derivOrder) { 00947 case 0: value = a*sin(arg); break; 00948 case 1: value = w*a*cos(arg); break; 00949 case 2: value = -w*w*a*sin(arg); break; 00950 case 3: value = -w*w*w*a*cos(arg); break; 00951 default: SimTK_ASSERT1_ALWAYS(!"out of range", 00952 "Measure::Sinusoid::Implementation::calcCachedValueVirtual():" 00953 " derivOrder %d is out of range 0-3.", derivOrder); 00954 } 00955 } 00956 00957 // There are no uncached values. 00958 00959 private: 00960 // TOPOLOGY STATE 00961 T a, w, p; 00962 00963 // TOPOLOGY CACHE 00964 // nothing 00965 }; 00966 00967 00968 00969 //============================================================================== 00970 // PLUS :: IMPLEMENTATION 00971 //============================================================================== 00972 template <class T> 00973 class Measure_<T>::Plus::Implementation : public Measure_<T>::Implementation { 00974 public: 00975 // TODO: Currently allocates just one cache entry. 00976 // left and right will be empty handles. 00977 Implementation() {} 00978 00979 Implementation(const Measure_<T>& left, 00980 const Measure_<T>& right) 00981 : left(left), right(right) {} 00982 00983 // Default copy constructor gives us a new Implementation object, 00984 // but with references to the *same* operand measures. 00985 00986 // Implementations of virtual methods. 00987 00988 // This uses the default copy constructor. 00989 Implementation* cloneVirtual() const OVERRIDE_11 00990 { return new Implementation(*this); } 00991 00992 // TODO: Let this be settable up to the min number of derivatives 00993 // provided by the arguments. 00994 int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;} 00995 //{ return std::min(left.getNumTimeDerivatives(), 00996 // right.getNumTimeDerivatives()); } 00997 00998 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 00999 { return Stage(std::max(left.getDependsOnStage(order), 01000 right.getDependsOnStage(order))); } 01001 01002 01003 void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const 01004 OVERRIDE_11 01005 { 01006 value = left.getValue(s,derivOrder) + right.getValue(s,derivOrder); 01007 } 01008 01009 // There are no uncached values. 01010 01011 private: 01012 // TOPOLOGY STATE 01013 Measure_<T> left; 01014 Measure_<T> right; 01015 01016 // TOPOLOGY CACHE 01017 // nothing 01018 }; 01019 01020 01021 01022 //============================================================================== 01023 // MINUS :: IMPLEMENTATION 01024 //============================================================================== 01025 template <class T> 01026 class Measure_<T>::Minus::Implementation : public Measure_<T>::Implementation { 01027 public: 01028 // TODO: Currently allocates just one cache entry. 01029 // left and right will be empty handles. 01030 Implementation() {} 01031 01032 Implementation(const Measure_<T>& left, 01033 const Measure_<T>& right) 01034 : left(left), right(right) {} 01035 01036 // Default copy constructor gives us a new Implementation object, 01037 // but with references to the *same* operand measures. 01038 01039 // Implementations of virtual methods. 01040 01041 // This uses the default copy constructor. 01042 Implementation* cloneVirtual() const OVERRIDE_11 01043 { return new Implementation(*this); } 01044 01045 // TODO: Let this be settable up to the min number of derivatives 01046 // provided by the arguments. 01047 int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;} 01048 //{ return std::min(left.getNumTimeDerivatives(), 01049 // right.getNumTimeDerivatives()); } 01050 01051 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 01052 { return Stage(std::max(left.getDependsOnStage(order), 01053 right.getDependsOnStage(order))); } 01054 01055 01056 void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const 01057 OVERRIDE_11 01058 { 01059 value = left.getValue(s,derivOrder) - right.getValue(s,derivOrder); 01060 } 01061 01062 // There are no uncached values. 01063 01064 private: 01065 // TOPOLOGY STATE 01066 Measure_<T> left; 01067 Measure_<T> right; 01068 01069 // TOPOLOGY CACHE 01070 // nothing 01071 }; 01072 01073 01074 01075 //============================================================================== 01076 // SCALE :: IMPLEMENTATION 01077 //============================================================================== 01078 template <class T> 01079 class Measure_<T>::Scale::Implementation 01080 : public Measure_<T>::Implementation 01081 { 01082 public: 01083 // TODO: Currently allocates just one cache entry. 01084 // scale will be uninitialized, operand will be empty handle. 01085 Implementation() : factor(NaN) {} 01086 01087 Implementation(Real factor, const Measure_<T>& operand) 01088 : factor(factor), operand(operand) {} 01089 01090 // Default copy constructor gives us a new Implementation object, 01091 // but with references to the *same* operand measure. 01092 01093 void setScaleFactor(Real sf) { 01094 factor = sf; 01095 this->invalidateTopologyCache(); 01096 } 01097 01098 const Measure_<T>& getOperandMeasure() const 01099 { 01100 return operand; 01101 } 01102 01103 // Implementations of virtual methods. 01104 01105 // This uses the default copy constructor. 01106 Implementation* cloneVirtual() const OVERRIDE_11 01107 { return new Implementation(*this); } 01108 01109 // TODO: Let this be settable up to the min number of derivatives 01110 // provided by the arguments. 01111 int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;} 01112 //{ return std::min(left.getNumTimeDerivatives(), 01113 // right.getNumTimeDerivatives()); } 01114 01115 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 01116 { return operand.getDependsOnStage(order); } 01117 01118 01119 void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const 01120 OVERRIDE_11 01121 { 01122 value = factor * operand.getValue(s,derivOrder); 01123 } 01124 01125 // There are no uncached values. 01126 01127 private: 01128 // TOPOLOGY STATE 01129 Real factor; 01130 Measure_<T> operand; 01131 01132 // TOPOLOGY CACHE 01133 // nothing 01134 }; 01135 01136 01137 01138 //============================================================================== 01139 // INTEGRATE :: IMPLEMENTATION 01140 //============================================================================== 01147 template <class T> 01148 class Measure_<T>::Integrate::Implementation 01149 : public Measure_<T>::Implementation { 01150 public: 01153 Implementation() : Measure_<T>::Implementation(1) {} 01154 01157 Implementation(const Measure_<T>& deriv, const Measure_<T>& ic, 01158 const T& defaultValue) 01159 : Measure_<T>::Implementation(defaultValue, 1), 01160 derivMeasure(deriv), icMeasure(ic) {} 01161 01164 Implementation(const Implementation& source) 01165 : Measure_<T>::Implementation(source.getDefaultValue(), 1), 01166 derivMeasure(source.derivMeasure), icMeasure(source.icMeasure) {} 01167 01171 void setValue(State& s, const T& value) const 01172 { assert(zIndex >= 0); 01173 for (int i=0; i < this->size(); ++i) 01174 this->getSubsystem().updZ(s)[zIndex+i] = 01175 Measure_Num<T>::get(value, i); } 01176 01177 const Measure_<T>& getDerivativeMeasure() const 01178 { SimTK_ERRCHK(!derivMeasure.isEmptyHandle(), 01179 "Measure_<T>::Integrate::getDerivativeMeasure()", 01180 "No derivative measure is available for this integrated measure."); 01181 return derivMeasure; } 01182 01183 const Measure_<T>& getInitialConditionMeasure() const 01184 { SimTK_ERRCHK(!icMeasure.isEmptyHandle(), 01185 "Measure_<T>::Integrate::getInitialConditionMeasure()", 01186 "No initial condition measure is available for this " 01187 "integrated measure."); 01188 return icMeasure; } 01189 01190 void setDerivativeMeasure(const Measure_<T>& d) 01191 { derivMeasure = d; this->invalidateTopologyCache(); } 01192 void setInitialConditionMeasure(const Measure_<T>& ic) 01193 { icMeasure = ic; this->invalidateTopologyCache(); } 01194 01195 // Implementations of virtuals. 01196 01197 // This uses the copy constructor defined above. 01198 Implementation* cloneVirtual() const OVERRIDE_11 01199 { return new Implementation(*this); } 01200 01202 int getNumTimeDerivativesVirtual() const OVERRIDE_11 01203 { int integralDerivs = getDerivativeMeasure().getNumTimeDerivatives(); 01204 // Careful - can't add 1 to max int and stay an int. 01205 if (integralDerivs < std::numeric_limits<int>::max()) 01206 ++integralDerivs; 01207 return integralDerivs; } 01208 01209 void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const 01210 OVERRIDE_11 01211 { assert(derivOrder == 0); // only one cache entry 01212 assert(Measure_Num<T>::size(value) == this->size()); 01213 assert(zIndex.isValid()); 01214 const Vector& allZ = this->getSubsystem().getZ(s); 01215 for (int i=0; i < this->size(); ++i) 01216 Measure_Num<T>::upd(value,i) = allZ[zIndex+i]; 01217 } 01218 01219 const T& getUncachedValueVirtual(const State& s, int derivOrder) const 01220 OVERRIDE_11 01221 { assert(derivOrder > 0); // 0th entry is cached 01222 return getDerivativeMeasure().getValue(s, derivOrder-1); 01223 } 01224 01225 Stage getDependsOnStageVirtual(int derivOrder) const OVERRIDE_11 01226 { return derivOrder>0 01227 ? getDerivativeMeasure().getDependsOnStage(derivOrder-1) 01228 : Stage::Time; } 01229 01232 void initializeVirtual(State& s) const OVERRIDE_11 { 01233 assert(zIndex.isValid()); 01234 Vector& allZ = this->getSubsystem().updZ(s); 01235 if (!icMeasure.isEmptyHandle()) { 01236 this->getSubsystem().getSystem() 01237 .realize(s, icMeasure.getDependsOnStage()); 01238 const T& ic = icMeasure.getValue(s); 01239 for (int i=0; i < this->size(); ++i) 01240 allZ[zIndex+i] = Measure_Num<T>::get(ic,i); 01241 } else { 01242 for (int i=0; i < this->size(); ++i) 01243 allZ[zIndex+i] = Measure_Num<T>::get(this->getDefaultValue(),i); 01244 } 01245 } 01246 01251 void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 { 01252 Vector init(this->size()); 01253 for (int i=0; i < this->size(); ++i) 01254 init[i] = Measure_Num<T>::get(this->getDefaultValue(),i); 01255 zIndex = this->getSubsystem().allocateZ(s, init); 01256 } 01257 01260 void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 { 01261 assert(zIndex.isValid()); 01262 Vector& allZDot = this->getSubsystem().updZDot(s); 01263 if (!derivMeasure.isEmptyHandle()) { 01264 const T& deriv = derivMeasure.getValue(s); 01265 for (int i=0; i < this->size(); ++i) 01266 allZDot[zIndex+i] = Measure_Num<T>::get(deriv,i); 01267 } else { 01268 allZDot(zIndex,this->size()) = 0; // derivative is zero 01269 } 01270 } 01271 01272 private: 01273 // TOPOLOGY STATE 01274 Measure_<T> derivMeasure; // just handles 01275 Measure_<T> icMeasure; 01276 01277 // TOPOLOGY CACHE 01278 mutable ZIndex zIndex; // This is the first index if more than one z. 01279 }; 01280 01281 01282 01283 //============================================================================== 01284 // DIFFERENTIATE :: IMPLEMENTATION 01285 //============================================================================== 01286 // Hide from Doxygen. 01288 // This helper class is the contents of the discrete state variable and 01289 // corresponding cache entry maintained by this measure. The variable is 01290 // auto-update, meaning the value of the cache entry replaces the state 01291 // variable at the start of each step. 01292 // TODO: This was a local class in Measure_<T>::Differentiate::Implementation 01293 // but VC++ 8 (2005) failed to properly instantiate the templatized operator<<() 01294 // in that case; doing it this way is a workaround. 01295 template <class T> 01296 class Measure_Differentiate_Result { 01297 public: 01298 Measure_Differentiate_Result() : derivIsGood(false) {} 01299 T operand; // previous value of operand 01300 T operandDot; // previous value of derivative 01301 bool derivIsGood; // do we think the deriv is a good one? 01302 }; 01305 template <class T> 01306 class Measure_<T>::Differentiate::Implementation 01307 : public Measure_<T>::Implementation 01308 { 01309 typedef Measure_Differentiate_Result<T> Result; 01310 public: 01311 // Don't allocate any cache entries in the base class. 01312 Implementation() : Measure_<T>::Implementation(0) {} 01313 01314 Implementation(const Measure_<T>& operand) 01315 : Measure_<T>::Implementation(0), 01316 operand(operand), forceUseApprox(false), isApproxInUse(false) {} 01317 01318 // Default copy constructor gives us a new Implementation object, 01319 // but with reference to the *same* operand measure. 01320 01321 void setForceUseApproximation(bool mustApproximate) { 01322 forceUseApprox = mustApproximate; 01323 this->invalidateTopologyCache(); 01324 } 01325 01326 void setOperandMeasure(const Measure_<T>& operand) { 01327 this->operand = operand; 01328 this->invalidateTopologyCache(); 01329 } 01330 01331 bool getForceUseApproximation() const {return forceUseApprox;} 01332 bool isUsingApproximation() const {return isApproxInUse;} 01333 const Measure_<T>& getOperandMeasure() const {return operand;} 01334 01335 // Implementations of virtual methods. 01336 01337 // This uses the default copy constructor. 01338 Implementation* cloneVirtual() const OVERRIDE_11 01339 { return new Implementation(*this); } 01340 01341 // This has one fewer than the operand. 01342 int getNumTimeDerivativesVirtual() const OVERRIDE_11 01343 { if (!isApproxInUse) return operand.getNumTimeDerivatives()-1; 01344 else return 0; } 01345 01346 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 01347 { if (!isApproxInUse) return operand.getDependsOnStage(order+1); 01348 else return operand.getDependsOnStage(order); } 01349 01350 01351 // We're not using the Measure_<T> base class cache services, but 01352 // we do have one of our own. It looks uncached from the base class 01353 // point of view which is why we're implementing it here. 01354 const T& getUncachedValueVirtual(const State& s, int derivOrder) const 01355 OVERRIDE_11 01356 { if (!isApproxInUse) 01357 return operand.getValue(s, derivOrder+1); 01358 01359 ensureDerivativeIsRealized(s); 01360 const Subsystem& subsys = this->getSubsystem(); 01361 const Result& result = Value<Result>::downcast 01362 (subsys.getDiscreteVarUpdateValue(s,resultIx)); 01363 return result.operandDot; // has a value but might not be a good one 01364 } 01365 01366 void initializeVirtual(State& s) const OVERRIDE_11 { 01367 if (!isApproxInUse) return; 01368 01369 assert(resultIx.isValid()); 01370 const Subsystem& subsys = this->getSubsystem(); 01371 Result& result = Value<Result>::updDowncast 01372 (subsys.updDiscreteVariable(s,resultIx)); 01373 this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage()); 01374 result.operand = operand.getValue(s); 01375 result.operandDot = this->getValueZero(); 01376 result.derivIsGood = false; 01377 } 01378 01379 void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 { 01380 isApproxInUse = (forceUseApprox || operand.getNumTimeDerivatives()==0); 01381 if (!isApproxInUse) 01382 return; 01383 01384 resultIx = this->getSubsystem() 01385 .allocateAutoUpdateDiscreteVariable(s, operand.getDependsOnStage(0), 01386 new Value<Result>(), operand.getDependsOnStage(0)); 01387 } 01388 01391 void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 { 01392 ensureDerivativeIsRealized(s); 01393 } 01394 01395 void ensureDerivativeIsRealized(const State& s) const { 01396 assert(resultIx.isValid()); 01397 const Subsystem& subsys = this->getSubsystem(); 01398 if (subsys.isDiscreteVarUpdateValueRealized(s,resultIx)) 01399 return; 01400 01401 const Real t0 = subsys.getDiscreteVarLastUpdateTime(s,resultIx); 01402 const Result& prevResult = Value<Result>::downcast 01403 (subsys.getDiscreteVariable(s,resultIx)); 01404 const T& f0 = prevResult.operand; 01405 const T& fdot0 = prevResult.operandDot; // may be invalid 01406 const bool good0 = prevResult.derivIsGood; 01407 01408 const Real t = s.getTime(); 01409 Result& result = Value<Result>::updDowncast 01410 (subsys.updDiscreteVarUpdateValue(s,resultIx)); 01411 T& f = result.operand; // renaming 01412 T& fdot = result.operandDot; 01413 bool& good = result.derivIsGood; 01414 01415 f = operand.getValue(s); 01416 good = false; 01417 if (!isFinite(t0)) 01418 fdot = this->getValueZero(); 01419 else if (t == t0) { 01420 fdot = fdot0; 01421 good = good0; 01422 } else { 01423 fdot = (f-f0)/(t-t0); // 1st order 01424 if (good0) 01425 fdot = Real(2)*fdot - fdot0; // now 2nd order 01426 good = true; // either 1st or 2nd order estimate 01427 } 01428 subsys.markDiscreteVarUpdateValueRealized(s,resultIx); 01429 } 01430 private: 01431 // TOPOLOGY STATE 01432 Measure_<T> operand; 01433 bool forceUseApprox; 01434 01435 // TOPOLOGY CACHE 01436 mutable bool isApproxInUse; 01437 mutable DiscreteVariableIndex resultIx; // auto-update 01438 }; 01439 01440 01441 01442 //============================================================================== 01443 // EXTREME :: IMPLEMENTATION 01444 //============================================================================== 01445 template <class T> 01446 class Measure_<T>::Extreme::Implementation : public Measure_<T>::Implementation 01447 { 01448 typedef typename Measure_<T>::Extreme Extreme; 01449 typedef typename Extreme::Operation Operation; 01450 public: 01453 Implementation() 01454 : Measure_<T>::Implementation(0), operation(Extreme::MaxAbs) {} 01455 01458 Implementation(const Measure_<T>& operand, Operation op) 01459 : Measure_<T>::Implementation(0), operand(operand), operation(op) {} 01460 01461 // Default copy constructor gives us a new Implementation object, 01462 // but with reference to the *same* operand measure. 01463 01467 void setOperandMeasure(const Measure_<T>& operand) { 01468 this->operand = operand; 01469 this->invalidateTopologyCache(); 01470 } 01471 01475 void setOperation(Operation op) { 01476 this->operation = op; 01477 this->invalidateTopologyCache(); 01478 } 01479 01481 const Measure_<T>& getOperandMeasure() const {return operand;} 01482 01485 Operation getOperation() const {return operation;} 01486 01489 void setValue(State& s, const T& value) const { 01490 assert(extremeIx.isValid()); 01491 const Subsystem& subsys = this->getSubsystem(); 01492 T& prevMin = Value<T>::updDowncast 01493 (subsys.updDiscreteVariable(s,extremeIx)); 01494 prevMin = value; 01495 } 01496 01500 Real getTimeOfExtremeValue(const State& s) const { 01501 const Subsystem& subsys = this->getSubsystem(); 01502 const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s); 01503 Real tUpdate; 01504 if (hasNewExtreme) 01505 tUpdate = s.getTime(); // i.e., now 01506 else 01507 tUpdate = subsys.getDiscreteVarLastUpdateTime(s,extremeIx); 01508 return tUpdate; 01509 } 01510 01511 // Implementations of virtual methods. 01512 01513 // This uses the default copy constructor. 01514 Implementation* cloneVirtual() const OVERRIDE_11 01515 { return new Implementation(*this); } 01516 01519 int getNumTimeDerivativesVirtual() const OVERRIDE_11 01520 { return operand.getNumTimeDerivatives(); } 01521 01524 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 01525 { return operand.getDependsOnStage(order); } 01526 01527 01531 const T& getUncachedValueVirtual(const State& s, int derivOrder) const 01532 OVERRIDE_11 01533 { 01534 const Subsystem& subsys = this->getSubsystem(); 01535 const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s); 01536 if (derivOrder > 0) { 01537 // TODO: should be handled elementwise and zero unless the 01538 // derivative is acting in the direction that changes the 01539 // extreme. 01540 return hasNewExtreme ? operand.getValue(s, derivOrder) 01541 : this->getValueZero(); 01542 } 01543 if (hasNewExtreme) { 01544 const T& newExt = Value<T>::downcast 01545 (subsys.getDiscreteVarUpdateValue(s,extremeIx)); 01546 return newExt; 01547 } else { 01548 const T& currentExt = Value<T>::downcast 01549 (subsys.getDiscreteVariable(s,extremeIx)); 01550 return currentExt; 01551 } 01552 } 01553 01556 void initializeVirtual(State& s) const OVERRIDE_11 { 01557 this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage()); 01558 setValue(s, operand.getValue(s)); 01559 } 01560 01568 void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 { 01569 // TODO: this should be NaN once initialization is working properly. 01570 T initVal = this->getDefaultValue(); 01571 switch(operation) { 01572 case Minimum: initVal = Infinity; break; 01573 case Maximum: initVal = -Infinity; break; 01574 case MinAbs: initVal = Infinity; break; 01575 case MaxAbs: initVal = 0; break; 01576 }; 01577 01578 extremeIx = this->getSubsystem() 01579 .allocateAutoUpdateDiscreteVariable(s, Stage::Dynamics, 01580 new Value<T>(initVal), operand.getDependsOnStage(0)); 01581 01582 isNewExtremeIx = this->getSubsystem() 01583 .allocateAutoUpdateDiscreteVariable(s, Stage::Dynamics, 01584 new Value<bool>(false), operand.getDependsOnStage(0)); 01585 } 01586 01589 void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 { 01590 ensureExtremeHasBeenUpdated(s); 01591 } 01592 01598 bool ensureExtremeHasBeenUpdated(const State& s) const { 01599 assert(extremeIx.isValid() && isNewExtremeIx.isValid()); 01600 const Subsystem& subsys = this->getSubsystem(); 01601 01602 // We may have already determined whether we're at a new extreme in 01603 // which case we don't need to do it again. 01604 if (subsys.isDiscreteVarUpdateValueRealized(s, isNewExtremeIx)) 01605 return Value<bool>::downcast 01606 (subsys.getDiscreteVarUpdateValue(s,isNewExtremeIx)); 01607 01608 // We're going to have to decide if we're at a new extreme, and if 01609 // so record the new extreme value in the auto-update cache entry of 01610 // the extreme value state variable. 01611 01612 // Get the previous extreme value and the current operand value. 01613 const T& prevExtreme = Value<T>::downcast 01614 (subsys.getDiscreteVariable(s,extremeIx)); 01615 const T& currentVal = operand.getValue(s); 01616 01617 // Search to see if any element has reached a new extreme. 01618 bool foundNewExt = false; 01619 for (int i=0; i < this->size() && !foundNewExt; ++i) 01620 foundNewExt = isNewExtreme(Measure_Num<T>::get(currentVal,i), 01621 Measure_Num<T>::get(prevExtreme,i)); 01622 01623 // Record the result and mark the auto-update cache entry valid 01624 // so we won't have to recalculate. When the integrator advances to the 01625 // next step this cache entry will be swapped with the corresponding 01626 // state and marked invalid so we'll be sure to recalculate each step. 01627 Value<bool>::updDowncast 01628 (subsys.updDiscreteVarUpdateValue(s,isNewExtremeIx)) = foundNewExt; 01629 subsys.markDiscreteVarUpdateValueRealized(s,isNewExtremeIx); 01630 01631 // Don't update the auto-update cache entry if we didn't see a new 01632 // extreme. That way no auto-update will occur and the state variable 01633 // will remain unchanged with the existing update time preserved. 01634 if (!foundNewExt) 01635 return false; 01636 01637 // We have encountered a new extreme. We'll record the new extreme 01638 // in the auto-update cache entry which will be used as the current 01639 // result until the integrator advances to the next step at which time 01640 // this will be swapped with the state variable to serve as the previous 01641 // extreme value until a further extreme is encountered. 01642 T& newExtreme = Value<T>::updDowncast 01643 (subsys.updDiscreteVarUpdateValue(s,extremeIx)); 01644 01645 for (int i=0; i < this->size(); ++i) 01646 Measure_Num<T>::upd(newExtreme,i) = 01647 extremeOf(Measure_Num<T>::get(currentVal,i), 01648 Measure_Num<T>::get(prevExtreme,i)); 01649 01650 // Marking this valid is what ensures that an auto-update occurs later. 01651 subsys.markDiscreteVarUpdateValueRealized(s,extremeIx); 01652 return true; 01653 } 01654 private: 01655 // Return true if newVal is "more extreme" than oldExtreme, according 01656 // to the operation we're performing. 01657 bool isNewExtreme(const typename Measure_Num<T>::Element& newVal, 01658 const typename Measure_Num<T>::Element& oldExtreme) const 01659 { 01660 switch (operation) { 01661 case Extreme::Maximum: return newVal > oldExtreme; 01662 case Extreme::Minimum: return newVal < oldExtreme; 01663 case Extreme::MaxAbs: return std::abs(newVal) > std::abs(oldExtreme); 01664 case Extreme::MinAbs: return std::abs(newVal) < std::abs(oldExtreme); 01665 }; 01666 SimTK_ASSERT1_ALWAYS(!"recognized", 01667 "Measure::Extreme::Implementation::isNewExtreme(): " 01668 "unrecognized operation %d", (int)operation); 01669 return false; /*NOTREACHED*/ 01670 } 01671 01672 // Given the value of one element of the operand, and that value's time 01673 // derivative, determine whether the derivative is moving the element 01674 // into the "more extreme" direction, according to the operation. 01675 bool isExtremeDir(const typename Measure_Num<T>::Element& value, 01676 const typename Measure_Num<T>::Element& deriv) const 01677 { 01678 const int sv = sign(value), sd = sign(deriv); 01679 if (sd == 0) return false; // derivative is zero; not changing 01680 switch (operation) { 01681 case Extreme::Maximum: return sd == 1; // getting larger 01682 case Extreme::Minimum: return sd == -1; // getting smaller 01683 case Extreme::MaxAbs: return sv==0 || sd==sv; // abs is growing 01684 case Extreme::MinAbs: return sd == -sv; 01685 }; 01686 SimTK_ASSERT1_ALWAYS(!"recognized", 01687 "Measure::Extreme::Implementation::isExtremeDir(): " 01688 "unrecognized operation %d", (int)operation); 01689 return false; /*NOTREACHED*/ 01690 } 01691 01692 typename Measure_Num<T>::Element 01693 extremeOf(const typename Measure_Num<T>::Element& newVal, 01694 const typename Measure_Num<T>::Element& oldExtreme) const 01695 { 01696 return isNewExtreme(newVal,oldExtreme) ? newVal : oldExtreme; 01697 } 01698 01699 // TOPOLOGY STATE 01700 Measure_<T> operand; 01701 Operation operation; 01702 01703 // TOPOLOGY CACHE 01704 mutable DiscreteVariableIndex extremeIx; // extreme so far; auto-update 01705 01706 // This auto-update flag records whether the current value is a new 01707 // extreme. We don't really need to save it as a state variable since you 01708 // can figure this out from the timestamp, but we need to to get invalidated 01709 // by the auto-update swap so that we'll figure it out anew each step. 01710 mutable DiscreteVariableIndex isNewExtremeIx; 01711 }; 01712 01713 01714 01715 //============================================================================== 01716 // DELAY :: IMPLEMENTATION 01717 //============================================================================== // Hide from Doxygen. 01719 // This helper class is the contents of the discrete state variable and 01720 // corresponding cache entry maintained by this measure. The variable is 01721 // auto-update, meaning the value of the cache entry replaces the state 01722 // variable at the start of each step. 01723 // 01724 // Circular buffers look like this: 01725 // 01726 // oldest=0, n=0 01727 // v 01728 // Empty buffer: | available | 01729 // 01730 // By convention, oldest=0 whenever the buffer is empty. 01731 // 01732 // 01733 // oldest next=(oldest+n)%capacity 01734 // v v 01735 // | available | | | | | | | available | 01736 // ^ n=6 ^ 01737 // 0 capacity 01738 // v v 01739 // or | | | | | | available | | | | | | | | n=12 01740 // ^ ^ 01741 // next oldest 01742 // = (oldest+n)%capacity 01743 // 01744 // Number of entries = n (called size() below) 01745 // Empty = n==0 01746 // Full = n==capacity() 01747 // Next available = (oldest+n)%capacity() 01748 template <class T> 01749 class Measure_Delay_Buffer { 01750 public: 01751 explicit Measure_Delay_Buffer() {initDataMembers();} 01752 void clear() {initDataMembers();} 01753 int size() const {return m_size;} // # saved entries, *not* size of arrays 01754 int capacity() const {return m_times.size();} 01755 bool empty() const {return size()==0;} 01756 bool full() const {return size()==capacity();} 01757 01758 double getEntryTime(int i) const 01759 { assert(i < size()); return m_times[getArrayIndex(i)];} 01760 const T& getEntryValue(int i) const 01761 { assert(i < size()); return m_values[getArrayIndex(i)];} 01762 01763 enum { 01764 InitialAllocation = 8, // smallest allocation 01765 GrowthFactor = 2, // how fast to grow (double) 01766 MaxShrinkProofSize = 16, // won't shrink unless bigger 01767 TooBigFactor = 5 // 5X too much->maybe shrink 01768 }; 01769 01770 // Add a new entry to the end of the list, throwing out old entries that 01771 // aren't needed to answer requests at tEarliest or later. 01772 void append(double tEarliest, double tNow, const T& valueNow) { 01773 forgetEntriesMuchOlderThan(tEarliest); 01774 removeEntriesLaterOrEq(tNow); 01775 if (full()) 01776 makeMoreRoom(); 01777 else if (capacity() > std::max((int)MaxShrinkProofSize, 01778 (int)TooBigFactor * (size()+1))) 01779 makeLessRoom(); // less than 1/TooBigFactor full 01780 const int nextFree = getArrayIndex(m_size++); 01781 m_times[nextFree] = tNow; 01782 m_values[nextFree] = valueNow; 01783 m_maxSize = std::max(m_maxSize, size()); 01784 } 01785 01786 // Prepend an older entry to the beginning of the list. No cleanup is done. 01787 void prepend(double tNewOldest, const T& value) { 01788 assert(empty() || tNewOldest < m_times[m_oldest]); 01789 if (full()) makeMoreRoom(); 01790 m_oldest = empty() ? 0 : getArrayIndex(-1); 01791 m_times[m_oldest] = tNewOldest; 01792 m_values[m_oldest] = value; 01793 ++m_size; 01794 m_maxSize = std::max(m_maxSize, size()); 01795 } 01796 01797 // This is a specialized copy assignment for copying an old buffer 01798 // to a new one with updated contents. We are told the earliest time we'll 01799 // be asked about from now on, and won't copy any entries older than those 01800 // needed to answer that earliest request. We won't copy anything at or 01801 // newer than tNow, and finally we'll push (tNow,valueNow) as the newest 01802 // entry. 01803 void copyInAndUpdate(const Measure_Delay_Buffer& oldBuf, double tEarliest, 01804 double tNow, const T& valueNow) { 01805 // clear all current entries (no heap activity) 01806 m_oldest = m_size = 0; 01807 01808 // determine how may old entries we have to keep 01809 int firstNeeded = oldBuf.countNumUnneededOldEntries(tEarliest); 01810 int lastNeeded = oldBuf.findLastEarlier(tNow); // might be -1 01811 int numOldEntriesToKeep = lastNeeded-firstNeeded+1; 01812 int newSize = numOldEntriesToKeep+1; // includes the new one 01813 01814 int newSizeRequest = -1; 01815 if (capacity() < newSize) { 01816 newSizeRequest = std::max((int)InitialAllocation, 01817 (int)GrowthFactor * newSize); 01818 ++m_nGrows; 01819 } else if (capacity() > std::max((int)MaxShrinkProofSize, 01820 (int)TooBigFactor * newSize)) { 01821 newSizeRequest = std::max((int)MaxShrinkProofSize, 01822 (int)GrowthFactor * newSize); 01823 ++m_nShrinks; 01824 } 01825 01826 // Reallocate space if advisable. 01827 if (newSizeRequest != -1) { 01828 const double dNaN = NTraits<double>::getNaN(); 01829 m_values.resize(newSizeRequest); 01830 if (m_values.capacity() > m_values.size()) 01831 m_values.resize(m_values.capacity()); // don't waste any 01832 m_times.resize(m_values.size(), dNaN); 01833 } 01834 01835 m_maxCapacity = std::max(m_maxCapacity, capacity()); 01836 01837 // Copy the entries we need to keep. 01838 int nxt = 0; 01839 for (int i=firstNeeded; i<=lastNeeded; ++i, ++nxt) { 01840 m_times[nxt] = oldBuf.getEntryTime(i); 01841 m_values[nxt] = oldBuf.getEntryValue(i); 01842 } 01843 // Now add the newest entry and set the size. 01844 m_times[nxt] = tNow; 01845 m_values[nxt] = valueNow; 01846 assert(nxt+1==newSize); 01847 m_size = nxt+1; 01848 m_maxSize = std::max(m_maxSize, size()); 01849 } 01850 01851 // Given the current time and value and the earlier time at which the 01852 // value is needed, use the buffer and (if necessary) the current value 01853 // to estimate the delayed value. 01854 T calcValueAtTime(double tDelay, double tNow, const T& valueNow) const; 01855 01856 // Given the current time but *not* the current value of the source measure, 01857 // provide an estimate for the value at tDelay=tNow-delay using only the 01858 // buffer contents and linear interpolation or extrapolation. 01859 void calcValueAtTimeLinearOnly(double tDelay, T& delayedValue) const { 01860 if (empty()) { 01861 // Nothing in the buffer?? Shouldn't happen. Return empty Vector 01862 // or NaN for fixed-size types. 01863 Measure_Num<T>::makeNaNLike(T(), delayedValue); 01864 return; 01865 } 01866 01867 int firstLater = findFirstLaterOrEq(tDelay); 01868 01869 if (firstLater > 0) { 01870 // Normal case: tDelay is between two buffer entries. 01871 int firstEarlier = firstLater-1; 01872 double t0=getEntryTime(firstEarlier), t1=getEntryTime(firstLater); 01873 const T& v0=getEntryValue(firstEarlier); 01874 const T& v1=getEntryValue(firstLater); 01875 Real fraction = Real((tDelay-t0)/(t1-t0)); 01876 delayedValue = T(v0 + fraction*(v1-v0)); 01877 return; 01878 } 01879 01880 if (firstLater==0) { 01881 // Startup case: tDelay is at or before the oldest buffer entry. 01882 // Assume the value was flat before that. 01883 delayedValue = getEntryValue(firstLater); 01884 return; 01885 } 01886 01887 // tDelay is later than the latest entry in the buffer. We are going 01888 // to have to extrapolate (yuck). 01889 01890 if (size() == 1) { 01891 // Just one entry; we'll have to assume the value is flat. 01892 delayedValue = getEntryValue(0); 01893 return; 01894 } 01895 01896 // Extrapolate using the last two entries. 01897 double t0=getEntryTime(size()-2), t1=getEntryTime(size()-1); 01898 const T& v0=getEntryValue(size()-2); 01899 const T& v1=getEntryValue(size()-1); 01900 Real fraction = Real((tDelay-t0)/(t1-t0)); // > 1 01901 assert(fraction > 1.0); 01902 delayedValue = T(v0 + fraction*(v1-v0)); // Extrapolate. 01903 } 01904 01905 // Return the number of times we had to grow the buffer. 01906 int getNumGrows() const {return m_nGrows;} 01907 // Return the number of times we decided the buffer was so overallocated 01908 // that we had to shrink it. 01909 int getNumShrinks() const {return m_nShrinks;} 01910 // Return the largest number of values we ever had in the buffer. 01911 int getMaxSize() const {return m_maxSize;} 01912 // Return the largest capacity the buffer ever had. 01913 int getMaxCapacity() const {return m_maxCapacity;} 01914 01915 private: 01916 // Return the i'th oldest entry 01917 // (0 -> oldest, size-1 -> newest, size -> first free, -1 -> last free) 01918 int getArrayIndex(int i) const 01919 { assert(-1<=i && i<=size()); 01920 const int rawIndex = m_oldest + i; 01921 if (rawIndex < 0) return rawIndex + capacity(); 01922 else return rawIndex % capacity(); } 01923 01924 // Remove all but two entries older than the given time. 01925 void forgetEntriesMuchOlderThan(double tEarliest) { 01926 const int numToRemove = countNumUnneededOldEntries(tEarliest); 01927 if (numToRemove) { 01928 m_oldest = getArrayIndex(numToRemove); 01929 m_size -= numToRemove; 01930 } 01931 } 01932 01933 // Count up how many old entries at the beginning of the buffer are so old 01934 // that they wouldn't be needed to respond to a request at time tEarliest or 01935 // later. We'll keep no more than two entries earlier than tEarliest. 01936 int countNumUnneededOldEntries(double tEarliest) const { 01937 const int firstLater = findFirstLaterOrEq(tEarliest); 01938 return std::max(0, firstLater-2); 01939 } 01940 01941 // Given the time now, delete anything at the end of the queue that is 01942 // at that same time or later. 01943 void removeEntriesLaterOrEq(double t) { 01944 int lastEarlier = findLastEarlier(t); 01945 m_size = lastEarlier+1; 01946 if (m_size==0) m_oldest=0; // restart at beginning of array 01947 } 01948 01949 // Return the entry number (0..size-1) of the first entry whose time 01950 // is >= the given time, or -1 if there is none such. 01951 int findFirstLaterOrEq(double tDelay) const { 01952 for (int i=0; i < size(); ++i) 01953 if (getEntryTime(i) >= tDelay) 01954 return i; 01955 return -1; 01956 } 01957 01958 // Return the entry number(size-1..0) of the last entry whose time 01959 // is < the given time, or -1 if there is none such. 01960 int findLastEarlier(double t) const { 01961 for (int i=size()-1; i>=0; --i) 01962 if (getEntryTime(i) < t) 01963 return i; 01964 return -1; 01965 } 01966 01967 // We don't have enough space. This is either the initial allocation or 01968 // we need to double the current space. 01969 void makeMoreRoom() { 01970 const int newSizeRequest = std::max((int)InitialAllocation, 01971 (int)GrowthFactor * size()); 01972 resize(newSizeRequest); 01973 ++m_nGrows; 01974 m_maxCapacity = std::max(m_maxCapacity, capacity()); 01975 } 01976 01977 // We are wasting a lot of space, reduce the heap allocation to just 01978 // double what we're using now. 01979 void makeLessRoom() { 01980 const int targetMaxSize = std::max((int)MaxShrinkProofSize, 01981 (int)GrowthFactor * size()); 01982 if (capacity() > targetMaxSize) { 01983 resize(targetMaxSize); 01984 ++m_nShrinks; 01985 } 01986 } 01987 01988 // Reallocate memory to get more space or stop wasting space. The new 01989 // size request must be big enough to hold all the current contents. The 01990 // amount we actually get may be somewhat larger than the request. On 01991 // return, the times and values arrays will have been resized and the 01992 // oldest entry will now be entry 0. 01993 void resize(int newSizeRequest) { 01994 assert(newSizeRequest >= size()); 01995 const double dNaN = NTraits<double>::getNaN(); 01996 Array_<T,int> newValues(newSizeRequest); 01997 if (newValues.capacity() > newValues.size()) 01998 newValues.resize(newValues.capacity()); // don't waste any 01999 Array_<double,int> newTimes(newValues.size(), dNaN); 02000 02001 // Pack existing values into start of new arrays. 02002 for (int i=0; i < size(); ++i) { 02003 const int ix = getArrayIndex(i); 02004 newTimes[i] = m_times[ix]; 02005 newValues[i] = m_values[ix]; 02006 } 02007 m_times.swap(newTimes); // switch heap space 02008 m_values.swap(newValues); 02009 m_oldest = 0; // starts at the beginning now; size unchanged 02010 } 02011 02012 // Initialize everything to its default-constructed state. 02013 void initDataMembers() { 02014 m_times.clear(); m_values.clear(); 02015 m_oldest=m_size=0; 02016 m_nGrows=m_nShrinks=m_maxSize=m_maxCapacity=0; 02017 } 02018 02019 // These are circular buffers of the same size. 02020 Array_<double,int> m_times; 02021 Array_<T,int> m_values; 02022 int m_oldest; // Array index of oldest (time,value) 02023 int m_size; // number of entries in use 02024 02025 // Statistics. 02026 int m_nGrows, m_nShrinks, m_maxSize, m_maxCapacity; 02027 }; 02030 template <class T> 02031 class Measure_<T>::Delay::Implementation: public Measure_<T>::Implementation { 02032 typedef Measure_Delay_Buffer<T> Buffer; 02033 public: 02034 // Allocate one cache entry in the base class for the value; we allocate 02035 // a specialized one for the buffer. 02036 Implementation() 02037 : Measure_<T>::Implementation(1), m_delay(NaN), 02038 m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {} 02039 02040 Implementation(const Measure_<T>& source, Real delay) 02041 : Measure_<T>::Implementation(1), m_source(source), m_delay(delay), 02042 m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {} 02043 02044 // Default copy constructor gives us a new Implementation object, 02045 // but with reference to the *same* source measure. 02046 02047 void setSourceMeasure(const Measure_<T>& source) { 02048 if (!source.isSameMeasure(this->m_source)) { 02049 this->m_source = source; 02050 this->invalidateTopologyCache(); 02051 } 02052 } 02053 02054 void setDelay(Real delay) { 02055 if (delay != this->m_delay) { 02056 this->m_delay = delay; 02057 this->invalidateTopologyCache(); 02058 } 02059 } 02060 02061 void setUseLinearInterpolationOnly(bool linearOnly) { 02062 if (linearOnly != this->m_useLinearInterpolationOnly) { 02063 this->m_useLinearInterpolationOnly = linearOnly; 02064 this->invalidateTopologyCache(); 02065 } 02066 } 02067 02068 void setCanUseCurrentValue(bool canUseCurrentValue) { 02069 if (canUseCurrentValue != this->m_canUseCurrentValue) { 02070 this->m_canUseCurrentValue = canUseCurrentValue; 02071 this->invalidateTopologyCache(); 02072 } 02073 } 02074 02075 const Measure_<T>& getSourceMeasure() const {return this->m_source;} 02076 Real getDelay() const {return this->m_delay;} 02077 bool getUseLinearInterpolationOnly() const 02078 { return this->m_useLinearInterpolationOnly; } 02079 bool getCanUseCurrentValue() const 02080 { return this->m_canUseCurrentValue; } 02081 02082 02083 // Implementations of virtual methods. 02084 02085 // This uses the default copy constructor. 02086 Implementation* cloneVirtual() const OVERRIDE_11 02087 { return new Implementation(*this); } 02088 02089 // Currently no derivative supported. 02090 int getNumTimeDerivativesVirtual() const OVERRIDE_11 02091 { return 0; } 02092 02093 // If we are allowed to use the current value of the source measure to 02094 // determine the delayed value, the depends-on stage here is the same as 02095 // for the source; otherwise it is Stage::Time. 02096 Stage getDependsOnStageVirtual(int order) const OVERRIDE_11 02097 { return this->m_canUseCurrentValue ? m_source.getDependsOnStage(order) 02098 : Stage::Time; } 02099 02100 // Calculate the delayed value and return it to the Measure base class to 02101 // be put in a cache entry. 02102 void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const 02103 OVERRIDE_11 02104 { const Subsystem& subsys = this->getSubsystem(); 02105 const Buffer& buffer = Value<Buffer>::downcast 02106 (subsys.getDiscreteVariable(s,m_bufferIx)); 02107 //TODO: use cubic interpolation if allowed 02108 buffer.calcValueAtTimeLinearOnly(s.getTime()-m_delay, value); 02109 } 02110 02111 void initializeVirtual(State& s) const OVERRIDE_11 { 02112 assert(m_bufferIx.isValid()); 02113 const Subsystem& subsys = this->getSubsystem(); 02114 Buffer& buffer = Value<Buffer>::updDowncast 02115 (subsys.updDiscreteVariable(s,m_bufferIx)); 02116 buffer.clear(); 02117 this->getSubsystem().getSystem().realize(s,m_source.getDependsOnStage()); 02118 buffer.append(s.getTime()-m_delay, s.getTime(), m_source.getValue(s)); 02119 } 02120 02121 void realizeMeasureTopologyVirtual(State& s) const OVERRIDE_11 { 02122 m_bufferIx = this->getSubsystem() 02123 .allocateAutoUpdateDiscreteVariable(s, Stage::Report, 02124 new Value<Buffer>(), getDependsOnStageVirtual(0)); 02125 } 02126 02129 void realizeMeasureAccelerationVirtual(const State& s) const OVERRIDE_11 { 02130 updateBuffer(s); 02131 } 02132 02133 // This uses the buffer from the state to update the one in the 02134 // corresponding cache entry. The update adds the current value of the 02135 // source to the end of the buffer and tosses out unneeded old entries. 02136 void updateBuffer(const State& s) const { 02137 assert(m_bufferIx.isValid()); 02138 const Subsystem& subsys = this->getSubsystem(); 02139 02140 const Buffer& prevBuffer = Value<Buffer>::downcast 02141 (subsys.getDiscreteVariable(s,m_bufferIx)); 02142 02143 Buffer& nextBuffer = Value<Buffer>::updDowncast 02144 (subsys.updDiscreteVarUpdateValue(s,m_bufferIx)); 02145 02146 const Real t = s.getTime(); 02147 nextBuffer.copyInAndUpdate(prevBuffer, t-m_delay, 02148 t, m_source.getValue(s)); 02149 02150 subsys.markDiscreteVarUpdateValueRealized(s,m_bufferIx); 02151 } 02152 private: 02153 // TOPOLOGY STATE 02154 Measure_<T> m_source; 02155 Real m_delay; 02156 bool m_canUseCurrentValue; 02157 bool m_useLinearInterpolationOnly; 02158 02159 // TOPOLOGY CACHE 02160 mutable DiscreteVariableIndex m_bufferIx; // auto-update 02161 }; 02162 02163 02164 } // namespace SimTK 02165 02166 02167 02168 02169 #endif // SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_