Simbody  3.6
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 MeasureIndex AbstractMeasure::
234 { return getImpl().getSubsystemMeasureIndex();}
235 
236 inline int AbstractMeasure::
238 { return getImpl().getNumTimeDerivatives(); }
239 
241 getDependsOnStage(int derivOrder) const
242 { return getImpl().getDependsOnStage(derivOrder); }
243 
244 inline int AbstractMeasure::
245 getRefCount() const
246 { return getImpl().getRefCount(); }
247 
248  // Hide from Doxygen.
250 // This is a helper class that makes it possible to treat Real, Vec, and
251 // Vector objects uniformly.
252 template <class T> class Measure_Num {
253 };
254 
255 template <> class Measure_Num<float> {
256 public:
257  typedef float Element;
258  static int size(const float&) {return 1;}
259  static const float& get(const float& v, int i) {assert(i==0); return v;}
260  static float& upd(float& v, int i) {assert(i==0); return v;}
261  static void makeNaNLike(const float&, float& nanValue)
262  { nanValue = CNT<float>::getNaN();}
263  static void makeZeroLike(const float&, float& zeroValue) {zeroValue=0.f;}
264 };
265 
266 template <> class Measure_Num<double> {
267 public:
268  typedef double Element;
269  static int size(const double&) {return 1;}
270  static const double& get(const double& v, int i) {assert(i==0); return v;}
271  static double& upd(double& v, int i) {assert(i==0); return v;}
272  static void makeNaNLike(const double&, double& nanValue)
273  { nanValue = CNT<double>::getNaN(); }
274  static void makeZeroLike(const double&, double& zeroValue) {zeroValue=0.;}
275 };
276 
277 // We only support stride 1 (densely packed) Vec types.
278 template <int M, class E>
279 class Measure_Num< Vec<M,E,1> > {
280  typedef Vec<M,E,1> T;
281 public:
282  typedef E Element;
283  static int size(const T&) {return M;}
284  static const E& get(const T& v, int i) {return v[i];}
285  static E& upd(T& v, int i) {return v[i];}
286  static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();}
287  static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();}
288 };
289 
290 // We only support column major (densely packed) Mat types.
291 template <int M, int N, class E>
292 class Measure_Num< Mat<M,N,E> > {
293  typedef Mat<M,N,E> T;
294 public:
295  typedef E Element;
296  static int size(const T&) {return N;} // number of columns
297  static const typename T::TCol& get(const T& m, int j) {return m.col(j);}
298  static typename T::TCol& upd(T& m, int j) {return m.col(j);}
299  static void makeNaNLike (const T&, T& nanValue) {nanValue.setToNaN();}
300  static void makeZeroLike(const T&, T& zeroValue) {zeroValue.setToZero();}
301 };
302 
303 
304 template <class E>
305 class Measure_Num< Vector_<E> > {
306  typedef Vector_<E> T;
307 public:
308  typedef E Element;
309  static int size(const T& v) {return v.size();}
310  static const E& get(const T& v, int i) {return v[i];}
311  static E& upd(T& v, int i) {return v[i];}
312  static void makeNaNLike(const T& v, T& nanValue)
313  { nanValue.resize(v.size()); nanValue.setToNaN(); }
314  static void makeZeroLike(const T& v, T& zeroValue)
315  { zeroValue.resize(v.size()); zeroValue.setToZero(); }
316 };
317 
318 
319 template <class E>
320 class Measure_Num< Rotation_<E> > {
321  typedef Rotation_<E> T;
322 public:
323  typedef T Element;
324  static int size(const T&) {return 1;}
325  static const T& get(const T& v, int i) {assert(i==0); return v;}
326  static T& upd(T& v, int i) {assert(i==0); return v;}
327  static void makeNaNLike(const T&, T& nanValue)
328  { nanValue.setRotationToNaN(); }
329  static void makeZeroLike(const T&, T& zeroValue)
330  { zeroValue.setRotationToIdentityMatrix(); }
331 };
332 
333 template <class E>
334 class Measure_Num< Transform_<E> > {
335  typedef Transform_<E> T;
336 public:
337  typedef T Element;
338  static int size(const T&) {return 1;}
339  static const T& get(const T& v, int i) {assert(i==0); return v;}
340  static T& upd(T& v, int i) {assert(i==0); return v;}
341  static void makeNaNLike(const T&, T& nanValue)
342  { nanValue.setToNaN(); }
343  static void makeZeroLike(const T&, T& zeroValue)
344  { zeroValue.setToZero(); }
345 };
346 
349 //==============================================================================
350 // MEASURE_<T> :: IMPLEMENTATION
351 //==============================================================================
358 template <class T>
360 public:
361  const T& getValue(const State& s, int derivOrder) const {
362  SimTK_ERRCHK2(0 <= derivOrder && derivOrder <= getNumTimeDerivatives(),
363  "Measure_<T>::getValue()",
364  "derivOrder %d was out of range; this Measure allows 0-%d.",
365  derivOrder, getNumTimeDerivatives());
366 
367  // We require the stage to have been advanced to at least the one
368  // before this measure's depends-on stage since this will get called
369  // towards the end of the depends-on stage realization.
370  if (getDependsOnStage(derivOrder) != Stage::Empty) {
371 #ifndef NDEBUG
372  Stage prevStage = getDependsOnStage(derivOrder).prev();
373 #endif
374 
376  ( ( isInSubsystem() && getStage(s)>=prevStage)
377  || (!isInSubsystem() && s.getSystemStage()>=prevStage),
378  "Measure_<T>::getValue()",
379  "Expected State to have been realized to at least stage "
380  "%s but stage was %s.",
381  prevStage.getName().c_str(),
382  (isInSubsystem() ? getStage(s) : s.getSystemStage())
383  .getName().c_str());
384  }
385 
386  if (derivOrder < getNumCacheEntries()) {
387  if (!isCacheValueRealized(s,derivOrder)) {
388  T& value = updCacheEntry(s,derivOrder);
389  calcCachedValueVirtual(s, derivOrder, value);
390  markCacheValueRealized(s,derivOrder);
391  return value;
392  }
393  return getCacheEntry(s,derivOrder);
394  }
395 
396  // We can't handle it here -- punt to the concrete Measure
397  // for higher order derivatives.
398  return getUncachedValueVirtual(s,derivOrder);
399  }
400 
403  void setDefaultValue(const T& defaultValue) {
404  this->defaultValue = defaultValue;
405  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
406  this->invalidateTopologyCache();
407  }
408 
412  const T& getDefaultValue() const {return defaultValue;}
413 
415  { presumeValidAtDependsOnStage = presume;
416  this->invalidateTopologyCache(); }
417 
419  { return presumeValidAtDependsOnStage; }
420 
421 protected:
422  explicit Implementation(const T& defaultValue, int numCacheEntries=1)
423  : presumeValidAtDependsOnStage(false),
424  defaultValue(defaultValue),
425  derivIx(numCacheEntries)
426  {
427  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
428  }
429 
433  explicit Implementation(int numCacheEntries=1)
434  : presumeValidAtDependsOnStage(false),
435  defaultValue(),
436  derivIx(numCacheEntries)
437  {
438  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
439  }
440 
445  : presumeValidAtDependsOnStage(source.presumeValidAtDependsOnStage),
446  defaultValue(source.defaultValue),
447  derivIx(source.derivIx.size())
448  {
449  Measure_Num<T>::makeZeroLike(defaultValue, zeroValue);
450  }
451 
452 
455  int size() const {return Measure_Num<T>::size(defaultValue);}
456 
459  int getNumCacheEntries() const {return (int)derivIx.size();}
460 
464  const T& getCacheEntry(const State& s, int derivOrder) const {
465  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
466  "Measure_<T>::Implementation::getCacheEntry()",
467  "Derivative order %d is out of range; only %d cache entries"
468  " were allocated.", derivOrder, getNumCacheEntries());
469 
470  return Value<T>::downcast(
471  this->getSubsystem().getCacheEntry(s, derivIx[derivOrder]));
472  }
473 
477  T& updCacheEntry(const State& s, int derivOrder) const {
478  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
479  "Measure_<T>::Implementation::updCacheEntry()",
480  "Derivative order %d is out of range; only %d cache entries"
481  " were allocated.", derivOrder, getNumCacheEntries());
482 
483  return Value<T>::updDowncast(
484  this->getSubsystem().updCacheEntry(s, derivIx[derivOrder]));
485  }
486 
489  bool isCacheValueRealized(const State& s, int derivOrder) const {
490  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
491  "Measure_<T>::Implementation::isCacheValueRealized()",
492  "Derivative order %d is out of range; only %d cache entries"
493  " were allocated.", derivOrder, getNumCacheEntries());
494 
495  return this->getSubsystem().isCacheValueRealized(s, derivIx[derivOrder]);
496  }
497 
501  void markCacheValueRealized(const State& s, int derivOrder) const {
502  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
503  "Measure_<T>::Implementation::markCacheValueRealized()",
504  "Derivative order %d is out of range; only %d cache entries"
505  " were allocated.", derivOrder, getNumCacheEntries());
506 
507  this->getSubsystem().markCacheValueRealized(s, derivIx[derivOrder]);
508  }
509 
514  void markCacheValueNotRealized(const State& s, int derivOrder) const {
515  SimTK_ERRCHK2(0 <= derivOrder && derivOrder < getNumCacheEntries(),
516  "Measure_<T>::Implementation::markCacheValueNotRealized()",
517  "Derivative order %d is out of range; only %d cache entries"
518  " were allocated.", derivOrder, getNumCacheEntries());
519 
520  this->getSubsystem().markCacheValueNotRealized(s, derivIx[derivOrder]);
521  }
522 
523  // VIRTUALS //
524 
527  virtual void realizeMeasureTopologyVirtual(State&) const {}
528 
531  virtual void
532  calcCachedValueVirtual(const State&, int derivOrder, T& value) const
533  { SimTK_ERRCHK1_ALWAYS(!"implemented",
534  "Measure_<T>::Implementation::calcCachedValueVirtual()",
535  "This method should have been overridden by the derived"
536  " Measure but was not. It is needed to calculate the"
537  " cached value for derivOrder=%d.", derivOrder); }
538 
544  virtual const T&
545  getUncachedValueVirtual(const State&, int derivOrder) const
546  { SimTK_ERRCHK1_ALWAYS(!"implemented",
547  "Measure_<T>::Implementation::getUncachedValueVirtual()",
548  "This method should have been overridden by the derived"
549  " Measure but was not. It is needed to return the uncached"
550  " value at derivOrder=%d.", derivOrder);
551  return *reinterpret_cast<T*>(0);
552  }
553 
556  const T& getValueZero() const {return zeroValue;}
557 
558 private:
559  // Satisfy the realizeTopology() pure virtual here now that we know the
560  // data type T. Allocate lazy- or auto-validated- cache entries depending
561  // on the setting of presumeValidAtDependsOnStage.
562  void realizeTopology(State& s) const override final {
563  // Allocate cache entries. Initialize the value cache entry to
564  // the given defaultValue; all the derivative cache entries should be
565  // initialized to a NaN of the same size.
566  if (getNumCacheEntries()) {
567  derivIx[0] = presumeValidAtDependsOnStage
569  (s, getDependsOnStage(0), new Value<T>(defaultValue))
571  (s, getDependsOnStage(0), new Value<T>(defaultValue));
572 
573  if (getNumCacheEntries() > 1) {
574  T nanValue; Measure_Num<T>::makeNaNLike(defaultValue, nanValue);
575  for (int i=1; i < getNumCacheEntries(); ++i) {
576  derivIx[i] = presumeValidAtDependsOnStage
578  (s, getDependsOnStage(i), new Value<T>(nanValue))
580  (s, getDependsOnStage(i), new Value<T>(nanValue));
581  }
582  }
583  }
584 
585  // Call the concrete class virtual if any.
586  realizeMeasureTopologyVirtual(s);
587  }
588 
589 //------------------------------------------------------------------------------
590 private:
591  // TOPOLOGY STATE
592  bool presumeValidAtDependsOnStage;
593  T defaultValue;
594  T zeroValue;
595 
596  // TOPOLOGY CACHE
597  mutable Array_<CacheEntryIndex> derivIx;
598 };
599 
600 
601 
602 //==============================================================================
603 // CONSTANT :: IMPLEMENTATION
604 //==============================================================================
605 template <class T>
607 : public Measure_<T>::Implementation
608 {
609 public:
610  // We don't want the base class to allocate *any* cache entries.
612  explicit Implementation(const T& value)
613  : Measure_<T>::Implementation(value,0) {}
614 
617  void setValue(const T& v) {this->setDefaultValue(v);}
618 
619  // Implementations of virtual methods.
620  // Measure_<T> virtuals:
621  // No cached values.
622 
623  const T& getUncachedValueVirtual(const State&, int derivOrder) const
624  override
625  { return derivOrder>0 ? this->getValueZero() : this->getDefaultValue(); }
626 
627  // AbstractMeasure virtuals:
628  Implementation* cloneVirtual() const override
629  { return new Implementation(*this); }
630  Stage getDependsOnStageVirtual(int derivOrder) const override
631  { return derivOrder>0 ? Stage::Empty : Stage::Topology; }
632  int getNumTimeDerivativesVirtual() const override
633  { return std::numeric_limits<int>::max(); }
634 };
635 
636 
637 
638 //==============================================================================
639 // MEASURE ZERO and ONE
640 //==============================================================================
641 // These had to wait for Constant::Implementation to be declared.
642 
643 template <class T> inline
645 template <class T> inline
647 
649 : Constant(Vector(size, Real(0))) {}
651 : Constant(sub, Vector(size, Real(0))) {}
652 
653 template <class T> inline
655 template <class T> inline
657 
659 : Constant(Vector(size, Real(1))) {}
661 : Constant(sub, Vector(size, Real(1))) {}
662 
663 
664 
665 //==============================================================================
666 // TIME :: IMPLEMENTATION
667 //==============================================================================
668 template <class T>
670 
671 template <>
672 class Measure_<Real>::Time::Implementation
673 : public Measure_<Real>::Implementation
674 {
675 public:
676  // We don't want the base class to allocate *any* cache entries.
678 
679  // Implementations of virtual methods.
680  // Measure_<Real> virtuals:
681  // No cached values.
682 
683  const Real& getUncachedValueVirtual(const State& s, int derivOrder) const
684  override
685  { return derivOrder==0 ? s.getTime()
686  : (derivOrder==1 ? SimTK::One
687  : SimTK::Zero); }
688 
689  // AbstractMeasure virtuals:
690  Implementation* cloneVirtual() const override
691  { return new Implementation(*this); }
692  Stage getDependsOnStageVirtual(int derivOrder) const override
693  { return derivOrder>0 ? Stage::Empty : Stage::Time; }
694 
695  // Value is t, 1st derivative is 1, the rest are 0.
696  int getNumTimeDerivativesVirtual() const override
697  { return std::numeric_limits<int>::max(); }
698 };
699 
700 
701 
702 //==============================================================================
703 // VARIABLE :: IMPLEMENTATION
704 //==============================================================================
705 template <class T>
707 : public Measure_<T>::Implementation
708 {
709 public:
710  // We don't want the base class to allocate *any* cache entries;
711  // we'll use the variable as its own value and zeroes for all
712  // the derivatives.
714  : Measure_<T>::Implementation(0),
715  invalidatedStage(Stage::Empty) {}
716 
717  Implementation(Stage invalidated, const T& defaultValue)
718  : Measure_<T>::Implementation(defaultValue, 0),
719  invalidatedStage(invalidated) {}
720 
721  // Copy constructor should not copy the variable.
723  : Measure_<T>::Implementation(source.getDefaultValue(), 0),
724  invalidatedStage(source.invalidatedStage) {}
725 
726  void setInvalidatedStage(Stage invalidates) {
727  invalidatedStage = invalidates;
728  this->invalidateTopologyCache();
729  }
730 
731  Stage getInvalidatedStage() const {return invalidatedStage;}
732 
736  void setValue(State& state, const T& value) const
737  { updVarValue(state) = value; }
738 
739  // Implementations of virtual methods.
740  Implementation* cloneVirtual() const override
741  { return new Implementation(*this); }
742 
743  int getNumTimeDerivativesVirtual() const override
744  { return std::numeric_limits<int>::max(); }
745 
746  // Discrete variable is available after Model stage; but all its
747  // derivatives are zero so are always available.
748  Stage getDependsOnStageVirtual(int derivOrder) const override
749  { return derivOrder>0 ? Stage::Empty : Stage::Model;}
750 
751  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
752  override
753  { return derivOrder>0 ? this->getValueZero() : getVarValue(s); }
754 
755  // No cached values.
756 
757  void realizeMeasureTopologyVirtual(State& s) const override {
758  discreteVarIndex = this->getSubsystem().allocateDiscreteVariable
759  (s, invalidatedStage, new Value<T>(this->getDefaultValue()));
760  }
761 private:
762  const T& getVarValue(const State& s) const {
763  assert(discreteVarIndex.isValid());
764  return Value<T>::downcast(
765  this->getSubsystem().getDiscreteVariable(s, discreteVarIndex));
766  }
767  T& updVarValue(State& s) const {
768  assert(discreteVarIndex.isValid());
769  return Value<T>::downcast(
770  this->getSubsystem().updDiscreteVariable(s, discreteVarIndex));
771  }
772 
773  // TOPOLOGY STATE
774  Stage invalidatedStage; // TODO this shouldn't be needed
775 
776  // TOPOLOGY CACHE
777  mutable DiscreteVariableIndex discreteVarIndex;
778 };
779 
780 
781 
782 //==============================================================================
783 // RESULT :: IMPLEMENTATION
784 //==============================================================================
785 template <class T>
787 : public Measure_<T>::Implementation
788 {
789 public:
790  // We want the base class to allocate a single cache entry of type T.
792  : Measure_<T>::Implementation(1),
793  dependsOnStage(Stage::Topology), invalidatedStage(Stage::Infinity) {}
794 
795  Implementation(Stage dependsOn, Stage invalidated)
796  : Measure_<T>::Implementation(1),
797  dependsOnStage(dependsOn==Stage::Empty ? Stage::Topology : dependsOn),
798  invalidatedStage(invalidated)
799  { SimTK_ERRCHK2_ALWAYS(invalidated > dependsOn,"Measure::Result::ctor()",
800  "Got invalidated stage %s and dependsOn stage %s which is illegal "
801  "because the invalidated stage must be later than dependsOn.",
802  invalidated.getName().c_str(), dependsOn.getName().c_str());
803  }
804 
805  // Copy constructor will not copy the cache entry index.
807  : Measure_<T>::Implementation(source),
808  dependsOnStage(source.dependsOnStage),
809  invalidatedStage(source.invalidatedStage) {}
810 
811  void setDependsOnStage(Stage dependsOn) {
812  if (dependsOn == Stage::Empty) dependsOn = Stage::Topology;
813  SimTK_ERRCHK2_ALWAYS(dependsOn < getInvalidatedStage(),
814  "Measure::Result::setDependsOnStage()",
815  "The provided dependsOn stage %s is illegal because it is not "
816  "less than the current invalidated stage %s. Change the "
817  "invalidated stage first with setInvalidatedStage().",
818  dependsOn.getName().c_str(),
819  getInvalidatedStage().getName().c_str());
820 
821  dependsOnStage = dependsOn;
822  this->invalidateTopologyCache();
823  }
824 
825  void setInvalidatedStage(Stage invalidated) {
826  SimTK_ERRCHK2_ALWAYS(invalidated > getDependsOnStage(),
827  "Measure::Result::setInvalidatedStage()",
828  "The provided invalidated stage %s is illegal because it is not "
829  "greater than the current dependsOn stage %s. Change the "
830  "dependsOn stage first with setDependsOnStage().",
831  invalidated.getName().c_str(),
832  getDependsOnStage().getName().c_str());
833 
834  invalidatedStage = invalidated;
835  this->invalidateTopologyCache();
836  }
837 
838 
839  Stage getDependsOnStage() const {return dependsOnStage;}
840  Stage getInvalidatedStage() const {return invalidatedStage;}
841 
842 
843  void markAsValid(const State& state) const
844  { const Stage subsystemStage = this->getSubsystem().getStage(state);
845  SimTK_ERRCHK3_ALWAYS(subsystemStage >= getDependsOnStage().prev(),
846  "Measure::Result::markAsValid()",
847  "This Result Measure cannot be marked valid in a State where this "
848  "measure's Subsystem has been realized only to stage %s, because "
849  "its value was declared to depend on stage %s. To mark it valid, "
850  "we require that the State have been realized at least to the "
851  "previous stage (%s in this case); that is, you must at least be "
852  "*working on* the dependsOn stage in order to claim this result is "
853  "available.",
854  subsystemStage.getName().c_str(),
855  getDependsOnStage().getName().c_str(),
856  getDependsOnStage().prev().getName().c_str());
857  this->markCacheValueRealized(state, 0); }
858 
859  bool isValid(const State& state) const
860  { return this->isCacheValueRealized(state, 0); }
861 
862  void markAsNotValid(const State& state) const
863  { this->markCacheValueNotRealized(state, 0);
864  state.invalidateAllCacheAtOrAbove(invalidatedStage); }
865 
866  T& updValue(const State& state) const
867  { markAsNotValid(state); return this->updCacheEntry(state, 0); }
868 
869 
870  // Implementations of virtual methods.
871  Implementation* cloneVirtual() const override
872  { return new Implementation(*this); }
873 
874  int getNumTimeDerivativesVirtual() const override {return 0;}
875 
878  Stage getDependsOnStageVirtual(int derivOrder) const override
879  { return derivOrder>0 ? Stage::Empty : dependsOnStage;}
880 
881  void calcCachedValueVirtual(const State&, int derivOrder, T& value) const
882  override
883  { SimTK_ERRCHK_ALWAYS(!"calcCachedValueVirtual() implemented",
884  "Measure_<T>::Result::getValue()",
885  "Measure_<T>::Result::getValue() was called when the value was not "
886  "yet valid. For most Measure types, this would have initiated "
887  "computation of the value, but Result measures must have their values "
888  "calculated and set externally, and then marked valid."); }
889 
890 private:
891  // TOPOLOGY STATE
892  Stage dependsOnStage;
893  Stage invalidatedStage;
894 };
895 
896 
897 
898 //==============================================================================
899 // SINUSOID :: IMPLEMENTATION
900 //==============================================================================
901 template <class T>
903 : public Measure_<T>::Implementation
904 {
905  static const int NumDerivs = 3;
906 public:
908  : Measure_<T>::Implementation(NumDerivs+1),
909  a(CNT<T>::getNaN()), w(CNT<T>::getNaN()), p(CNT<T>::getNaN()) {}
910 
911  Implementation(const T& amplitude,
912  const T& frequency,
913  const T& phase=T(0))
914  : Measure_<T>::Implementation(NumDerivs+1),
915  a(amplitude), w(frequency), p(phase) {}
916 
917  // Default copy constructor is fine.
918 
919  // Implementations of virtual methods.
920  Implementation* cloneVirtual() const override
921  { return new Implementation(*this); }
922 
923  int getNumTimeDerivativesVirtual() const override {return NumDerivs;}
924 
925  Stage getDependsOnStageVirtual(int order) const override
926  { return Stage::Time; }
927 
928  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
929  override
930  {
931  // We need to allow the compiler to select std::sin or SimTK::sin
932  // based on the argument type.
933  using std::sin; using std::cos;
934 
935  assert(NumDerivs == 3);
936  const Real t = s.getTime();
937  const T arg = w*t + p;
938 
939  switch (derivOrder) {
940  case 0: value = a*sin(arg); break;
941  case 1: value = w*a*cos(arg); break;
942  case 2: value = -w*w*a*sin(arg); break;
943  case 3: value = -w*w*w*a*cos(arg); break;
944  default: SimTK_ASSERT1_ALWAYS(!"out of range",
945  "Measure::Sinusoid::Implementation::calcCachedValueVirtual():"
946  " derivOrder %d is out of range 0-3.", derivOrder);
947  }
948  }
949 
950  // There are no uncached values.
951 
952 private:
953  // TOPOLOGY STATE
954  T a, w, p;
955 
956  // TOPOLOGY CACHE
957  // nothing
958 };
959 
960 
961 
962 //==============================================================================
963 // PLUS :: IMPLEMENTATION
964 //==============================================================================
965 template <class T>
967 public:
968  // TODO: Currently allocates just one cache entry.
969  // left and right will be empty handles.
971 
973  const Measure_<T>& right)
974  : left(left), right(right) {}
975 
976  // Default copy constructor gives us a new Implementation object,
977  // but with references to the *same* operand measures.
978 
979  // Implementations of virtual methods.
980 
981  // This uses the default copy constructor.
982  Implementation* cloneVirtual() const override
983  { return new Implementation(*this); }
984 
985  // TODO: Let this be settable up to the min number of derivatives
986  // provided by the arguments.
987  int getNumTimeDerivativesVirtual() const override {return 0;}
988  //{ return std::min(left.getNumTimeDerivatives(),
989  // right.getNumTimeDerivatives()); }
990 
991  Stage getDependsOnStageVirtual(int order) const override
992  { return Stage(std::max(left.getDependsOnStage(order),
993  right.getDependsOnStage(order))); }
994 
995 
996  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
997  override
998  {
999  value = left.getValue(s,derivOrder) + right.getValue(s,derivOrder);
1000  }
1001 
1002  // There are no uncached values.
1003 
1004 private:
1005  // TOPOLOGY STATE
1006  Measure_<T> left;
1007  Measure_<T> right;
1008 
1009  // TOPOLOGY CACHE
1010  // nothing
1011 };
1012 
1013 
1014 
1015 //==============================================================================
1016 // MINUS :: IMPLEMENTATION
1017 //==============================================================================
1018 template <class T>
1020 public:
1021  // TODO: Currently allocates just one cache entry.
1022  // left and right will be empty handles.
1024 
1026  const Measure_<T>& right)
1027  : left(left), right(right) {}
1028 
1029  // Default copy constructor gives us a new Implementation object,
1030  // but with references to the *same* operand measures.
1031 
1032  // Implementations of virtual methods.
1033 
1034  // This uses the default copy constructor.
1035  Implementation* cloneVirtual() const override
1036  { return new Implementation(*this); }
1037 
1038  // TODO: Let this be settable up to the min number of derivatives
1039  // provided by the arguments.
1040  int getNumTimeDerivativesVirtual() const override {return 0;}
1041  //{ return std::min(left.getNumTimeDerivatives(),
1042  // right.getNumTimeDerivatives()); }
1043 
1044  Stage getDependsOnStageVirtual(int order) const override
1045  { return Stage(std::max(left.getDependsOnStage(order),
1046  right.getDependsOnStage(order))); }
1047 
1048 
1049  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1050  override
1051  {
1052  value = left.getValue(s,derivOrder) - right.getValue(s,derivOrder);
1053  }
1054 
1055  // There are no uncached values.
1056 
1057 private:
1058  // TOPOLOGY STATE
1059  Measure_<T> left;
1060  Measure_<T> right;
1061 
1062  // TOPOLOGY CACHE
1063  // nothing
1064 };
1065 
1066 
1067 
1068 //==============================================================================
1069 // SCALE :: IMPLEMENTATION
1070 //==============================================================================
1071 template <class T>
1073 : public Measure_<T>::Implementation
1074 {
1075 public:
1076  // TODO: Currently allocates just one cache entry.
1077  // scale will be uninitialized, operand will be empty handle.
1078  Implementation() : factor(NaN) {}
1079 
1080  Implementation(Real factor, const Measure_<T>& operand)
1081  : factor(factor), operand(operand) {}
1082 
1083  // Default copy constructor gives us a new Implementation object,
1084  // but with references to the *same* operand measure.
1085 
1086  void setScaleFactor(Real sf) {
1087  factor = sf;
1088  this->invalidateTopologyCache();
1089  }
1090 
1092  {
1093  return operand;
1094  }
1095 
1096  // Implementations of virtual methods.
1097 
1098  // This uses the default copy constructor.
1099  Implementation* cloneVirtual() const override
1100  { return new Implementation(*this); }
1101 
1102  // TODO: Let this be settable up to the min number of derivatives
1103  // provided by the arguments.
1104  int getNumTimeDerivativesVirtual() const override {return 0;}
1105  //{ return std::min(left.getNumTimeDerivatives(),
1106  // right.getNumTimeDerivatives()); }
1107 
1108  Stage getDependsOnStageVirtual(int order) const override
1109  { return operand.getDependsOnStage(order); }
1110 
1111 
1112  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1113  override
1114  {
1115  value = factor * operand.getValue(s,derivOrder);
1116  }
1117 
1118  // There are no uncached values.
1119 
1120 private:
1121  // TOPOLOGY STATE
1122  Real factor;
1123  Measure_<T> operand;
1124 
1125  // TOPOLOGY CACHE
1126  // nothing
1127 };
1128 
1129 
1130 
1131 //==============================================================================
1132 // INTEGRATE :: IMPLEMENTATION
1133 //==============================================================================
1140 template <class T>
1142 : public Measure_<T>::Implementation {
1143 public:
1147 
1150  Implementation(const Measure_<T>& deriv, const Measure_<T>& ic,
1151  const T& defaultValue)
1152  : Measure_<T>::Implementation(defaultValue, 1),
1153  derivMeasure(deriv), icMeasure(ic) {}
1154 
1158  : Measure_<T>::Implementation(source.getDefaultValue(), 1),
1159  derivMeasure(source.derivMeasure), icMeasure(source.icMeasure) {}
1160 
1164  void setValue(State& s, const T& value) const
1165  { assert(zIndex >= 0);
1166  for (int i=0; i < this->size(); ++i)
1167  this->getSubsystem().updZ(s)[zIndex+i] =
1168  Measure_Num<T>::get(value, i); }
1169 
1171  { SimTK_ERRCHK(!derivMeasure.isEmptyHandle(),
1172  "Measure_<T>::Integrate::getDerivativeMeasure()",
1173  "No derivative measure is available for this integrated measure.");
1174  return derivMeasure; }
1175 
1177  { SimTK_ERRCHK(!icMeasure.isEmptyHandle(),
1178  "Measure_<T>::Integrate::getInitialConditionMeasure()",
1179  "No initial condition measure is available for this "
1180  "integrated measure.");
1181  return icMeasure; }
1182 
1184  { derivMeasure = d; this->invalidateTopologyCache(); }
1186  { icMeasure = ic; this->invalidateTopologyCache(); }
1187 
1188  // Implementations of virtuals.
1189 
1190  // This uses the copy constructor defined above.
1191  Implementation* cloneVirtual() const override
1192  { return new Implementation(*this); }
1193 
1195  int getNumTimeDerivativesVirtual() const override
1196  { int integralDerivs = getDerivativeMeasure().getNumTimeDerivatives();
1197  // Careful - can't add 1 to max int and stay an int.
1198  if (integralDerivs < std::numeric_limits<int>::max())
1199  ++integralDerivs;
1200  return integralDerivs; }
1201 
1202  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
1203  override
1204  { assert(derivOrder == 0); // only one cache entry
1205  assert(Measure_Num<T>::size(value) == this->size());
1206  assert(zIndex.isValid());
1207  const Vector& allZ = this->getSubsystem().getZ(s);
1208  for (int i=0; i < this->size(); ++i)
1209  Measure_Num<T>::upd(value,i) = allZ[zIndex+i];
1210  }
1211 
1212  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1213  override
1214  { assert(derivOrder > 0); // 0th entry is cached
1215  return getDerivativeMeasure().getValue(s, derivOrder-1);
1216  }
1217 
1218  Stage getDependsOnStageVirtual(int derivOrder) const override
1219  { return derivOrder>0
1220  ? getDerivativeMeasure().getDependsOnStage(derivOrder-1)
1221  : Stage::Time; }
1222 
1225  void initializeVirtual(State& s) const override {
1226  assert(zIndex.isValid());
1227  Vector& allZ = this->getSubsystem().updZ(s);
1228  if (!icMeasure.isEmptyHandle()) {
1229  this->getSubsystem().getSystem()
1230  .realize(s, icMeasure.getDependsOnStage());
1231  const T& ic = icMeasure.getValue(s);
1232  for (int i=0; i < this->size(); ++i)
1233  allZ[zIndex+i] = Measure_Num<T>::get(ic,i);
1234  } else {
1235  for (int i=0; i < this->size(); ++i)
1236  allZ[zIndex+i] = Measure_Num<T>::get(this->getDefaultValue(),i);
1237  }
1238  }
1239 
1244  void realizeMeasureTopologyVirtual(State& s) const override {
1245  Vector init(this->size());
1246  for (int i=0; i < this->size(); ++i)
1247  init[i] = Measure_Num<T>::get(this->getDefaultValue(),i);
1248  zIndex = this->getSubsystem().allocateZ(s, init);
1249  }
1250 
1253  void realizeMeasureAccelerationVirtual(const State& s) const override {
1254  assert(zIndex.isValid());
1255  Vector& allZDot = this->getSubsystem().updZDot(s);
1256  if (!derivMeasure.isEmptyHandle()) {
1257  const T& deriv = derivMeasure.getValue(s);
1258  for (int i=0; i < this->size(); ++i)
1259  allZDot[zIndex+i] = Measure_Num<T>::get(deriv,i);
1260  } else {
1261  allZDot(zIndex,this->size()) = 0; // derivative is zero
1262  }
1263  }
1264 
1265 private:
1266  // TOPOLOGY STATE
1267  Measure_<T> derivMeasure; // just handles
1268  Measure_<T> icMeasure;
1269 
1270  // TOPOLOGY CACHE
1271  mutable ZIndex zIndex; // This is the first index if more than one z.
1272 };
1273 
1274 
1275 
1276 //==============================================================================
1277 // DIFFERENTIATE :: IMPLEMENTATION
1278 //==============================================================================
1279  // Hide from Doxygen.
1281 // This helper class is the contents of the discrete state variable and
1282 // corresponding cache entry maintained by this measure. The variable is
1283 // auto-update, meaning the value of the cache entry replaces the state
1284 // variable at the start of each step.
1285 // TODO: This was a local class in Measure_<T>::Differentiate::Implementation
1286 // but VC++ 8 (2005) failed to properly instantiate the templatized operator<<()
1287 // in that case; doing it this way is a workaround.
1288 template <class T>
1289 class Measure_Differentiate_Result {
1290 public:
1291  Measure_Differentiate_Result() : derivIsGood(false) {}
1292  T operand; // previous value of operand
1293  T operandDot; // previous value of derivative
1294  bool derivIsGood; // do we think the deriv is a good one?
1295 };
1298 template <class T>
1300 : public Measure_<T>::Implementation
1301 {
1302  typedef Measure_Differentiate_Result<T> Result;
1303 public:
1304  // Don't allocate any cache entries in the base class.
1306 
1307  Implementation(const Measure_<T>& operand)
1308  : Measure_<T>::Implementation(0),
1309  operand(operand), forceUseApprox(false), isApproxInUse(false) {}
1310 
1311  // Default copy constructor gives us a new Implementation object,
1312  // but with reference to the *same* operand measure.
1313 
1314  void setForceUseApproximation(bool mustApproximate) {
1315  forceUseApprox = mustApproximate;
1316  this->invalidateTopologyCache();
1317  }
1318 
1319  void setOperandMeasure(const Measure_<T>& operand) {
1320  this->operand = operand;
1321  this->invalidateTopologyCache();
1322  }
1323 
1324  bool getForceUseApproximation() const {return forceUseApprox;}
1325  bool isUsingApproximation() const {return isApproxInUse;}
1326  const Measure_<T>& getOperandMeasure() const {return operand;}
1327 
1328  // Implementations of virtual methods.
1329 
1330  // This uses the default copy constructor.
1331  Implementation* cloneVirtual() const override
1332  { return new Implementation(*this); }
1333 
1334  // This has one fewer than the operand.
1335  int getNumTimeDerivativesVirtual() const override
1336  { if (!isApproxInUse) return operand.getNumTimeDerivatives()-1;
1337  else return 0; }
1338 
1339  Stage getDependsOnStageVirtual(int order) const override
1340  { if (!isApproxInUse) return operand.getDependsOnStage(order+1);
1341  else return operand.getDependsOnStage(order); }
1342 
1343 
1344  // We're not using the Measure_<T> base class cache services, but
1345  // we do have one of our own. It looks uncached from the base class
1346  // point of view which is why we're implementing it here.
1347  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1348  override
1349  { if (!isApproxInUse)
1350  return operand.getValue(s, derivOrder+1);
1351 
1352  ensureDerivativeIsRealized(s);
1353  const Subsystem& subsys = this->getSubsystem();
1354  const Result& result = Value<Result>::downcast
1355  (subsys.getDiscreteVarUpdateValue(s,resultIx));
1356  return result.operandDot; // has a value but might not be a good one
1357  }
1358 
1359  void initializeVirtual(State& s) const override {
1360  if (!isApproxInUse) return;
1361 
1362  assert(resultIx.isValid());
1363  const Subsystem& subsys = this->getSubsystem();
1364  Result& result = Value<Result>::updDowncast
1365  (subsys.updDiscreteVariable(s,resultIx));
1366  this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
1367  result.operand = operand.getValue(s);
1368  result.operandDot = this->getValueZero();
1369  result.derivIsGood = false;
1370  }
1371 
1372  void realizeMeasureTopologyVirtual(State& s) const override {
1373  isApproxInUse = (forceUseApprox || operand.getNumTimeDerivatives()==0);
1374  if (!isApproxInUse)
1375  return;
1376 
1377  resultIx = this->getSubsystem()
1378  .allocateAutoUpdateDiscreteVariable(s, operand.getDependsOnStage(0),
1379  new Value<Result>(), operand.getDependsOnStage(0));
1380  }
1381 
1384  void realizeMeasureAccelerationVirtual(const State& s) const override {
1385  ensureDerivativeIsRealized(s);
1386  }
1387 
1388  void ensureDerivativeIsRealized(const State& s) const {
1389  assert(resultIx.isValid());
1390  const Subsystem& subsys = this->getSubsystem();
1391  if (subsys.isDiscreteVarUpdateValueRealized(s,resultIx))
1392  return;
1393 
1394  const Real t0 = subsys.getDiscreteVarLastUpdateTime(s,resultIx);
1395  const Result& prevResult = Value<Result>::downcast
1396  (subsys.getDiscreteVariable(s,resultIx));
1397  const T& f0 = prevResult.operand;
1398  const T& fdot0 = prevResult.operandDot; // may be invalid
1399  const bool good0 = prevResult.derivIsGood;
1400 
1401  const Real t = s.getTime();
1402  Result& result = Value<Result>::updDowncast
1403  (subsys.updDiscreteVarUpdateValue(s,resultIx));
1404  T& f = result.operand; // renaming
1405  T& fdot = result.operandDot;
1406  bool& good = result.derivIsGood;
1407 
1408  f = operand.getValue(s);
1409  good = false;
1410  if (!isFinite(t0))
1411  fdot = this->getValueZero();
1412  else if (t == t0) {
1413  fdot = fdot0;
1414  good = good0;
1415  } else {
1416  fdot = (f-f0)/(t-t0); // 1st order
1417  if (good0)
1418  fdot = Real(2)*fdot - fdot0; // now 2nd order
1419  good = true; // either 1st or 2nd order estimate
1420  }
1421  subsys.markDiscreteVarUpdateValueRealized(s,resultIx);
1422  }
1423 private:
1424  // TOPOLOGY STATE
1425  Measure_<T> operand;
1426  bool forceUseApprox;
1427 
1428  // TOPOLOGY CACHE
1429  mutable bool isApproxInUse;
1430  mutable DiscreteVariableIndex resultIx; // auto-update
1431 };
1432 
1433 
1434 
1435 //==============================================================================
1436 // EXTREME :: IMPLEMENTATION
1437 //==============================================================================
1438 template <class T>
1440 {
1441  typedef typename Measure_<T>::Extreme Extreme;
1442  typedef typename Extreme::Operation Operation;
1443 public:
1447  : Measure_<T>::Implementation(0), operation(Extreme::MaxAbs) {}
1448 
1451  Implementation(const Measure_<T>& operand, Operation op)
1452  : Measure_<T>::Implementation(0), operand(operand), operation(op) {}
1453 
1454  // Default copy constructor gives us a new Implementation object,
1455  // but with reference to the *same* operand measure.
1456 
1460  void setOperandMeasure(const Measure_<T>& operand) {
1461  this->operand = operand;
1462  this->invalidateTopologyCache();
1463  }
1464 
1468  void setOperation(Operation op) {
1469  this->operation = op;
1470  this->invalidateTopologyCache();
1471  }
1472 
1474  const Measure_<T>& getOperandMeasure() const {return operand;}
1475 
1478  Operation getOperation() const {return operation;}
1479 
1482  void setValue(State& s, const T& value) const {
1483  assert(extremeIx.isValid());
1484  const Subsystem& subsys = this->getSubsystem();
1485  T& prevMin = Value<T>::updDowncast
1486  (subsys.updDiscreteVariable(s,extremeIx));
1487  prevMin = value;
1488  }
1489 
1493  Real getTimeOfExtremeValue(const State& s) const {
1494  const Subsystem& subsys = this->getSubsystem();
1495  const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
1496  Real tUpdate;
1497  if (hasNewExtreme)
1498  tUpdate = s.getTime(); // i.e., now
1499  else
1500  tUpdate = subsys.getDiscreteVarLastUpdateTime(s,extremeIx);
1501  return tUpdate;
1502  }
1503 
1504  // Implementations of virtual methods.
1505 
1506  // This uses the default copy constructor.
1507  Implementation* cloneVirtual() const override
1508  { return new Implementation(*this); }
1509 
1512  int getNumTimeDerivativesVirtual() const override
1513  { return operand.getNumTimeDerivatives(); }
1514 
1517  Stage getDependsOnStageVirtual(int order) const override
1518  { return operand.getDependsOnStage(order); }
1519 
1520 
1524  const T& getUncachedValueVirtual(const State& s, int derivOrder) const
1525  override
1526  {
1527  const Subsystem& subsys = this->getSubsystem();
1528  const bool hasNewExtreme = ensureExtremeHasBeenUpdated(s);
1529  if (derivOrder > 0) {
1530  // TODO: should be handled elementwise and zero unless the
1531  // derivative is acting in the direction that changes the
1532  // extreme.
1533  return hasNewExtreme ? operand.getValue(s, derivOrder)
1534  : this->getValueZero();
1535  }
1536  if (hasNewExtreme) {
1537  const T& newExt = Value<T>::downcast
1538  (subsys.getDiscreteVarUpdateValue(s,extremeIx));
1539  return newExt;
1540  } else {
1541  const T& currentExt = Value<T>::downcast
1542  (subsys.getDiscreteVariable(s,extremeIx));
1543  return currentExt;
1544  }
1545  }
1546 
1549  void initializeVirtual(State& s) const override {
1550  this->getSubsystem().getSystem().realize(s,operand.getDependsOnStage());
1551  setValue(s, operand.getValue(s));
1552  }
1553 
1561  void realizeMeasureTopologyVirtual(State& s) const override {
1562  // TODO: this should be NaN once initialization is working properly.
1563  T initVal = this->getDefaultValue();
1564  switch(operation) {
1565  case Minimum: initVal = Infinity; break;
1566  case Maximum: initVal = -Infinity; break;
1567  case MinAbs: initVal = Infinity; break;
1568  case MaxAbs: initVal = 0; break;
1569  };
1570 
1571  extremeIx = this->getSubsystem()
1573  new Value<T>(initVal), operand.getDependsOnStage(0));
1574 
1575  isNewExtremeIx = this->getSubsystem()
1577  new Value<bool>(false), operand.getDependsOnStage(0));
1578  }
1579 
1582  void realizeMeasureAccelerationVirtual(const State& s) const override {
1583  ensureExtremeHasBeenUpdated(s);
1584  }
1585 
1591  bool ensureExtremeHasBeenUpdated(const State& s) const {
1592  assert(extremeIx.isValid() && isNewExtremeIx.isValid());
1593  const Subsystem& subsys = this->getSubsystem();
1594 
1595  // We may have already determined whether we're at a new extreme in
1596  // which case we don't need to do it again.
1597  if (subsys.isDiscreteVarUpdateValueRealized(s, isNewExtremeIx))
1598  return Value<bool>::downcast
1599  (subsys.getDiscreteVarUpdateValue(s,isNewExtremeIx));
1600 
1601  // We're going to have to decide if we're at a new extreme, and if
1602  // so record the new extreme value in the auto-update cache entry of
1603  // the extreme value state variable.
1604 
1605  // Get the previous extreme value and the current operand value.
1606  const T& prevExtreme = Value<T>::downcast
1607  (subsys.getDiscreteVariable(s,extremeIx));
1608  const T& currentVal = operand.getValue(s);
1609 
1610  // Search to see if any element has reached a new extreme.
1611  bool foundNewExt = false;
1612  for (int i=0; i < this->size() && !foundNewExt; ++i)
1613  foundNewExt = isNewExtreme(Measure_Num<T>::get(currentVal,i),
1614  Measure_Num<T>::get(prevExtreme,i));
1615 
1616  // Record the result and mark the auto-update cache entry valid
1617  // so we won't have to recalculate. When the integrator advances to the
1618  // next step this cache entry will be swapped with the corresponding
1619  // state and marked invalid so we'll be sure to recalculate each step.
1621  (subsys.updDiscreteVarUpdateValue(s,isNewExtremeIx)) = foundNewExt;
1622  subsys.markDiscreteVarUpdateValueRealized(s,isNewExtremeIx);
1623 
1624  // Don't update the auto-update cache entry if we didn't see a new
1625  // extreme. That way no auto-update will occur and the state variable
1626  // will remain unchanged with the existing update time preserved.
1627  if (!foundNewExt)
1628  return false;
1629 
1630  // We have encountered a new extreme. We'll record the new extreme
1631  // in the auto-update cache entry which will be used as the current
1632  // result until the integrator advances to the next step at which time
1633  // this will be swapped with the state variable to serve as the previous
1634  // extreme value until a further extreme is encountered.
1635  T& newExtreme = Value<T>::updDowncast
1636  (subsys.updDiscreteVarUpdateValue(s,extremeIx));
1637 
1638  for (int i=0; i < this->size(); ++i)
1639  Measure_Num<T>::upd(newExtreme,i) =
1640  extremeOf(Measure_Num<T>::get(currentVal,i),
1641  Measure_Num<T>::get(prevExtreme,i));
1642 
1643  // Marking this valid is what ensures that an auto-update occurs later.
1644  subsys.markDiscreteVarUpdateValueRealized(s,extremeIx);
1645  return true;
1646  }
1647 private:
1648  // Return true if newVal is "more extreme" than oldExtreme, according
1649  // to the operation we're performing.
1650  bool isNewExtreme(const typename Measure_Num<T>::Element& newVal,
1651  const typename Measure_Num<T>::Element& oldExtreme) const
1652  {
1653  switch (operation) {
1654  case Extreme::Maximum: return newVal > oldExtreme;
1655  case Extreme::Minimum: return newVal < oldExtreme;
1656  case Extreme::MaxAbs: return std::abs(newVal) > std::abs(oldExtreme);
1657  case Extreme::MinAbs: return std::abs(newVal) < std::abs(oldExtreme);
1658  };
1659  SimTK_ASSERT1_ALWAYS(!"recognized",
1660  "Measure::Extreme::Implementation::isNewExtreme(): "
1661  "unrecognized operation %d", (int)operation);
1662  return false; /*NOTREACHED*/
1663  }
1664 
1665  // Given the value of one element of the operand, and that value's time
1666  // derivative, determine whether the derivative is moving the element
1667  // into the "more extreme" direction, according to the operation.
1668  bool isExtremeDir(const typename Measure_Num<T>::Element& value,
1669  const typename Measure_Num<T>::Element& deriv) const
1670  {
1671  const int sv = sign(value), sd = sign(deriv);
1672  if (sd == 0) return false; // derivative is zero; not changing
1673  switch (operation) {
1674  case Extreme::Maximum: return sd == 1; // getting larger
1675  case Extreme::Minimum: return sd == -1; // getting smaller
1676  case Extreme::MaxAbs: return sv==0 || sd==sv; // abs is growing
1677  case Extreme::MinAbs: return sd == -sv;
1678  };
1679  SimTK_ASSERT1_ALWAYS(!"recognized",
1680  "Measure::Extreme::Implementation::isExtremeDir(): "
1681  "unrecognized operation %d", (int)operation);
1682  return false; /*NOTREACHED*/
1683  }
1684 
1685  typename Measure_Num<T>::Element
1686  extremeOf(const typename Measure_Num<T>::Element& newVal,
1687  const typename Measure_Num<T>::Element& oldExtreme) const
1688  {
1689  return isNewExtreme(newVal,oldExtreme) ? newVal : oldExtreme;
1690  }
1691 
1692  // TOPOLOGY STATE
1693  Measure_<T> operand;
1694  Operation operation;
1695 
1696  // TOPOLOGY CACHE
1697  mutable DiscreteVariableIndex extremeIx; // extreme so far; auto-update
1698 
1699  // This auto-update flag records whether the current value is a new
1700  // extreme. We don't really need to save it as a state variable since you
1701  // can figure this out from the timestamp, but we need to to get invalidated
1702  // by the auto-update swap so that we'll figure it out anew each step.
1703  mutable DiscreteVariableIndex isNewExtremeIx;
1704 };
1705 
1706 
1707 
1708 //==============================================================================
1709 // DELAY :: IMPLEMENTATION
1710 //============================================================================== // Hide from Doxygen.
1712 // This helper class is the contents of the discrete state variable and
1713 // corresponding cache entry maintained by this measure. The variable is
1714 // auto-update, meaning the value of the cache entry replaces the state
1715 // variable at the start of each step.
1716 //
1717 // Circular buffers look like this:
1718 //
1719 // oldest=0, n=0
1720 // v
1721 // Empty buffer: | available |
1722 //
1723 // By convention, oldest=0 whenever the buffer is empty.
1724 //
1725 //
1726 // oldest next=(oldest+n)%capacity
1727 // v v
1728 // | available | | | | | | | available |
1729 // ^ n=6 ^
1730 // 0 capacity
1731 // v v
1732 // or | | | | | | available | | | | | | | | n=12
1733 // ^ ^
1734 // next oldest
1735 // = (oldest+n)%capacity
1736 //
1737 // Number of entries = n (called size() below)
1738 // Empty = n==0
1739 // Full = n==capacity()
1740 // Next available = (oldest+n)%capacity()
1741 template <class T>
1742 class Measure_Delay_Buffer {
1743 public:
1744  explicit Measure_Delay_Buffer() {initDataMembers();}
1745  void clear() {initDataMembers();}
1746  int size() const {return m_size;} // # saved entries, *not* size of arrays
1747  int capacity() const {return m_times.size();}
1748  bool empty() const {return size()==0;}
1749  bool full() const {return size()==capacity();}
1750 
1751  double getEntryTime(int i) const
1752  { assert(i < size()); return m_times[getArrayIndex(i)];}
1753  const T& getEntryValue(int i) const
1754  { assert(i < size()); return m_values[getArrayIndex(i)];}
1755 
1756  enum {
1757  InitialAllocation = 8, // smallest allocation
1758  GrowthFactor = 2, // how fast to grow (double)
1759  MaxShrinkProofSize = 16, // won't shrink unless bigger
1760  TooBigFactor = 5 // 5X too much->maybe shrink
1761  };
1762 
1763  // Add a new entry to the end of the list, throwing out old entries that
1764  // aren't needed to answer requests at tEarliest or later.
1765  void append(double tEarliest, double tNow, const T& valueNow) {
1766  forgetEntriesMuchOlderThan(tEarliest);
1767  removeEntriesLaterOrEq(tNow);
1768  if (full())
1769  makeMoreRoom();
1770  else if (capacity() > std::max((int)MaxShrinkProofSize,
1771  (int)TooBigFactor * (size()+1)))
1772  makeLessRoom(); // less than 1/TooBigFactor full
1773  const int nextFree = getArrayIndex(m_size++);
1774  m_times[nextFree] = tNow;
1775  m_values[nextFree] = valueNow;
1776  m_maxSize = std::max(m_maxSize, size());
1777  }
1778 
1779  // Prepend an older entry to the beginning of the list. No cleanup is done.
1780  void prepend(double tNewOldest, const T& value) {
1781  assert(empty() || tNewOldest < m_times[m_oldest]);
1782  if (full()) makeMoreRoom();
1783  m_oldest = empty() ? 0 : getArrayIndex(-1);
1784  m_times[m_oldest] = tNewOldest;
1785  m_values[m_oldest] = value;
1786  ++m_size;
1787  m_maxSize = std::max(m_maxSize, size());
1788  }
1789 
1790  // This is a specialized copy assignment for copying an old buffer
1791  // to a new one with updated contents. We are told the earliest time we'll
1792  // be asked about from now on, and won't copy any entries older than those
1793  // needed to answer that earliest request. We won't copy anything at or
1794  // newer than tNow, and finally we'll push (tNow,valueNow) as the newest
1795  // entry.
1796  void copyInAndUpdate(const Measure_Delay_Buffer& oldBuf, double tEarliest,
1797  double tNow, const T& valueNow) {
1798  // clear all current entries (no heap activity)
1799  m_oldest = m_size = 0;
1800 
1801  // determine how may old entries we have to keep
1802  int firstNeeded = oldBuf.countNumUnneededOldEntries(tEarliest);
1803  int lastNeeded = oldBuf.findLastEarlier(tNow); // might be -1
1804  int numOldEntriesToKeep = lastNeeded-firstNeeded+1;
1805  int newSize = numOldEntriesToKeep+1; // includes the new one
1806 
1807  int newSizeRequest = -1;
1808  if (capacity() < newSize) {
1809  newSizeRequest = std::max((int)InitialAllocation,
1810  (int)GrowthFactor * newSize);
1811  ++m_nGrows;
1812  } else if (capacity() > std::max((int)MaxShrinkProofSize,
1813  (int)TooBigFactor * newSize)) {
1814  newSizeRequest = std::max((int)MaxShrinkProofSize,
1815  (int)GrowthFactor * newSize);
1816  ++m_nShrinks;
1817  }
1818 
1819  // Reallocate space if advisable.
1820  if (newSizeRequest != -1) {
1821  const double dNaN = NTraits<double>::getNaN();
1822  m_values.resize(newSizeRequest);
1823  if (m_values.capacity() > m_values.size())
1824  m_values.resize(m_values.capacity()); // don't waste any
1825  m_times.resize(m_values.size(), dNaN);
1826  }
1827 
1828  m_maxCapacity = std::max(m_maxCapacity, capacity());
1829 
1830  // Copy the entries we need to keep.
1831  int nxt = 0;
1832  for (int i=firstNeeded; i<=lastNeeded; ++i, ++nxt) {
1833  m_times[nxt] = oldBuf.getEntryTime(i);
1834  m_values[nxt] = oldBuf.getEntryValue(i);
1835  }
1836  // Now add the newest entry and set the size.
1837  m_times[nxt] = tNow;
1838  m_values[nxt] = valueNow;
1839  assert(nxt+1==newSize);
1840  m_size = nxt+1;
1841  m_maxSize = std::max(m_maxSize, size());
1842  }
1843 
1844  // Given the current time and value and the earlier time at which the
1845  // value is needed, use the buffer and (if necessary) the current value
1846  // to estimate the delayed value.
1847  T calcValueAtTime(double tDelay, double tNow, const T& valueNow) const;
1848 
1849  // Given the current time but *not* the current value of the source measure,
1850  // provide an estimate for the value at tDelay=tNow-delay using only the
1851  // buffer contents and linear interpolation or extrapolation.
1852  void calcValueAtTimeLinearOnly(double tDelay, T& delayedValue) const {
1853  if (empty()) {
1854  // Nothing in the buffer?? Shouldn't happen. Return empty Vector
1855  // or NaN for fixed-size types.
1856  Measure_Num<T>::makeNaNLike(T(), delayedValue);
1857  return;
1858  }
1859 
1860  int firstLater = findFirstLaterOrEq(tDelay);
1861 
1862  if (firstLater > 0) {
1863  // Normal case: tDelay is between two buffer entries.
1864  int firstEarlier = firstLater-1;
1865  double t0=getEntryTime(firstEarlier), t1=getEntryTime(firstLater);
1866  const T& v0=getEntryValue(firstEarlier);
1867  const T& v1=getEntryValue(firstLater);
1868  Real fraction = Real((tDelay-t0)/(t1-t0));
1869  delayedValue = T(v0 + fraction*(v1-v0));
1870  return;
1871  }
1872 
1873  if (firstLater==0) {
1874  // Startup case: tDelay is at or before the oldest buffer entry.
1875  // Assume the value was flat before that.
1876  delayedValue = getEntryValue(firstLater);
1877  return;
1878  }
1879 
1880  // tDelay is later than the latest entry in the buffer. We are going
1881  // to have to extrapolate (yuck).
1882 
1883  if (size() == 1) {
1884  // Just one entry; we'll have to assume the value is flat.
1885  delayedValue = getEntryValue(0);
1886  return;
1887  }
1888 
1889  // Extrapolate using the last two entries.
1890  double t0=getEntryTime(size()-2), t1=getEntryTime(size()-1);
1891  const T& v0=getEntryValue(size()-2);
1892  const T& v1=getEntryValue(size()-1);
1893  Real fraction = Real((tDelay-t0)/(t1-t0)); // > 1
1894  assert(fraction > 1.0);
1895  delayedValue = T(v0 + fraction*(v1-v0)); // Extrapolate.
1896  }
1897 
1898  // Return the number of times we had to grow the buffer.
1899  int getNumGrows() const {return m_nGrows;}
1900  // Return the number of times we decided the buffer was so overallocated
1901  // that we had to shrink it.
1902  int getNumShrinks() const {return m_nShrinks;}
1903  // Return the largest number of values we ever had in the buffer.
1904  int getMaxSize() const {return m_maxSize;}
1905  // Return the largest capacity the buffer ever had.
1906  int getMaxCapacity() const {return m_maxCapacity;}
1907 
1908 private:
1909  // Return the i'th oldest entry
1910  // (0 -> oldest, size-1 -> newest, size -> first free, -1 -> last free)
1911  int getArrayIndex(int i) const
1912  { assert(-1<=i && i<=size());
1913  const int rawIndex = m_oldest + i;
1914  if (rawIndex < 0) return rawIndex + capacity();
1915  else return rawIndex % capacity(); }
1916 
1917  // Remove all but two entries older than the given time.
1918  void forgetEntriesMuchOlderThan(double tEarliest) {
1919  const int numToRemove = countNumUnneededOldEntries(tEarliest);
1920  if (numToRemove) {
1921  m_oldest = getArrayIndex(numToRemove);
1922  m_size -= numToRemove;
1923  }
1924  }
1925 
1926  // Count up how many old entries at the beginning of the buffer are so old
1927  // that they wouldn't be needed to respond to a request at time tEarliest or
1928  // later. We'll keep no more than two entries earlier than tEarliest.
1929  int countNumUnneededOldEntries(double tEarliest) const {
1930  const int firstLater = findFirstLaterOrEq(tEarliest);
1931  return std::max(0, firstLater-2);
1932  }
1933 
1934  // Given the time now, delete anything at the end of the queue that is
1935  // at that same time or later.
1936  void removeEntriesLaterOrEq(double t) {
1937  int lastEarlier = findLastEarlier(t);
1938  m_size = lastEarlier+1;
1939  if (m_size==0) m_oldest=0; // restart at beginning of array
1940  }
1941 
1942  // Return the entry number (0..size-1) of the first entry whose time
1943  // is >= the given time, or -1 if there is none such.
1944  int findFirstLaterOrEq(double tDelay) const {
1945  for (int i=0; i < size(); ++i)
1946  if (getEntryTime(i) >= tDelay)
1947  return i;
1948  return -1;
1949  }
1950 
1951  // Return the entry number(size-1..0) of the last entry whose time
1952  // is < the given time, or -1 if there is none such.
1953  int findLastEarlier(double t) const {
1954  for (int i=size()-1; i>=0; --i)
1955  if (getEntryTime(i) < t)
1956  return i;
1957  return -1;
1958  }
1959 
1960  // We don't have enough space. This is either the initial allocation or
1961  // we need to double the current space.
1962  void makeMoreRoom() {
1963  const int newSizeRequest = std::max((int)InitialAllocation,
1964  (int)GrowthFactor * size());
1965  resize(newSizeRequest);
1966  ++m_nGrows;
1967  m_maxCapacity = std::max(m_maxCapacity, capacity());
1968  }
1969 
1970  // We are wasting a lot of space, reduce the heap allocation to just
1971  // double what we're using now.
1972  void makeLessRoom() {
1973  const int targetMaxSize = std::max((int)MaxShrinkProofSize,
1974  (int)GrowthFactor * size());
1975  if (capacity() > targetMaxSize) {
1976  resize(targetMaxSize);
1977  ++m_nShrinks;
1978  }
1979  }
1980 
1981  // Reallocate memory to get more space or stop wasting space. The new
1982  // size request must be big enough to hold all the current contents. The
1983  // amount we actually get may be somewhat larger than the request. On
1984  // return, the times and values arrays will have been resized and the
1985  // oldest entry will now be entry 0.
1986  void resize(int newSizeRequest) {
1987  assert(newSizeRequest >= size());
1988  const double dNaN = NTraits<double>::getNaN();
1989  Array_<T,int> newValues(newSizeRequest);
1990  if (newValues.capacity() > newValues.size())
1991  newValues.resize(newValues.capacity()); // don't waste any
1992  Array_<double,int> newTimes(newValues.size(), dNaN);
1993 
1994  // Pack existing values into start of new arrays.
1995  for (int i=0; i < size(); ++i) {
1996  const int ix = getArrayIndex(i);
1997  newTimes[i] = m_times[ix];
1998  newValues[i] = m_values[ix];
1999  }
2000  m_times.swap(newTimes); // switch heap space
2001  m_values.swap(newValues);
2002  m_oldest = 0; // starts at the beginning now; size unchanged
2003  }
2004 
2005  // Initialize everything to its default-constructed state.
2006  void initDataMembers() {
2007  m_times.clear(); m_values.clear();
2008  m_oldest=m_size=0;
2009  m_nGrows=m_nShrinks=m_maxSize=m_maxCapacity=0;
2010  }
2011 
2012  // These are circular buffers of the same size.
2013  Array_<double,int> m_times;
2014  Array_<T,int> m_values;
2015  int m_oldest; // Array index of oldest (time,value)
2016  int m_size; // number of entries in use
2017 
2018  // Statistics.
2019  int m_nGrows, m_nShrinks, m_maxSize, m_maxCapacity;
2020 };
2023 template <class T>
2025  typedef Measure_Delay_Buffer<T> Buffer;
2026 public:
2027  // Allocate one cache entry in the base class for the value; we allocate
2028  // a specialized one for the buffer.
2030  : Measure_<T>::Implementation(1), m_delay(NaN),
2031  m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
2032 
2033  Implementation(const Measure_<T>& source, Real delay)
2034  : Measure_<T>::Implementation(1), m_source(source), m_delay(delay),
2035  m_canUseCurrentValue(false), m_useLinearInterpolationOnly(false) {}
2036 
2037  // Default copy constructor gives us a new Implementation object,
2038  // but with reference to the *same* source measure.
2039 
2040  void setSourceMeasure(const Measure_<T>& source) {
2041  if (!source.isSameMeasure(this->m_source)) {
2042  this->m_source = source;
2043  this->invalidateTopologyCache();
2044  }
2045  }
2046 
2047  void setDelay(Real delay) {
2048  if (delay != this->m_delay) {
2049  this->m_delay = delay;
2050  this->invalidateTopologyCache();
2051  }
2052  }
2053 
2054  void setUseLinearInterpolationOnly(bool linearOnly) {
2055  if (linearOnly != this->m_useLinearInterpolationOnly) {
2056  this->m_useLinearInterpolationOnly = linearOnly;
2057  this->invalidateTopologyCache();
2058  }
2059  }
2060 
2061  void setCanUseCurrentValue(bool canUseCurrentValue) {
2062  if (canUseCurrentValue != this->m_canUseCurrentValue) {
2063  this->m_canUseCurrentValue = canUseCurrentValue;
2064  this->invalidateTopologyCache();
2065  }
2066  }
2067 
2068  const Measure_<T>& getSourceMeasure() const {return this->m_source;}
2069  Real getDelay() const {return this->m_delay;}
2071  { return this->m_useLinearInterpolationOnly; }
2073  { return this->m_canUseCurrentValue; }
2074 
2075 
2076  // Implementations of virtual methods.
2077 
2078  // This uses the default copy constructor.
2079  Implementation* cloneVirtual() const override
2080  { return new Implementation(*this); }
2081 
2082  // Currently no derivative supported.
2083  int getNumTimeDerivativesVirtual() const override
2084  { return 0; }
2085 
2086  // If we are allowed to use the current value of the source measure to
2087  // determine the delayed value, the depends-on stage here is the same as
2088  // for the source; otherwise it is Stage::Time.
2089  Stage getDependsOnStageVirtual(int order) const override
2090  { return this->m_canUseCurrentValue ? m_source.getDependsOnStage(order)
2091  : Stage::Time; }
2092 
2093  // Calculate the delayed value and return it to the Measure base class to
2094  // be put in a cache entry.
2095  void calcCachedValueVirtual(const State& s, int derivOrder, T& value) const
2096  override
2097  { const Subsystem& subsys = this->getSubsystem();
2098  const Buffer& buffer = Value<Buffer>::downcast
2099  (subsys.getDiscreteVariable(s,m_bufferIx));
2100  //TODO: use cubic interpolation if allowed
2101  buffer.calcValueAtTimeLinearOnly(s.getTime()-m_delay, value);
2102  }
2103 
2104  void initializeVirtual(State& s) const override {
2105  assert(m_bufferIx.isValid());
2106  const Subsystem& subsys = this->getSubsystem();
2107  Buffer& buffer = Value<Buffer>::updDowncast
2108  (subsys.updDiscreteVariable(s,m_bufferIx));
2109  buffer.clear();
2110  this->getSubsystem().getSystem().realize(s,m_source.getDependsOnStage());
2111  buffer.append(s.getTime()-m_delay, s.getTime(), m_source.getValue(s));
2112  }
2113 
2114  void realizeMeasureTopologyVirtual(State& s) const override {
2115  m_bufferIx = this->getSubsystem()
2117  new Value<Buffer>(), getDependsOnStageVirtual(0));
2118  }
2119 
2122  void realizeMeasureAccelerationVirtual(const State& s) const override {
2123  updateBuffer(s);
2124  }
2125 
2126  // This uses the buffer from the state to update the one in the
2127  // corresponding cache entry. The update adds the current value of the
2128  // source to the end of the buffer and tosses out unneeded old entries.
2129  void updateBuffer(const State& s) const {
2130  assert(m_bufferIx.isValid());
2131  const Subsystem& subsys = this->getSubsystem();
2132 
2133  const Buffer& prevBuffer = Value<Buffer>::downcast
2134  (subsys.getDiscreteVariable(s,m_bufferIx));
2135 
2136  Buffer& nextBuffer = Value<Buffer>::updDowncast
2137  (subsys.updDiscreteVarUpdateValue(s,m_bufferIx));
2138 
2139  const Real t = s.getTime();
2140  nextBuffer.copyInAndUpdate(prevBuffer, t-m_delay,
2141  t, m_source.getValue(s));
2142 
2143  subsys.markDiscreteVarUpdateValueRealized(s,m_bufferIx);
2144  }
2145 private:
2146  // TOPOLOGY STATE
2147  Measure_<T> m_source;
2148  Real m_delay;
2149  bool m_canUseCurrentValue;
2150  bool m_useLinearInterpolationOnly;
2151 
2152  // TOPOLOGY CACHE
2153  mutable DiscreteVariableIndex m_bufferIx; // auto-update
2154 };
2155 
2156 
2157 } // namespace SimTK
2158 
2159 
2160 
2161 
2162 #endif // SimTK_SimTKCOMMON_MEASURE_IMPLEMENTATION_H_
Definition: MeasureImplementation.h:2024
void setIsPresumedValidAtDependsOnStage(bool presume)
Definition: MeasureImplementation.h:414
Definition: MeasureImplementation.h:606
Implementation()
Definition: MeasureImplementation.h:907
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:925
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1335
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:1512
Operation getOperation() const
Return the particular operation being performed by this Extreme measure.
Definition: MeasureImplementation.h:1478
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:1460
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:751
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:245
int size() const
Return the number of elements in the data type of this Measure; for Vector measures this is determine...
Definition: MeasureImplementation.h:455
const Measure_< T > & getOperandMeasure() const
Definition: MeasureImplementation.h:1326
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:743
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
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:1549
#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:532
This is the vector class intended to appear in user code for large, variable size column vectors...
Definition: BigMatrix.h:171
Implementation(Stage invalidated, const T &defaultValue)
Definition: MeasureImplementation.h:717
Definition: MeasureImplementation.h:669
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:991
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:2069
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:444
friend class Implementation
Definition: Measure.h:251
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:987
Definition: Measure.h:843
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:748
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:2079
DiscreteVariableIndex allocateDiscreteVariable(State &s, Stage g, AbstractValue *v) const
Definition: Subsystem.h:96
Implementation(const Measure_< T > &operand)
Definition: MeasureImplementation.h:1307
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:422
void ensureDerivativeIsRealized(const State &s) const
Definition: MeasureImplementation.h:1388
Definition: Measure.h:840
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:996
void realizeMeasureAccelerationVirtual(const State &s) const override
Set the zdots to the integrand (derivative measure) value.
Definition: MeasureImplementation.h:1253
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:1524
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:464
bool isUsingApproximation() const
Definition: MeasureImplementation.h:1325
void resize(size_type n)
Change the size of this Array, preserving all the elements that will still fit, and default construct...
Definition: Array.h:2091
void setInvalidatedStage(Stage invalidated)
Definition: MeasureImplementation.h:825
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1044
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:288
const Real Zero
Real(0)
void setOperandMeasure(const Measure_< T > &operand)
Definition: MeasureImplementation.h:1319
void setCanUseCurrentValue(bool canUseCurrentValue)
Definition: MeasureImplementation.h:2061
void setValue(State &s, const T &value) const
Set the value of the state variables(s) that hold the integral.
Definition: MeasureImplementation.h:1164
void setInitialConditionMeasure(const Measure_< T > &ic)
Definition: MeasureImplementation.h:1185
The Rotation class is a Mat33 that guarantees that the matrix can be interpreted as a legitimate 3x3 ...
Definition: Quaternion.h:40
Implementation(const Measure_< T > &source, Real delay)
Definition: MeasureImplementation.h:2033
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:1384
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:1049
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:1195
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:2089
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:1202
Track the value of the operand that is of maximum absolute value.
Definition: Measure.h:903
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:1482
virtual void realizeMeasureTopologyVirtual(State &) const
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:527
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:359
void updateBuffer(const State &s) const
Definition: MeasureImplementation.h:2129
Definition: MeasureImplementation.h:786
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:1112
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:859
Implementation()
The derivative and initialConditions Measures will be empty handles if this is default constructed...
Definition: MeasureImplementation.h:1146
void setDelay(Real delay)
Definition: MeasureImplementation.h:2047
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:1372
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:835
void calcCachedValueVirtual(const State &s, int derivOrder, T &value) const override
Concrete measures must override this if the state cache is used for precalculated values or derivativ...
Definition: MeasureImplementation.h:928
void realizeReport(const State &s) const
Definition: MeasureImplementation.h:96
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:696
Stage getDependsOnStageVirtual(int order) const override
The depends-on stage for this measure is the same as for its operand.
Definition: MeasureImplementation.h:1517
void setValue(State &state, const T &value) const
Change the value of this Measure in the given state.
Definition: MeasureImplementation.h:736
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:1591
int getRefCount() const
Definition: MeasureImplementation.h:79
Track the minimum value of the operand (signed).
Definition: Measure.h:883
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1108
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:1347
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
Definition: MeasureImplementation.h:1019
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:920
bool getCanUseCurrentValue() const
Definition: MeasureImplementation.h:2072
int getCopyNumber() const
Definition: MeasureImplementation.h:81
void initializeVirtual(State &s) const override
Definition: MeasureImplementation.h:2104
Implementation(const T &amplitude, const T &frequency, const T &phase=T(0))
Definition: MeasureImplementation.h:911
Implementation()
Definition: MeasureImplementation.h:1305
Stage getDependsOnStage() const
Definition: MeasureImplementation.h:839
const Stage & getSystemStage() const
This returns the global stage for this State.
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:982
bool isFinite(const negator< float > &x)
Definition: negator.h:285
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1191
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:881
const AbstractValue & getDiscreteVarUpdateValue(const State &s, DiscreteVariableIndex dx) const
Definition: Subsystem.h:259
Stage getInvalidatedStage() const
Definition: MeasureImplementation.h:731
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1331
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:2095
const Measure_< T > & getOperandMeasure() const
Definition: MeasureImplementation.h:1091
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1040
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:617
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:628
bool getIsPresumedValidAtDependsOnStage() const
Definition: MeasureImplementation.h:418
void setSourceMeasure(const Measure_< T > &source)
Definition: MeasureImplementation.h:2040
MeasureIndex getSubsystemMeasureIndex() const
Return the MeasureIndex by which this Measure is known to the Subsystem that owns it...
Definition: MeasureImplementation.h:233
Implementation(const Implementation &source)
Definition: MeasureImplementation.h:722
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:740
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:630
void initializeVirtual(State &s) const override
Definition: MeasureImplementation.h:1359
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:618
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:683
Definition: MeasureImplementation.h:966
Definition: Measure.h:842
Definition: MeasureImplementation.h:1072
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:795
Implementation()
Definition: MeasureImplementation.h:713
Forces calculated.
Definition: Stage.h:76
Implementation()
Definition: MeasureImplementation.h:2029
bool hasImpl() const
Definition: Measure.h:242
Implementation()
Definition: MeasureImplementation.h:1023
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
#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:2114
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:1468
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:623
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:1561
Implementation(const Measure_< T > &left, const Measure_< T > &right)
Definition: MeasureImplementation.h:1025
T & updValue(const State &state) const
Definition: MeasureImplementation.h:866
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:677
#define SimTK_ERRCHK3_ALWAYS(cond, whereChecked, fmt, a1, a2, a3)
Definition: ExceptionMacros.h:293
This class represents the rotate-and-shift transform which gives the location and orientation of a ne...
Definition: Transform.h:43
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:871
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:1035
Implementation(const Measure_< T > &left, const Measure_< T > &right)
Definition: MeasureImplementation.h:972
This creates a Measure whose value is a Topology-stage constant of any type T.
Definition: Measure.h:339
virtual void realizeMeasureReportVirtual(const State &) const
Definition: MeasureImplementation.h:144
void setForceUseApproximation(bool mustApproximate)
Definition: MeasureImplementation.h:1314
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:1582
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:489
bool getForceUseApproximation() const
Definition: MeasureImplementation.h:1324
Zero()
Definition: MeasureImplementation.h:644
int getNumCacheEntries() const
Return the number of cache entries allocated for the value and derivatives of this Measure...
Definition: MeasureImplementation.h:459
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:874
Implementation()
Definition: MeasureImplementation.h:970
int getNumTimeDerivatives() const
Every Measure can produce a value, and some can provide one or more total derivatives with respect to...
Definition: MeasureImplementation.h:237
~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:240
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:1218
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:690
void swap(Array_ &other)
This is a specialized algorithm providing constant time exchange of data with another array that has ...
Definition: Array.h:1983
#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:1212
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:902
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:654
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:1244
void realizeModel(State &s) const
Definition: MeasureImplementation.h:89
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:1439
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:1150
const Measure_< T > & getSourceMeasure() const
Definition: MeasureImplementation.h:2068
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:2122
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:878
This unique integer type is for selecting discrete variables.
void realizeAcceleration(const State &s) const
Definition: MeasureImplementation.h:95
Definition: Measure.h:841
Stage getInvalidatedStage() const
Definition: MeasureImplementation.h:840
The implementation for Integrate measures allocates a continuous state variable or variables from the...
Definition: MeasureImplementation.h:1141
Implementation(const T &value)
Definition: MeasureImplementation.h:612
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:1099
Implementation(const Implementation &source)
Definition: MeasureImplementation.h:806
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:556
Implementation()
Default constructor leaves the operand measure unspecified; no base class cache entries are allocated...
Definition: MeasureImplementation.h:1446
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:862
Implementation * cloneVirtual() const override
Definition: MeasureImplementation.h:1507
void setDerivativeMeasure(const Measure_< T > &d)
Definition: MeasureImplementation.h:1183
int getNumTimeDerivatives() const
Definition: MeasureImplementation.h:103
Track the maximum value of the operand (signed).
Definition: Measure.h:893
bool getUseLinearInterpolationOnly() const
Definition: MeasureImplementation.h:2070
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:433
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:2083
Stage getDependsOnStageVirtual(int order) const override
Definition: MeasureImplementation.h:1339
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:241
void realizeMeasureTopologyVirtual(State &s) const override
Concrete measures can override this to allocate Topology-stage resources.
Definition: MeasureImplementation.h:757
Provide a unique integer type for identifying Subsystems.
void setUseLinearInterpolationOnly(bool linearOnly)
Definition: MeasureImplementation.h:2054
const T & getValue(const State &s, int derivOrder=0) const
Retrieve the Value of this Measure or one of its time derivatives, assuming the supplied State has be...
Definition: Measure.h:274
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:1451
void markCacheValueNotRealized(const State &s, int derivOrder) const
Invalidate one of this Measure&#39;s cache entries.
Definition: MeasureImplementation.h:514
Operation
Definition: Measure.h:839
Track the value of the operand that is of minimum absolute value (not very useful).
Definition: Measure.h:914
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
Implementation()
Definition: MeasureImplementation.h:1078
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:1086
void setDependsOnStage(Stage dependsOn)
Definition: MeasureImplementation.h:811
Implementation(Real factor, const Measure_< T > &operand)
Definition: MeasureImplementation.h:1080
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:1474
void setDefaultValue(const T &defaultValue)
Set a new default value for this Measure.
Definition: MeasureImplementation.h:403
const T & getValue(const State &s, int derivOrder) const
Definition: MeasureImplementation.h:361
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:412
AbstractMeasure & shallowAssign(const AbstractMeasure &)
Shallow assignment operator destructs the current Implementation object (meaning its reference count ...
Definition: MeasureImplementation.h:195
Definition: MeasureImplementation.h:706
Constant & setValue(const T &value)
Change the value returned by this Measure.
Definition: Measure.h:356
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:1493
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:501
const Measure_< T > & getDerivativeMeasure() const
Definition: MeasureImplementation.h:1170
size_type capacity() const
Return the number of elements this array can currently hold without requiring reallocation.
Definition: Array.h:2085
Stage getDependsOnStageVirtual(int derivOrder) const override
Definition: MeasureImplementation.h:692
const Subsystem & getSubsystem() const
Definition: MeasureImplementation.h:119
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:632
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:1104
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:1157
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:726
int getNumTimeDerivativesVirtual() const override
Definition: MeasureImplementation.h:923
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:1225
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:545
Definition: MeasureImplementation.h:1299
Implementation()
Definition: MeasureImplementation.h:791
Definition: negator.h:64
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:477
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:1176
This is the base handle class for all Measures whose value type is known, including all the Simbody b...
Definition: Measure.h:261
void markAsValid(const State &state) const
Definition: MeasureImplementation.h:843
Implementation()
Definition: MeasureImplementation.h:611
Concrete templatized class derived from AbstractValue, adding generic value type-specific functionali...
Definition: Value.h:39