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