Track.h

Go to the documentation of this file.

#pragma once
 
#include "VariantValue.h"
#include "interpolate/linear.h"
 
 
namespace armarx::trajectory
{
 
    /**
     * @brief A keyframe, representing a value at a given time.
     */
    template <typename ValueT>
    struct Keyframe
    {
        /// Constructor.
        Keyframe(float time, const ValueT& value) : time(time), value(value)
        {}
 
        float time;   ///< The time on the timeline.
        ValueT value; ///< The value.
    };
 
    /// A keyframe with of type TValue.
    using VariantKeyframe = Keyframe<VariantValue>;
 
 
    /**
     * @brief A track represents the timeline of a single value, identified by a track ID.
     * A track is comprised of a sequence of keyframes and used to call a
     * single update function.
     */
    template <typename ValueT>
    class Track
    {
    public:
        /// The update function type.
        using UpdateFunc = std::function<void(ValueT)>;
        // Forward declaration.
        struct KeyframeProxy;
 
    public:
 
        /// Construct a track with given ID (and no update function).
        Track(const TrackID& id) :
            id(id)
        {}
 
        /// Construct a track with given ID and update function.
        Track(const TrackID& id, UpdateFunc updateFunc) :
            id(id), updateFunc(updateFunc)
        {}
 
 
        /// Indicate whether this track does not contain any keyframes.
        bool empty() const;
        /// Clear the track of all keyframes.
        void clear();
 
        /// Add a keyframe to this track.
        void addKeyframe(const Keyframe<ValueT>& keyframe);
        /// Add a keyframe to this track.
        void addKeyframe(float time, const ValueT& value);
 
        /// Add a keyframe by assignment: `track[time] = value;`
        KeyframeProxy operator[](float time);
 
 
        /**
         * @brief Get the interpolated value at the given time.
         * @throws `error::EmptyTrack` If the track is empty.
         */
        ValueT at(float time) const;
 
        /**
         * @brief Call the update function with the interpolated value at the given time.
         *
         * @param ignoreIfEmpty If true and the track is empty, the method does nothing.
         * @throws `error::EmptyTrack` If the track is empty and `ignoreIfEmpty` is false.
         */
        void update(float time, bool ignoreIfEmpty = false);
 
        template <typename V>
        friend std::ostream& operator<<(std::ostream& os, const Track<V>& track);
 
 
    private:
 
        /// Sort the keyframes.
        void sortKeyframes();
 
        /**
         * @throw `error::WrongValueTypeInKeyframe` If the given value's type
         * conflicts with contained keyframes.
         */
        void checkValueType(const ValueT& value);
 
 
        /// The track ID.
        TrackID id;
        /// The update function.
        UpdateFunc updateFunc;
        /// The sorted array of keyframes.
        std::vector<Keyframe<ValueT>> keyframes;
 
 
    public:
 
        /// A proxy allowing for adding keyframes by: `track[time] = value;`
        struct KeyframeProxy
        {
        public:
            /// Get the value at time.
            operator ValueT() const;
            /// Add a keyframe on assignment.
            void operator= (const ValueT& value);
            /// Add a keyframe on assignment.
            void operator= (const KeyframeProxy& value);
 
            /// Get the value at time.
            ValueT value() const;
 
        private:
            friend Track; // Allow TrackBase to use constructor.
 
            KeyframeProxy(Track& track, float time);
 
            Track& track;
            float time;
        };
 
    };
 
    /// A track with value type TValue.
    using VariantTrack = Track<VariantValue>;
 
    template <typename V>
    bool Track<V>::empty() const
    {
        return keyframes.empty();
    }
 
    template<typename V>
    void Track<V>::clear()
    {
        keyframes.clear();
    }
 
    template <typename V>
    void Track<V>::addKeyframe(const Keyframe<V>& keyframe)
    {
        checkValueType(keyframe.value);
        keyframes.push_back(keyframe);
        sortKeyframes();
    }
 
    template <typename V>
    void Track<V>::addKeyframe(float time, const V& value)
    {
        addKeyframe(Keyframe<V>(time, value));
    }
 
    template<typename V>
    auto Track<V>::operator[](float time) -> KeyframeProxy
    {
        return KeyframeProxy{*this, time};
    }
 
    template <typename V>
    void Track<V>::sortKeyframes()
    {
        std::sort(keyframes.begin(), keyframes.end(),
                  [](const Keyframe<V>& lhs, const Keyframe<V>& rhs)
        {
            return lhs.time < rhs.time;
        });
    }
 
    /// In general, do nothing.
    template <typename V>
    void Track<V>::checkValueType(const V&)
    {}
 
    /**
     * If the track is not empty, check whether the given type of the given
     * value is the same as the type of the values already in the track.
     *
     * @throws `error::WrongValueTypeInKeyframe` if the types do not match.
     */
    template <>
    void Track<VariantValue>::checkValueType(const VariantValue& value);
 
 
    template <typename V>
    V Track<V>::at(float time) const
    {
        if (empty())
        {
            throw error::EmptyTrack(id);
        }
 
        if (keyframes.size() == 1)
        {
            return keyframes.front().value;
        }
 
        if (time <= keyframes.front().time)
        {
            return keyframes.front().value;
        }
        if (time >= keyframes.back().time)
        {
            return keyframes.back().value;
        }
 
 
        std::size_t i = 0;
        while (i + 1 <= keyframes.size() && keyframes[i + 1].time < time)
        {
            i++;
        }
 
        // interpolate between i and i+1
        const Keyframe<V>& kf1 = keyframes.at(i);
        const Keyframe<V>& kf2 = keyframes.at(i + 1);
 
        float t = (time - kf1.time) / (kf2.time - kf1.time);
        // t = 0 =>  full kf1, t = 1 => full kf2
 
        return interpolate::linear<V>(t, kf1.value, kf2.value);
    }
 
 
    template <typename V>
    void Track<V>::update(float time, bool ignoreIfEmpty)
    {
        if (updateFunc && !(ignoreIfEmpty && empty()))
        {
            updateFunc(at(time));
        }
    }
 
 
    template<typename V>
    Track<V>::KeyframeProxy::KeyframeProxy(Track& track, float time) :
        track(track), time(time)
    {}
 
    template <typename V>
    Track<V>::KeyframeProxy::operator V() const
    {
        return track.at(time);
    }
 
    template <typename V>
    void Track<V>::KeyframeProxy::operator=(const V& value)
    {
        track.addKeyframe(time, value);
    }
 
    template <typename V>
    void Track<V>::KeyframeProxy::operator=(const KeyframeProxy& other)
    {
        track.addKeyframe(time, other.value());
    }
 
    template <typename V>
    auto Track<V>::KeyframeProxy::value() const -> V
    {
        return track.at(time);
    }
 
    template <typename ValueT>
    std::ostream& operator<<(std::ostream& os, const Track<ValueT>& track)
    {
        os << "Track '" << track.id << "' with " << track.keyframes.size() << " keyframes: [";
        for (const Keyframe<ValueT>& kf : track.keyframes)
        {
            os << kf.time << ", ";
        }
        return os << "]";
    }
 
}