Measure.h

Go to the documentation of this file.

#ifndef SimTK_SimTKCOMMON_MEASURE_H_
#define SimTK_SimTKCOMMON_MEASURE_H_

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2008-13 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon/basics.h"
#include "SimTKcommon/Simmatrix.h"

#include <cassert>

// Helper macro shared by SimTK_MEASURE_HANDLE_PREAMBLE and
// SimTK_MEASURE_HANDLE_PREAMBLE_ABSTRACT.
#define SimTK_MEASURE_HANDLE_PREAMBLE_BASE(MH,PH) \
    class Implementation;                                           \
    explicit MH(Implementation* imp) : PH(imp) {}                   \
    MH(SimTK::Subsystem& sub, Implementation* imp,                  \
       const SimTK::AbstractMeasure::SetHandle& sh)                 \
    :   PH(sub,imp,sh) {}                                           \
    MH& operator=(const MH& src) {PH::operator=(src); return *this;}\
    MH& shallowAssign(const MH& src) {PH::shallowAssign(src); return *this;}\
    MH& deepAssign(const MH& src) {PH::deepAssign(src); return *this;}


// The default constructor for concrete classes should instantiate
// a default-constructed Implementation object if no Implementation object
// is provided.
#define SimTK_MEASURE_HANDLE_PREAMBLE(MH,PH)    \
    SimTK_MEASURE_HANDLE_PREAMBLE_BASE(MH,PH)   \
    MH() : PH(new Implementation()) {}          \
    explicit MH(SimTK::Subsystem& sub)          \
    : PH(sub,new Implementation(), typename PH::SetHandle()) {}



// The default constructor for a still-abstract derived class can't
// instantiate an Implementation.
#define SimTK_MEASURE_HANDLE_PREAMBLE_ABSTRACT(MH,PH)   \
    SimTK_MEASURE_HANDLE_PREAMBLE_BASE(MH,PH)           \
    MH() : PH() {}

#define SimTK_MEASURE_HANDLE_POSTSCRIPT(MH,PH) \
    static bool isA(const SimTK::AbstractMeasure& m)                        \
    {   return dynamic_cast<const Implementation*>(&m.getImpl()) != 0; }    \
    static const MH& getAs(const SimTK::AbstractMeasure& m)                 \
    {   assert(isA(m)); return static_cast<const MH&>(m); }                 \
    static MH& updAs(SimTK::AbstractMeasure& m)                             \
    {   assert(isA(m)); return static_cast<MH&>(m); }                       \
    const Implementation& getImpl() const                                   \
    {   return SimTK_DYNAMIC_CAST_DEBUG<const Implementation&>              \
                    (SimTK::AbstractMeasure::getImpl());}                   \
    Implementation& updImpl()                                               \
    {   return SimTK_DYNAMIC_CAST_DEBUG<Implementation&>                    \
                    (SimTK::AbstractMeasure::updImpl());} 

namespace SimTK {

class State;
class Subsystem;
class System;
class EventId;

SimTK_DEFINE_UNIQUE_INDEX_TYPE(MeasureIndex);

//==============================================================================
//                            ABSTRACT MEASURE
//==============================================================================
class SimTK_SimTKCOMMON_EXPORT AbstractMeasure {
protected:
    class SetHandle {};

public:
    class Implementation; // local; name is AbstractMeasure::Implementation

    explicit AbstractMeasure(Implementation* g=0);

    AbstractMeasure(Subsystem&, Implementation* g, const SetHandle&);

    AbstractMeasure(const AbstractMeasure&);

    AbstractMeasure& operator=(const AbstractMeasure& source)
    {   return shallowAssign(source); }

    ~AbstractMeasure();

    AbstractMeasure& shallowAssign(const AbstractMeasure&);

    AbstractMeasure& deepAssign(const AbstractMeasure& source);

    int getNumTimeDerivatives() const;

    Stage getDependsOnStage(int derivOrder=0) const;


