path.hpp

Go to the documentation of this file.

#pragma once
 
#include <cmath>
#include <functional>
#include <initializer_list>
#include <memory>
#include <optional>
#include <utility>
#include <vector>
 
#include "copp/core.hpp"
 
namespace copp
{
 
    class Path;
 
    namespace detail
    {
        COPP_API const void *path_handle(const Path &path) noexcept;
    } // namespace detail

    struct Jet3
    {
        double v = 0.0;
        double d1 = 0.0;
        double d2 = 0.0;
        double d3 = 0.0;
 
        static constexpr Jet3 constant(double value) noexcept
        {
            return Jet3{value, 0.0, 0.0, 0.0};
        }
 
        static constexpr Jet3 seed(double value) noexcept
        {
            return Jet3{value, 1.0, 0.0, 0.0};
        }
    };
 
    inline constexpr Jet3 operator+(Jet3 lhs, Jet3 rhs) noexcept
    {
        return Jet3{
            lhs.v + rhs.v,
            lhs.d1 + rhs.d1,
            lhs.d2 + rhs.d2,
            lhs.d3 + rhs.d3,
        };
    }
 
    inline constexpr Jet3 operator+(Jet3 lhs, double rhs) noexcept
    {
        return Jet3{lhs.v + rhs, lhs.d1, lhs.d2, lhs.d3};
    }
 
    inline constexpr Jet3 operator+(double lhs, Jet3 rhs) noexcept
    {
        return rhs + lhs;
    }
 
    inline constexpr Jet3 operator-(Jet3 lhs, Jet3 rhs) noexcept
    {
        return Jet3{
            lhs.v - rhs.v,
            lhs.d1 - rhs.d1,
            lhs.d2 - rhs.d2,
            lhs.d3 - rhs.d3,
        };
    }
 
    inline constexpr Jet3 operator-(Jet3 lhs, double rhs) noexcept
    {
        return Jet3{lhs.v - rhs, lhs.d1, lhs.d2, lhs.d3};
    }
 
    inline constexpr Jet3 operator-(double lhs, Jet3 rhs) noexcept
    {
        return Jet3{lhs - rhs.v, -rhs.d1, -rhs.d2, -rhs.d3};
    }
 
    inline constexpr Jet3 operator-(Jet3 value) noexcept
    {
        return Jet3{-value.v, -value.d1, -value.d2, -value.d3};
    }
 
    inline constexpr Jet3 operator*(Jet3 lhs, Jet3 rhs) noexcept
    {
        return Jet3{
            lhs.v * rhs.v,
            lhs.d1 * rhs.v + lhs.v * rhs.d1,
            lhs.d2 * rhs.v + 2.0 * lhs.d1 * rhs.d1 + lhs.v * rhs.d2,
            lhs.d3 * rhs.v + 3.0 * (lhs.d2 * rhs.d1 + lhs.d1 * rhs.d2) + lhs.v * rhs.d3,
        };
    }
 
    inline constexpr Jet3 operator*(Jet3 lhs, double rhs) noexcept
    {
        return Jet3{
            lhs.v * rhs,
            lhs.d1 * rhs,
            lhs.d2 * rhs,
            lhs.d3 * rhs,
        };
    }
 
    inline constexpr Jet3 operator*(double lhs, Jet3 rhs) noexcept
    {
        return rhs * lhs;
    }
 
    inline Jet3 operator/(double lhs, Jet3 rhs) noexcept
    {
        const double inv = 1.0 / rhs.v;
        const double inv2 = inv * inv;
        const double inv3 = inv2 * inv;
        const double inv4 = inv3 * inv;
        const double d1sq = rhs.d1 * rhs.d1;
        return Jet3{
            lhs * inv,
            -lhs * inv2 * rhs.d1,
            lhs * (2.0 * inv3 * d1sq - inv2 * rhs.d2),
            lhs * (-6.0 * inv4 * d1sq * rhs.d1 + 6.0 * inv3 * rhs.d1 * rhs.d2 - inv2 * rhs.d3),
        };
    }
 
    inline constexpr Jet3 operator/(Jet3 lhs, double rhs) noexcept
    {
        return lhs * (1.0 / rhs);
    }
 
    inline Jet3 operator/(Jet3 lhs, Jet3 rhs) noexcept
    {
        return lhs * (1.0 / rhs);
    }
 
    inline Jet3 sin(Jet3 x) noexcept
    {
        const double sv = std::sin(x.v);
        const double cv = std::cos(x.v);
        const double d1sq = x.d1 * x.d1;
        return Jet3{
            sv,
            cv * x.d1,
            -sv * d1sq + cv * x.d2,
            -cv * d1sq * x.d1 - 3.0 * sv * x.d1 * x.d2 + cv * x.d3,
        };
    }
 
    inline Jet3 cos(Jet3 x) noexcept
    {
        const double sv = std::sin(x.v);
        const double cv = std::cos(x.v);
        const double d1sq = x.d1 * x.d1;
        return Jet3{
            cv,
            -sv * x.d1,
            -cv * d1sq - sv * x.d2,
            sv * d1sq * x.d1 - 3.0 * cv * x.d1 * x.d2 - sv * x.d3,
        };
    }
 
    inline Jet3 exp(Jet3 x) noexcept
    {
        const double ev = std::exp(x.v);
        const double d1sq = x.d1 * x.d1;
        return Jet3{
            ev,
            ev * x.d1,
            ev * (d1sq + x.d2),
            ev * (d1sq * x.d1 + 3.0 * x.d1 * x.d2 + x.d3),
        };
    }
 
    inline Jet3 log(Jet3 x) noexcept
    {
        const double inv = 1.0 / x.v;
        const double inv2 = inv * inv;
        const double inv3 = inv2 * inv;
        const double d1sq = x.d1 * x.d1;
        return Jet3{
            std::log(x.v),
            inv * x.d1,
            -inv2 * d1sq + inv * x.d2,
            2.0 * inv3 * d1sq * x.d1 - 3.0 * inv2 * x.d1 * x.d2 + inv * x.d3,
        };
    }
 
    inline Jet3 ln(Jet3 x) noexcept
    {
        return log(x);
    }
 
    inline Jet3 sqrt(Jet3 x) noexcept
    {
        const double sqrtv = std::sqrt(x.v);
        const double inv_sqrt = 1.0 / sqrtv;
        const double inv_v_sqrt = inv_sqrt / x.v;
        const double inv_v2_sqrt = inv_v_sqrt / x.v;
        const double d1sq = x.d1 * x.d1;
        return Jet3{
            sqrtv,
            0.5 * inv_sqrt * x.d1,
            -0.25 * inv_v_sqrt * d1sq + 0.5 * inv_sqrt * x.d2,
            0.375 * inv_v2_sqrt * d1sq * x.d1 - 0.75 * inv_v_sqrt * x.d1 * x.d2 +
                0.5 * inv_sqrt * x.d3,
        };
    }
 
    inline Jet3 powi(Jet3 x, int n) noexcept
    {
        if (n == 0)
        {
            return Jet3::constant(1.0);
        }
 
        const double nf = static_cast<double>(n);
        const double coeff2 = nf * (nf - 1.0);
        const double coeff3 = coeff2 * (nf - 2.0);
        const double dv = nf * std::pow(x.v, nf - 1.0);
        const double ddv = coeff2 == 0.0 ? 0.0 : coeff2 * std::pow(x.v, nf - 2.0);
        const double dddv = coeff3 == 0.0 ? 0.0 : coeff3 * std::pow(x.v, nf - 3.0);
        const double d1sq = x.d1 * x.d1;
        return Jet3{
            std::pow(x.v, nf),
            dv * x.d1,
            ddv * d1sq + dv * x.d2,
            dddv * d1sq * x.d1 + 3.0 * ddv * x.d1 * x.d2 + dv * x.d3,
        };
    }

    using PathParametric = std::function<std::vector<Jet3>(Jet3 s)>;

    using PathParametricWriter = std::function<void(Jet3 s, Span<Jet3> q)>;

    using PathEvaluator2nd = std::function<void(
        Span<const double> s,
        MatrixRef q,
        MatrixRef dq,
        MatrixRef ddq)>;

    using PathEvaluator3rd = std::function<void(
        Span<const double> s,
        MatrixRef q,
        MatrixRef dq,
        MatrixRef ddq,
        MatrixRef dddq)>;

    enum class OutOfRangeMode
    {
        Error,
        Clamp,
    };

    struct SplineConfig
    {
        std::size_t order = 5;
        double s_min = 0.0;
        double s_max = 1.0;
        OutOfRangeMode out_of_range = OutOfRangeMode::Error;
        std::optional<Matrix> start_state;
        std::optional<Matrix> end_state;
    };

    struct PathDerivatives
    {
        Matrix q;
        std::optional<Matrix> dq;
        std::optional<Matrix> ddq;
        std::optional<Matrix> dddq;
    };

    class COPP_API Path
    {
    public:
        static Path from_waypoints(MatrixView waypoints, SplineConfig config = {});
        static Expected<Path> from_waypoints(MatrixView waypoints, SplineConfig config, NoThrowTag);
        static Expected<Path> from_waypoints(MatrixView waypoints, NoThrowTag);

        static Path from_waypoints(
            std::initializer_list<std::initializer_list<double>> waypoints,
            SplineConfig config = {});
        static Expected<Path> from_waypoints(
            std::initializer_list<std::initializer_list<double>> waypoints,
            SplineConfig config,
            NoThrowTag);
        static Expected<Path> from_waypoints(
            std::initializer_list<std::initializer_list<double>> waypoints,
            NoThrowTag);

