Simbody  3.7
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-14 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 
132  virtual ~Implementation() {}
133  virtual Implementation* cloneVirtual() const = 0;
134 
135  virtual void realizeTopology(State&)const = 0;
136 
137  virtual void realizeMeasureModelVirtual(State&) const {}
138  virtual void realizeMeasureInstanceVirtual(const State&) const {}
139  virtual void realizeMeasureTimeVirtual(const State&) const {}
140  virtual void realizeMeasurePositionVirtual(const State&) const {}
141  virtual void realizeMeasureVelocityVirtual(const State&) const {}
142  virtual void realizeMeasureDynamicsVirtual(const State&) const {}
143  virtual void realizeMeasureAccelerationVirtual(const State&) const {}
144  virtual void realizeMeasureReportVirtual(const State&) const {}
145 
146  virtual void initializeVirtual(State&) const {}
147  virtual int getNumTimeDerivativesVirtual() const {return 0;}
148  virtual Stage getDependsOnStageVirtual(int order) const = 0;
149 
150 private:
151  int copyNumber; // bumped each time we do a deep copy
152 
153  // These are set when this Measure is adopted by a Subsystem.
154  Subsystem* mySubsystem;
155  MeasureIndex myIndex;
156 
157  // Measures have shallow copy semantics so they share the Implementation
158  // objects, which are only deleted when the refCount goes to zero.
159  mutable int refCount;
160 
161 friend class AbstractMeasure;
162 friend class Subsystem::Guts;
163 };
164 
165 //==============================================================================
166 // ABSTRACT MEASURE DEFINITIONS
167 //==============================================================================
168 // These had to wait for AbstractMeasure::Implementation to be defined.
169 
170 inline AbstractMeasure::
172 : impl(g)
173 { if (impl) impl->incrRefCount(); }
174 
175 inline AbstractMeasure::
177 : impl(g) {
178  SimTK_ERRCHK(hasImpl(), "AbstractMeasure::AbstractMeasure()",
179  "An empty Measure handle can't be put in a Subsystem.");
180  impl->incrRefCount();
181  sub.adoptMeasure(*this);
182 }
183 
184 // Shallow copy constructor.
186 : impl(0) {
187  if (src.impl) {
188  impl = src.impl;
189  impl->incrRefCount();
190  }
191 }
192 
193 // Shallow assignment.
196  if (impl != src.impl) {
197  if (impl && impl->decrRefCount()==0) delete impl;
198  impl = src.impl;
199  impl->incrRefCount();
200  }
201  return *this;
202 }
203 
204 // Note that even if the source and destination are currently pointing
205 // to the same Implementation, we still have to make a new copy so that
206 // afterwards the destination has its own, refcount==1 copy.
209  if (&src != this) {
210  if (impl && impl->decrRefCount()==0) delete impl;
211  if (src.impl) {
212  impl = src.impl->clone();
213  impl->incrRefCount();
214  } else
215  impl = 0;
216  }
217  return *this;
218 }
219 
220 inline AbstractMeasure::
222 { if (impl && impl->decrRefCount()==0) delete impl;}
223 
224 inline bool AbstractMeasure::
226 { return hasImpl() && getImpl().isInSubsystem(); }
227 
228 inline const Subsystem& AbstractMeasure::
230 { return getImpl().getSubsystem(); }
231 
232 inline bool AbstractMeasure::
233 isSameSubsystem(const Subsystem& other) const
234 { return getSubsystem().isSameSubsystem(other); }
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 
531  virtual void realizeMeasureTopologyVirtual(State&) const {}
532 
535  virtual void
536  calcCachedValueVirtual(const State&, int derivOrder, T& value) const
537  { SimTK_ERRCHK1_ALWAYS(!"implemented",
538  "Measure_<T>::Implementation::calcCachedValueVirtual()",
539  "This method should have been overridden by the derived"
540  " Measure but was not. It is needed to calculate the"
541  " cached value for derivOrder=%d.", derivOrder); }
542 
548  virtual const T&
549  getUncachedValueVirtual(const State&, int derivOrder) const
550  { SimTK_ERRCHK1_ALWAYS(!"implemented",
551  "Measure_<T>::Implementation::getUncachedValueVirtual()",
552  "This method should have been overridden by the derived"
553  " Measure but was not. It is needed to return the uncached"
554  " value at derivOrder=%d.", derivOrder);
555  return *reinterpret_cast<T*>(0);
556  }
557 
560  const T& getValueZero() const {return zeroValue;}
561 
562 private:
563  // Satisfy the realizeTopology() pure virtual here now that we know the
564  // data type T. Allocate lazy- or auto-validated- cache entries depending
565  // on the setting of presumeValidAtDependsOnStage.
566  void realizeTopology(State& s) const override final {
567  // Allocate cache entries. Initialize the value cache entry to
568  // the given defaultValue; all the derivative cache entries should be
569  // initialized to a NaN of the same size.
570  if (getNumCacheEntries()) {
571  derivIx[0] = presumeValidAtDependsOnStage
573  (s, getDependsOnStage(0), new Value<T>(defaultValue))
575  (s, getDependsOnStage(0), new Value<T>(defaultValue));
576 
577  if (getNumCacheEntries() > 1) {
578  T nanValue; Measure_Num<T>::makeNaNLike(defaultValue, nanValue);
579  for (int i=1; i < getNumCacheEntries(); ++i) {
580  derivIx[i] = presumeValidAtDependsOnStage
582  (s, getDependsOnStage(i), new Value<T>(nanValue))
584  (s, getDependsOnStage(i), new Value<T>(nanValue));
585  }
586  }
587  }
588 
589  // Call the concrete class virtual if any.
590  realizeMeasureTopologyVirtual(s);
591  }
592 
593 //------------------------------------------------------------------------------
594 private:
595  // TOPOLOGY STATE
596  bool presumeValidAtDependsOnStage;
597  T defaultValue;
598  T zeroValue;
599 
600  // TOPOLOGY CACHE
601  mutable Array_<CacheEntryIndex> derivIx;
602 };
603 
604 
605 
606 //==============================================================================
607 // CONSTANT :: IMPLEMENTATION
608 //==============================================================================
609 template <class T>
611 : public Measure_<T>::Implementation
612 {
613 public:
614  // We don't want the base class to allocate *any* cache entries.
616  explicit Implementation(const T& value)
617  : Measure_<T>::Implementation(value,0) {}
618 
621  void setValue(const T& v) {this->setDefaultValue(v);}
622 
623  // Implementations of virtual methods.
624  // Measure_<T> virtuals:
625  // No cached values.
626 
627  const T& getUncachedValueVirtual(const State&, int derivOrder) const
628  override
629  { return derivOrder>0 ? this->getValueZero() : this->getDefaultValue(); }
630 
631  // AbstractMeasure virtuals:
632  Implementation* cloneVirtual() const override
633  { return new Implementation(*this); }
634  Stage getDependsOnStageVirtual(int derivOrder) const override
635  { return derivOrder>0 ? Stage::Empty : Stage::Topology; }
636  int getNumTimeDerivativesVirtual() const override
637  { return std::numeric_limits<int>::max(); }
638 };
639 
640 
641 
642 //==============================================================================
643 // MEASURE ZERO and ONE
644 //==============================================================================
645 // These had to wait for Constant::Implementation to be declared.
646 
647 template <class T> inline
649 template <class T> inline
651 
653 : Constant(Vector(size, Real(0))) {}
655 : Constant(sub, Vector(size, Real(0))) {}
656 
657 template <class T> inline
659 template <class T> inline
661 
663 : Constant(Vector(size, Real(1))) {}
665 : Constant(sub, Vector(size, Real(1))) {}
666 
667 
668 
669 //==============================================================================
670 // TIME :: IMPLEMENTATION
671 //==============================================================================
672 template <class T>
674 
675 template <>
677 : public Measure_<Real>::Implementation
678 {
679 public:
680  // We don't want the base class to allocate *any* cache entries.
682 
683  // Implementations of virtual methods.
684  // Measure_<Real> virtuals:
685  // No cached values.
686 
687  const Real& getUncachedValueVirtual(const State& s, int derivOrder) const
688  override
689  { return derivOrder==0 ? s.getTime()
690  : (derivOrder==1 ? SimTK::One
691  : SimTK::Zero); }
692 
693  // AbstractMeasure virtuals:
694  Implementation* cloneVirtual() const override
695  { return new Implementation(*this); }
696  Stage getDependsOnStageVirtual(int derivOrder) const override
697  { return derivOrder>0 ? Stage::Empty : Stage::Time; }
698 
699  // Value is t, 1st derivative is 1, the rest are 0.
700  int getNumTimeDerivativesVirtual() const override
701  { return std::numeric_limits<int>::max(); }
702 };
703 
704 
705 
706 //==============================================================================
707 // VARIABLE :: IMPLEMENTATION
708 //==============================================================================
709 template <class T>
711 : public Measure_<T>::Implementation
712 {
713 public:
714  // We don't want the base class to allocate *any* cache entries;
715  // we'll use the variable as its own value and zeroes for all
716  // the derivatives.
718  : Measure_<T>::Implementation(0),
719  invalidatedStage(Stage::Empty) {}
720 
721  Implementation(Stage invalidated, const T& defaultValue)
722  : Measure_<T>::Implementation(defaultValue, 0),
723  invalidatedStage(invalidated) {}
724 
725  // Copy constructor should not copy the variable.
727  : Measure_<T>::Implementation(source.getDefaultValue(), 0),
728  invalidatedStage(source.invalidatedStage) {}
729 
730  void setInvalidatedStage(Stage invalidates) {
731  invalidatedStage = invalidates;
732  this->invalidateTopologyCache();
733  }
734 
735  Stage getInvalidatedStage() const {return invalidatedStage;}
736 
740  void setValue(State& state, const T& value) const
741  { updVarValue(state) = value; }
742 
743  // Implementations of virtual methods.
744  Implementation* cloneVirtual() const override
745  { return new Implementation(*this); }
746 
747  int getNumTimeDerivativesVirtual() const override
748  { return std::numeric_limits<int>::max(); }
749 
750  // Discrete variable is available after Model stage; but all its
751  // derivatives are zero so are always available.
752  Stage getDependsOnStageVirtual(int derivOrder) const override
753  { return derivOrder>0 ? Stage::Empty : Stage::Model;}
754 
755  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
756  override
757  { return derivOrder>0 ? this->getValueZero() : getVarValue(s); }
758 
759  // No cached values.
760 
761  void realizeMeasureTopologyVirtual(State& s) const override {
762  discreteVarIndex = this->getSubsystem().allocateDiscreteVariable
763  (s, invalidatedStage, new Value<T>(this->getDefaultValue()));
764  }
765 private:
766  const T& getVarValue(const State& s) const {
767  assert(discreteVarIndex.isValid());
768  return Value<T>::downcast(
769  this->getSubsystem().getDiscreteVariable(s, discreteVarIndex));
770  }
771  T& updVarValue(State& s) const {
772  assert(discreteVarIndex.isValid());
773  return Value<T>::downcast(
774  this->getSubsystem().updDiscreteVariable(s, discreteVarIndex));
775  }
776 
777  // TOPOLOGY STATE
778  Stage invalidatedStage; // TODO this shouldn't be needed
779 
780  // TOPOLOGY CACHE
781  mutable DiscreteVariableIndex discreteVarIndex;
782 };
783 
784 
785 
786 //==============================================================================
787 // RESULT :: IMPLEMENTATION
788 //==============================================================================
789 template <class T>
791 : public Measure_<T>::Implementation
792 {
793 public:
794  // We want the base class to allocate a single cache entry of type T.
796  : Measure_<T>::Implementation(1),
797  dependsOnStage(Stage::Topology), invalidatedStage(Stage::Infinity) {}
798 
799  Implementation(Stage dependsOn, Stage invalidated)
800  : Measure_<T>::Implementation(1),
801  dependsOnStage(dependsOn==Stage::Empty ? Stage::Topology : dependsOn),
802  invalidatedStage(invalidated)
803  { SimTK_ERRCHK2_ALWAYS(invalidated > dependsOn,"Measure::Result::ctor()",
804  "Got invalidated stage %s and dependsOn stage %s which is illegal "
805  "because the invalidated stage must be later than dependsOn.",
806  invalidated.getName().c_str(), dependsOn.getName().c_str());
807  }
808 
809  // Copy constructor will not copy the cache entry index.
811  : Measure_<T>::Implementation(source),
812  dependsOnStage(source.dependsOnStage),
813  invalidatedStage(source.invalidatedStage) {}
814 
815  void setDependsOnStage(Stage dependsOn) {
816  if (dependsOn == Stage::Empty) dependsOn = Stage::Topology;
817  SimTK_ERRCHK2_ALWAYS(dependsOn < getInvalidatedStage(),
818  "Measure::Result::setDependsOnStage()",
819  "The provided dependsOn stage %s is illegal because it is not "
820  "less than the current invalidated stage %s. Change the "
821  "invalidated stage first with setInvalidatedStage().",
822  dependsOn.getName().c_str(),
823  getInvalidatedStage().getName().c_str());
824 
825  dependsOnStage = dependsOn;
826  this->invalidateTopologyCache();
827  }
828 
829  void setInvalidatedStage(Stage invalidated) {
830  SimTK_ERRCHK2_ALWAYS(invalidated > getDependsOnStage(),
831  "Measure::Result::setInvalidatedStage()",
832  "The provided invalidated stage %s is illegal because it is not "
833  "greater than the current dependsOn stage %s. Change the "
834  "dependsOn stage first with setDependsOnStage().",
835  invalidated.getName().c_str(),
836  getDependsOnStage().getName().c_str());
837 
838  invalidatedStage = invalidated;
839  this->invalidateTopologyCache();
840  }
841 
842 
843  Stage getDependsOnStage() const {return dependsOnStage;}
844  Stage getInvalidatedStage() const {return invalidatedStage;}
845 
846 
847  void markAsValid(const State& state) const
848  { const Stage subsystemStage = this->getSubsystem().getStage(state);
849  SimTK_ERRCHK3_ALWAYS(subsystemStage >= getDependsOnStage().prev(),
850  "Measure::Result::markAsValid()",
851  "This Result Measure cannot be marked valid in a State where this "
852  "measure's Subsystem has been realized only to stage %s, because "
853  "its value was declared to depend on stage %s. To mark it valid, "
854  "we require that the State have been realized at least to the "
855  "previous stage (%s in this case); that is, you must at least be "
856  "*working on* the dependsOn stage in order to claim this result is "
857  "available.",
858  subsystemStage.getName().c_str(),
859  getDependsOnStage().getName().c_str(),
860  getDependsOnStage().prev().getName().c_str());
861  this->markCacheValueRealized(state, 0); }
862 
863  bool isValid(const State& state) const
864  { return this->isCacheValueRealized(state, 0); }
865 
866  void markAsNotValid(const State& state) const
867  { this->markCacheValueNotRealized(state, 0);
868  state.invalidateAllCacheAtOrAbove(invalidatedStage); }
869 
870  T& updValue(const State& state) const
871  { markAsNotValid(state); return this->updCacheEntry(state, 0); }
872 
873 
874  // Implementations of virtual methods.
875  Implementation* cloneVirtual() const override
876  { return new Implementation(*this); }
877 
878  int getNumTimeDerivativesVirtual() const override {return 0;}
879 
882  Stage getDependsOnStageVirtual(int derivOrder) const override
883  { return derivOrder>0 ? Stage::Empty : dependsOnStage;}
884 
885  void calcCachedValueVirtual(const State&, int derivOrder, T& value) const
886  override
887  { SimTK_ERRCHK_ALWAYS(!"calcCachedValueVirtual() implemented",
888  "Measure_<T>::Result::getValue()",
889  "Measure_<T>::Result::getValue() was called when the value was not "
890  "yet valid. For most Measure types, this would have initiated "
891  "computation of the value, but Result measures must have their values "
892  "calculated and set externally, and then marked valid."); }
893 
894 private:
895  // TOPOLOGY STATE
896  Stage dependsOnStage;
897  Stage invalidatedStage;
898 };
899 
900 
901 
902 //==============================================================================
903 // SINUSOID :: IMPLEMENTATION
904 //==============================================================================
905 template <class T>
907 : public Measure_<T>::Implementation
908 {
909  static const int NumDerivs = 3;
910 public:
912  : Measure_<T>::Implementation(NumDerivs+1),
913  a(CNT<T>::getNaN()), w(CNT<T>::getNaN()), p(CNT<T>::getNaN()) {}
914 
915  Implementation(const T& amplitude,
916  const T& frequency,
917  const T& phase=T(0))
918  : Measure_<T>::Implementation(NumDerivs+1),
919  a(amplitude), w(frequency), p(phase) {}
920 
921  // Default copy constructor is fine.
922 
923  // Implementations of virtual methods.
924  Implementation* cloneVirtual() const override
925  { return new Implementation(*this); }
926 
927  int getNumTimeDerivativesVirtual() const override {return NumDerivs;}
928 
929  Stage getDependsOnStageVirtual(int order) const override
930  { return Stage::Time; }
931 
932  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
933  override
934  {
935  // We need to allow the compiler to select std::sin or SimTK::sin
936  // based on the argument type.
937  using std::sin; using std::cos;
938 
939  assert(NumDerivs == 3);
940  const Real t = s.getTime();
941  const T arg = w*t + p;
942 
943  switch (derivOrder) {
944  case 0: value = a*sin(arg); break;
945  case 1: value = w*a*cos(arg); break;
946  case 2: value = -w*w*a*sin(arg); break;
947  case 3: value = -w*w*w*a*cos(arg); break;
948  default: SimTK_ASSERT1_ALWAYS(!"out of range",
949  "Measure::Sinusoid::Implementation::calcCachedValueVirtual():"
950  " derivOrder %d is out of range 0-3.", derivOrder);
951  }
952  }
953 
954  // There are no uncached values.
955 
956 private:
957  // TOPOLOGY STATE
958  T a, w, p;
959 
960  // TOPOLOGY CACHE
961  // nothing
962 };
963 
964 
965 
966 //==============================================================================
967 // PLUS :: IMPLEMENTATION
968 //==============================================================================
969 template <class T>
971 public:
972  // TODO: Currently allocates just one cache entry.
973  // left and right will be empty handles.
975 
977  const Measure_<T>& right)
978  : left(left), right(right) {}
979 
980  // Default copy constructor gives us a new Implementation object,
981  // but with references to the *same* operand measures.
982 
983  // Implementations of virtual methods.
984 
985  // This uses the default copy constructor.
986  Implementation* cloneVirtual() const override
987  { return new Implementation(*this); }
988 
989  // TODO: Let this be settable up to the min number of derivatives
990  // provided by the arguments.
991  int getNumTimeDerivativesVirtual() const override {return 0;}
992  //{ return std::min(left.getNumTimeDerivatives(),
993  // right.getNumTimeDerivatives()); }
994 
995  Stage getDependsOnStageVirtual(int order) const override
996  { return Stage(std::max(left.getDependsOnStage(order),
997  right.getDependsOnStage(order))); }
998 
999 
1000  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1001  override
1002  {
1003  value = left.getValue(s,derivOrder) + right.getValue(s,derivOrder);
1004  }
1005 
1006  // There are no uncached values.
1007 
1008 private:
1009  // TOPOLOGY STATE
1010  Measure_<T> left;
1011  Measure_<T> right;
1012 
1013  // TOPOLOGY CACHE
1014  // nothing
1015 };
1016 
1017 
1018 
1019 //==============================================================================
1020 // MINUS :: IMPLEMENTATION
1021 //==============================================================================
1022 template <class T>
1024 public:
1025  // TODO: Currently allocates just one cache entry.
1026  // left and right will be empty handles.
1028 
1030  const Measure_<T>& right)
1031  : left(left), right(right) {}
1032 
1033  // Default copy constructor gives us a new Implementation object,
1034  // but with references to the *same* operand measures.
1035 
1036  // Implementations of virtual methods.
1037 
1038  // This uses the default copy constructor.
1039  Implementation* cloneVirtual() const override
1040  { return new Implementation(*this); }
1041 
1042  // TODO: Let this be settable up to the min number of derivatives
1043  // provided by the arguments.
1044  int getNumTimeDerivativesVirtual() const override {return 0;}
1045  //{ return std::min(left.getNumTimeDerivatives(),
1046  // right.getNumTimeDerivatives()); }
1047 
1048  Stage getDependsOnStageVirtual(int order) const override
1049  { return Stage(std::max(left.getDependsOnStage(order),
1050  right.getDependsOnStage(order))); }
1051 
1052 
1053  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1054  override
1055  {
1056  value = left.getValue(s,derivOrder) - right.getValue(s,derivOrder);
1057  }
1058 
1059  // There are no uncached values.
1060 
1061 private:
1062  // TOPOLOGY STATE
1063  Measure_<T> left;
1064  Measure_<T> right;
1065 
1066  // TOPOLOGY CACHE
1067  // nothing
1068 };
1069 
1070 
1071 
1072 //==============================================================================
1073 // SCALE :: IMPLEMENTATION
1074 //==============================================================================
1075 template <class T>
1077 : public Measure_<T>::Implementation
1078 {
1079 public:
1080  // TODO: Currently allocates just one cache entry.
1081  // scale will be uninitialized, operand will be empty handle.
1082  Implementation() : factor(NaN) {}
1083 
1084  Implementation(Real factor, const Measure_<T>& operand)
1085  : factor(factor), operand(operand) {}
1086 
1087  // Default copy constructor gives us a new Implementation object,
1088  // but with references to the *same* operand measure.
1089 
1091  factor = sf;
1092  this->invalidateTopologyCache();
1093  }
1094 
1096  {
1097  return operand;
1098  }
1099 
1100  // Implementations of virtual methods.
1101 
1102  // This uses the default copy constructor.
1103  Implementation* cloneVirtual() const override
1104  { return new Implementation(*this); }
1105 
1106  // TODO: Let this be settable up to the min number of derivatives
1107  // provided by the arguments.
1108  int getNumTimeDerivativesVirtual() const override {return 0;}
1109  //{ return std::min(left.getNumTimeDerivatives(),
1110  // right.getNumTimeDerivatives()); }
1111 
1112  Stage getDependsOnStageVirtual(int order) const override
1113  { return operand.getDependsOnStage(order); }
1114 
1115 
1116  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1117  override
1118  {
1119  value = factor * operand.getValue(s,derivOrder);
1120  }
1121 
1122  // There are no uncached values.
1123 
1124 private:
1125  // TOPOLOGY STATE
1126  Real factor;
1127  Measure_<T> operand;
1128 
1129  // TOPOLOGY CACHE
1130  // nothing
1131 };
1132 
1133 
1134 
1135 //==============================================================================
1136 // INTEGRATE :: IMPLEMENTATION
1137 //==============================================================================
1144 template <class T>
1146 : public Measure_<T>::Implementation {
1147 public:
1151 
1154  Implementation(const Measure_<T>& deriv, const Measure_<T>& ic,
1155  const T& defaultValue)
1156  : Measure_<T>::Implementation(defaultValue, 1),
1157  derivMeasure(deriv), icMeasure(ic) {}
1158 
1162  : Measure_<T>::Implementation(source.getDefaultValue(), 1),
1163  derivMeasure(source.derivMeasure), icMeasure(source.icMeasure) {}
1164 
1168  void setValue(State& s, const T& value) const
1169  { assert(zIndex >= 0);
1170  for (int i=0; i < this->size(); ++i)
1171  this->getSubsystem().updZ(s)[zIndex+i] =
1172  Measure_Num<T>::get(value, i); }
1173 
1175  { SimTK_ERRCHK(!derivMeasure.isEmptyHandle(),
1176  "Measure_<T>::Integrate::getDerivativeMeasure()",
1177  "No derivative measure is available for this integrated measure.");
1178  return derivMeasure; }
1179 
1181  { SimTK_ERRCHK(!icMeasure.isEmptyHandle(),
1182  "Measure_<T>::Integrate::getInitialConditionMeasure()",
1183  "No initial condition measure is available for this "
1184  "integrated measure.");
1185  return icMeasure; }
1186 
1188  { derivMeasure = d; this->invalidateTopologyCache(); }
1190  { icMeasure = ic; this->invalidateTopologyCache(); }
1191 
1192  // Implementations of virtuals.
1193 
1194  // This uses the copy constructor defined above.
1195  Implementation* cloneVirtual() const override
1196  { return new Implementation(*this); }
1197 
1199  int getNumTimeDerivativesVirtual() const override
1200  { int integralDerivs = getDerivativeMeasure().getNumTimeDerivatives();
1201  // Careful - can't add 1 to max int and stay an int.
1202  if (integralDerivs < std::numeric_limits<int>::max())
1203  ++integralDerivs;
1204  return integralDerivs; }
1205 
1206  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1207  override
1208  { assert(derivOrder == 0); // only one cache entry
1209  assert(Measure_Num<T>::size(value) == this->size());
1210  assert(zIndex.isValid());
1211  const Vector& allZ = this->getSubsystem().getZ(s);
1212  for (int i=0; i < this->size(); ++i)
1213  Measure_Num<T>::upd(value,i) = allZ[zIndex+i];
1214  }
1215 
1216  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1217  override
1218  { assert(derivOrder > 0); // 0th entry is cached
1219  return getDerivativeMeasure().getValue(s, derivOrder-1);
1220  }
1221 
1222  Stage getDependsOnStageVirtual(int derivOrder) const override
1223  { return derivOrder>0
1224  ? getDerivativeMeasure().getDependsOnStage(derivOrder-1)
1225  : Stage::Time; }
1226 
1229  void initializeVirtual(State& s) const override {
1230  assert(zIndex.isValid());
1231  Vector& allZ = this->getSubsystem().updZ(s);
1232  if (!icMeasure.isEmptyHandle()) {
1233  this->getSubsystem().getSystem()
1234  .realize(s, icMeasure.getDependsOnStage());
1235  const T& ic = icMeasure.getValue(s);
1236  for (int i=0; i < this->size(); ++i)
1237  allZ[zIndex+i] = Measure_Num<T>::get(ic,i);
1238  } else {
1239  for (int i=0; i < this->size(); ++i)
1240  allZ[zIndex+i] = Measure_Num<T>::get(this->getDefaultValue(),i);
1241  }
1242  }
1243 
1248  void realizeMeasureTopologyVirtual(State& s) const override {
1249  Vector init(this->size());
1250  for (int i=0; i < this->size(); ++i)
1251  init[i] = Measure_Num<T>::get(this->getDefaultValue(),i);
1252  zIndex = this->getSubsystem().allocateZ(s, init);
1253  }
1254 
1257  void realizeMeasureAccelerationVirtual(const State& s) const override {
1258  assert(zIndex.isValid());
1259  Vector& allZDot = this->getSubsystem().updZDot(s);
1260  if (!derivMeasure.isEmptyHandle()) {
1261  const T& deriv = derivMeasure.getValue(s);
1262  for (int i=0; i < this->size(); ++i)
1263  allZDot[zIndex+i] = Measure_Num<T>::get(deriv,i);
1264  } else {
1265  allZDot(zIndex,this->size()) = 0; // derivative is zero
1266  }
1267  }
1268 
1269 private:
1270  // TOPOLOGY STATE
1271  Measure_<T> derivMeasure; // just handles
1272  Measure_<T> icMeasure;
1273 
1274  // TOPOLOGY CACHE
1275  mutable ZIndex zIndex; // This is the first index if more than one z.
1276 };
1277 
1278 
1279 
1280 //==============================================================================
1281 // DIFFERENTIATE :: IMPLEMENTATION
1282 //==============================================================================
1283  // Hide from Doxygen.
1285 // This helper class is the contents of the discrete state variable and
1286 // corresponding cache entry maintained by this measure. The variable is
1287 // auto-update, meaning the value of the cache entry replaces the state
1288 // variable at the start of each step.
1289 // TODO: This was a local class in Measure_<T>::Differentiate::Implementation
1290 // but VC++ 8 (2005) failed to properly instantiate the templatized operator<<()
1291 // in that case; doing it this way is a workaround.
1292 template <class T>
1293 class Measure_Differentiate_Result {
1294 public:
1295  Measure_Differentiate_Result() : derivIsGood(false) {}
1296  T operand; // previous value of operand
1297  T operandDot; // previous value of derivative
1298  bool derivIsGood; // do we think the deriv is a good one?
1299 };
1302 template <class T>
1304 : public Measure_<T>::Implementation
1305 {
1306  typedef Measure_Differentiate_Result<T> Result;
1307 public:
1308  // Don't allocate any cache entries in the base class.
1310 
1311  Implementation(const Measure_<T>& operand)
1312  : Measure_<T>::Implementation(0),
1313  operand(operand), forceUseApprox(false), isApproxInUse(false) {}
1314 
1315  // Default copy constructor gives us a new Implementation object,
1316  // but with reference to the *same* operand measure.
1317 
1318  void setForceUseApproximation(bool mustApproximate) {
1319  forceUseApprox = mustApproximate;
1320  this->invalidateTopologyCache();
1321  }
1322 
1323  void setOperandMeasure(const Measure_<T>& operand) {
1324  this->operand = operand;
1325  this->invalidateTopologyCache();
1326  }
1327 
1328  bool getForceUseApproximation() const {return forceUseApprox;}
1329  bool isUsingApproximation() const {return isApproxInUse;}
1330  const Measure_<T>& getOperandMeasure() const {return operand;}
1331 
1332  // Implementations of virtual methods.
1333 
1334  // This uses the default copy constructor.
1335  Implementation* cloneVirtual() const override
1336  { return new Implementation(*this); }
1337 
1338  // This has one fewer than the operand.
1339  int getNumTimeDerivativesVirtual() const override
1340  { if (!isApproxInUse) return operand.getNumTimeDerivatives()-1;
1341  else return 0; }
1342 
1343  Stage getDependsOnStageVirtual(int order) const override
1344  { if (!isApproxInUse) return operand.getDependsOnStage(order+1);
1345  else return operand.getDependsOnStage(order); }
1346 
1347 
1348  // We're not using the Measure_<T> base class cache services, but
1349  // we do have one of our own. It looks uncached from the base class
1350  // point of view which is why we're implementing it here.
1351  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1352  override
1353  { if (!isApproxInUse)
1354  return operand.getValue(s, derivOrder+1);
1355 
1356  ensureDerivativeIsRealized(s);
1357  const Subsystem& subsys = this->getSubsystem();
1358  const Result& result = Value<Result>::downcast
1359  (subsys.getDiscreteVarUpdateValue(s,resultIx));
1360  return result.operandDot; // has a value but might not be a good one
1361  }
1362 
1363  void initializeVirtual(State& s) const override {
1364  if (!isApproxInUse) return;
1365 
1366  assert(resultIx.isValid());
1367  const Subsystem& subsys = this->getSubsystem();
1368  Result& result = Value<Result>::updDowncast
1369  (subsys.updDiscreteVariable(s,resultIx));
1370  this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
1371  result.operand = operand.getValue(s);
1372  result.operandDot = this->getValueZero();
1373  result.derivIsGood = false;
1374  }
1375 
1376  void realizeMeasureTopologyVirtual(State& s) const override {
1377  isApproxInUse = (forceUseApprox || operand.getNumTimeDerivatives()==0);
1378  if (!isApproxInUse)
1379  return;
1380 
1381  resultIx = this->getSubsystem()
1382  .allocateAutoUpdateDiscreteVariable(s, operand.getDependsOnStage(0),
1383  new Value<Result>(), operand.getDependsOnStage(0));
1384  }
1385 
1388  void realizeMeasureAccelerationVirtual(const State& s) const override {
1389  ensureDerivativeIsRealized(s);
1390  }
1391 
1392  void ensureDerivativeIsRealized(const State& s) const {
1393  assert(resultIx.isValid());
1394  const Subsystem& subsys = this->getSubsystem();
1395  if (subsys.isDiscreteVarUpdateValueRealized(s,resultIx))
1396  return;
1397 
1398  const Real t0 = subsys.getDiscreteVarLastUpdateTime(s,resultIx);
1399  const Result& prevResult = Value<Result>::downcast
1400  (subsys.getDiscreteVariable(s,resultIx));
1401  const T& f0 = prevResult.operand;
1402  const T& fdot0 = prevResult.operandDot; // may be invalid
1403  const bool good0 = prevResult.derivIsGood;
1404 
1405  const Real t = s.getTime();
1406  Result& result = Value<Result>::updDowncast
1407  (subsys.updDiscreteVarUpdateValue(s,resultIx));
1408  T& f = result.operand; // renaming
1409  T& fdot = result.operandDot;
1410  bool& good = result.derivIsGood;
1411 
1412  f = operand.getValue(s);
1413  good = false;
1414  if (!isFinite(t0))
1415  fdot = this->getValueZero();
1416  else if (t == t0) {
1417  fdot = fdot0;
1418  good = good0;
1419  } else {
1420  fdot = (f-f0)/(t-t0); // 1st order
1421  if (good0)
1422  fdot = Real(2)*fdot - fdot0; // now 2nd order
1423  good = true; // either 1st or 2nd order estimate
1424  }
1425  subsys.markDiscreteVarUpdateValueRealized(s,resultIx);
1426  }
1427 private:
1428  // TOPOLOGY STATE
1429  Measure_<T> operand;
1430  bool forceUseApprox;
1431 
1432  // TOPOLOGY CACHE
1433  mutable bool isApproxInUse;
1434  mutable DiscreteVariableIndex resultIx; // auto-update
1435 };
1436 
1437 
1438 
1439 //==============================================================================
1440 // EXTREME :: IMPLEMENTATION
1441 //==============================================================================
1442 template <class T>
1444 {
1445  typedef typename Measure_<T>::Extreme Extreme;
1446  typedef typename Extreme::Operation Operation;
1447 public:
1451  : Measure_<T>::Implementation(0), operation(Extreme::MaxAbs) {}
1452 
1455  Implementation(const Measure_<T>& operand, Operation op)
1456  : Measure_<T>::Implementation(0), operand(operand), operation(op) {}
1457 
1458  // Default copy constructor gives us a new Implementation object,
1459  // but with reference to the *same* operand measure.
1460 
1464  void setOperandMeasure(const Measure_<T>& operand) {
1465  this->operand = operand;
1466  this->invalidateTopologyCache();
1467  }
1468 
1472  void setOperation(Operation op) {
1473  this->operation = op;
1474  this->invalidateTopologyCache();
1475  }
1476 
1478  const Measure_<T>& getOperandMeasure() const {return operand;}
1479 
1482  Operation getOperation() const {return operation;}
1483 
1486  void setValue(State& s, const T& value) const {
1487  assert(extremeIx.isValid());
1488  const Subsystem& subsys = this->getSubsystem();
1489  T& prevMin = Value<T>::updDowncast
1490  (subsys.updDiscreteVariable(s,extremeIx));
1491  prevMin = value;
1492  }
1493 
1498  const Subsystem& subsys = this->getSubsystem();
1499  const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
1500  Real tUpdate;
1501  if (hasNewExtreme)
1502  tUpdate = s.getTime(); // i.e., now
1503  else
1504  tUpdate = subsys.getDiscreteVarLastUpdateTime(s,extremeIx);
1505  return tUpdate;
1506  }
1507 
1508  // Implementations of virtual methods.
1509 
1510  // This uses the default copy constructor.
1511  Implementation* cloneVirtual() const override
1512  { return new Implementation(*this); }
1513 
1516  int getNumTimeDerivativesVirtual() const override
1517  { return operand.getNumTimeDerivatives(); }
1518 
1521  Stage getDependsOnStageVirtual(int order) const override
1522  { return operand.getDependsOnStage(order); }
1523 
1524 
1528  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1529  override
1530  {
1531  const Subsystem& subsys = this->getSubsystem();
1532  const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
1533  if (derivOrder > 0) {
1534  // TODO: should be handled elementwise and zero unless the
1535  // derivative is acting in the direction that changes the
1536  // extreme.
1537  return hasNewExtreme ? operand.getValue(s, derivOrder)
1538  : this->getValueZero();
1539  }
1540  if (hasNewExtreme) {
1541  const T& newExt = Value<T>::downcast
1542  (subsys.getDiscreteVarUpdateValue(s,extremeIx));
1543  return newExt;
1544  } else {
1545  const T& currentExt = Value<T>::downcast
1546  (subsys.getDiscreteVariable(s,extremeIx));
1547  return currentExt;
1548  }
1549  }
1550 
1553  void initializeVirtual(State& s) const override {
1554  this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
1555  setValue(s, operand.getValue(s));
1556  }
1557 
1565  void realizeMeasureTopologyVirtual(State& s) const override {
1566  // TODO: this should be NaN once initialization is working properly.
1567  T initVal = this->getDefaultValue();
1568  switch(operation) {
1569  case Minimum: initVal = Infinity; break;
1570  case Maximum: initVal = -Infinity; break;
1571  case MinAbs: initVal = Infinity; break;
1572  case MaxAbs: initVal = 0; break;
1573  };
1574 
1575  extremeIx = this->getSubsystem()
1577  new Value<T>(initVal), operand.getDependsOnStage(0));
1578 
1579  isNewExtremeIx = this->getSubsystem()
1581  new Value<bool>(false), operand.getDependsOnStage(0));
1582  }
1583 
1586  void realizeMeasureAccelerationVirtual(const State& s) const override {
1587  ensureExtremeHasBeenUpdated(s);
1588  }
1589 
1595  bool ensureExtremeHasBeenUpdated(const State& s) const {
1596  assert(extremeIx.isValid() && isNewExtremeIx.isValid());
1597  const Subsystem& subsys = this->getSubsystem();
1598 
1599  // We may have already determined whether we're at a new extreme in
1600  // which case we don't need to do it again.
1601  if (subsys.isDiscreteVarUpdateValueRealized(s, isNewExtremeIx))
1602  return Value<bool>::downcast
1603  (subsys.getDiscreteVarUpdateValue(s,isNewExtremeIx));
1604 
1605  // We're going to have to decide if we're at a new extreme, and if
1606  // so record the new extreme value in the auto-update cache entry of
1607  // the extreme value state variable.
1608 
1609  // Get the previous extreme value and the current operand value.
1610  const T& prevExtreme = Value<T>::downcast
1611  (subsys.getDiscreteVariable(s,extremeIx));
1612  const T& currentVal = operand.getValue(s);
1613 
1614  // Search to see if any element has reached a new extreme.
1615  bool foundNewExt = false;
1616  for (int i=0; i < this->size() && !foundNewExt; ++i)
1617  foundNewExt = isNewExtreme(Measure_Num<T>::get(currentVal,i),
1618  Measure_Num<T>::get(prevExtreme,i));
1619 
1620  // Record the result and mark the auto-update cache entry valid
1621  // so we won't have to recalculate. When the integrator advances to the
1622  // next step this cache entry will be swapped with the corresponding
1623  // state and marked invalid so we'll be sure to recalculate each step.
1625  (subsys.updDiscreteVarUpdateValue(s,isNewExtremeIx)) = foundNewExt;
1626  subsys.markDiscreteVarUpdateValueRealized(s,isNewExtremeIx);
1627 
1628  // Don't update the auto-update cache entry if we didn't see a new
1629  // extreme. That way no auto-update will occur and the state variable
1630  // will remain unchanged with the existing update time preserved.
1631  if (!foundNewExt)
1632  return false;
1633 
1634  // We have encountered a new extreme. We'll record the new extreme
1635  // in the auto-update cache entry which will be used as the current
1636  // result until the integrator advances to the next step at which time
1637  // this will be swapped with the state variable to serve as the previous
1638  // extreme value until a further extreme is encountered.
1639  T& newExtreme = Value<T>::updDowncast
1640  (subsys.updDiscreteVarUpdateValue(s,extremeIx));
1641 
1642  for (int i=0; i < this->size(); ++i)
1643  Measure_Num<T>::upd(newExtreme,i) =
1644  extremeOf(Measure_Num<T>::get(currentVal,i),
1645  Measure_Num<T>::get(prevExtreme,i));
1646 
1647  // Marking this valid is what ensures that an auto-update occurs later.
1648  subsys.markDiscreteVarUpdateValueRealized(s,extremeIx);
1649  return true;
1650  }
1651 private:
1652  // Return true if newVal is "more extreme" than oldExtreme, according
1653  // to the operation we're performing.
1654  bool isNewExtreme(const typename Measure_Num<T>::Element& newVal,
1655  const typename Measure_Num<T>::Element& oldExtreme) const
1656  {
1657  switch (operation) {
1658  case Extreme::Maximum: return newVal > oldExtreme;
1659  case Extreme::Minimum: return newVal < oldExtreme;
1660  case Extreme::MaxAbs: return std::abs(newVal) > std::abs(oldExtreme);
1661  case Extreme::MinAbs: return std::abs(newVal) < std::abs(oldExtreme);
1662  };
1663  SimTK_ASSERT1_ALWAYS(!"recognized",
1664  "Measure::Extreme::Implementation::isNewExtreme(): "
1665  "unrecognized operation %d", (int)operation);
1666  return false; /*NOTREACHED*/
1667  }
1668 
1669  // Given the value of one element of the operand, and that value's time
1670  // derivative, determine whether the derivative is moving the element
1671  // into the "more extreme" direction, according to the operation.
1672  bool isExtremeDir(const typename Measure_Num<T>::Element& value,
1673  const typename Measure_Num<T>::Element& deriv) const
1674  {
1675  const int sv = sign(value), sd = sign(deriv);
1676  if (sd == 0) return false; // derivative is zero; not changing
1677  switch (operation) {
1678  case Extreme::Maximum: return sd == 1; // getting larger
1679  case Extreme::Minimum: return sd == -1; // getting smaller
1680  case Extreme::MaxAbs: return sv==0 || sd==sv; // abs is growing
1681  case Extreme::MinAbs: return sd == -sv;
1682  };
1683  SimTK_ASSERT1_ALWAYS(!"recognized",
1684  "Measure::Extreme::Implementation::isExtremeDir(): "
1685  "unrecognized operation %d", (int)operation);
1686  return false; /*NOTREACHED*/
1687  }
1688 
1689  typename Measure_Num<T>::Element
1690  extremeOf(const typename Measure_Num<T>::Element& newVal,
1691  const typename Measure_Num<T>::Element& oldExtreme) const
1692  {
1693  return isNewExtreme(newVal,oldExtreme) ? newVal : oldExtreme;
1694  }
1695 
1696  // TOPOLOGY STATE
1697  Measure_<T> operand;
1698  Operation operation;
1699 
1700  // TOPOLOGY CACHE
1701  mutable DiscreteVariableIndex extremeIx; // extreme so far; auto-update
1702 
1703  // This auto-update flag records whether the current value is a new
1704  // extreme. We don't really need to save it as a state variable since you
1705  // can figure this out from the timestamp, but we need to to get invalidated
1706  // by the auto-update swap so that we'll figure it out anew each step.
1707  mutable DiscreteVariableIndex isNewExtremeIx;
1708 };
1709 
1710 
1711 
1712 //==============================================================================
1713 // DELAY :: IMPLEMENTATION
1714 //============================================================================== // Hide from Doxygen.
1716 // This helper class is the contents of the discrete state variable and
1717 // corresponding cache entry maintained by this measure. The variable is
1718 // auto-update, meaning the value of the cache entry replaces the state
1719 // variable at the start of each step.
1720 //
1721 // Circular buffers look like this:
1722 //
1723 // oldest=0, n=0
1724 // v
1725 // Empty buffer: | available |
1726 //
1727 // By convention, oldest=0 whenever the buffer is empty.
1728 //
1729 //
1730 // oldest next=(oldest+n)%capacity
1731 // v v
1732 // | available | | | | | | | available |
1733 // ^ n=6 ^
1734 // 0 capacity
1735 // v v
1736 // or | | | | | | available | | | | | | | | n=12
1737 // ^ ^
1738 // next oldest
1739 // = (oldest+n)%capacity
1740 //
1741 // Number of entries = n (called size() below)
1742 // Empty = n==0
1743 // Full = n==capacity()
1744 // Next available = (oldest+n)%capacity()
1745 template <class T>
1746 class Measure_Delay_Buffer {
1747 public:
1748  explicit Measure_Delay_Buffer() {initDataMembers();}
1749  void clear() {initDataMembers();}
1750  int size() const {return m_size;} // # saved entries, *not* size of arrays
1751  int capacity() const {return m_times.size();}
1752  bool empty() const {return size()==0;}
1753  bool full() const {return size()==capacity();}
1754 
1755  double getEntryTime(int i) const
1756  { assert(i < size()); return m_times[getArrayIndex(i)];}
1757  const T& getEntryValue(int i) const
1758  { assert(i < size()); return m_values[getArrayIndex(i)];}
1759 
1760  enum {
1761  InitialAllocation = 8, // smallest allocation
1762  GrowthFactor = 2, // how fast to grow (double)
1763  MaxShrinkProofSize = 16, // won't shrink unless bigger
1764  TooBigFactor = 5 // 5X too much->maybe shrink
1765  };
1766 
1767  // Add a new entry to the end of the list, throwing out old entries that
1768  // aren't needed to answer requests at tEarliest or later.
1769  void append(double tEarliest, double tNow, const T& valueNow) {
1770  forgetEntriesMuchOlderThan(tEarliest);
1771  removeEntriesLaterOrEq(tNow);
1772  if (full())
1773  makeMoreRoom();
1774  else if (capacity() > std::max((int)MaxShrinkProofSize,
1775  (int)TooBigFactor * (size()+1)))
1776  makeLessRoom(); // less than 1/TooBigFactor full
1777  const int nextFree = getArrayIndex(m_size++);
1778  m_times[nextFree] = tNow;
1779  m_values[nextFree] = valueNow;
1780  m_maxSize = std::max(m_maxSize, size());
1781  }
1782 
1783  // Prepend an older entry to the beginning of the list. No cleanup is done.
1784  void prepend(double tNewOldest, const T& value) {
1785  assert(empty() || tNewOldest < m_times[m_oldest]);
1786  if (full()) makeMoreRoom();
1787  m_oldest = empty() ? 0 : getArrayIndex(-1);
1788  m_times[m_oldest] = tNewOldest;
1789  m_values[m_oldest] = value;
1790  ++m_size;
1791  m_maxSize = std::max(m_maxSize, size());
1792  }
1793 
1794  // This is a specialized copy assignment for copying an old buffer
1795  // to a new one with updated contents. We are told the earliest time we'll
1796  // be asked about from now on, and won't copy any entries older than those
1797  // needed to answer that earliest request. We won't copy anything at or
1798  // newer than tNow, and finally we'll push (tNow,valueNow) as the newest
1799  // entry.
1800  void copyInAndUpdate(const Measure_Delay_Buffer& oldBuf, double tEarliest,
1801  double tNow, const T& valueNow) {
1802  // clear all current entries (no heap activity)
1803  m_oldest = m_size = 0;
1804 
1805  // determine how may old entries we have to keep
1806  int firstNeeded = oldBuf.countNumUnneededOldEntries(tEarliest);
1807  int lastNeeded = oldBuf.findLastEarlier(tNow); // might be -1
1808  int numOldEntriesToKeep = lastNeeded-firstNeeded+1;
1809  int newSize = numOldEntriesToKeep+1; // includes the new one
1810 
1811  int newSizeRequest = -1;
1812  if (capacity() < newSize) {
1813  newSizeRequest = std::max((int)InitialAllocation,
1814  (int)GrowthFactor * newSize);
1815  ++m_nGrows;
1816  } else if (capacity() > std::max((int)MaxShrinkProofSize,
1817  (int)TooBigFactor * newSize)) {
1818  newSizeRequest = std::max((int)MaxShrinkProofSize,
1819  (int)GrowthFactor * newSize);
1820  ++m_nShrinks;
1821  }
1822 
1823  // Reallocate space if advisable.
1824  if (newSizeRequest != -1) {
1825  const double dNaN = NTraits<double>::getNaN();
1826  m_values.resize(newSizeRequest);
1827  if (m_values.capacity() > m_values.size())
1828  m_values.resize(m_values.capacity()); // don't waste any
1829  m_times.resize(m_values.size(), dNaN);
1830  }
1831 
1832  m_maxCapacity = std::max(m_maxCapacity, capacity());
1833 
1834  // Copy the entries we need to keep.
1835  int nxt = 0;
1836  for (int i=firstNeeded; i<=lastNeeded; ++i, ++nxt) {
1837  m_times[nxt] = oldBuf.getEntryTime(i);
1838  m_values[nxt] = oldBuf.getEntryValue(i);
1839  }
1840  // Now add the newest entry and set the size.
1841  m_times[nxt] = tNow;
1842  m_values[nxt] = valueNow;
1843  assert(nxt+1==newSize);
1844  m_size = nxt+1;
1845  m_maxSize = std::max(m_maxSize, size());
1846  }
1847 
1848  // Given the current time and value and the earlier time at which the
1849  // value is needed, use the buffer and (if necessary) the current value
1850  // to estimate the delayed value.
1851  T calcValueAtTime(double tDelay, double tNow, const T& valueNow) const;
1852 
1853  // Given the current time but *not* the current value of the source measure,
1854  // provide an estimate for the value at tDelay=tNow-delay using only the
1855  // buffer contents and linear interpolation or extrapolation.
1856  void calcValueAtTimeLinearOnly(double tDelay, T& delayedValue) const {
1857  if (empty()) {
1858  // Nothing in the buffer?? Shouldn't happen. Return empty Vector
1859  // or NaN for fixed-size types.
1860  Measure_Num<T>::makeNaNLike(T(), delayedValue);
1861  return;
1862  }
1863 
1864  int firstLater = findFirstLaterOrEq(tDelay);
1865 
1866  if (firstLater > 0) {
1867  // Normal case: tDelay is between two buffer entries.
1868  int firstEarlier = firstLater-1;
1869  double t0=getEntryTime(firstEarlier), t1=getEntryTime(firstLater);
1870  const T& v0=getEntryValue(firstEarlier);
1871  const T& v1=getEntryValue(firstLater);
1872  Real fraction = Real((tDelay-t0)/(t1-t0));
1873  delayedValue = T(v0 + fraction*(v1-v0));
1874  return;
1875  }
1876 
1877  if (firstLater==0) {
1878  // Startup case: tDelay is at or before the oldest buffer entry.
1879  // Assume the value was flat before that.
1880  delayedValue = getEntryValue(firstLater);
1881  return;
1882  }
1883 
1884  // tDelay is later than the latest entry in the buffer. We are going
1885  // to have to extrapolate (yuck).
1886 
1887  if (size() == 1) {
1888  // Just one entry; we'll have to assume the value is flat.
1889  delayedValue = getEntryValue(0);
1890  return;
1891  }
1892 
1893  // Extrapolate using the last two entries.
1894  double t0=getEntryTime(size()-2), t1=getEntryTime(size()-1);
1895  const T& v0=getEntryValue(size()-2);
1896  const T& v1=getEntryValue(size()-1);
1897  Real fraction = Real((tDelay-t0)/(t1-t0)); // > 1
1898  assert(fraction > 1.0);
1899  delayedValue = T(v0 + fraction*(v1-v0)); // Extrapolate.
1900  }
1901 
1902  // Return the number of times we had to grow the buffer.
1903  int getNumGrows() const {return m_nGrows;}
1904  // Return the number of times we decided the buffer was so overallocated
1905  // that we had to shrink it.
1906  int getNumShrinks() const {return m_nShrinks;}
1907  // Return the largest number of values we ever had in the buffer.
1908  int getMaxSize() const {return m_maxSize;}
1909  // Return the largest capacity the buffer ever had.
1910  int getMaxCapacity() const {return m_maxCapacity;}
1911 
1912 private:
1913  // Return the i'th oldest entry
1914  // (0 -> oldest, size-1 -> newest, size -> first free, -1 -> last free)
1915  int getArrayIndex(int i) const
1916  { assert(-1<=i && i<=size());
1917  const int rawIndex = m_oldest + i;
1918  if (rawIndex < 0) return rawIndex + capacity();
1919  else return rawIndex % capacity(); }
1920 
1921  // Remove all but two entries older than the given time.
1922  void forgetEntriesMuchOlderThan(double tEarliest) {
1923  const int numToRemove = countNumUnneededOldEntries(tEarliest);
1924  if (numToRemove) {
1925  m_oldest = getArrayIndex(numToRemove);
1926  m_size -= numToRemove;
1927  }
1928  }
1929 
1930  // Count up how many old entries at the beginning of the buffer are so old
1931  // that they wouldn't be needed to respond to a request at time tEarliest or
1932  // later. We'll keep no more than two entries earlier than tEarliest.
1933  int countNumUnneededOldEntries(double tEarliest) const {
1934  const int firstLater = findFirstLaterOrEq(tEarliest);
1935  return std::max(0, firstLater-2);
1936  }
1937 
1938  // Given the time now, delete anything at the end of the queue that is
1939  // at that same time or later.
1940  void removeEntriesLaterOrEq(double t) {
1941  int lastEarlier = findLastEarlier(t);
1942  m_size = lastEarlier+1;
1943  if (m_size==0) m_oldest=0; // restart at beginning of array
1944  }
1945 
1946  // Return the entry number (0..size-1) of the first entry whose time
1947  // is >= the given time, or -1 if there is none such.
1948  int findFirstLaterOrEq(double tDelay) const {
1949  for (int i=0; i < size(); ++i)
1950  if (getEntryTime(i) >= tDelay)
1951  return i;
1952  return -1;
1953  }
1954 
1955  // Return the entry number(size-1..0) of the last entry whose time
1956  // is < the given time, or -1 if there is none such.
1957  int findLastEarlier(double t) const {
1958  for (int i=size()-1; i>=0; --i)
1959  if (getEntryTime(i) < t)
1960  return i;
1961  return -1;
1962  }
1963 
1964  // We don't have enough space. This is either the initial allocation or
1965  // we need to double the current space.
1966  void makeMoreRoom() {
1967  const int newSizeRequest = std::max((int)InitialAllocation,
1968  (int)GrowthFactor * size());
1969  resize(newSizeRequest);
1970  ++m_nGrows;
1971  m_maxCapacity = std::max(m_maxCapacity, capacity());
1972  }
1973 
1974  // We are wasting a lot of space, reduce the heap allocation to just
1975  // double what we're using now.
1976  void makeLessRoom() {
1977  const int targetMaxSize = std::max((int)MaxShrinkProofSize,
1978  (int)GrowthFactor * size());
1979  if (capacity() > targetMaxSize) {
1980  resize(targetMaxSize);
1981  ++m_nShrinks;
1982  }
1983  }
1984 
1985  // Reallocate memory to get more space or stop wasting space. The new
1986  // size request must be big enough to hold all the current contents. The
1987  // amount we actually get may be somewhat larger than the request. On
1988  // return, the times and values arrays will have been resized and the
1989  // oldest entry will now be entry 0.
1990  void resize(int newSizeRequest) {
1991  assert(newSizeRequest >= size());
1992  const double dNaN = NTraits<double>::getNaN();
1993  Array_<T,int> newValues(newSizeRequest);
1994  if (newValues.capacity() > newValues.size())
1995  newValues.resize(newValues.capacity()); // don't waste any
1996  Array_<double,int> newTimes(newValues.size(), dNaN);
1997 
1998  // Pack existing values into start of new arrays.
1999  for (int i=0; i < size(); ++i) {
2000  const int ix = getArrayIndex(i);
2001  newTimes[i] = m_times[ix];
2002  newValues[i] = m_values[ix];
2003  }
2004  m_times.swap(newTimes); // switch heap space
2005  m_values.swap(newValues);
2006  m_oldest = 0; // starts at the beginning now; size unchanged
2007  }
2008 
2009  // Initialize everything to its default-constructed state.
2010  void initDataMembers() {
2011  m_times.clear(); m_values.clear();
2012  m_oldest=m_size=0;
2013  m_nGrows=m_nShrinks=m_maxSize=m_maxCapacity=0;
2014  }
2015 
2016  // These are circular buffers of the same size.
2017  Array_<double,int> m_times;
2018  Array_<T,int> m_values;
2019  int m_oldest; // Array index of oldest (time,value)
2020  int m_size; // number of entries in use
2021 
2022  // Statistics.
2023  int m_nGrows, m_nShrinks, m_maxSize, m_maxCapacity;
2024 };
2027 template <class T>
2029  typedef Measure_Delay_Buffer<T> Buffer;
2030 public:
2031  // Allocate one cache entry in the base class for the value; we allocate
2032  // a specialized one for the buffer.
2034  : Measure_<T>::Implementation(1), m_delay(NaN),
2035  m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
2036 
2037  Implementation(const Measure_<T>& source, Real delay)
2038  : Measure_<T>::Implementation(1), m_source(source), m_delay(delay),
2039  m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
2040 
2041  // Default copy constructor gives us a new Implementation object,
2042  // but with reference to the *same* source measure.
2043 
2044  void setSourceMeasure(const Measure_<T>& source) {
2045  if (!source.isSameMeasure(this->m_source)) {
2046  this->m_source = source;
2047  this->invalidateTopologyCache();
2048  }
2049  }
2050 
2051  void setDelay(Real delay) {
2052  if (delay != this->m_delay) {
2053  this->m_delay = delay;
2054  this->invalidateTopologyCache();
2055  }
2056  }
2057 
2058  void setUseLinearInterpolationOnly(bool linearOnly) {
2059  if (linearOnly != this->m_useLinearInterpolationOnly) {
2060  this->m_useLinearInterpolationOnly = linearOnly;
2061  this->invalidateTopologyCache();
2062  }
2063  }
2064 
2065  void setCanUseCurrentValue(bool canUseCurrentValue) {
2066  if (canUseCurrentValue != this->m_canUseCurrentValue) {
2067  this->m_canUseCurrentValue = canUseCurrentValue;
2068  this->invalidateTopologyCache();
2069  }
2070  }
2071 
2072  const Measure_<T>& getSourceMeasure() const {return this->m_source;}
2073  Real getDelay() const {return this->m_delay;}
2075  { return this->m_useLinearInterpolationOnly; }
2077  { return this->m_canUseCurrentValue; }
2078 
2079 
2080  // Implementations of virtual methods.
2081 
2082  // This uses the default copy constructor.
2083  Implementation* cloneVirtual() const override
2084  { return new Implementation(*this); }
2085 
2086  // Currently no derivative supported.
2087  int getNumTimeDerivativesVirtual() const override
2088  { return 0; }
2089 
2090  // If we are allowed to use the current value of the source measure to
2091  // determine the delayed value, the depends-on stage here is the same as
2092  // for the source; otherwise it is Stage::Time.
2093  Stage getDependsOnStageVirtual(int order) const override
2094  { return this->m_canUseCurrentValue ? m_source.getDependsOnStage(order)
2095  : Stage::Time; }
2096 
2097  // Calculate the delayed value and return it to the Measure base class to
2098  // be put in a cache entry.
2099  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
2100  override
2101  { const Subsystem& subsys = this->getSubsystem();
2102  const Buffer& buffer = Value<Buffer>::downcast
2103  (subsys.getDiscreteVariable(s,m_bufferIx));
2104  //TODO: use cubic interpolation if allowed
2105  buffer.calcValueAtTimeLinearOnly(s.getTime()-m_delay, value);
2106  }
2107 
2108  void initializeVirtual(State& s) const override {
2109  assert(m_bufferIx.isValid());
2110  const Subsystem& subsys = this->getSubsystem();
2111  Buffer& buffer = Value<Buffer>::updDowncast
2112  (subsys.updDiscreteVariable(s,m_bufferIx));
2113  buffer.clear();
2114  this->getSubsystem().getSystem().realize(s,m_source.getDependsOnStage());
2115  buffer.append(s.getTime()-m_delay, s.getTime(), m_source.getValue(s));
2116  }
2117 
2118  void realizeMeasureTopologyVirtual(State& s) const override {
2119  m_bufferIx = this->getSubsystem()
2121  new Value<Buffer>(), getDependsOnStageVirtual(0));
2122  }
2123 
2126  void realizeMeasureAccelerationVirtual(const State& s) const override {
2127  updateBuffer(s);
2128  }
2129 
2130  // This uses the buffer from the state to update the one in the
2131  // corresponding cache entry. The update adds the current value of the
2132  // source to the end of the buffer and tosses out unneeded old entries.
2133  void updateBuffer(const State& s) const {
2134  assert(m_bufferIx.isValid());
2135  const Subsystem& subsys = this->getSubsystem();
2136 
2137  const Buffer& prevBuffer = Value<Buffer>::downcast
2138  (subsys.getDiscreteVariable(s,m_bufferIx));
2139 
2140  Buffer& nextBuffer = Value<Buffer>::updDowncast
2141  (subsys.updDiscreteVarUpdateValue(s,m_bufferIx));
2142 
2143  const Real t = s.getTime();
2144  nextBuffer.copyInAndUpdate(prevBuffer, t-m_delay,
2145  t, m_source.getValue(s));
2146 
2147  subsys.markDiscreteVarUpdateValueRealized(s,m_bufferIx);
2148  }
2149 private:
2150  // TOPOLOGY STATE
2151  Measure_<T> m_source;
2152  Real m_delay;
2153  bool m_canUseCurrentValue;
2154  bool m_useLinearInterpolationOnly;
2155 
2156  // TOPOLOGY CACHE
2157  mutable DiscreteVariableIndex m_bufferIx; // auto-update
2158 };
2159 
2160 
2161 } // namespace SimTK
2162 
2163 
2164 
2165 
2166 #endif // SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
Definition: MeasureImplementation.h:2028
void setIsPresumedValidAtDependsOnStage(bool presume)
Definition: MeasureImplementation.h:418
Definition: MeasureImplementation.h:610
Implementation()
Definition: MeasureImplementation.h:911
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:929
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1339
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:1516
Operation getOperation() const
Return the particular operation being performed by this Extreme measure.
Definition: MeasureImplementation.h:1482
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:1464
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:755
virtual void realizeMeasureModelVirtual(State &) const
Definition: MeasureImplementation.h:137
virtual void realizeMeasureTimeVirtual(const State &) const
Definition: MeasureImplementation.h:139
#define SimTK_SimTKCOMMON_EXPORT
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:224
bool isInSubsystem() const
Definition: MeasureImplementation.h:118
Stage getStage(const State &s) const
Definition: MeasureImplementation.h:128
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
int getRefCount() const
Definition: MeasureImplementation.h:249
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
const Measure_< T > & getOperandMeasure() const
Definition: MeasureImplementation.h:1330
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:747
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:1553
#define SimTK_ASSERT1_ALWAYS(cond, msg, a1)
Definition: ExceptionMacros.h:351
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:536
Implementation(Stage invalidated, const T &defaultValue)
Definition: MeasureImplementation.h:721
Definition: MeasureImplementation.h:673
virtual void realizeMeasureInstanceVirtual(const State &) const
Definition: MeasureImplementation.h:138
static const Value & downcast(const AbstractValue &value)
Downcast a const reference to an AbstractValue to a const reference to this type Value<T>.
Definition: Value.h:219
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:995
Modeling choices made.
Definition: Stage.h:71
static Value & updDowncast(AbstractValue &value)
Downcast a writable reference to an AbstractValue to a writable reference to this type Value<T>...
Definition: Value.h:225
int incrRefCount() const
Definition: MeasureImplementation.h:73
Vector & updZ(State &s) const
Definition: Subsystem.h:137
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
MeasureIndex getSubsystemMeasureIndex() const
Definition: MeasureImplementation.h:121
Implementation(const Implementation &src)
Base class copy constructor removes the Subsystem and sets the reference count to zero...
Definition: MeasureImplementation.h:57
Real getDelay() const
Definition: MeasureImplementation.h:2073
const Real & getTime() const
You can call these as long as system stage >= Model.
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:253
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:991
Definition: Measure.h:845
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:752
MeasureIndex adoptMeasure(AbstractMeasure &)
Obtain the Subsystem name if one was given on construction of the concrete Subsystem.
Definition: SubsystemGuts.h:509
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:2083
DiscreteVariableIndex allocateDiscreteVariable(State &s, Stage g, AbstractValue *v) const
Definition: Subsystem.h:96
Implementation(const Measure_< T > &operand)
Definition: MeasureImplementation.h:1311
String getName() const
Return a printable name corresponding to the stage level currently stored in this Stage...
Definition: Stage.h:144
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Implementation(const T &defaultValue, int numCacheEntries=1)
Definition: MeasureImplementation.h:426
void ensureDerivativeIsRealized(const State &s) const
Definition: MeasureImplementation.h:1392
Definition: Measure.h:842
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:1000
void realizeMeasureAccelerationVirtual(const State &s) const override
Set the zdots to the integrand (derivative measure) value.
Definition: MeasureImplementation.h:1257
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:1528
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
bool isUsingApproximation() const
Definition: MeasureImplementation.h:1329
void setInvalidatedStage(Stage invalidated)
Definition: MeasureImplementation.h:829
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1048
const Subsystem & getSubsystem() const
Return a reference to the Subsystem that owns this Measure.
Definition: MeasureImplementation.h:229
virtual ~Implementation()
Definition: MeasureImplementation.h:132
const AbstractValue & getDiscreteVariable(const State &s, DiscreteVariableIndex index) const
Definition: Subsystem.h:245
Lower than any legitimate Stage.
Definition: Stage.h:69
const T & getDefaultValue() const
Obtain a reference to the default value associated with this Measure.
Definition: Measure.h:290
const Real Zero
Real(0)
void setOperandMeasure(const Measure_< T > &operand)
Definition: MeasureImplementation.h:1323
void setCanUseCurrentValue(bool canUseCurrentValue)
Definition: MeasureImplementation.h:2065
void setValue(State &s, const T &value) const
Set the value of the state variables(s) that hold the integral.
Definition: MeasureImplementation.h:1168
void setInitialConditionMeasure(const Measure_< T > &ic)
Definition: MeasureImplementation.h:1189
Implementation(const Measure_< T > &source, Real delay)
Definition: MeasureImplementation.h:2037
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
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:1388
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:1053
bool isInSubsystem() const
Test whether this Measure object has been adopted by a Subsystem.
Definition: MeasureImplementation.h:225
int getNumTimeDerivativesVirtual() const override
This measure has one more time derivative than the integrand.
Definition: MeasureImplementation.h:1199
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:2093
DiscreteVariableIndex allocateAutoUpdateDiscreteVariable(State &s, Stage invalidates, AbstractValue *v, Stage updateDependsOn) const
Definition: Subsystem.h:99
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:1206
Track the value of the operand that is of maximum absolute value.
Definition: Measure.h:905
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:1486
virtual void realizeMeasureTopologyVirtual(State &) const
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:531
The abstract parent of all Measure Implementation classes.
Definition: MeasureImplementation.h:48
This is the base Implementation class for all Measures whose value type is known. ...
Definition: MeasureImplementation.h:363
Measure_ & setDefaultValue(const T &defaultValue)
Change the default value associated with this Measure.
Definition: Measure.h:285
void updateBuffer(const State &s) const
Definition: MeasureImplementation.h:2133
Definition: MeasureImplementation.h:790
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:1116
Real getDiscreteVarLastUpdateTime(const State &s, DiscreteVariableIndex dx) const
Definition: Subsystem.h:253
void markCacheValueNotRealized(const State &s, CacheEntryIndex cx) const
Definition: Subsystem.h:275
bool isValid(const State &state) const
Definition: MeasureImplementation.h:863
Implementation()
The derivative and initialConditions Measures will be empty handles if this is default constructed...
Definition: MeasureImplementation.h:1150
void setDelay(Real delay)
Definition: MeasureImplementation.h:2051
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:1376
void realizeTime(const State &s) const
Definition: MeasureImplementation.h:91
This is the base class for all Measure handle classes.
Definition: Measure.h:151
Stage getDependsOnStage(int derivOrder) const
Definition: MeasureImplementation.h:105
Implementation()
This default constructor is for use by concrete measure implementation classes.
Definition: MeasureImplementation.h:52
Implementation * clone() const
This is a deep copy of the concrete Implementation object, except the Subsystem will have been remove...
Definition: MeasureImplementation.h:86
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:606
ZIndex allocateZ(State &s, const Vector &zInit) const
Definition: Subsystem.h:92
This Measure tracks extreme values attained by the elements of its source operand since the last init...
Definition: Measure.h:837
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:932
void realizeReport(const State &s) const
Definition: MeasureImplementation.h:96
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:700
Stage getDependsOnStageVirtual(int order) const override
The depends-on stage for this measure is the same as for its operand.
Definition: MeasureImplementation.h:1521
void setValue(State &state, const T &value) const
Change the value of this Measure in the given state.
Definition: MeasureImplementation.h:740
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:1595
int getRefCount() const
Definition: MeasureImplementation.h:79
Track the minimum value of the operand (signed).
Definition: Measure.h:885
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1112
bool isSameSubsystem(const Subsystem &otherSubsystem) const
Determine if this Subsystem handle refers to the same Subsystem::Guts object as handle otherSubsystem...
Definition: Subsystem.h:322
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
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:1351
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
Definition: MeasureImplementation.h:1023
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:924
bool getCanUseCurrentValue() const
Definition: MeasureImplementation.h:2076
int getCopyNumber() const
Definition: MeasureImplementation.h:81
void initializeVirtual(State &s) const override
Definition: MeasureImplementation.h:2108
Implementation(const T &amplitude, const T &frequency, const T &phase=T(0))
Definition: MeasureImplementation.h:915
Implementation()
Definition: MeasureImplementation.h:1309
Stage getDependsOnStage() const
Definition: MeasureImplementation.h:843
const Stage & getSystemStage() const
This returns the global stage for this State.
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:986
bool isFinite(const negator< float > &x)
Definition: negator.h:285
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1195
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:885
const AbstractValue & getDiscreteVarUpdateValue(const State &s, DiscreteVariableIndex dx) const
Definition: Subsystem.h:259
Stage getInvalidatedStage() const
Definition: MeasureImplementation.h:735
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1335
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:2099
const Measure_< T > & getOperandMeasure() const
Definition: MeasureImplementation.h:1095
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1044
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:621
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:632
bool getIsPresumedValidAtDependsOnStage() const
Definition: MeasureImplementation.h:422
void setSourceMeasure(const Measure_< T > &source)
Definition: MeasureImplementation.h:2044
MeasureIndex getSubsystemMeasureIndex() const
Return the MeasureIndex by which this Measure is known to the Subsystem that owns it...
Definition: MeasureImplementation.h:237
Implementation(const Implementation &source)
Definition: MeasureImplementation.h:726
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:744
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:634
void initializeVirtual(State &s) const override
Definition: MeasureImplementation.h:1363
Report-only quantities evaluated.
Definition: Stage.h:78
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:687
Definition: MeasureImplementation.h:970
Definition: Measure.h:844
Definition: MeasureImplementation.h:1076
System topology realized.
Definition: Stage.h:70
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
Stage getStage(const State &s) const
Definition: Subsystem.h:239
Implementation(Stage dependsOn, Stage invalidated)
Definition: MeasureImplementation.h:799
Implementation()
Definition: MeasureImplementation.h:717
Forces calculated.
Definition: Stage.h:76
Implementation()
Definition: MeasureImplementation.h:2033
bool hasImpl() const
Definition: Measure.h:244
Implementation()
Definition: MeasureImplementation.h:1027
#define SimTK_ERRCHK_ALWAYS(cond, whereChecked, msg)
Definition: ExceptionMacros.h:281
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:2118
void realize(const State &state, Stage stage=Stage::HighestRuntime) const
Realize the given state to the indicated stage.
void setOperation(Operation op)
Set the particular operation to be performed by this Extreme measure; this is a Topology stage change...
Definition: MeasureImplementation.h:1472
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:627
Unique integer type for Subsystem-local z indexing.
void realizeMeasureTopologyVirtual(State &s) const override
Allocate the auto-updated state variable that holds the extreme seen so far.
Definition: MeasureImplementation.h:1565
Implementation(const Measure_< T > &left, const Measure_< T > &right)
Definition: MeasureImplementation.h:1029
T & updValue(const State &state) const
Definition: MeasureImplementation.h:870
const T & getValue() const
Retrieve the original (type-erased)thing as read-only.
Definition: Value.h:79
Implementation & operator=(const Implementation &src)
Base class copy assignment operator removes the Subsystem, and sets the reference count to zero...
Definition: MeasureImplementation.h:63
Implementation()
Definition: MeasureImplementation.h:681
#define SimTK_ERRCHK3_ALWAYS(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:293
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:875
bool isCacheValueRealized(const State &s, CacheEntryIndex cx) const
Definition: Subsystem.h:271
CacheEntryIndex allocateCacheEntry(const State &s, Stage dependsOn, Stage computedBy, AbstractValue *v) const
Definition: Subsystem.h:103
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1039
Implementation(const Measure_< T > &left, const Measure_< T > &right)
Definition: MeasureImplementation.h:976
This creates a Measure whose value is a Topology-stage constant of any type T.
Definition: Measure.h:341
virtual void realizeMeasureReportVirtual(const State &) const
Definition: MeasureImplementation.h:144
void setForceUseApproximation(bool mustApproximate)
Definition: MeasureImplementation.h:1318
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:1586
SubsystemIndex getSubsystemIndex() const
Definition: MeasureImplementation.h:122
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
bool getForceUseApproximation() const
Definition: MeasureImplementation.h:1328
Zero()
Definition: MeasureImplementation.h:648
int getNumCacheEntries() const
Return the number of cache entries allocated for the value and derivatives of this Measure...
Definition: MeasureImplementation.h:463
virtual void realizeMeasurePositionVirtual(const State &) const
Definition: MeasureImplementation.h:140
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:139
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:878
Implementation()
Definition: MeasureImplementation.h:974
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
~AbstractMeasure()
Destructor decrements the Implementation&#39;s reference count and deletes the object if the count goes t...
Definition: MeasureImplementation.h:221
const Implementation & getImpl() const
Definition: Measure.h:242
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:1222
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:694
#define SimTK_ERRCHK(cond, whereChecked, msg)
Definition: ExceptionMacros.h:324
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:1216
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:906
const System & getSystem() const
Return a const reference to the System that contains this Subsystem.
Definition: SubsystemGuts.h:520
const Vector & getZ(const State &s) const
Definition: Subsystem.h:128
One()
Definition: MeasureImplementation.h:658
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:1248
void realizeModel(State &s) const
Definition: MeasureImplementation.h:89
bool isSameSubsystem(const Subsystem &) const
Is getSubsystem() the same as the passed-in Subsystem?
Definition: MeasureImplementation.h:233
void realizeVelocity(const State &s) const
Definition: MeasureImplementation.h:93
void markCacheValueRealized(const State &s, CacheEntryIndex cx) const
Definition: Subsystem.h:273
const Real One
Real(1)
virtual void realizeMeasureAccelerationVirtual(const State &) const
Definition: MeasureImplementation.h:143
Declares the user-visible part of a SimTK::State, the implementation is done in a separate internal c...
Definition: MeasureImplementation.h:1443
const Real E
e = Real(exp(1))
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:1154
const Measure_< T > & getSourceMeasure() const
Definition: MeasureImplementation.h:2072
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:2126
void invalidateAllCacheAtOrAbove(Stage) const
If any subsystem or the system stage is currently at or higher than the passed-in one...
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:882
This unique integer type is for selecting discrete variables.
void realizeAcceleration(const State &s) const
Definition: MeasureImplementation.h:95
Definition: Measure.h:843
Stage getInvalidatedStage() const
Definition: MeasureImplementation.h:844
The implementation for Integrate measures allocates a continuous state variable or variables from the...
Definition: MeasureImplementation.h:1145
Implementation(const T &value)
Definition: MeasureImplementation.h:616
Vector & updZDot(const State &s) const
Definition: Subsystem.h:152
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1103
Implementation(const Implementation &source)
Definition: MeasureImplementation.h:810
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:560
Implementation()
Default constructor leaves the operand measure unspecified; no base class cache entries are allocated...
Definition: MeasureImplementation.h:1450
void invalidateTopologyCache() const
Definition: MeasureImplementation.h:125
void realizeDynamics(const State &s) const
Definition: MeasureImplementation.h:94
void markAsNotValid(const State &state) const
Definition: MeasureImplementation.h:866
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1511
void setDerivativeMeasure(const Measure_< T > &d)
Definition: MeasureImplementation.h:1187
int getNumTimeDerivatives() const
Definition: MeasureImplementation.h:103
Track the maximum value of the operand (signed).
Definition: Measure.h:895
bool getUseLinearInterpolationOnly() const
Definition: MeasureImplementation.h:2074
AbstractMeasure & deepAssign(const AbstractMeasure &source)
Deep assignment clones the Implementation object pointed to by the source handle, so that this handle...
Definition: MeasureImplementation.h:208
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:2087
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1343
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 realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:761
Provide a unique integer type for identifying Subsystems.
void setUseLinearInterpolationOnly(bool linearOnly)
Definition: MeasureImplementation.h:2058
CacheEntryIndex allocateLazyCacheEntry(const State &state, Stage earliest, AbstractValue *v) const
Definition: Subsystem.h:111
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:1455
void markCacheValueNotRealized(const State &s, int derivOrder) const
Invalidate one of this Measure&#39;s cache entries.
Definition: MeasureImplementation.h:518
Operation
Definition: Measure.h:841
Track the value of the operand that is of minimum absolute value (not very useful).
Definition: Measure.h:916
Implementation()
Definition: MeasureImplementation.h:1082
An object of this type is used as a dummy argument to make sure the automatically-generated handle co...
Definition: Measure.h:156
void setScaleFactor(Real sf)
Definition: MeasureImplementation.h:1090
void setDependsOnStage(Stage dependsOn)
Definition: MeasureImplementation.h:815
Implementation(Real factor, const Measure_< T > &operand)
Definition: MeasureImplementation.h:1084
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:171
const Measure_< T > & getOperandMeasure() const
Return a reference to the operand measure for this Extreme measure.
Definition: MeasureImplementation.h:1478
void setDefaultValue(const T &defaultValue)
Set a new default value for this Measure.
Definition: MeasureImplementation.h:407
const T & getValue(const State &s, int derivOrder) const
Definition: MeasureImplementation.h:365
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
AbstractMeasure & shallowAssign(const AbstractMeasure &)
Shallow assignment operator destructs the current Implementation object (meaning its reference count ...
Definition: MeasureImplementation.h:195
Definition: MeasureImplementation.h:710
A new time has been realized.
Definition: Stage.h:73
void realizeInstance(const State &s) const
Definition: MeasureImplementation.h:90
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
Real getTimeOfExtremeValue(const State &s) const
Return the time at which the extreme was last updated.
Definition: MeasureImplementation.h:1497
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
const Measure_< T > & getDerivativeMeasure() const
Definition: MeasureImplementation.h:1174
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:696
const Subsystem & getSubsystem() const
Definition: MeasureImplementation.h:119
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:636
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1108
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:1161
void realizePosition(const State &s) const
Definition: MeasureImplementation.h:92
virtual int getNumTimeDerivativesVirtual() const
Definition: MeasureImplementation.h:147
Includes internal headers providing declarations for the basic SimTK Core classes.
void setInvalidatedStage(Stage invalidates)
Definition: MeasureImplementation.h:730
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:927
virtual void realizeMeasureVelocityVirtual(const State &) const
Definition: MeasureImplementation.h:141
virtual void initializeVirtual(State &) const
Definition: MeasureImplementation.h:146
bool isSameMeasure(const AbstractMeasure &other) const
There can be multiple handles on the same Measure.
Definition: Measure.h:220
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:1229
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:549
Definition: MeasureImplementation.h:1303
Implementation()
Definition: MeasureImplementation.h:795
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
virtual void realizeMeasureDynamicsVirtual(const State &) const
Definition: MeasureImplementation.h:142
int decrRefCount() const
Definition: MeasureImplementation.h:76
const Measure_< T > & getInitialConditionMeasure() const
Definition: MeasureImplementation.h:1180
This is the base handle class for all Measures whose value type is known, including all the Simbody b...
Definition: Measure.h:263
void markAsValid(const State &state) const
Definition: MeasureImplementation.h:847
Implementation()
Definition: MeasureImplementation.h:615
Concrete templatized class derived from AbstractValue, adding generic value type-specific functionali...
Definition: Value.h:39