    bool isSameMeasure(const AbstractMeasure& other) const
    {   return impl && impl==other.impl;}

    bool isEmptyHandle() const {return !hasImpl();}

    bool isInSubsystem() const;
    const Subsystem& getSubsystem()  const;
    bool isSameSubsystem(const Subsystem&) const;
    MeasureIndex getSubsystemMeasureIndex() const;

    // Internal use only

    // dynamic_cast the returned reference to a reference to your concrete
    // Implementation class.
    const Implementation& getImpl() const {assert(impl); return *impl;}
    Implementation&       updImpl()       {assert(impl); return *impl;}
    bool                  hasImpl() const {return impl!=0;}

    int getRefCount() const;
private:
    // This is the only data member in this class. Also, any class derived 
    // from AbstractMeasure must have *NO* data members at all (data goes 
    // in the Implementation class).
    Implementation* impl;

friend class Implementation;
};


//==============================================================================
//                               MEASURE <T>
//==============================================================================
template <class T>
class Measure_ : public AbstractMeasure {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE_ABSTRACT(Measure_, AbstractMeasure);

    const T& getValue(const State& s, int derivOrder=0) const 
    {   return getImpl().getValue(s,derivOrder); }

    Measure_& setDefaultValue(const T& defaultValue) 
    {   updImpl().setDefaultValue(defaultValue); return *this; }

    const T& getDefaultValue() const
    {   return getImpl().getDefaultValue(); }

    // These are built-in Measures with local class names. 

    // Templatized measures may have restrictions on the allowable template
    // type and may be specialized for particular types.
    class Zero;         // T is any numerical type
    class One;          // T is any numerical type
    class Constant;     // T is any assignable type
    class Time;         // T is any type for which T(t) makes sense.
    class Variable;     // T is any assignable type (state)
    class Result;       // T is any assignable type (cache)
    class SampleAndHold;// T is any assignable type
    class Delay;        // T is any assignable type

    // This requires any numerical type.
    class Plus;
    class Minus;
    class Scale;
    class Differentiate;

    // These find extreme values *in time*, not among inputs at the same
    // time. They perform elementwise on aggregate types.
    class Extreme;  // base class for min/max/minabs/maxabs
    class Minimum;  // most positive value
    class Maximum;  // most negative value
    class MinAbs;   // the signed quantity whose absolute value was min
    class MaxAbs;   // the signed quantity whose absolute value was max

    // These accept floating point numerical template arguments only.
    class Integrate;
    class Sinusoid;

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Measure_, AbstractMeasure);
};

typedef Measure_<Real> Measure;


//==============================================================================
//                                CONSTANT
//==============================================================================
template <class T>
class Measure_<T>::Constant : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Constant, Measure_<T>);

    explicit Constant(const T& value)
    :   Measure_<T>(new Implementation(value)) {}

    Constant(Subsystem& sub, const T& value)
    :   Measure_<T>(sub, new Implementation(value), 
                    AbstractMeasure::SetHandle()) {}

    Constant& setValue(const T& value) 
    {   updImpl().setValue(value); return *this; }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Constant, Measure_<T>);
};

//==============================================================================
//                                   ZERO
//==============================================================================
template <class T>
class Measure_<T>::Zero : public Measure_<T>::Constant {
public:
    Zero();
    explicit Zero(Subsystem& sub);
};

template <>
class Measure_< Vector >::Zero : public Measure_< Vector >::Constant {
public:
    explicit Zero(int size);
    Zero(Subsystem& sub, int size);
};

//==============================================================================
//                                    ONE
//==============================================================================
template <class T>
class Measure_<T>::One : public Measure_<T>::Constant {
public:
    One();
    explicit One(Subsystem& sub);
};

template <>
class Measure_< Vector >::One : public Measure_< Vector >::Constant {
public:
    explicit One(int size);
    One(Subsystem& sub, int size);
};

