Simbody  3.5
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  *                       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 = &sub; 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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines