spline.hpp

Go to the documentation of this file.

// Copyright (C) 2021-2022 Petter Nilsson. MIT License.
 
#pragma once
 
#include <ranges>
 
#include "../lie_groups.hpp"
#include "common.hpp"
 
SMOOTH_BEGIN_NAMESPACE
 
template<int K, LieGroup G>
class Spline
{
public:
  static_assert(Dof<G> > 0, "Dof<G> must be known at compile time.");
 
  Spline(const G & ga = Identity<G>());
 
  Spline(double T, Eigen::Matrix<double, Dof<G>, K> && V, G && ga = Identity<G>());
 
  template<typename Derived>
  Spline(double T, const Eigen::MatrixBase<Derived> & V, const G & ga = Identity<G>());
 
  template<std::ranges::range Rv>
    requires(std::is_same_v<std::ranges::range_value_t<Rv>, Tangent<G>>)
  Spline(double T, const Rv & vs, const G & ga = Identity<G>());
 
  Spline(const Spline &) = default;
 
  Spline(Spline &&) = default;
 
  Spline & operator=(const Spline &) = default;
 
  Spline & operator=(Spline &&) = default;
 
  ~Spline() = default;
 
  [[nodiscard]] static Spline ConstantVelocityGoal(const G & gb, double T = 1, const G & ga = Identity<G>());
 
  [[nodiscard]] static Spline ConstantVelocity(const Tangent<G> & v, double T = 1, const G & ga = Identity<G>());
 
  [[nodiscard]] static Spline
  FixedCubic(const G & gb, const Tangent<G> & va, const Tangent<G> & vb, double T = 1, const G & ga = Identity<G>())
    requires(K == 3);
 
  [[nodiscard]] std::size_t size() const;
 
  [[nodiscard]] bool empty() const;
 
  void reserve(std::size_t capacity);
 
  [[nodiscard]] double t_min() const;
 
  [[nodiscard]] double t_max() const;
 
  [[nodiscard]] G start() const;
 
  [[nodiscard]] G end() const;
 
  void make_local();
 
  Spline & concat_global(const Spline & other);
 
  Spline & concat_local(const Spline & other);
 
  Spline & operator+=(const Spline & other);
 
  [[nodiscard]] Spline operator+(const Spline & other);
 
  template<typename S = double>
  CastT<S, G> operator()(const S & t, OptTangent<CastT<S, G>> vel = {}, OptTangent<CastT<S, G>> acc = {}) const;
 
  [[nodiscard]] Tangent<G> arclength(double t) const
    requires(K == 3);
 
  [[nodiscard]] Spline crop(double ta, double tb = std::numeric_limits<double>::infinity(), bool localize = true) const;
 
private:
  std::size_t find_idx(double t) const;
 
  // segment i is defined by
  //
  //  - time interval:  m_end_t[i-1], m_end_t[i]
  //  - g interval:     m_end_g[i-1], m_end_g[i]
  //  - velocities:     m_Vs[i]
  //  - crop:           m_seg_T0[i], m_seg_Del[i]
  //
  // s.t.
  //
  //  u(t) = T0[i] + Del[i] * (t - end_t[i-1]) / (end_t[i] - end_t[i-1])
  //
  //  x(t) = xu(u(t))  where xu is spline defined in [0, 1]
 
  // Spline starting point
  G m_g0;
 
  // segment end times
  std::vector<double> m_end_t;
 
  // segment end points
  std::vector<G> m_end_g;
 
  // segment bezier velocities
  std::vector<Eigen::Matrix<double, Dof<G>, K>> m_Vs;
 
  // segment crop information
  std::vector<double> m_seg_T0, m_seg_Del;
};
 
template<LieGroup G>
using CubicSpline = Spline<3, G>;
 
SMOOTH_END_NAMESPACE
#include "detail/spline_impl.hpp"