//==============================================================================
//                                   TIME
//==============================================================================
template <class T>
class Measure_<T>::Time : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Time, Measure_<T>);

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Time, Measure_<T>);
};

//==============================================================================
//                                 VARIABLE
//==============================================================================
template <class T>
class Measure_<T>::Variable : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Variable, Measure_<T>);

    // TODO: should not require invalidated Stage here. Instead, 
    // should have a unique "generation" counter for this variable
    // and allow subsequent users to check it.
    Variable(Subsystem& sub, Stage invalidates, const T& defaultValue)
    :   Measure_<T>(sub, new Implementation(invalidates, defaultValue), 
                    AbstractMeasure::SetHandle()) {}


    void setValue(State& state, const T& value) const
    {   getImpl().setValue(state, value); }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Variable, Measure_<T>);
};

//==============================================================================
//                                 RESULT
//==============================================================================
template <class T>
class Measure_<T>::Result : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Result, Measure_<T>);

    // TODO: should not require invalidated Stage here. Instead, 
    // should have a unique "generation" counter for this cache entry
    // and allow subsequent users of the value to check it.

    Result(Subsystem& sub, Stage dependsOn, Stage invalidated)
    :   Measure_<T>(sub, new Implementation(dependsOn, invalidated), 
                    AbstractMeasure::SetHandle()) {}

    Stage getDependsOnStage() const {return getImpl().getDependsOnStage();}
    Stage getInvalidatedStage() const {return getImpl().getInvalidatedStage();}
    Result& setDependsOnStage(Stage dependsOn) 
    {   updImpl().setDependsOnStage(dependsOn); return *this; }
    Result& setInvalidatedStage(Stage invalidated) 
    {   updImpl().setInvalidatedStage(invalidated); return *this; }

    Result& setIsPresumedValidAtDependsOnStage(bool presume)
    {   updImpl().setIsPresumedValidAtDependsOnStage(presume); return *this; }

    bool getIsPresumedValidAtDependsOnStage() const
    {   return getImpl().getIsPresumedValidAtDependsOnStage(); }


    T& updValue(const State& state) const
    {   return getImpl().updValue(state); }

    void markAsValid(const State& state) const {getImpl().markAsValid(state);}

    bool isValid(const State& state) const {return getImpl().isValid(state);}

    void markAsNotValid(const State& state) const 
    {   getImpl().markAsNotValid(state); }

    void setValue(const State& state, const T& value) const
    {   updValue(state) = value; markAsValid(state); }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Result, Measure_<T>);
};

//==============================================================================
//                                 SINUSOID
//==============================================================================
template <class T>
class Measure_<T>::Sinusoid : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Sinusoid, Measure_<T>);

    Sinusoid(Subsystem& sub,
             const T& amplitude, 
             const T& frequency,
             const T& phase=T(0))
    :   Measure_<T>(sub, new Implementation(amplitude,frequency,phase), 
                    AbstractMeasure::SetHandle()) {}

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Sinusoid, Measure_<T>);
};

//==============================================================================
//                                   PLUS
//==============================================================================
template <class T>
class Measure_<T>::Plus : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Plus, Measure_<T>);

    Plus(Subsystem& sub, const Measure_<T>& left, const Measure_<T>& right)
    :   Measure_<T>(sub, new Implementation(left, right), 
                    AbstractMeasure::SetHandle())
    {   SimTK_ERRCHK_ALWAYS
           (   this->isSameSubsystem(left.getSubsystem())
            && this->isSameSubsystem(right.getSubsystem()),
            "Measure_<T>::Plus::ctor()",
            "Arguments must be in the same Subsystem as this Measure.");
    }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Plus, Measure_<T>);
};

//==============================================================================
//                                   MINUS
//==============================================================================
template <class T>
class Measure_<T>::Minus : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Minus, Measure_<T>);

    Minus(Subsystem& sub, const Measure_<T>& left, const Measure_<T>& right)
    :   Measure_<T>(sub, new Implementation(left, right), 
                    AbstractMeasure::SetHandle())
    {   SimTK_ERRCHK_ALWAYS
           (   this->isSameSubsystem(left.getSubsystem())
            && this->isSameSubsystem(right.getSubsystem()),
            "Measure_<T>::Minus::ctor()",
            "Arguments must be in the same Subsystem as this Measure.");
    }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Minus, Measure_<T>);
};

//==============================================================================
//                                   SCALE
//==============================================================================
template <class T>
class Measure_<T>::Scale : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Scale, Measure_<T>);

    Scale(Subsystem& sub, Real factor, const Measure_<T>& operand)
    :   Measure_<T>(sub, new Implementation(factor, operand), 
                    AbstractMeasure::SetHandle())
    {   SimTK_ERRCHK_ALWAYS
           (this->isSameSubsystem(operand.getSubsystem()),
            "Measure_<T>::Scale::ctor()",
            "Argument must be in the same Subsystem as this Measure.");
    }

    const Measure_<T>& getOperandMeasure() const 
    { return getImpl().getOperandMeasure(); }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Scale, Measure_<T>);
};

//==============================================================================
//                                 INTEGRATE
//==============================================================================
template <class T>
class Measure_<T>::Integrate : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Integrate, Measure_<T>);

    Integrate(Subsystem&            subsystem, 
              const Measure_<T>&    deriv, 
              const Measure_<T>&    ic,
              const T&              initAlloc=T(0))
    :   Measure_<T>(subsystem, new Implementation(deriv,ic,initAlloc), 
                    AbstractMeasure::SetHandle()) {}

    void setValue(State& s, const T& value) const 
    {   return getImpl().setValue(s, value); }

    const Measure_<T>& getDerivativeMeasure() const 

    {   return getImpl().getDerivativeMeasure(); }
    const Measure_<T>& getInitialConditionMeasure() const 
    {   return getImpl().getInitialConditionMeasure(); }

    Integrate& setDerivativeMeasure(const Measure_<T>& d)
    {   updImpl().setDerivativeMeasure(d); return *this; }
    Integrate& setInitialConditionMeasure(const Measure_<T>& ic)
    {   updImpl().setInitialConditionMeasure(ic); return *this; }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Integrate, Measure_<T>);
};

//==============================================================================
//                               DIFFERENTIATE
//==============================================================================
template <class T>
class Measure_<T>::Differentiate : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Differentiate, Measure_<T>);

    Differentiate(Subsystem& subsystem, const Measure_<T>& operand)
    :   Measure_<T>(subsystem, new Implementation(operand), 
                    AbstractMeasure::SetHandle()) {}

    bool isUsingApproximation() const 
    {   return getImpl().isUsingApproximation(); }

    const Measure_<T>& getOperandMeasure() const 
    {   return getImpl().getOperandMeasure(); }

    Differentiate& setOperandMeasure(const Measure_<T>& operand)
    {   updImpl().setOperandMeasure(operand); return *this; }

    void setForceUseApproximation(bool mustApproximate)
    {   updImpl().setForceUseApproximation(mustApproximate); }

    bool getForceUseApproximation() const 
    {   return getImpl().getForceUseApproximation(); }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Differentiate, Measure_<T>);
};

//==============================================================================
//                 EXTREME, MINIMUM, MAXIMUM, MINABS, MAXABS
//==============================================================================
template <class T>
class Measure_<T>::Extreme : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Extreme, Measure_<T>);

    enum Operation {
        MaxAbs, // default
        Maximum,
        MinAbs,
        Minimum
    };

    Extreme(Subsystem& sub, const Measure_<T>& operand, Operation op=MaxAbs)
    :   Measure_<T>(sub, new Implementation(operand, op), 
                    AbstractMeasure::SetHandle()) {}

    Extreme& setOperation(Operation op)
    {   updImpl().setOperation(op); return *this; }

    Operation getOperation() const {return getImpl().getOperation();}

    Real getTimeOfExtremeValue(const State& state) const
    {   return getImpl().getTimeOfExtremeValue(state); }

    void setValue(State& s, const T& value) const 
    {   return getImpl().setValue(s, value); }

    const Measure_<T>& getOperandMeasure() const 
    {   return getImpl().getOperandMeasure(); }

    Extreme& setOperandMeasure(const Measure_<T>& s)
    {   updImpl().setOperandMeasure(s); return *this; }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Extreme, Measure_<T>);
};

