Simbody  3.5
MeasureImplementation.h
Go to the documentation of this file.
1 #ifndef SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
2 #define SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): SimTKcommon *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2008-13 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKcommon/basics.h"
28 #include "SimTKcommon/Simmatrix.h"
34 
35 #include <cmath>
36 
37 
38 namespace SimTK {
39 
40 
41 //==============================================================================
42 // ABSTRACT MEASURE :: IMPLEMENTATION
43 //==============================================================================
44 
49 protected:
52  Implementation() : copyNumber(0), mySubsystem(0), refCount(0) {}
53 
58  : copyNumber(src.copyNumber+1), mySubsystem(0), refCount(0) {}
59 
64  if (&src != this)
65  { copyNumber=src.copyNumber+1;
66  refCount=0; mySubsystem=0; }
67  return *this;
68  }
69 
70  // destructor is virtual; below
71 
72  // Increment the reference count and return its new value.
73  int incrRefCount() const {return ++refCount;}
74 
75  // Decrement the reference count and return its new value.
76  int decrRefCount() const {return --refCount;}
77 
78  // Get the current value of the reference counter.
79  int getRefCount() const {return refCount;}
80 
81  int getCopyNumber() const {return copyNumber;}
82 
86  Implementation* clone() const {return cloneVirtual();}
87 
88  // realizeTopology() is pure virtual below for Measure_<T> to supply.
89  void realizeModel (State& s) const {realizeMeasureModelVirtual(s);}
90  void realizeInstance (const State& s) const {realizeMeasureInstanceVirtual(s);}
91  void realizeTime (const State& s) const {realizeMeasureTimeVirtual(s);}
92  void realizePosition (const State& s) const {realizeMeasurePositionVirtual(s);}
93  void realizeVelocity (const State& s) const {realizeMeasureVelocityVirtual(s);}
94  void realizeDynamics (const State& s) const {realizeMeasureDynamicsVirtual(s);}
95  void realizeAcceleration(const State& s) const {realizeMeasureAccelerationVirtual(s);}
96  void realizeReport (const State& s) const {realizeMeasureReportVirtual(s);}
97 
101  void initialize(State& s) const {initializeVirtual(s);}
102 
103  int getNumTimeDerivatives() const {return getNumTimeDerivativesVirtual();}
104 
105  Stage getDependsOnStage(int derivOrder) const {
106  SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(),
107  "Measure::getDependsOnStage()",
108  "derivOrder %d was out of range; this Measure allows 0-%d.",
109  derivOrder, getNumTimeDerivatives());
110  return getDependsOnStageVirtual(derivOrder);
111  }
112 
113 
114  void setSubsystem(Subsystem& sub, MeasureIndex mx)
115  { assert(!mySubsystem && mx.isValid());
116  mySubsystem = &sub; myIndex = mx; }
117 
118  bool isInSubsystem() const {return mySubsystem != 0;}
119  const Subsystem& getSubsystem() const {assert(mySubsystem); return *mySubsystem;}
120  Subsystem& updSubsystem() {assert(mySubsystem); return *mySubsystem;}
121  MeasureIndex getSubsystemMeasureIndex() const {assert(mySubsystem); return myIndex;}
123  { return getSubsystem().getMySubsystemIndex(); }
124 
126  { if (isInSubsystem()) getSubsystem().invalidateSubsystemTopologyCache(); }
127 
128  Stage getStage(const State& s) const {return getSubsystem().getStage(s);}
129 
130  // VIRTUALS //
131  // Ordinals must retain the same meaning from release to release
132  // to preserve binary compatibility.
133 
134  /* 0*/virtual ~Implementation() {}
135  /* 1*/virtual Implementation* cloneVirtual() const = 0;
136 
137  /* 2*/virtual void realizeTopology(State&)const = 0;
138 
139  /* 3*/virtual void realizeMeasureModelVirtual(State&) const {}
140  /* 4*/virtual void realizeMeasureInstanceVirtual(const State&) const {}
141  /* 5*/virtual void realizeMeasureTimeVirtual(const State&) const {}
142  /* 6*/virtual void realizeMeasurePositionVirtual(const State&) const {}
143  /* 7*/virtual void realizeMeasureVelocityVirtual(const State&) const {}
144  /* 8*/virtual void realizeMeasureDynamicsVirtual(const State&) const {}
145  /* 9*/virtual void realizeMeasureAccelerationVirtual(const State&) const {}
146  /*10*/virtual void realizeMeasureReportVirtual(const State&) const {}
147 
148  /*11*/virtual void initializeVirtual(State&) const {}
149  /*12*/virtual int
150  getNumTimeDerivativesVirtual() const {return 0;}
151  /*13*/virtual Stage
152  getDependsOnStageVirtual(int order) const = 0;
153 
154 private:
155  int copyNumber; // bumped each time we do a deep copy
156 
157  // These are set when this Measure is adopted by a Subsystem.
158  Subsystem* mySubsystem;
159  MeasureIndex myIndex;
160 
161  // Measures have shallow copy semantics so they share the Implementation
162  // objects, which are only deleted when the refCount goes to zero.
163  mutable int refCount;
164 
165 friend class AbstractMeasure;
166 friend class Subsystem::Guts;
167 };
168 
169 //==============================================================================
170 // ABSTRACT MEASURE DEFINITIONS
171 //==============================================================================
172 // These had to wait for AbstractMeasure::Implementation to be defined.
173 
174 inline AbstractMeasure::
176 : impl(g)
177 { if (impl) impl->incrRefCount(); }
178 
179 inline AbstractMeasure::
181 : impl(g) {
182  SimTK_ERRCHK(hasImpl(), "AbstractMeasure::AbstractMeasure()",
183  "An empty Measure handle can't be put in a Subsystem.");
184  impl->incrRefCount();
185  sub.adoptMeasure(*this);
186 }
187 
188 // Shallow copy constructor.
190 : impl(0) {
191  if (src.impl) {
192  impl = src.impl;
193  impl->incrRefCount();
194  }
195 }
196 
197 // Shallow assignment.
200  if (impl != src.impl) {
201  if (impl && impl->decrRefCount()==0) delete impl;
202  impl = src.impl;
203  impl->incrRefCount();
204  }
205  return *this;
206 }
207 
208 // Note that even if the source and destination are currently pointing
209 // to the same Implementation, we still have to make a new copy so that
210 // afterwards the destination has its own, refcount==1 copy.
213  if (&src != this) {
214  if (impl && impl->decrRefCount()==0) delete impl;
215  if (src.impl) {
216  impl = src.impl->clone();
217  impl->incrRefCount();
218  } else
219  impl = 0;
220  }
221  return *this;
222 }
223 
224 inline AbstractMeasure::
226 { if (impl && impl->decrRefCount()==0) delete impl;}
227 
228 inline bool AbstractMeasure::
230 { return hasImpl() && getImpl().isInSubsystem(); }
231 
232 inline const Subsystem& AbstractMeasure::
234 { return getImpl().getSubsystem(); }
235 
236 inline MeasureIndex AbstractMeasure::
238 { return getImpl().getSubsystemMeasureIndex();}
239 
240 inline int AbstractMeasure::
242 { return getImpl().getNumTimeDerivatives(); }
243 
245 getDependsOnStage(int derivOrder) const
246 { return getImpl().getDependsOnStage(derivOrder); }
247 
248 inline int AbstractMeasure::
249 getRefCount() const
250 { return getImpl().getRefCount(); }
251 
252  // Hide from Doxygen.
254 // This is a helper class that makes it possible to treat Real, Vec, and
255 // Vector objects uniformly.
256 template <class T> class Measure_Num {
257 };
258 
259 template <> class Measure_Num<float> {
260 public:
261  typedef float Element;
262  static int size(const float&) {return 1;}
263  static const float& get(const float& v, int i) {assert(i==0); return v;}
264  static float& upd(float& v, int i) {assert(i==0); return v;}
265  static void makeNaNLike(const float&, float& nanValue)
266  { nanValue = CNT<float>::getNaN();}
267  static void makeZeroLike(const float&, float& zeroValue) {zeroValue=0.f;}
268 };
269 
270 template <> class Measure_Num<double> {
271 public:
272  typedef double Element;
273  static int size(const double&) {return 1;}
274  static const double& get(const double& v, int i) {assert(i==0); return v;}
275  static double& upd(double& v, int i) {assert(i==0); return v;}
276  static void makeNaNLike(const double&, double& nanValue)
277  { nanValue = CNT<double>::getNaN(); }
278  static void makeZeroLike(const double&, double& zeroValue) {zeroValue=0.;}
279 };
280 
281 // We only support stride 1 (densely packed) Vec types.
282 template <int M, class E>
283 class Measure_Num< Vec<M,E,1> > {
284  typedef Vec<M,E,1> T;
285 public:
286  typedef E Element;
287  static int size(const T&) {return M;}
288  static const E& get(const T& v, int i) {return v[i];}
289  static E& upd(T& v, int i) {return v[i];}
290  static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();}
291  static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();}
292 };
293 
294 // We only support column major (densely packed) Mat types.
295 template <int M, int N, class E>
296 class Measure_Num< Mat<M,N,E> > {
297  typedef Mat<M,N,E> T;
298 public:
299  typedef E Element;
300  static int size(const T&) {return N;} // number of columns
301  static const typename T::TCol& get(const T& m, int j) {return m.col(j);}
302  static typename T::TCol& upd(T& m, int j) {return m.col(j);}
303  static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();}
304  static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();}
305 };
306 
307 
308 template <class E>
309 class Measure_Num< Vector_<E> > {
310  typedef Vector_<E> T;
311 public:
312  typedef E Element;
313  static int size(const T& v) {return v.size();}
314  static const E& get(const T& v, int i) {return v[i];}
315  static E& upd(T& v, int i) {return v[i];}
316  static void makeNaNLike(const T& v, T& nanValue)
317  { nanValue.resize(v.size()); nanValue.setToNaN(); }
318  static void makeZeroLike(const T& v, T& zeroValue)
319  { zeroValue.resize(v.size()); zeroValue.setToZero(); }
320 };
321 
322 
323 template <class E>
324 class Measure_Num< Rotation_<E> > {
325  typedef Rotation_<E> T;
326 public:
327  typedef T Element;
328  static int size(const T&) {return 1;}
329  static const T& get(const T& v, int i) {assert(i==0); return v;}
330  static T& upd(T& v, int i) {assert(i==0); return v;}
331  static void makeNaNLike(const T&, T& nanValue)
332  { nanValue.setRotationToNaN(); }
333  static void makeZeroLike(const T&, T& zeroValue)
334  { zeroValue.setRotationToIdentityMatrix(); }
335 };
336 
337 template <class E>
338 class Measure_Num< Transform_<E> > {
339  typedef Transform_<E> T;
340 public:
341  typedef T Element;
342  static int size(const T&) {return 1;}
343  static const T& get(const T& v, int i) {assert(i==0); return v;}
344  static T& upd(T& v, int i) {assert(i==0); return v;}
345  static void makeNaNLike(const T&, T& nanValue)
346  { nanValue.setToNaN(); }
347  static void makeZeroLike(const T&, T& zeroValue)
348  { zeroValue.setToZero(); }
349 };
350 
353 //==============================================================================
354 // MEASURE_<T> :: IMPLEMENTATION
355 //==============================================================================
362 template <class T>
364 public:
365  const T& getValue(const State& s, int derivOrder) const {
366  SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(),
367  "Measure_<T>::getValue()",
368  "derivOrder %d was out of range; this Measure allows 0-%d.",
369  derivOrder, getNumTimeDerivatives());
370 
371  // We require the stage to have been advanced to at least the one
372  // before this measure's depends-on stage since this will get called
373  // towards the end of the depends-on stage realization.
374  if (getDependsOnStage(derivOrder) != Stage::Empty) {
375 #ifndef NDEBUG
376  Stage prevStage = getDependsOnStage(derivOrder).prev();
377 #endif
378 
380  ( ( isInSubsystem() && getStage(s)>=prevStage)
381  || (!isInSubsystem() && s.getSystemStage()>=prevStage),
382  "Measure_<T>::getValue()",
383  "Expected State to have been realized to at least stage "
384  "%s but stage was %s.",
385  prevStage.getName().c_str(),
386  (isInSubsystem() ? getStage(s) : s.getSystemStage())
387  .getName().c_str());
388  }
389 
390  if (derivOrder < getNumCacheEntries()) {
391  if (!isCacheValueRealized(s,derivOrder)) {
392  T& value = updCacheEntry(s,derivOrder);
393  calcCachedValueVirtual(s, derivOrder, value);
394  markCacheValueRealized(s,derivOrder);
395  return value;
396  }
397  return getCacheEntry(s,derivOrder);
398  }
399 
400  // We can't handle it here -- punt to the concrete Measure
401  // for higher order derivatives.
402  return getUncachedValueVirtual(s,derivOrder);
403  }
404 
407  void setDefaultValue(const T& defaultValue) {
408  this->defaultValue = defaultValue;
409  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
410  this->invalidateTopologyCache();
411  }
412 
416  const T& getDefaultValue() const {return defaultValue;}
417 
419  { presumeValidAtDependsOnStage = presume;
420  this->invalidateTopologyCache(); }
421 
423  { return presumeValidAtDependsOnStage; }
424 
425 protected:
426  explicit Implementation(const T& defaultValue, int numCacheEntries=1)
427  : presumeValidAtDependsOnStage(false),
428  defaultValue(defaultValue),
429  derivIx(numCacheEntries)
430  {
431  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
432  }
433 
437  explicit Implementation(int numCacheEntries=1)
438  : presumeValidAtDependsOnStage(false),
439  defaultValue(),
440  derivIx(numCacheEntries)
441  {
442  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
443  }
444 
449  : presumeValidAtDependsOnStage(source.presumeValidAtDependsOnStage),
450  defaultValue(source.defaultValue),
451  derivIx(source.derivIx.size())
452  {
453  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
454  }
455 
456 
459  int size() const {return Measure_Num<T>::size(defaultValue);}
460 
463  int getNumCacheEntries() const {return (int)derivIx.size();}
464 
468  const T& getCacheEntry(const State& s, int derivOrder) const {
469  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
470  "Measure_<T>::Implementation::getCacheEntry()",
471  "Derivative order %d is out of range; only %d cache entries"
472  " were allocated.", derivOrder, getNumCacheEntries());
473 
474  return Value<T>::downcast(
475  this->getSubsystem().getCacheEntry(s, derivIx[derivOrder]));
476  }
477 
481  T& updCacheEntry(const State& s, int derivOrder) const {
482  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
483  "Measure_<T>::Implementation::updCacheEntry()",
484  "Derivative order %d is out of range; only %d cache entries"
485  " were allocated.", derivOrder, getNumCacheEntries());
486 
487  return Value<T>::updDowncast(
488  this->getSubsystem().updCacheEntry(s, derivIx[derivOrder]));
489  }
490 
493  bool isCacheValueRealized(const State& s, int derivOrder) const {
494  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
495  "Measure_<T>::Implementation::isCacheValueRealized()",
496  "Derivative order %d is out of range; only %d cache entries"
497  " were allocated.", derivOrder, getNumCacheEntries());
498 
499  return this->getSubsystem().isCacheValueRealized(s, derivIx[derivOrder]);
500  }
501 
505  void markCacheValueRealized(const State& s, int derivOrder) const {
506  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
507  "Measure_<T>::Implementation::markCacheValueRealized()",
508  "Derivative order %d is out of range; only %d cache entries"
509  " were allocated.", derivOrder, getNumCacheEntries());
510 
511  this->getSubsystem().markCacheValueRealized(s, derivIx[derivOrder]);
512  }
513 
518  void markCacheValueNotRealized(const State& s, int derivOrder) const {
519  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
520  "Measure_<T>::Implementation::markCacheValueNotRealized()",
521  "Derivative order %d is out of range; only %d cache entries"
522  " were allocated.", derivOrder, getNumCacheEntries());
523 
524  this->getSubsystem().markCacheValueNotRealized(s, derivIx[derivOrder]);
525  }
526 
527  // VIRTUALS //
528  // Ordinals must retain the same meaning from release to release
529  // to preserve binary compatibility.
530 
533  /* 0*/virtual void realizeMeasureTopologyVirtual(State&) const
534  {}
535 
538  /* 1*/virtual void
539  calcCachedValueVirtual(const State&, int derivOrder, T& value) const
540  { SimTK_ERRCHK1_ALWAYS(!"implemented",
541  "Measure_<T>::Implementation::calcCachedValueVirtual()",
542  "This method should have been overridden by the derived"
543  " Measure but was not. It is needed to calculate the"
544  " cached value for derivOrder=%d.", derivOrder); }
545 
551  /* 2*/virtual const T&
552  getUncachedValueVirtual(const State&, int derivOrder) const
553  { SimTK_ERRCHK1_ALWAYS(!"implemented",
554  "Measure_<T>::Implementation::getUncachedValueVirtual()",
555  "This method should have been overridden by the derived"
556  " Measure but was not. It is needed to return the uncached"
557  " value at derivOrder=%d.", derivOrder);
558  return *reinterpret_cast<T*>(0);
559  }
560 
563  const T& getValueZero() const {return zeroValue;}
564 
565 private:
566  // Satisfy the realizeTopology() pure virtual here now that we know the
567  // data type T. Allocate lazy- or auto-validated- cache entries depending
568  // on the setting of presumeValidAtDependsOnStage.
569  void realizeTopology(State& s) const FINAL_11 {
570  // Allocate cache entries. Initialize the value cache entry to
571  // the given defaultValue; all the derivative cache entries should be
572  // initialized to a NaN of the same size.
573  if (getNumCacheEntries()) {
574  derivIx[0] = presumeValidAtDependsOnStage
576  (s, getDependsOnStage(0), new Value<T>(defaultValue))
578  (s, getDependsOnStage(0), new Value<T>(defaultValue));
579 
580  if (getNumCacheEntries() > 1) {
581  T nanValue; Measure_Num<T>::makeNaNLike(defaultValue, nanValue);
582  for (int i=1; i < getNumCacheEntries(); ++i) {
583  derivIx[i] = presumeValidAtDependsOnStage
585  (s, getDependsOnStage(i), new Value<T>(nanValue))
587  (s, getDependsOnStage(i), new Value<T>(nanValue));
588  }
589  }
590  }
591 
592  // Call the concrete class virtual if any.
593  realizeMeasureTopologyVirtual(s);
594  }
595 
596 //------------------------------------------------------------------------------
597 private:
598  // TOPOLOGY STATE
599  bool presumeValidAtDependsOnStage;
600  T defaultValue;
601  T zeroValue;
602 
603  // TOPOLOGY CACHE
604  mutable Array_<CacheEntryIndex> derivIx;
605 };
606 
607 
608 
609 //==============================================================================
610 // CONSTANT :: IMPLEMENTATION
611 //==============================================================================
612 template <class T>
614 : public Measure_<T>::Implementation
615 {
616 public:
617  // We don't want the base class to allocate *any* cache entries.
619  explicit Implementation(const T& value)
620  : Measure_<T>::Implementation(value,0) {}
621 
624  void setValue(const T& v) {this->setDefaultValue(v);}
625 
626  // Implementations of virtual methods.
627  // Measure_<T> virtuals:
628  // No cached values.
629 
630  const T& getUncachedValueVirtual(const State&, int derivOrder) const
632  { return derivOrder>0 ? this->getValueZero() : this->getDefaultValue(); }
633 
634  // AbstractMeasure virtuals:
636  { return new Implementation(*this); }
638  { return derivOrder>0 ? Stage::Empty : Stage::Topology; }
640  { return std::numeric_limits<int>::max(); }
641 };
642 
643 
644 
645 //==============================================================================
646 // MEASURE ZERO and ONE
647 //==============================================================================
648 // These had to wait for Constant::Implementation to be declared.
649 
650 template <class T> inline
652 template <class T> inline
654 
656 : Constant(Vector(size, Real(0))) {}
658 : Constant(sub, Vector(size, Real(0))) {}
659 
660 template <class T> inline
662 template <class T> inline
664 
666 : Constant(Vector(size, Real(1))) {}
668 : Constant(sub, Vector(size, Real(1))) {}
669 
670 
671 
672 //==============================================================================
673 // TIME :: IMPLEMENTATION
674 //==============================================================================
675 template <class T>
677 
678 template <>
679 class Measure_<Real>::Time::Implementation
680 : public Measure_<Real>::Implementation
681 {
682 public:
683  // We don't want the base class to allocate *any* cache entries.
685 
686  // Implementations of virtual methods.
687  // Measure_<Real> virtuals:
688  // No cached values.
689 
690  const Real& getUncachedValueVirtual(const State& s, int derivOrder) const
692  { return derivOrder==0 ? s.getTime()
693  : (derivOrder==1 ? SimTK::One
694  : SimTK::Zero); }
695 
696  // AbstractMeasure virtuals:
698  { return new Implementation(*this); }
700  { return derivOrder>0 ? Stage::Empty : Stage::Time; }
701 
702  // Value is t, 1st derivative is 1, the rest are 0.
704  { return std::numeric_limits<int>::max(); }
705 };
706 
707 
708 
709 //==============================================================================
710 // VARIABLE :: IMPLEMENTATION
711 //==============================================================================
712 template <class T>
714 : public Measure_<T>::Implementation
715 {
716 public:
717  // We don't want the base class to allocate *any* cache entries;
718  // we'll use the variable as its own value and zeroes for all
719  // the derivatives.
721  : Measure_<T>::Implementation(0),
722  invalidatedStage(Stage::Empty) {}
723 
724  Implementation(Stage invalidated, const T& defaultValue)
725  : Measure_<T>::Implementation(defaultValue, 0),
726  invalidatedStage(invalidated) {}
727 
728  // Copy constructor should not copy the variable.
730  : Measure_<T>::Implementation(source.getDefaultValue(), 0),
731  invalidatedStage(source.invalidatedStage) {}
732 
733  void setInvalidatedStage(Stage invalidates) {
734  invalidatedStage = invalidates;
735  this->invalidateTopologyCache();
736  }
737 
738  Stage getInvalidatedStage() const {return invalidatedStage;}
739 
743  void setValue(State& state, const T& value) const
744  { updVarValue(state) = value; }
745 
746  // Implementations of virtual methods.
748  { return new Implementation(*this); }
749 
751  { return std::numeric_limits<int>::max(); }
752 
753  // Discrete variable is available after Model stage; but all its
754  // derivatives are zero so are always available.
756  { return derivOrder>0 ? Stage::Empty : Stage::Model;}
757 
758  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
760  { return derivOrder>0 ? this->getValueZero() : getVarValue(s); }
761 
762  // No cached values.
763 
765  discreteVarIndex = this->getSubsystem().allocateDiscreteVariable
766  (s, invalidatedStage, new Value<T>(this->getDefaultValue()));
767  }
768 private:
769  const T& getVarValue(const State& s) const {
770  assert(discreteVarIndex.isValid());
771  return Value<T>::downcast(
772  this->getSubsystem().getDiscreteVariable(s, discreteVarIndex));
773  }
774  T& updVarValue(State& s) const {
775  assert(discreteVarIndex.isValid());
776  return Value<T>::downcast(
777  this->getSubsystem().updDiscreteVariable(s, discreteVarIndex));
778  }
779 
780  // TOPOLOGY STATE
781  Stage invalidatedStage; // TODO this shouldn't be needed
782 
783  // TOPOLOGY CACHE
784  mutable DiscreteVariableIndex discreteVarIndex;
785 };
786 
787 
788 
789 //==============================================================================
790 // RESULT :: IMPLEMENTATION
791 //==============================================================================
792 template <class T>
794 : public Measure_<T>::Implementation
795 {
796 public:
797  // We want the base class to allocate a single cache entry of type T.
799  : Measure_<T>::Implementation(1),
800  dependsOnStage(Stage::Topology), invalidatedStage(Stage::Infinity) {}
801 
802  Implementation(Stage dependsOn, Stage invalidated)
803  : Measure_<T>::Implementation(1),
804  dependsOnStage(dependsOn==Stage::Empty ? Stage::Topology : dependsOn),
805  invalidatedStage(invalidated)
806  { SimTK_ERRCHK2_ALWAYS(invalidated > dependsOn,"Measure::Result::ctor()",
807  "Got invalidated stage %s and dependsOn stage %s which is illegal "
808  "because the invalidated stage must be later than dependsOn.",
809  invalidated.getName().c_str(), dependsOn.getName().c_str());
810  }
811 
812  // Copy constructor will not copy the cache entry index.
814  : Measure_<T>::Implementation(source),
815  dependsOnStage(source.dependsOnStage),
816  invalidatedStage(source.invalidatedStage) {}
817 
818  void setDependsOnStage(Stage dependsOn) {
819  if (dependsOn == Stage::Empty) dependsOn = Stage::Topology;
820  SimTK_ERRCHK2_ALWAYS(dependsOn < getInvalidatedStage(),
821  "Measure::Result::setDependsOnStage()",
822  "The provided dependsOn stage %s is illegal because it is not "
823  "less than the current invalidated stage %s. Change the "
824  "invalidated stage first with setInvalidatedStage().",
825  dependsOn.getName().c_str(),
826  getInvalidatedStage().getName().c_str());
827 
828  dependsOnStage = dependsOn;
829  this->invalidateTopologyCache();
830  }
831 
832  void setInvalidatedStage(Stage invalidated) {
833  SimTK_ERRCHK2_ALWAYS(invalidated > getDependsOnStage(),
834  "Measure::Result::setInvalidatedStage()",
835  "The provided invalidated stage %s is illegal because it is not "
836  "greater than the current dependsOn stage %s. Change the "
837  "dependsOn stage first with setDependsOnStage().",
838  invalidated.getName().c_str(),
839  getDependsOnStage().getName().c_str());
840 
841  invalidatedStage = invalidated;
842  this->invalidateTopologyCache();
843  }
844 
845 
846  Stage getDependsOnStage() const {return dependsOnStage;}
847  Stage getInvalidatedStage() const {return invalidatedStage;}
848 
849 
850  void markAsValid(const State& state) const
851  { const Stage subsystemStage = this->getSubsystem().getStage(state);
852  SimTK_ERRCHK3_ALWAYS(subsystemStage >= getDependsOnStage().prev(),
853  "Measure::Result::markAsValid()",
854  "This Result Measure cannot be marked valid in a State where this "
855  "measure's Subsystem has been realized only to stage %s, because "
856  "its value was declared to depend on stage %s. To mark it valid, "
857  "we require that the State have been realized at least to the "
858  "previous stage (%s in this case); that is, you must at least be "
859  "*working on* the dependsOn stage in order to claim this result is "
860  "available.",
861  subsystemStage.getName().c_str(),
862  getDependsOnStage().getName().c_str(),
863  getDependsOnStage().prev().getName().c_str());
864  this->markCacheValueRealized(state, 0); }
865 
866  bool isValid(const State& state) const
867  { return this->isCacheValueRealized(state, 0); }
868 
869  void markAsNotValid(const State& state) const
870  { this->markCacheValueNotRealized(state, 0);
871  state.invalidateAllCacheAtOrAbove(invalidatedStage); }
872 
873  T& updValue(const State& state) const
874  { markAsNotValid(state); return this->updCacheEntry(state, 0); }
875 
876 
877  // Implementations of virtual methods.
879  { return new Implementation(*this); }
880 
881  int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;}
882 
886  { return derivOrder>0 ? Stage::Empty : dependsOnStage;}
887 
888  void calcCachedValueVirtual(const State&, int derivOrder, T& value) const
890  { SimTK_ERRCHK_ALWAYS(!"calcCachedValueVirtual() implemented",
891  "Measure_<T>::Result::getValue()",
892  "Measure_<T>::Result::getValue() was called when the value was not "
893  "yet valid. For most Measure types, this would have initiated "
894  "computation of the value, but Result measures must have their values "
895  "calculated and set externally, and then marked valid."); }
896 
897 private:
898  // TOPOLOGY STATE
899  Stage dependsOnStage;
900  Stage invalidatedStage;
901 };
902 
903 
904 
905 //==============================================================================
906 // SINUSOID :: IMPLEMENTATION
907 //==============================================================================
908 template <class T>
910 : public Measure_<T>::Implementation
911 {
912  static const int NumDerivs = 3;
913 public:
915  : Measure_<T>::Implementation(NumDerivs+1),
916  a(CNT<T>::getNaN()), w(CNT<T>::getNaN()), p(CNT<T>::getNaN()) {}
917 
918  Implementation(const T& amplitude,
919  const T& frequency,
920  const T& phase=T(0))
921  : Measure_<T>::Implementation(NumDerivs+1),
922  a(amplitude), w(frequency), p(phase) {}
923 
924  // Default copy constructor is fine.
925 
926  // Implementations of virtual methods.
928  { return new Implementation(*this); }
929 
930  int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return NumDerivs;}
931 
933  { return Stage::Time; }
934 
935  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
937  {
938  // We need to allow the compiler to select std::sin or SimTK::sin
939  // based on the argument type.
940  using std::sin; using std::cos;
941 
942  assert(NumDerivs == 3);
943  const Real t = s.getTime();
944  const T arg = w*t + p;
945 
946  switch (derivOrder) {
947  case 0: value = a*sin(arg); break;
948  case 1: value = w*a*cos(arg); break;
949  case 2: value = -w*w*a*sin(arg); break;
950  case 3: value = -w*w*w*a*cos(arg); break;
951  default: SimTK_ASSERT1_ALWAYS(!"out of range",
952  "Measure::Sinusoid::Implementation::calcCachedValueVirtual():"
953  " derivOrder %d is out of range 0-3.", derivOrder);
954  }
955  }
956 
957  // There are no uncached values.
958 
959 private:
960  // TOPOLOGY STATE
961  T a, w, p;
962 
963  // TOPOLOGY CACHE
964  // nothing
965 };
966 
967 
968 
969 //==============================================================================
970 // PLUS :: IMPLEMENTATION
971 //==============================================================================
972 template <class T>
974 public:
975  // TODO: Currently allocates just one cache entry.
976  // left and right will be empty handles.
978 
980  const Measure_<T>& right)
981  : left(left), right(right) {}
982 
983  // Default copy constructor gives us a new Implementation object,
984  // but with references to the *same* operand measures.
985 
986  // Implementations of virtual methods.
987 
988  // This uses the default copy constructor.
990  { return new Implementation(*this); }
991 
992  // TODO: Let this be settable up to the min number of derivatives
993  // provided by the arguments.
994  int getNumTimeDerivativesVirtual() const OVERRIDE_11 {return 0;}
995  //{ return std::min(left.getNumTimeDerivatives(),
996  // right.getNumTimeDerivatives()); }
997 
999  { return Stage(std::max(left.getDependsOnStage(order),
1000  right.getDependsOnStage(order))); }
1001 
1002 
1003  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1004  OVERRIDE_11
1005  {
1006  value = left.getValue(s,derivOrder) + right.getValue(s,derivOrder);
1007  }
1008 
1009  // There are no uncached values.
1010 
1011 private:
1012  // TOPOLOGY STATE
1013  Measure_<T> left;
1014  Measure_<T> right;
1015 
1016  // TOPOLOGY CACHE
1017  // nothing
1018 };
1019 
1020 
1021 
1022 //==============================================================================
1023 // MINUS :: IMPLEMENTATION
1024 //==============================================================================
1025 template <class T>
1027 public:
1028  // TODO: Currently allocates just one cache entry.
1029  // left and right will be empty handles.
1031 
1033  const Measure_<T>& right)
1034  : left(left), right(right) {}
1035 
1036  // Default copy constructor gives us a new Implementation object,
1037  // but with references to the *same* operand measures.
1038 
1039  // Implementations of virtual methods.
1040 
1041  // This uses the default copy constructor.
1043  { return new Implementation(*this); }
1044 
1045  // TODO: Let this be settable up to the min number of derivatives
1046  // provided by the arguments.
1048  //{ return std::min(left.getNumTimeDerivatives(),
1049  // right.getNumTimeDerivatives()); }
1050 
1052  { return Stage(std::max(left.getDependsOnStage(order),
1053  right.getDependsOnStage(order))); }
1054 
1055 
1056  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1057  OVERRIDE_11
1058  {
1059  value = left.getValue(s,derivOrder) - right.getValue(s,derivOrder);
1060  }
1061 
1062  // There are no uncached values.
1063 
1064 private:
1065  // TOPOLOGY STATE
1066  Measure_<T> left;
1067  Measure_<T> right;
1068 
1069  // TOPOLOGY CACHE
1070  // nothing
1071 };
1072 
1073 
1074 
1075 //==============================================================================
1076 // SCALE :: IMPLEMENTATION
1077 //==============================================================================
1078 template <class T>
1080 : public Measure_<T>::Implementation
1081 {
1082 public:
1083  // TODO: Currently allocates just one cache entry.
1084  // scale will be uninitialized, operand will be empty handle.
1085  Implementation() : factor(NaN) {}
1086 
1087  Implementation(Real factor, const Measure_<T>& operand)
1088  : factor(factor), operand(operand) {}
1089 
1090  // Default copy constructor gives us a new Implementation object,
1091  // but with references to the *same* operand measure.
1092 
1093  void setScaleFactor(Real sf) {
1094  factor = sf;
1095  this->invalidateTopologyCache();
1096  }
1097 
1099  {
1100  return operand;
1101  }
1102 
1103  // Implementations of virtual methods.
1104 
1105  // This uses the default copy constructor.
1107  { return new Implementation(*this); }
1108 
1109  // TODO: Let this be settable up to the min number of derivatives
1110  // provided by the arguments.
1112  //{ return std::min(left.getNumTimeDerivatives(),
1113  // right.getNumTimeDerivatives()); }
1114 
1116  { return operand.getDependsOnStage(order); }
1117 
1118 
1119  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1120  OVERRIDE_11
1121  {
1122  value = factor * operand.getValue(s,derivOrder);
1123  }
1124 
1125  // There are no uncached values.
1126 
1127 private:
1128  // TOPOLOGY STATE
1129  Real factor;
1130  Measure_<T> operand;
1131 
1132  // TOPOLOGY CACHE
1133  // nothing
1134 };
1135 
1136 
1137 
1138 //==============================================================================
1139 // INTEGRATE :: IMPLEMENTATION
1140 //==============================================================================
1147 template <class T>
1149 : public Measure_<T>::Implementation {
1150 public:
1154 
1157  Implementation(const Measure_<T>& deriv, const Measure_<T>& ic,
1158  const T& defaultValue)
1159  : Measure_<T>::Implementation(defaultValue, 1),
1160  derivMeasure(deriv), icMeasure(ic) {}
1161 
1165  : Measure_<T>::Implementation(source.getDefaultValue(), 1),
1166  derivMeasure(source.derivMeasure), icMeasure(source.icMeasure) {}
1167 
1171  void setValue(State& s, const T& value) const
1172  { assert(zIndex >= 0);
1173  for (int i=0; i < this->size(); ++i)
1174  this->getSubsystem().updZ(s)[zIndex+i] =
1175  Measure_Num<T>::get(value, i); }
1176 
1178  { SimTK_ERRCHK(!derivMeasure.isEmptyHandle(),
1179  "Measure_<T>::Integrate::getDerivativeMeasure()",
1180  "No derivative measure is available for this integrated measure.");
1181  return derivMeasure; }
1182 
1184  { SimTK_ERRCHK(!icMeasure.isEmptyHandle(),
1185  "Measure_<T>::Integrate::getInitialConditionMeasure()",
1186  "No initial condition measure is available for this "
1187  "integrated measure.");
1188  return icMeasure; }
1189 
1191  { derivMeasure = d; this->invalidateTopologyCache(); }
1193  { icMeasure = ic; this->invalidateTopologyCache(); }
1194 
1195  // Implementations of virtuals.
1196 
1197  // This uses the copy constructor defined above.
1199  { return new Implementation(*this); }
1200 
1203  { int integralDerivs = getDerivativeMeasure().getNumTimeDerivatives();
1204  // Careful - can't add 1 to max int and stay an int.
1205  if (integralDerivs < std::numeric_limits<int>::max())
1206  ++integralDerivs;
1207  return integralDerivs; }
1208 
1209  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1210  OVERRIDE_11
1211  { assert(derivOrder == 0); // only one cache entry
1212  assert(Measure_Num<T>::size(value) == this->size());
1213  assert(zIndex.isValid());
1214  const Vector& allZ = this->getSubsystem().getZ(s);
1215  for (int i=0; i < this->size(); ++i)
1216  Measure_Num<T>::upd(value,i) = allZ[zIndex+i];
1217  }
1218 
1219  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1220  OVERRIDE_11
1221  { assert(derivOrder > 0); // 0th entry is cached
1222  return getDerivativeMeasure().getValue(s, derivOrder-1);
1223  }
1224 
1226  { return derivOrder>0
1227  ? getDerivativeMeasure().getDependsOnStage(derivOrder-1)
1228  : Stage::Time; }
1229 
1233  assert(zIndex.isValid());
1234  Vector& allZ = this->getSubsystem().updZ(s);
1235  if (!icMeasure.isEmptyHandle()) {
1236  this->getSubsystem().getSystem()
1237  .realize(s, icMeasure.getDependsOnStage());
1238  const T& ic = icMeasure.getValue(s);
1239  for (int i=0; i < this->size(); ++i)
1240  allZ[zIndex+i] = Measure_Num<T>::get(ic,i);
1241  } else {
1242  for (int i=0; i < this->size(); ++i)
1243  allZ[zIndex+i] = Measure_Num<T>::get(this->getDefaultValue(),i);
1244  }
1245  }
1246 
1252  Vector init(this->size());
1253  for (int i=0; i < this->size(); ++i)
1254  init[i] = Measure_Num<T>::get(this->getDefaultValue(),i);
1255  zIndex = this->getSubsystem().allocateZ(s, init);
1256  }
1257 
1261  assert(zIndex.isValid());
1262  Vector& allZDot = this->getSubsystem().updZDot(s);
1263  if (!derivMeasure.isEmptyHandle()) {
1264  const T& deriv = derivMeasure.getValue(s);
1265  for (int i=0; i < this->size(); ++i)
1266  allZDot[zIndex+i] = Measure_Num<T>::get(deriv,i);
1267  } else {
1268  allZDot(zIndex,this->size()) = 0; // derivative is zero
1269  }
1270  }
1271 
1272 private:
1273  // TOPOLOGY STATE
1274  Measure_<T> derivMeasure; // just handles
1275  Measure_<T> icMeasure;
1276 
1277  // TOPOLOGY CACHE
1278  mutable ZIndex zIndex; // This is the first index if more than one z.
1279 };
1280 
1281 
1282 
1283 //==============================================================================
1284 // DIFFERENTIATE :: IMPLEMENTATION
1285 //==============================================================================
1286  // Hide from Doxygen.
1288 // This helper class is the contents of the discrete state variable and
1289 // corresponding cache entry maintained by this measure. The variable is
1290 // auto-update, meaning the value of the cache entry replaces the state
1291 // variable at the start of each step.
1292 // TODO: This was a local class in Measure_<T>::Differentiate::Implementation
1293 // but VC++ 8 (2005) failed to properly instantiate the templatized operator<<()
1294 // in that case; doing it this way is a workaround.
1295 template <class T>
1296 class Measure_Differentiate_Result {
1297 public:
1298  Measure_Differentiate_Result() : derivIsGood(false) {}
1299  T operand; // previous value of operand
1300  T operandDot; // previous value of derivative
1301  bool derivIsGood; // do we think the deriv is a good one?
1302 };
1305 template <class T>
1307 : public Measure_<T>::Implementation
1308 {
1309  typedef Measure_Differentiate_Result<T> Result;
1310 public:
1311  // Don't allocate any cache entries in the base class.
1313 
1314  Implementation(const Measure_<T>& operand)
1315  : Measure_<T>::Implementation(0),
1316  operand(operand), forceUseApprox(false), isApproxInUse(false) {}
1317 
1318  // Default copy constructor gives us a new Implementation object,
1319  // but with reference to the *same* operand measure.
1320 
1321  void setForceUseApproximation(bool mustApproximate) {
1322  forceUseApprox = mustApproximate;
1323  this->invalidateTopologyCache();
1324  }
1325 
1326  void setOperandMeasure(const Measure_<T>& operand) {
1327  this->operand = operand;
1328  this->invalidateTopologyCache();
1329  }
1330 
1331  bool getForceUseApproximation() const {return forceUseApprox;}
1332  bool isUsingApproximation() const {return isApproxInUse;}
1333  const Measure_<T>& getOperandMeasure() const {return operand;}
1334 
1335  // Implementations of virtual methods.
1336 
1337  // This uses the default copy constructor.
1339  { return new Implementation(*this); }
1340 
1341  // This has one fewer than the operand.
1343  { if (!isApproxInUse) return operand.getNumTimeDerivatives()-1;
1344  else return 0; }
1345 
1347  { if (!isApproxInUse) return operand.getDependsOnStage(order+1);
1348  else return operand.getDependsOnStage(order); }
1349 
1350 
1351  // We're not using the Measure_<T> base class cache services, but
1352  // we do have one of our own. It looks uncached from the base class
1353  // point of view which is why we're implementing it here.
1354  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1355  OVERRIDE_11
1356  { if (!isApproxInUse)
1357  return operand.getValue(s, derivOrder+1);
1358 
1359  ensureDerivativeIsRealized(s);
1360  const Subsystem& subsys = this->getSubsystem();
1361  const Result& result = Value<Result>::downcast
1362  (subsys.getDiscreteVarUpdateValue(s,resultIx));
1363  return result.operandDot; // has a value but might not be a good one
1364  }
1365 
1367  if (!isApproxInUse) return;
1368 
1369  assert(resultIx.isValid());
1370  const Subsystem& subsys = this->getSubsystem();
1371  Result& result = Value<Result>::updDowncast
1372  (subsys.updDiscreteVariable(s,resultIx));
1373  this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
1374  result.operand = operand.getValue(s);
1375  result.operandDot = this->getValueZero();
1376  result.derivIsGood = false;
1377  }
1378 
1380  isApproxInUse = (forceUseApprox || operand.getNumTimeDerivatives()==0);
1381  if (!isApproxInUse)
1382  return;
1383 
1384  resultIx = this->getSubsystem()
1385  .allocateAutoUpdateDiscreteVariable(s, operand.getDependsOnStage(0),
1386  new Value<Result>(), operand.getDependsOnStage(0));
1387  }
1388 
1392  ensureDerivativeIsRealized(s);
1393  }
1394 
1395  void ensureDerivativeIsRealized(const State& s) const {
1396  assert(resultIx.isValid());
1397  const Subsystem& subsys = this->getSubsystem();
1398  if (subsys.isDiscreteVarUpdateValueRealized(s,resultIx))
1399  return;
1400 
1401  const Real t0 = subsys.getDiscreteVarLastUpdateTime(s,resultIx);
1402  const Result& prevResult = Value<Result>::downcast
1403  (subsys.getDiscreteVariable(s,resultIx));
1404  const T& f0 = prevResult.operand;
1405  const T& fdot0 = prevResult.operandDot; // may be invalid
1406  const bool good0 = prevResult.derivIsGood;
1407 
1408  const Real t = s.getTime();
1409  Result& result = Value<Result>::updDowncast
1410  (subsys.updDiscreteVarUpdateValue(s,resultIx));
1411  T& f = result.operand; // renaming
1412  T& fdot = result.operandDot;
1413  bool& good = result.derivIsGood;
1414 
1415  f = operand.getValue(s);
1416  good = false;
1417  if (!isFinite(t0))
1418  fdot = this->getValueZero();
1419  else if (t == t0) {
1420  fdot = fdot0;
1421  good = good0;
1422  } else {
1423  fdot = (f-f0)/(t-t0); // 1st order
1424  if (good0)
1425  fdot = Real(2)*fdot - fdot0; // now 2nd order
1426  good = true; // either 1st or 2nd order estimate
1427  }
1428  subsys.markDiscreteVarUpdateValueRealized(s,resultIx);
1429  }
1430 private:
1431  // TOPOLOGY STATE
1432  Measure_<T> operand;
1433  bool forceUseApprox;
1434 
1435  // TOPOLOGY CACHE
1436  mutable bool isApproxInUse;
1437  mutable DiscreteVariableIndex resultIx; // auto-update
1438 };
1439 
1440 
1441 
1442 //==============================================================================
1443 // EXTREME :: IMPLEMENTATION
1444 //==============================================================================
1445 template <class T>
1447 {
1448  typedef typename Measure_<T>::Extreme Extreme;
1449  typedef typename Extreme::Operation Operation;
1450 public:
1454  : Measure_<T>::Implementation(0), operation(Extreme::MaxAbs) {}
1455 
1458  Implementation(const Measure_<T>& operand, Operation op)
1459  : Measure_<T>::Implementation(0), operand(operand), operation(op) {}
1460 
1461  // Default copy constructor gives us a new Implementation object,
1462  // but with reference to the *same* operand measure.
1463 
1467  void setOperandMeasure(const Measure_<T>& operand) {
1468  this->operand = operand;
1469  this->invalidateTopologyCache();
1470  }
1471 
1475  void setOperation(Operation op) {
1476  this->operation = op;
1477  this->invalidateTopologyCache();
1478  }
1479 
1481  const Measure_<T>& getOperandMeasure() const {return operand;}
1482 
1485  Operation getOperation() const {return operation;}
1486 
1489  void setValue(State& s, const T& value) const {
1490  assert(extremeIx.isValid());
1491  const Subsystem& subsys = this->getSubsystem();
1492  T& prevMin = Value<T>::updDowncast
1493  (subsys.updDiscreteVariable(s,extremeIx));
1494  prevMin = value;
1495  }
1496 
1500  Real getTimeOfExtremeValue(const State& s) const {
1501  const Subsystem& subsys = this->getSubsystem();
1502  const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
1503  Real tUpdate;
1504  if (hasNewExtreme)
1505  tUpdate = s.getTime(); // i.e., now
1506  else
1507  tUpdate = subsys.getDiscreteVarLastUpdateTime(s,extremeIx);
1508  return tUpdate;
1509  }
1510 
1511  // Implementations of virtual methods.
1512 
1513  // This uses the default copy constructor.
1515  { return new Implementation(*this); }
1516 
1520  { return operand.getNumTimeDerivatives(); }
1521 
1525  { return operand.getDependsOnStage(order); }
1526 
1527 
1531  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1532  OVERRIDE_11
1533  {
1534  const Subsystem& subsys = this->getSubsystem();
1535  const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
1536  if (derivOrder > 0) {
1537  // TODO: should be handled elementwise and zero unless the
1538  // derivative is acting in the direction that changes the
1539  // extreme.
1540  return hasNewExtreme ? operand.getValue(s, derivOrder)
1541  : this->getValueZero();
1542  }
1543  if (hasNewExtreme) {
1544  const T& newExt = Value<T>::downcast
1545  (subsys.getDiscreteVarUpdateValue(s,extremeIx));
1546  return newExt;
1547  } else {
1548  const T& currentExt = Value<T>::downcast
1549  (subsys.getDiscreteVariable(s,extremeIx));
1550  return currentExt;
1551  }
1552  }
1553 
1557  this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
1558  setValue(s, operand.getValue(s));
1559  }
1560 
1569  // TODO: this should be NaN once initialization is working properly.
1570  T initVal = this->getDefaultValue();
1571  switch(operation) {
1572  case Minimum: initVal = Infinity; break;
1573  case Maximum: initVal = -Infinity; break;
1574  case MinAbs: initVal = Infinity; break;
1575  case MaxAbs: initVal = 0; break;
1576  };
1577 
1578  extremeIx = this->getSubsystem()
1580  new Value<T>(initVal), operand.getDependsOnStage(0));
1581 
1582  isNewExtremeIx = this->getSubsystem()
1584  new Value<bool>(false), operand.getDependsOnStage(0));
1585  }
1586 
1590  ensureExtremeHasBeenUpdated(s);
1591  }
1592 
1598  bool ensureExtremeHasBeenUpdated(const State& s) const {
1599  assert(extremeIx.isValid() && isNewExtremeIx.isValid());
1600  const Subsystem& subsys = this->getSubsystem();
1601 
1602  // We may have already determined whether we're at a new extreme in
1603  // which case we don't need to do it again.
1604  if (subsys.isDiscreteVarUpdateValueRealized(s, isNewExtremeIx))
1605  return Value<bool>::downcast
1606  (subsys.getDiscreteVarUpdateValue(s,isNewExtremeIx));
1607 
1608  // We're going to have to decide if we're at a new extreme, and if
1609  // so record the new extreme value in the auto-update cache entry of
1610  // the extreme value state variable.
1611 
1612  // Get the previous extreme value and the current operand value.
1613  const T& prevExtreme = Value<T>::downcast
1614  (subsys.getDiscreteVariable(s,extremeIx));
1615  const T& currentVal = operand.getValue(s);
1616 
1617  // Search to see if any element has reached a new extreme.
1618  bool foundNewExt = false;
1619  for (int i=0; i < this->size() && !foundNewExt; ++i)
1620  foundNewExt = isNewExtreme(Measure_Num<T>::get(currentVal,i),
1621  Measure_Num<T>::get(prevExtreme,i));
1622 
1623  // Record the result and mark the auto-update cache entry valid
1624  // so we won't have to recalculate. When the integrator advances to the
1625  // next step this cache entry will be swapped with the corresponding
1626  // state and marked invalid so we'll be sure to recalculate each step.
1628  (subsys.updDiscreteVarUpdateValue(s,isNewExtremeIx)) = foundNewExt;
1629  subsys.markDiscreteVarUpdateValueRealized(s,isNewExtremeIx);
1630 
1631  // Don't update the auto-update cache entry if we didn't see a new
1632  // extreme. That way no auto-update will occur and the state variable
1633  // will remain unchanged with the existing update time preserved.
1634  if (!foundNewExt)
1635  return false;
1636 
1637  // We have encountered a new extreme. We'll record the new extreme
1638  // in the auto-update cache entry which will be used as the current
1639  // result until the integrator advances to the next step at which time
1640  // this will be swapped with the state variable to serve as the previous
1641  // extreme value until a further extreme is encountered.
1642  T& newExtreme = Value<T>::updDowncast
1643  (subsys.updDiscreteVarUpdateValue(s,extremeIx));
1644 
1645  for (int i=0; i < this->size(); ++i)
1646  Measure_Num<T>::upd(newExtreme,i) =
1647  extremeOf(Measure_Num<T>::get(currentVal,i),
1648  Measure_Num<T>::get(prevExtreme,i));
1649 
1650  // Marking this valid is what ensures that an auto-update occurs later.
1651  subsys.markDiscreteVarUpdateValueRealized(s,extremeIx);
1652  return true;
1653  }
1654 private:
1655  // Return true if newVal is "more extreme" than oldExtreme, according
1656  // to the operation we're performing.
1657  bool isNewExtreme(const typename Measure_Num<T>::Element& newVal,
1658  const typename Measure_Num<T>::Element& oldExtreme) const
1659  {
1660  switch (operation) {
1661  case Extreme::Maximum: return newVal > oldExtreme;
1662  case Extreme::Minimum: return newVal < oldExtreme;
1663  case Extreme::MaxAbs: return std::abs(newVal) > std::abs(oldExtreme);
1664  case Extreme::MinAbs: return std::abs(newVal) < std::abs(oldExtreme);
1665  };
1666  SimTK_ASSERT1_ALWAYS(!"recognized",
1667  "Measure::Extreme::Implementation::isNewExtreme(): "
1668  "unrecognized operation %d", (int)operation);
1669  return false; /*NOTREACHED*/
1670  }
1671 
1672  // Given the value of one element of the operand, and that value's time
1673  // derivative, determine whether the derivative is moving the element
1674  // into the "more extreme" direction, according to the operation.
1675  bool isExtremeDir(const typename Measure_Num<T>::Element& value,
1676  const typename Measure_Num<T>::Element& deriv) const
1677  {
1678  const int sv = sign(value), sd = sign(deriv);
1679  if (sd == 0) return false; // derivative is zero; not changing
1680  switch (operation) {
1681  case Extreme::Maximum: return sd == 1; // getting larger
1682  case Extreme::Minimum: return sd == -1; // getting smaller
1683  case Extreme::MaxAbs: return sv==0 || sd==sv; // abs is growing
1684  case Extreme::MinAbs: return sd == -sv;
1685  };
1686  SimTK_ASSERT1_ALWAYS(!"recognized",
1687  "Measure::Extreme::Implementation::isExtremeDir(): "
1688  "unrecognized operation %d", (int)operation);
1689  return false; /*NOTREACHED*/
1690  }
1691 
1692  typename Measure_Num<T>::Element
1693  extremeOf(const typename Measure_Num<T>::Element& newVal,
1694  const typename Measure_Num<T>::Element& oldExtreme) const
1695  {
1696  return isNewExtreme(newVal,oldExtreme) ? newVal : oldExtreme;
1697  }
1698 
1699  // TOPOLOGY STATE
1700  Measure_<T> operand;
1701  Operation operation;
1702 
1703  // TOPOLOGY CACHE
1704  mutable DiscreteVariableIndex extremeIx; // extreme so far; auto-update
1705 
1706  // This auto-update flag records whether the current value is a new
1707  // extreme. We don't really need to save it as a state variable since you
1708  // can figure this out from the timestamp, but we need to to get invalidated
1709  // by the auto-update swap so that we'll figure it out anew each step.
1710  mutable DiscreteVariableIndex isNewExtremeIx;
1711 };
1712 
1713 
1714 
1715 //==============================================================================
1716 // DELAY :: IMPLEMENTATION
1717 //============================================================================== // Hide from Doxygen.
1719 // This helper class is the contents of the discrete state variable and
1720 // corresponding cache entry maintained by this measure. The variable is
1721 // auto-update, meaning the value of the cache entry replaces the state
1722 // variable at the start of each step.
1723 //
1724 // Circular buffers look like this:
1725 //
1726 // oldest=0, n=0
1727 // v
1728 // Empty buffer: | available |
1729 //
1730 // By convention, oldest=0 whenever the buffer is empty.
1731 //
1732 //
1733 // oldest next=(oldest+n)%capacity
1734 // v v
1735 // | available | | | | | | | available |
1736 // ^ n=6 ^
1737 // 0 capacity
1738 // v v
1739 // or | | | | | | available | | | | | | | | n=12
1740 // ^ ^
1741 // next oldest
1742 // = (oldest+n)%capacity
1743 //
1744 // Number of entries = n (called size() below)
1745 // Empty = n==0
1746 // Full = n==capacity()
1747 // Next available = (oldest+n)%capacity()
1748 template <class T>
1749 class Measure_Delay_Buffer {
1750 public:
1751  explicit Measure_Delay_Buffer() {initDataMembers();}
1752  void clear() {initDataMembers();}
1753  int size() const {return m_size;} // # saved entries, *not* size of arrays
1754  int capacity() const {return m_times.size();}
1755  bool empty() const {return size()==0;}
1756  bool full() const {return size()==capacity();}
1757 
1758  double getEntryTime(int i) const
1759  { assert(i < size()); return m_times[getArrayIndex(i)];}
1760  const T& getEntryValue(int i) const
1761  { assert(i < size()); return m_values[getArrayIndex(i)];}
1762 
1763  enum {
1764  InitialAllocation = 8, // smallest allocation
1765  GrowthFactor = 2, // how fast to grow (double)
1766  MaxShrinkProofSize = 16, // won't shrink unless bigger
1767  TooBigFactor = 5 // 5X too much->maybe shrink
1768  };
1769 
1770  // Add a new entry to the end of the list, throwing out old entries that
1771  // aren't needed to answer requests at tEarliest or later.
1772  void append(double tEarliest, double tNow, const T& valueNow) {
1773  forgetEntriesMuchOlderThan(tEarliest);
1774  removeEntriesLaterOrEq(tNow);
1775  if (full())
1776  makeMoreRoom();
1777  else if (capacity() > std::max((int)MaxShrinkProofSize,
1778  (int)TooBigFactor * (size()+1)))
1779  makeLessRoom(); // less than 1/TooBigFactor full
1780  const int nextFree = getArrayIndex(m_size++);
1781  m_times[nextFree] = tNow;
1782  m_values[nextFree] = valueNow;
1783  m_maxSize = std::max(m_maxSize, size());
1784  }
1785 
1786  // Prepend an older entry to the beginning of the list. No cleanup is done.
1787  void prepend(double tNewOldest, const T& value) {
1788  assert(empty() || tNewOldest < m_times[m_oldest]);
1789  if (full()) makeMoreRoom();
1790  m_oldest = empty() ? 0 : getArrayIndex(-1);
1791  m_times[m_oldest] = tNewOldest;
1792  m_values[m_oldest] = value;
1793  ++m_size;
1794  m_maxSize = std::max(m_maxSize, size());
1795  }
1796 
1797  // This is a specialized copy assignment for copying an old buffer
1798  // to a new one with updated contents. We are told the earliest time we'll
1799  // be asked about from now on, and won't copy any entries older than those
1800  // needed to answer that earliest request. We won't copy anything at or
1801  // newer than tNow, and finally we'll push (tNow,valueNow) as the newest
1802  // entry.
1803  void copyInAndUpdate(const Measure_Delay_Buffer& oldBuf, double tEarliest,
1804  double tNow, const T& valueNow) {
1805  // clear all current entries (no heap activity)
1806  m_oldest = m_size = 0;
1807 
1808  // determine how may old entries we have to keep
1809  int firstNeeded = oldBuf.countNumUnneededOldEntries(tEarliest);
1810  int lastNeeded = oldBuf.findLastEarlier(tNow); // might be -1
1811  int numOldEntriesToKeep = lastNeeded-firstNeeded+1;
1812  int newSize = numOldEntriesToKeep+1; // includes the new one
1813 
1814  int newSizeRequest = -1;
1815  if (capacity() < newSize) {
1816  newSizeRequest = std::max((int)InitialAllocation,
1817  (int)GrowthFactor * newSize);
1818  ++m_nGrows;
1819  } else if (capacity() > std::max((int)MaxShrinkProofSize,
1820  (int)TooBigFactor * newSize)) {
1821  newSizeRequest = std::max((int)MaxShrinkProofSize,
1822  (int)GrowthFactor * newSize);
1823  ++m_nShrinks;
1824  }
1825 
1826  // Reallocate space if advisable.
1827  if (newSizeRequest != -1) {
1828  const double dNaN = NTraits<double>::getNaN();
1829  m_values.resize(newSizeRequest);
1830  if (m_values.capacity() > m_values.size())
1831  m_values.resize(m_values.capacity()); // don't waste any
1832  m_times.resize(m_values.size(), dNaN);
1833  }
1834 
1835  m_maxCapacity = std::max(m_maxCapacity, capacity());
1836 
1837  // Copy the entries we need to keep.
1838  int nxt = 0;
1839  for (int i=firstNeeded; i<=lastNeeded; ++i, ++nxt) {
1840  m_times[nxt] = oldBuf.getEntryTime(i);
1841  m_values[nxt] = oldBuf.getEntryValue(i);
1842  }
1843  // Now add the newest entry and set the size.
1844  m_times[nxt] = tNow;
1845  m_values[nxt] = valueNow;
1846  assert(nxt+1==newSize);
1847  m_size = nxt+1;
1848  m_maxSize = std::max(m_maxSize, size());
1849  }
1850 
1851  // Given the current time and value and the earlier time at which the
1852  // value is needed, use the buffer and (if necessary) the current value
1853  // to estimate the delayed value.
1854  T calcValueAtTime(double tDelay, double tNow, const T& valueNow) const;
1855 
1856  // Given the current time but *not* the current value of the source measure,
1857  // provide an estimate for the value at tDelay=tNow-delay using only the
1858  // buffer contents and linear interpolation or extrapolation.
1859  void calcValueAtTimeLinearOnly(double tDelay, T& delayedValue) const {
1860  if (empty()) {
1861  // Nothing in the buffer?? Shouldn't happen. Return empty Vector
1862  // or NaN for fixed-size types.
1863  Measure_Num<T>::makeNaNLike(T(), delayedValue);
1864  return;
1865  }
1866 
1867  int firstLater = findFirstLaterOrEq(tDelay);
1868 
1869  if (firstLater > 0) {
1870  // Normal case: tDelay is between two buffer entries.
1871  int firstEarlier = firstLater-1;
1872  double t0=getEntryTime(firstEarlier), t1=getEntryTime(firstLater);
1873  const T& v0=getEntryValue(firstEarlier);
1874  const T& v1=getEntryValue(firstLater);
1875  Real fraction = Real((tDelay-t0)/(t1-t0));
1876  delayedValue = T(v0 + fraction*(v1-v0));
1877  return;
1878  }
1879 
1880  if (firstLater==0) {
1881  // Startup case: tDelay is at or before the oldest buffer entry.
1882  // Assume the value was flat before that.
1883  delayedValue = getEntryValue(firstLater);
1884  return;
1885  }
1886 
1887  // tDelay is later than the latest entry in the buffer. We are going
1888  // to have to extrapolate (yuck).
1889 
1890  if (size() == 1) {
1891  // Just one entry; we'll have to assume the value is flat.
1892  delayedValue = getEntryValue(0);
1893  return;
1894  }
1895 
1896  // Extrapolate using the last two entries.
1897  double t0=getEntryTime(size()-2), t1=getEntryTime(size()-1);
1898  const T& v0=getEntryValue(size()-2);
1899  const T& v1=getEntryValue(size()-1);
1900  Real fraction = Real((tDelay-t0)/(t1-t0)); // > 1
1901  assert(fraction > 1.0);
1902  delayedValue = T(v0 + fraction*(v1-v0)); // Extrapolate.
1903  }
1904 
1905  // Return the number of times we had to grow the buffer.
1906  int getNumGrows() const {return m_nGrows;}
1907  // Return the number of times we decided the buffer was so overallocated
1908  // that we had to shrink it.
1909  int getNumShrinks() const {return m_nShrinks;}
1910  // Return the largest number of values we ever had in the buffer.
1911  int getMaxSize() const {return m_maxSize;}
1912  // Return the largest capacity the buffer ever had.
1913  int getMaxCapacity() const {return m_maxCapacity;}
1914 
1915 private:
1916  // Return the i'th oldest entry
1917  // (0 -> oldest, size-1 -> newest, size -> first free, -1 -> last free)
1918  int getArrayIndex(int i) const
1919  { assert(-1<=i && i<=size());
1920  const int rawIndex = m_oldest + i;
1921  if (rawIndex < 0) return rawIndex + capacity();
1922  else return rawIndex % capacity(); }
1923 
1924  // Remove all but two entries older than the given time.
1925  void forgetEntriesMuchOlderThan(double tEarliest) {
1926  const int numToRemove = countNumUnneededOldEntries(tEarliest);
1927  if (numToRemove) {
1928  m_oldest = getArrayIndex(numToRemove);
1929  m_size -= numToRemove;
1930  }
1931  }
1932 
1933  // Count up how many old entries at the beginning of the buffer are so old
1934  // that they wouldn't be needed to respond to a request at time tEarliest or
1935  // later. We'll keep no more than two entries earlier than tEarliest.
1936  int countNumUnneededOldEntries(double tEarliest) const {
1937  const int firstLater = findFirstLaterOrEq(tEarliest);
1938  return std::max(0, firstLater-2);
1939  }
1940 
1941  // Given the time now, delete anything at the end of the queue that is
1942  // at that same time or later.
1943  void removeEntriesLaterOrEq(double t) {
1944  int lastEarlier = findLastEarlier(t);
1945  m_size = lastEarlier+1;
1946  if (m_size==0) m_oldest=0; // restart at beginning of array
1947  }
1948 
1949  // Return the entry number (0..size-1) of the first entry whose time
1950  // is >= the given time, or -1 if there is none such.
1951  int findFirstLaterOrEq(double tDelay) const {
1952  for (int i=0; i < size(); ++i)
1953  if (getEntryTime(i) >= tDelay)
1954  return i;
1955  return -1;
1956  }
1957 
1958  // Return the entry number(size-1..0) of the last entry whose time
1959  // is < the given time, or -1 if there is none such.
1960  int findLastEarlier(double t) const {
1961  for (int i=size()-1; i>=0; --i)
1962  if (getEntryTime(i) < t)
1963  return i;
1964  return -1;
1965  }
1966 
1967  // We don't have enough space. This is either the initial allocation or
1968  // we need to double the current space.
1969  void makeMoreRoom() {
1970  const int newSizeRequest = std::max((int)InitialAllocation,
1971  (int)GrowthFactor * size());
1972  resize(newSizeRequest);
1973  ++m_nGrows;
1974  m_maxCapacity = std::max(m_maxCapacity, capacity());
1975  }
1976 
1977  // We are wasting a lot of space, reduce the heap allocation to just
1978  // double what we're using now.
1979  void makeLessRoom() {
1980  const int targetMaxSize = std::max((int)MaxShrinkProofSize,
1981  (int)GrowthFactor * size());
1982  if (capacity() > targetMaxSize) {
1983  resize(targetMaxSize);
1984  ++m_nShrinks;
1985  }
1986  }
1987 
1988  // Reallocate memory to get more space or stop wasting space. The new
1989  // size request must be big enough to hold all the current contents. The
1990  // amount we actually get may be somewhat larger than the request. On
1991  // return, the times and values arrays will have been resized and the
1992  // oldest entry will now be entry 0.
1993  void resize(int newSizeRequest) {
1994  assert(newSizeRequest >= size());
1995  const double dNaN = NTraits<double>::getNaN();
1996  Array_<T,int> newValues(newSizeRequest);
1997  if (newValues.capacity() > newValues.size())
1998  newValues.resize(newValues.capacity()); // don't waste any
1999  Array_<double,int> newTimes(newValues.size(), dNaN);
2000 
2001  // Pack existing values into start of new arrays.
2002  for (int i=0; i < size(); ++i) {
2003  const int ix = getArrayIndex(i);
2004  newTimes[i] = m_times[ix];
2005  newValues[i] = m_values[ix];
2006  }
2007  m_times.swap(newTimes); // switch heap space
2008  m_values.swap(newValues);
2009  m_oldest = 0; // starts at the beginning now; size unchanged
2010  }
2011 
2012  // Initialize everything to its default-constructed state.
2013  void initDataMembers() {
2014  m_times.clear(); m_values.clear();
2015  m_oldest=m_size=0;
2016  m_nGrows=m_nShrinks=m_maxSize=m_maxCapacity=0;
2017  }
2018 
2019  // These are circular buffers of the same size.
2020  Array_<double,int> m_times;
2021  Array_<T,int> m_values;
2022  int m_oldest; // Array index of oldest (time,value)
2023  int m_size; // number of entries in use
2024 
2025  // Statistics.
2026  int m_nGrows, m_nShrinks, m_maxSize, m_maxCapacity;
2027 };
2030 template <class T>
2032  typedef Measure_Delay_Buffer<T> Buffer;
2033 public:
2034  // Allocate one cache entry in the base class for the value; we allocate
2035  // a specialized one for the buffer.
2037  : Measure_<T>::Implementation(1), m_delay(NaN),
2038  m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
2039 
2040  Implementation(const Measure_<T>& source, Real delay)
2041  : Measure_<T>::Implementation(1), m_source(source), m_delay(delay),
2042  m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
2043 
2044  // Default copy constructor gives us a new Implementation object,
2045  // but with reference to the *same* source measure.
2046 
2048  if (!source.isSameMeasure(this->m_source)) {
2049  this->m_source = source;
2050  this->invalidateTopologyCache();
2051  }
2052  }
2053 
2054  void setDelay(Real delay) {
2055  if (delay != this->m_delay) {
2056  this->m_delay = delay;
2057  this->invalidateTopologyCache();
2058  }
2059  }
2060 
2061  void setUseLinearInterpolationOnly(bool linearOnly) {
2062  if (linearOnly != this->m_useLinearInterpolationOnly) {
2063  this->m_useLinearInterpolationOnly = linearOnly;
2064  this->invalidateTopologyCache();
2065  }
2066  }
2067 
2068  void setCanUseCurrentValue(bool canUseCurrentValue) {
2069  if (canUseCurrentValue != this->m_canUseCurrentValue) {
2070  this->m_canUseCurrentValue = canUseCurrentValue;
2071  this->invalidateTopologyCache();
2072  }
2073  }
2074 
2075  const Measure_<T>& getSourceMeasure() const {return this->m_source;}
2076  Real getDelay() const {return this->m_delay;}
2078  { return this->m_useLinearInterpolationOnly; }
2080  { return this->m_canUseCurrentValue; }
2081 
2082 
2083  // Implementations of virtual methods.
2084 
2085  // This uses the default copy constructor.
2087  { return new Implementation(*this); }
2088 
2089  // Currently no derivative supported.
2091  { return 0; }
2092 
2093  // If we are allowed to use the current value of the source measure to
2094  // determine the delayed value, the depends-on stage here is the same as
2095  // for the source; otherwise it is Stage::Time.
2097  { return this->m_canUseCurrentValue ? m_source.getDependsOnStage(order)
2098  : Stage::Time; }
2099 
2100  // Calculate the delayed value and return it to the Measure base class to
2101  // be put in a cache entry.
2102  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
2103  OVERRIDE_11
2104  { const Subsystem& subsys = this->getSubsystem();
2105  const Buffer& buffer = Value<Buffer>::downcast
2106  (subsys.getDiscreteVariable(s,m_bufferIx));
2107  //TODO: use cubic interpolation if allowed
2108  buffer.calcValueAtTimeLinearOnly(s.getTime()-m_delay, value);
2109  }
2110 
2112  assert(m_bufferIx.isValid());
2113  const Subsystem& subsys = this->getSubsystem();
2114  Buffer& buffer = Value<Buffer>::updDowncast
2115  (subsys.updDiscreteVariable(s,m_bufferIx));
2116  buffer.clear();
2117  this->getSubsystem().getSystem().realize(s,m_source.getDependsOnStage());
2118  buffer.append(s.getTime()-m_delay, s.getTime(), m_source.getValue(s));
2119  }
2120 
2122  m_bufferIx = this->getSubsystem()
2124  new Value<Buffer>(), getDependsOnStageVirtual(0));
2125  }
2126 
2130  updateBuffer(s);
2131  }
2132 
2133  // This uses the buffer from the state to update the one in the
2134  // corresponding cache entry. The update adds the current value of the
2135  // source to the end of the buffer and tosses out unneeded old entries.
2136  void updateBuffer(const State& s) const {
2137  assert(m_bufferIx.isValid());
2138  const Subsystem& subsys = this->getSubsystem();
2139 
2140  const Buffer& prevBuffer = Value<Buffer>::downcast
2141  (subsys.getDiscreteVariable(s,m_bufferIx));
2142 
2143  Buffer& nextBuffer = Value<Buffer>::updDowncast
2144  (subsys.updDiscreteVarUpdateValue(s,m_bufferIx));
2145 
2146  const Real t = s.getTime();
2147  nextBuffer.copyInAndUpdate(prevBuffer, t-m_delay,
2148  t, m_source.getValue(s));
2149 
2150  subsys.markDiscreteVarUpdateValueRealized(s,m_bufferIx);
2151  }
2152 private:
2153  // TOPOLOGY STATE
2154  Measure_<T> m_source;
2155  Real m_delay;
2156  bool m_canUseCurrentValue;
2157  bool m_useLinearInterpolationOnly;
2158 
2159  // TOPOLOGY CACHE
2160  mutable DiscreteVariableIndex m_bufferIx; // auto-update
2161 };
2162 
2163 
2164 } // namespace SimTK
2165 
2166 
2167 
2168 
2169 #endif // SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
virtual void realizeMeasureModelVirtual(State &) const
Definition: MeasureImplementation.h:139
Definition: MeasureImplementation.h:2031
virtual void realizeMeasureReportVirtual(const State &) const
Definition: MeasureImplementation.h:146
void setIsPresumedValidAtDependsOnStage(bool presume)
Definition: MeasureImplementation.h:418
Definition: MeasureImplementation.h:613
Implementation()
Definition: MeasureImplementation.h:914
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:932
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1342
int getNumTimeDerivativesVirtual() const override
Extreme(f(t)) has the same number of derivatives as f except that they are all zero unless f(t) is a ...
Definition: MeasureImplementation.h:1519
const Measure_< T > & getOperandMeasure() const
Definition: MeasureImplementation.h:1098
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55
void setOperandMeasure(const Measure_< T > &operand)
Set the operand measure for this Extreme measure; this is a Topology stage change so you&#39;ll have to c...
Definition: MeasureImplementation.h:1467
const T & getUncachedValueVirtual(const State &s, int derivOrder) const override
This is only called when derivOrder >= the number of cache entries we have, but still <= the number o...
Definition: MeasureImplementation.h:758
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:218
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
bool isUsingApproximation() const
Definition: MeasureImplementation.h:1332
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable(State &s, Stage invalidates, AbstractValue *v, Stage updateDependsOn) const
Definition: Subsystem.h:99
virtual void realizeMeasureTimeVirtual(const State &) const
Definition: MeasureImplementation.h:141
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:750
void initializeVirtual(State &s) const override
At start of a time stepping study, this should be called to set the current extreme value to the curr...
Definition: MeasureImplementation.h:1556
#define SimTK_ASSERT1_ALWAYS(cond, msg, a1)
Definition: ExceptionMacros.h:351
This is the vector class intended to appear in user code for large, variable size column vectors...
Definition: BigMatrix.h:171
Apache License January AND DISTRIBUTION Definitions License shall mean the terms and conditions for and distribution as defined by Sections through of this document Licensor shall mean the copyright owner or entity authorized by the copyright owner that is granting the License Legal Entity shall mean the union of the acting entity and all other entities that control are controlled by or are under common control with that entity For the purposes of this definition control direct or to cause the direction or management of such whether by contract or including but not limited to software source documentation source
Definition: LICENSE.txt:26
Implementation(Stage invalidated, const T &defaultValue)
Definition: MeasureImplementation.h:724
Definition: MeasureImplementation.h:676
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:998
Modeling choices made.
Definition: Stage.h:55
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
Implementation(const Implementation &src)
Base class copy constructor removes the Subsystem and sets the reference count to zero...
Definition: MeasureImplementation.h:57
Implementation(const Implementation &source)
Copy constructor copies the number of cache entries from the source, but not the cache indices themse...
Definition: MeasureImplementation.h:448
friend class Implementation
Definition: Measure.h:251
bool getIsPresumedValidAtDependsOnStage() const
Definition: MeasureImplementation.h:422
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:994
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:755
bool isInSubsystem() const
Definition: MeasureImplementation.h:118
MeasureIndex adoptMeasure(AbstractMeasure &)
Obtain the Subsystem name if one was given on construction of the concrete Subsystem.
Definition: SubsystemGuts.h:507
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:2086
Implementation(const Measure_< T > &operand)
Definition: MeasureImplementation.h:1314
int decrRefCount() const
Definition: MeasureImplementation.h:76
Implementation * clone() const
This is a deep copy of the concrete Implementation object, except the Subsystem will have been remove...
Definition: MeasureImplementation.h:86
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
void setValue(State &s, const T &value) const
Set the value of the state variables(s) that hold the integral.
Definition: MeasureImplementation.h:1171
Implementation(const T &defaultValue, int numCacheEntries=1)
Definition: MeasureImplementation.h:426
Stage prev() const
Return the Stage before this one, with Stage::Empty returned if this Stage is already at its lowest v...
Definition: Stage.h:123
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:1003
void realizeMeasureAccelerationVirtual(const State &s) const override
Set the zdots to the integrand (derivative measure) value.
Definition: MeasureImplementation.h:1260
const T & getUncachedValueVirtual(const State &s, int derivOrder) const override
We&#39;re not using the Measure_<T> base class cache services, but we do have one of our own...
Definition: MeasureImplementation.h:1531
const Stage & getSystemStage() const
This returns the *global* stage for this State.
int size() const
Return the number of elements in the data type of this Measure; for Vector measures this is determine...
Definition: MeasureImplementation.h:459
void resize(size_type n)
Change the size of this Array, preserving all the elements that will still fit, and default construct...
Definition: Array.h:2053
void setInvalidatedStage(Stage invalidated)
Definition: MeasureImplementation.h:832
const Subsystem & getSubsystem() const
Definition: MeasureImplementation.h:119
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1051
virtual ~Implementation()
Definition: MeasureImplementation.h:134
Real getDiscreteVarLastUpdateTime(const State &s, DiscreteVariableIndex dx) const
Definition: Subsystem.h:253
void realize(const State &state, Stage stage=Stage::HighestRuntime) const
Realize the given state to the indicated stage.
Lower than any legitimate Stage.
Definition: Stage.h:53
virtual void realizeMeasureVelocityVirtual(const State &) const
Definition: MeasureImplementation.h:143
const Real Zero
Real(0)
void setOperandMeasure(const Measure_< T > &operand)
Definition: MeasureImplementation.h:1326
void setCanUseCurrentValue(bool canUseCurrentValue)
Definition: MeasureImplementation.h:2068
virtual void realizeMeasurePositionVirtual(const State &) const
Definition: MeasureImplementation.h:142
void setInitialConditionMeasure(const Measure_< T > &ic)
Definition: MeasureImplementation.h:1192
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
void realizeDynamics(const State &s) const
Definition: MeasureImplementation.h:94
Implementation(const Measure_< T > &source, Real delay)
Definition: MeasureImplementation.h:2040
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50
void realizeMeasureAccelerationVirtual(const State &s) const override
In case no one has updated the value of this measure yet, we have to make sure it gets updated before...
Definition: MeasureImplementation.h:1391
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:1056
int getNumTimeDerivativesVirtual() const override
This measure has one more time derivative than the integrand.
Definition: MeasureImplementation.h:1202
void realizeInstance(const State &s) const
Definition: MeasureImplementation.h:90
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:2096
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:1209
Track the value of the operand that is of maximum absolute value.
Definition: Measure.h:903
virtual const T & getUncachedValueVirtual(const State &, int derivOrder) const
This is only called when derivOrder >= the number of cache entries we have, but still <= the number o...
Definition: MeasureImplementation.h:552
The abstract parent of all Measure Implementation classes.
Definition: MeasureImplementation.h:48
MeasureIndex getSubsystemMeasureIndex() const
Return the MeasureIndex by which this Measure is known to the Subsystem that owns it...
Definition: MeasureImplementation.h:237
void ensureDerivativeIsRealized(const State &s) const
Definition: MeasureImplementation.h:1395
This is the base Implementation class for all Measures whose value type is known. ...
Definition: MeasureImplementation.h:363
Definition: MeasureImplementation.h:793
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:1119
String getName() const
Return a printable name corresponding to the stage level currently stored in this Stage...
Definition: Stage.h:128
void setValue(State &state, const T &value) const
Change the value of this Measure in the given state.
Definition: MeasureImplementation.h:743
bool isCacheValueRealized(const State &s, int derivOrder) const
Determine whether a particular one of this Measure&#39;s cache entries has already been realized since th...
Definition: MeasureImplementation.h:493
const T & getDefaultValue() const
Obtain a reference to the default value associated with this Measure.
Definition: Measure.h:288
Implementation()
The derivative and initialConditions Measures will be empty handles if this is default constructed...
Definition: MeasureImplementation.h:1153
void setDelay(Real delay)
Definition: MeasureImplementation.h:2054
int incrRefCount() const
Definition: MeasureImplementation.h:73
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:1379
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2037
const AbstractValue & getDiscreteVariable(const State &s, DiscreteVariableIndex index) const
Definition: Subsystem.h:245
This is the base class for all Measure handle classes.
Definition: Measure.h:151
Implementation()
This default constructor is for use by concrete measure implementation classes.
Definition: MeasureImplementation.h:52
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
virtual void realizeMeasureTopologyVirtual(State &) const
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:533
bool isInSubsystem() const
Test whether this Measure object has been adopted by a Subsystem.
Definition: MeasureImplementation.h:229
This Measure tracks extreme values attained by the elements of its source operand since the last init...
Definition: Measure.h:835
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:935
Vector & updZ(State &s) const
Definition: Subsystem.h:137
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:703
CacheEntryIndex allocateCacheEntry(const State &s, Stage dependsOn, Stage computedBy, AbstractValue *v) const
Definition: Subsystem.h:103
Stage getDependsOnStageVirtual(int order) const override
The depends-on stage for this measure is the same as for its operand.
Definition: MeasureImplementation.h:1524
Track the minimum value of the operand (signed).
Definition: Measure.h:883
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1115
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
bool getCanUseCurrentValue() const
Definition: MeasureImplementation.h:2079
This file declares the base class AbstractMeasure for all derived Measure handle classes, and the handle classes for built-in Measures.
const T & getUncachedValueVirtual(const State &s, int derivOrder) const override
This is only called when derivOrder >= the number of cache entries we have, but still <= the number o...
Definition: MeasureImplementation.h:1354
int getRefCount() const
Definition: MeasureImplementation.h:79
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
Definition: MeasureImplementation.h:1026
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:927
m
Definition: CMakeCache.txt:469
const Measure_< T > & getDerivativeMeasure() const
Definition: MeasureImplementation.h:1177
Real getTimeOfExtremeValue(const State &s) const
Return the time at which the extreme was last updated.
Definition: MeasureImplementation.h:1500
void markAsNotValid(const State &state) const
Definition: MeasureImplementation.h:869
Stage getStage(const State &s) const
Definition: Subsystem.h:239
void initializeVirtual(State &s) const override
Definition: MeasureImplementation.h:2111
Implementation(const T &amplitude, const T &frequency, const T &phase=T(0))
Definition: MeasureImplementation.h:918
Implementation()
Definition: MeasureImplementation.h:1312
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:989
bool isFinite(const negator< float > &x)
Definition: negator.h:287
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1198
void calcCachedValueVirtual(const State &, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:888
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1338
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:2102
Stage getDependsOnStage(int derivOrder) const
Definition: MeasureImplementation.h:105
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1047
void setValue(const T &v)
Changing the value of a Constant measure is a topological change; if this is a Vector measure you can...
Definition: MeasureImplementation.h:624
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:635
void setSourceMeasure(const Measure_< T > &source)
Definition: MeasureImplementation.h:2047
void markCacheValueNotRealized(const State &s, CacheEntryIndex cx) const
Definition: Subsystem.h:275
Implementation(const Implementation &source)
Definition: MeasureImplementation.h:729
const Measure_< T > & getOperandMeasure() const
Return a reference to the operand measure for this Extreme measure.
Definition: MeasureImplementation.h:1481
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:747
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:637
void initializeVirtual(State &s) const override
Definition: MeasureImplementation.h:1366
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:605
Report-only quantities evaluated.
Definition: Stage.h:62
ZIndex allocateZ(State &s, const Vector &zInit) const
Definition: Subsystem.h:92
int getCopyNumber() const
Definition: MeasureImplementation.h:81
const Real & getUncachedValueVirtual(const State &s, int derivOrder) const override
This is only called when derivOrder >= the number of cache entries we have, but still <= the number o...
Definition: MeasureImplementation.h:690
bool isSameMeasure(const AbstractMeasure &other) const
There can be multiple handles on the same Measure.
Definition: Measure.h:220
const Measure_< T > & getOperandMeasure() const
Definition: MeasureImplementation.h:1333
bool isCacheValueRealized(const State &s, CacheEntryIndex cx) const
Definition: Subsystem.h:271
Definition: MeasureImplementation.h:973
void markAsValid(const State &state) const
Definition: MeasureImplementation.h:850
const AbstractValue & getDiscreteVarUpdateValue(const State &s, DiscreteVariableIndex dx) const
Definition: Subsystem.h:259
Definition: MeasureImplementation.h:1079
const T & getCacheEntry(const State &s, int derivOrder) const
Get a const reference to the value stored in one of this Measure&#39;s cache entries, indexed by the deri...
Definition: MeasureImplementation.h:468
System topology realized.
Definition: Stage.h:54
SubsystemIndex getSubsystemIndex() const
Definition: MeasureImplementation.h:122
#define OVERRIDE_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:267
bool hasImpl() const
Definition: Measure.h:242
DiscreteVariableIndex allocateDiscreteVariable(State &s, Stage g, AbstractValue *v) const
Definition: Subsystem.h:96
bool getForceUseApproximation() const
Definition: MeasureImplementation.h:1331
Implementation(Stage dependsOn, Stage invalidated)
Definition: MeasureImplementation.h:802
Implementation()
Definition: MeasureImplementation.h:720
Forces calculated.
Definition: Stage.h:60
Implementation()
Definition: MeasureImplementation.h:2036
Implementation()
Definition: MeasureImplementation.h:1030
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
#define SimTK_ERRCHK_ALWAYS(cond, whereChecked, msg)
Definition: ExceptionMacros.h:281
void realizeAcceleration(const State &s) const
Definition: MeasureImplementation.h:95
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:2121
void setOperation(Operation op)
Set the particular operation to be performed by this Extreme measure; this is a Topology stage change...
Definition: MeasureImplementation.h:1475
int getNumTimeDerivatives() const
Every Measure can produce a value, and some can provide one or more total derivatives with respect to...
Definition: MeasureImplementation.h:241
const T & getUncachedValueVirtual(const State &, int derivOrder) const override
This is only called when derivOrder >= the number of cache entries we have, but still <= the number o...
Definition: MeasureImplementation.h:630
Unique integer type for Subsystem-local z indexing.
CacheEntryIndex allocateLazyCacheEntry(const State &state, Stage earliest, AbstractValue *v) const
Definition: Subsystem.h:111
void realizeMeasureTopologyVirtual(State &s) const override
Allocate the auto-updated state variable that holds the extreme seen so far.
Definition: MeasureImplementation.h:1568
void initialize(State &s) const
This should be called at the start of a time stepping study to cause this Measure to set its state va...
Definition: MeasureImplementation.h:101
Implementation(const Measure_< T > &left, const Measure_< T > &right)
Definition: MeasureImplementation.h:1032
Implementation & operator=(const Implementation &src)
Base class copy assignment operator removes the Subsystem, and sets the reference count to zero...
Definition: MeasureImplementation.h:63
Stage getInvalidatedStage() const
Definition: MeasureImplementation.h:847
Operation getOperation() const
Return the particular operation being performed by this Extreme measure.
Definition: MeasureImplementation.h:1485
Implementation()
Definition: MeasureImplementation.h:684
int getRefCount() const
Definition: MeasureImplementation.h:249
#define SimTK_ERRCHK3_ALWAYS(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:293
virtual void realizeMeasureDynamicsVirtual(const State &) const
Definition: MeasureImplementation.h:144
const Subsystem & getSubsystem() const
Return a reference to the Subsystem that owns this Measure.
Definition: MeasureImplementation.h:233
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:43
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:878
void markCacheValueRealized(const State &s, CacheEntryIndex cx) const
Definition: Subsystem.h:273
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1042
void invalidateTopologyCache() const
Definition: MeasureImplementation.h:125
Implementation(const Measure_< T > &left, const Measure_< T > &right)
Definition: MeasureImplementation.h:979
╨╧ рб▒ с ■  ╖ ╣ ■    │ ┤ ╡ ╢                                                                                                                                                                                                                                                                                                                                                                                                                                     ье┴ А ° ┐ ч bjbjcTcT ┌┘ │ ├ ╗ t          ╖ Я ┴ K K K D      П П П А Л2 Ф П Z╞ j J a n u a r A b s t r a c t W e d e s c r i b e t h e g o a l s a n d d e s i g n d e c i s i o n b e h i n d S i m m a t r i t h e S i m T K m a t r i x a n d l i n e a r a l g e b r a l i b r a r a n d p r o v i d e r e f e r e n c e i n f o r m a t i o n f o r u s i n g i t T h e i d e a i s t o p r o v i d e t h e p o w e n a t u r a l n e s s
Definition: Simmatrix.doc:7
This creates a Measure whose value is a Topology-stage constant of any type T.
Definition: Measure.h:339
void updateBuffer(const State &s) const
Definition: MeasureImplementation.h:2136
virtual void calcCachedValueVirtual(const State &, int derivOrder, T &value) const
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:539
void realizeReport(const State &s) const
Definition: MeasureImplementation.h:96
void setForceUseApproximation(bool mustApproximate)
Definition: MeasureImplementation.h:1321
Subsystem & updSubsystem()
Definition: MeasureImplementation.h:120
The abstract parent of all Subsystem implementation classes.
Definition: SubsystemGuts.h:47
void realizeMeasureAccelerationVirtual(const State &s) const override
In case no one has updated the value of this measure yet, we have to make sure it gets updated before...
Definition: MeasureImplementation.h:1589
const Vector & getZ(const State &s) const
Definition: Subsystem.h:128
Zero()
Definition: MeasureImplementation.h:651
virtual void realizeMeasureAccelerationVirtual(const State &) const
Definition: MeasureImplementation.h:145
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:881
MeasureIndex getSubsystemMeasureIndex() const
Definition: MeasureImplementation.h:121
Implementation()
Definition: MeasureImplementation.h:977
~AbstractMeasure()
Destructor decrements the Implementation&#39;s reference count and deletes the object if the count goes t...
Definition: MeasureImplementation.h:225
T & updValue(const State &state) const
Definition: MeasureImplementation.h:873
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:1225
const System & getSystem() const
Return a const reference to the System that contains this Subsystem.
Definition: SubsystemGuts.h:518
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:697
void swap(Array_ &other)
This is a specialized algorithm providing constant time exchange of data with another array that has ...
Definition: Array.h:1946
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
Stage getStage(const State &s) const
Definition: MeasureImplementation.h:128
const T & getValue(const State &s, int derivOrder) const
Definition: MeasureImplementation.h:365
const Measure_< T > & getSourceMeasure() const
Definition: MeasureImplementation.h:2075
const Measure_< T > & getInitialConditionMeasure() const
Definition: MeasureImplementation.h:1183
const T & getUncachedValueVirtual(const State &s, int derivOrder) const override
This is only called when derivOrder >= the number of cache entries we have, but still <= the number o...
Definition: MeasureImplementation.h:1219
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
#define SimTK_ERRCHK2(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:328
Specialized information about Composite Numerical Types which allows us to define appropriate templat...
Definition: CompositeNumericalTypes.h:136
Definition: MeasureImplementation.h:909
int getNumCacheEntries() const
Return the number of cache entries allocated for the value and derivatives of this Measure...
Definition: MeasureImplementation.h:463
One()
Definition: MeasureImplementation.h:661
void realizeMeasureTopologyVirtual(State &s) const override
Allocate one Real continuous state variable z per element of this Measure&#39;s data type T...
Definition: MeasureImplementation.h:1251
const Real One
Real(1)
Declares the user-visible part of a SimTK::State, the implementation is done in a separate internal c...
Definition: MeasureImplementation.h:1446
bool isValid(const State &state) const
Definition: MeasureImplementation.h:866
const T & getDefaultValue() const
Return a reference to the value that this Measure will use to initialize its value-level state resour...
Definition: MeasureImplementation.h:416
Implementation(const Measure_< T > &deriv, const Measure_< T > &ic, const T &defaultValue)
Here we&#39;re shallow-copying the Measure handles so we&#39;ll be referring to the original Measures...
Definition: MeasureImplementation.h:1157
void realizeMeasureAccelerationVirtual(const State &s) const override
In case no one has updated the value of this measure yet, we have to make sure it gets updated before...
Definition: MeasureImplementation.h:2129
Stage getDependsOnStageVirtual(int derivOrder) const override
Cache value is available after its "depends on" stage has been realized; but all its derivatives are ...
Definition: MeasureImplementation.h:885
This unique integer type is for selecting discrete variables.
The implementation for Integrate measures allocates a continuous state variable or variables from the...
Definition: MeasureImplementation.h:1148
Implementation(const T &value)
Definition: MeasureImplementation.h:619
void realizeVelocity(const State &s) const
Definition: MeasureImplementation.h:93
T & updCacheEntry(const State &s, int derivOrder) const
Get a writable reference to the value stored in one of this Measure&#39;s cache entries, indexed by the derivative order (with the value treated as the 0th derivative).
Definition: MeasureImplementation.h:481
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1106
Implementation(const Implementation &source)
Definition: MeasureImplementation.h:813
#define FINAL_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:268
Implementation()
Default constructor leaves the operand measure unspecified; no base class cache entries are allocated...
Definition: MeasureImplementation.h:1453
const T & getValue(const State &s, int derivOrder=0) const
Retrieve the Value of this Measure or one of its time derivatives, assuming the supplied State has be...
Definition: Measure.h:274
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1514
void setDerivativeMeasure(const Measure_< T > &d)
Definition: MeasureImplementation.h:1190
Track the maximum value of the operand (signed).
Definition: Measure.h:893
AbstractMeasure & deepAssign(const AbstractMeasure &source)
Deep assignment clones the Implementation object pointed to by the source handle, so that this handle...
Definition: MeasureImplementation.h:212
Implementation(int numCacheEntries=1)
Argument numCacheEntries should be one greater than the number of derivatives; that is...
Definition: MeasureImplementation.h:437
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:2090
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1346
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:764
Provide a unique integer type for identifying Subsystems.
const Implementation & getImpl() const
Definition: Measure.h:240
void setUseLinearInterpolationOnly(bool linearOnly)
Definition: MeasureImplementation.h:2061
int getNumTimeDerivatives() const
Definition: MeasureImplementation.h:103
Implementation(const Measure_< T > &operand, Operation op)
Construct a measure that returns the extreme value taken on by the operand measure during a time step...
Definition: MeasureImplementation.h:1458
Operation
Definition: Measure.h:839
Track the value of the operand that is of minimum absolute value (not very useful).
Definition: Measure.h:914
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607
Implementation()
Definition: MeasureImplementation.h:1085
An object of this type is used as a dummy argument to make sure the automatically-generated handle co...
Definition: Measure.h:156
const Real & getTime() const
You can call these as long as *system* stage >= Model.
void setScaleFactor(Real sf)
Definition: MeasureImplementation.h:1093
void setDependsOnStage(Stage dependsOn)
Definition: MeasureImplementation.h:818
Implementation(Real factor, const Measure_< T > &operand)
Definition: MeasureImplementation.h:1087
void setSubsystem(Subsystem &sub, MeasureIndex mx)
Definition: MeasureImplementation.h:114
AbstractMeasure(Implementation *g=0)
Provide an Implementation for this AbstractMeasure and bump its reference count.
Definition: MeasureImplementation.h:175
void setDefaultValue(const T &defaultValue)
Set a new default value for this Measure.
Definition: MeasureImplementation.h:407
AbstractMeasure & shallowAssign(const AbstractMeasure &)
Shallow assignment operator destructs the current Implementation object (meaning its reference count ...
Definition: MeasureImplementation.h:199
Definition: MeasureImplementation.h:713
Constant & setValue(const T &value)
Change the value returned by this Measure.
Definition: Measure.h:356
Stage getInvalidatedStage() const
Definition: MeasureImplementation.h:738
void realizeTime(const State &s) const
Definition: MeasureImplementation.h:91
A new time has been realized.
Definition: Stage.h:57
const T & getValueZero() const
Return a reference to a zero of the same type and size as this Measure&#39;s value.
Definition: MeasureImplementation.h:563
This is the header which should be included in user programs that would like to make use of all the S...
static K getNaN()
Definition: CompositeNumericalTypes.h:246
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:699
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:639
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1111
Implementation(const Implementation &source)
Copy constructor shallow-copies the referenced measures, but we don&#39;t want to share our state variabl...
Definition: MeasureImplementation.h:1164
virtual void realizeMeasureInstanceVirtual(const State &) const
Definition: MeasureImplementation.h:140
bool getUseLinearInterpolationOnly() const
Definition: MeasureImplementation.h:2077
Includes internal headers providing declarations for the basic SimTK Core classes.
size_type capacity() const
Return the number of elements this array can currently hold without requiring reallocation.
Definition: Array.h:2047
void setValue(State &s, const T &value) const
Set the current extreme value stored in this Extreme measure&#39;s state variable.
Definition: MeasureImplementation.h:1489
void setInvalidatedStage(Stage invalidates)
Definition: MeasureImplementation.h:733
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:930
bool ensureExtremeHasBeenUpdated(const State &s) const
Here we make sure that the cache entry is updated if the current value of the operand is more extreme...
Definition: MeasureImplementation.h:1598
void invalidateAllCacheAtOrAbove(Stage) const
If any subsystem or the system stage is currently at or higher than the passed-in one...
Real getDelay() const
Definition: MeasureImplementation.h:2076
Stage getDependsOnStage() const
Definition: MeasureImplementation.h:846
virtual void initializeVirtual(State &) const
Definition: MeasureImplementation.h:148
void realizePosition(const State &s) const
Definition: MeasureImplementation.h:92
void initializeVirtual(State &s) const override
Initialize the state to the current value of the initial condition measure, if there is one...
Definition: MeasureImplementation.h:1232
void realizeModel(State &s) const
Definition: MeasureImplementation.h:89
Vector & updZDot(const State &s) const
Definition: Subsystem.h:152
Definition: MeasureImplementation.h:1306
Implementation()
Definition: MeasureImplementation.h:798
Definition: negator.h:64
Stage getDependsOnStage(int derivOrder=0) const
At what Stage can we expect the value of this AbstractMeasure or one of its time derivatives to be av...
Definition: MeasureImplementation.h:245
void markCacheValueNotRealized(const State &s, int derivOrder) const
Invalidate one of this Measure&#39;s cache entries.
Definition: MeasureImplementation.h:518
void markCacheValueRealized(const State &s, int derivOrder) const
Mark one of this Measure&#39;s cache entries up to date; call this after you have calculated a value or d...
Definition: MeasureImplementation.h:505
This is the base handle class for all Measures whose value type is known, including all the Simbody b...
Definition: Measure.h:261
virtual int getNumTimeDerivativesVirtual() const
Definition: MeasureImplementation.h:150
Implementation()
Definition: MeasureImplementation.h:618
Templatized version of the abstract class, providing generic type-specific functionality that does no...
Definition: Value.h:67