        static Path from_parametric(
            PathParametric parametric,
            double s_min,
            double s_max);
        static Expected<Path> from_parametric(
            PathParametric parametric,
            double s_min,
            double s_max,
            NoThrowTag);

        static Path from_parametric(
            std::size_t dim,
            double s_min,
            double s_max,
            PathParametricWriter parametric);
        static Expected<Path> from_parametric(
            std::size_t dim,
            double s_min,
            double s_max,
            PathParametricWriter parametric,
            NoThrowTag);

        static Path from_evaluator_2nd(
            std::size_t dim,
            double s_min,
            double s_max,
            PathEvaluator2nd evaluator);
        static Expected<Path> from_evaluator_2nd(
            std::size_t dim,
            double s_min,
            double s_max,
            PathEvaluator2nd evaluator,
            NoThrowTag);

        static Path from_evaluator_3rd(
            std::size_t dim,
            double s_min,
            double s_max,
            PathEvaluator3rd evaluator);
        static Expected<Path> from_evaluator_3rd(
            std::size_t dim,
            double s_min,
            double s_max,
            PathEvaluator3rd evaluator,
            NoThrowTag);
 
        Path(Path &&) noexcept;
        Path &operator=(Path &&) noexcept;
        ~Path();
 
        Path(const Path &) = delete;
        Path &operator=(const Path &) = delete;

        std::size_t dim() const;

        std::pair<double, double> s_range() const;

        PathDerivatives evaluate_q(Span<const double> s) const;
        Expected<PathDerivatives> evaluate_q(Span<const double> s, NoThrowTag) const;

        PathDerivatives evaluate_up_to_2nd(Span<const double> s) const;
        Expected<PathDerivatives> evaluate_up_to_2nd(Span<const double> s, NoThrowTag) const;

        PathDerivatives evaluate_up_to_3rd(Span<const double> s) const;
        Expected<PathDerivatives> evaluate_up_to_3rd(Span<const double> s, NoThrowTag) const;
 
    private:
        struct Impl;
 
        explicit Path(std::unique_ptr<Impl> impl);
 
        std::unique_ptr<Impl> impl_;
 
        friend const void *detail::path_handle(const Path &path) noexcept;
    };
 
    inline Expected<Path> Path::from_waypoints(
        MatrixView waypoints,
        SplineConfig config,
        NoThrowTag)
    {
        return detail::expected_from([&] { return from_waypoints(waypoints, config); });
    }
 
    inline Expected<Path> Path::from_waypoints(MatrixView waypoints, NoThrowTag tag)
    {
        return from_waypoints(waypoints, SplineConfig{}, tag);
    }
 
    inline Expected<Path> Path::from_waypoints(
        std::initializer_list<std::initializer_list<double>> waypoints,
        SplineConfig config,
        NoThrowTag)
    {
        return detail::expected_from([&] { return from_waypoints(waypoints, config); });
    }
 
    inline Expected<Path> Path::from_waypoints(
        std::initializer_list<std::initializer_list<double>> waypoints,
        NoThrowTag tag)
    {
        return from_waypoints(waypoints, SplineConfig{}, tag);
    }
 
    inline Expected<Path> Path::from_parametric(
        PathParametric parametric,
        double s_min,
        double s_max,
        NoThrowTag)
    {
        return detail::expected_from(
            [&] { return from_parametric(std::move(parametric), s_min, s_max); });
    }
 
    inline Expected<Path> Path::from_parametric(
        std::size_t dim,
        double s_min,
        double s_max,
        PathParametricWriter parametric,
        NoThrowTag)
    {
        return detail::expected_from(
            [&] { return from_parametric(dim, s_min, s_max, std::move(parametric)); });
    }
 
    inline Expected<Path> Path::from_evaluator_2nd(
        std::size_t dim,
        double s_min,
        double s_max,
        PathEvaluator2nd evaluator,
        NoThrowTag)
    {
        return detail::expected_from(
            [&] { return from_evaluator_2nd(dim, s_min, s_max, std::move(evaluator)); });
    }
 
    inline Expected<Path> Path::from_evaluator_3rd(
        std::size_t dim,
        double s_min,
        double s_max,
        PathEvaluator3rd evaluator,
        NoThrowTag)
    {
        return detail::expected_from(
            [&] { return from_evaluator_3rd(dim, s_min, s_max, std::move(evaluator)); });
    }
 
    inline Expected<PathDerivatives> Path::evaluate_q(Span<const double> s, NoThrowTag) const
    {
        return detail::expected_from([&] { return evaluate_q(s); });
    }
 
    inline Expected<PathDerivatives> Path::evaluate_up_to_2nd(Span<const double> s, NoThrowTag) const
    {
        return detail::expected_from([&] { return evaluate_up_to_2nd(s); });
    }
 
    inline Expected<PathDerivatives> Path::evaluate_up_to_3rd(Span<const double> s, NoThrowTag) const
    {
        return detail::expected_from([&] { return evaluate_up_to_3rd(s); });
    }
 
} // namespace copp