template <class T>
class Measure_<T>::Minimum : public Measure_<T>::Extreme {
    typedef typename Measure_<T>::Extreme Super;
public:
    Minimum(Subsystem& sub, const Measure_<T>& operand)
    :   Super(sub, operand, Super::Minimum) {}
};

template <class T>
class Measure_<T>::Maximum : public Measure_<T>::Extreme {
    typedef typename Measure_<T>::Extreme Super;
public:
    Maximum(Subsystem& sub, const Measure_<T>& operand)
    :   Super(sub, operand, Super::Maximum) {}
};

template <class T>
class Measure_<T>::MaxAbs : public Measure_<T>::Extreme {
    typedef typename Measure_<T>::Extreme Super;
public:
    MaxAbs(Subsystem& sub, const Measure_<T>& operand)
    :   Super(sub, operand, Super::MaxAbs) {}
};

template <class T>
class Measure_<T>::MinAbs : public Measure_<T>::Extreme {
    typedef typename Measure_<T>::Extreme Super;
public:
    MinAbs(Subsystem& sub, const Measure_<T>& operand)
    :   Super(sub, operand, Super::MinAbs) {}
};

//==============================================================================
//                                 DELAY
//==============================================================================
template <class T>
class Measure_<T>::Delay : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(Delay, Measure_<T>);
    Delay(Subsystem& sub, const Measure_<T>& source, Real delay)
    :   Measure_<T>(sub, new Implementation(source, delay), 
                    AbstractMeasure::SetHandle()) {}

    Delay& setUseLinearInterpolationOnly(bool linearOnly)
    {   updImpl().setUseLinearInterpolationOnly(linearOnly); return *this; }

    Delay& setCanUseCurrentValue(bool canUseCurrentValue)
    {   updImpl().setCanUseCurrentValue(canUseCurrentValue); return *this; }

    Delay& setSourceMeasure(const Measure_<T>& source)
    {   updImpl().setSourceMeasure(source); return *this; }

    Delay& setDelay(Real delay)
    {   updImpl().setDelay(delay); return *this; }

    bool getUseLinearInterpolationOnly() const
    {   return getImpl().getUseLinearInterpolationOnly(); }

    bool getCanUseCurrentValue() const
    {   return getImpl().getCanUseCurrentValue(); }

    const Measure_<T>& getSourceMeasure() const
    {   return getImpl().getSourceMeasure(); }

    Real getDelay() const
    {   return getImpl().getDelay(); }

    SimTK_MEASURE_HANDLE_POSTSCRIPT(Delay, Measure_<T>);
};

//==============================================================================
//                              SAMPLE AND HOLD
//==============================================================================
template <class T>
class Measure_<T>::SampleAndHold : public Measure_<T> {
public:
    SimTK_MEASURE_HANDLE_PREAMBLE(SampleAndHold, Measure_<T>);

    SampleAndHold(Subsystem& sub, const Measure_<T>& source, EventId e);

    void setValue(State& s, const T& value) const;

    void sample(State& s) const;

    const Measure_<T>& getSource() const;
    EventId            getEventId() const;

    SampleAndHold& setSource(const Measure_<T>& s);
    SampleAndHold& setEventId(EventId);

    SimTK_MEASURE_HANDLE_POSTSCRIPT(SampleAndHold, Measure_<T>);
};

} // namespace SimTK

#endif // SimTK_SimTKCOMMON_MEASURE_H_