linear.h

Go to the documentation of this file.

#pragma once
 
#include "../VariantValue.h"
#include "../exceptions.h"
 
namespace armarx::trajectory::interpolate
{
 
    /**
     * @brief Linear interpolation visitor: Interpolates between the given values linearly.
     */
    class Linear
    {
    public:
        using result_type = VariantValue; ///< Exposed result type.
 
    public:
        /**
         * @brief Interpolator
         * @param t in [0, 1], where `t = 0` for `lhs` and `t = 1` for `rhs`.
         */
        Linear(float t) : t(t)
        {
        }
 
        template <typename U, typename V>
        VariantValue
        operator()(const U&, const V&) const
        {
            throw error::InterpolateDifferentTypesError();
        }
 
        VariantValue
        operator()(const float& lhs, const float& rhs) const
        {
            return (1 - t) * lhs + t * rhs;
        }
 
        VariantValue
        operator()(const Eigen::MatrixXf& lhs, const Eigen::MatrixXf& rhs) const
        {
            return (1 - t) * lhs + t * rhs;
        }
 
        VariantValue
        operator()(const Eigen::Quaternionf& lhs, const Eigen::Quaternionf& rhs) const
        {
            return lhs.slerp(t, rhs);
        }
 
 
    private:
        float t;
    };
 
    template <typename ReturnT>
    ReturnT
    linear(float t, const VariantValue& lhs, const VariantValue& rhs)
    {
        VariantValue result = std::visit(Linear{t}, lhs, rhs);
        return std::get<ReturnT>(result);
    }
 
    template <>
    VariantValue linear<VariantValue>(float t, const VariantValue& lhs, const VariantValue& rhs);
 
} // namespace armarx::trajectory::interpolate