cubic_hermite_spline.h

Go to the documentation of this file.

 
#ifndef _CLASS_CUBICHERMITESPLINE
#define _CLASS_CUBICHERMITESPLINE
 
#include <boost/serialization/utility.hpp>  // To serialize std::pair
#include <iostream>
#include <stdexcept>
#include <vector>
 
#include "MathDefs.h"
#include "bezier_curve.h"
#include "curve_abc.h"
#include "piecewise_curve.h"
 
namespace ndcurves {
template <typename Time = double, typename Numeric = Time, bool Safe = false,
          typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1> >
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
  typedef Point point_t;
  typedef std::pair<Point, Point> pair_point_tangent_t;
  typedef std::vector<pair_point_tangent_t,
                      Eigen::aligned_allocator<pair_point_tangent_t> >
      t_pair_point_tangent_t;
  typedef std::vector<Time> vector_time_t;
  typedef Time time_t;
  typedef Numeric num_t;
  typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t;  // parent class
  typedef cubic_hermite_spline<Time, Numeric, Safe, point_t>
      cubic_hermite_spline_t;
  typedef bezier_curve<Time, Numeric, Safe, point_t> bezier_t;
  typedef typename bezier_t::t_point_t t_point_t;
  typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_t>
      piecewise_bezier_t;
 
 public:
  cubic_hermite_spline() : dim_(0), T_min_(0), T_max_(0) {}
 
  template <typename In>
  cubic_hermite_spline(In PairsBegin, In PairsEnd,
                       const vector_time_t& time_control_points)
      : size_(std::distance(PairsBegin, PairsEnd)), degree_(3) {
    // Check size of pairs container.
    if (Safe && size_ < 1) {
      throw std::length_error(
          "can not create cubic_hermite_spline, number of pairs is inferior to "
          "2.");
    }
    // Set dimension according to size of points
    dim_ = PairsBegin->first.size();
    // Push all pairs in controlPoints
    In it(PairsBegin);
    for (; it != PairsEnd; ++it) {
      if (Safe && (static_cast<size_t>(it->first.size()) != dim_ ||
                   static_cast<size_t>(it->second.size()) != dim_))
        throw std::invalid_argument(
            "All the control points and their derivatives must have the same "
            "dimension.");
      control_points_.push_back(*it);
    }
    // Set time
    setTime(time_control_points);
  }
 
  cubic_hermite_spline(const cubic_hermite_spline& other)
      : dim_(other.dim_),
        control_points_(other.control_points_),
        time_control_points_(other.time_control_points_),
        duration_splines_(other.duration_splines_),
        T_min_(other.T_min_),
        T_max_(other.T_max_),
        size_(other.size_),
        degree_(other.degree_) {}
 
  virtual ~cubic_hermite_spline() {}
 
  /*Operations*/
 public:
  virtual Point operator()(const time_t t) const {
    check_conditions();
    if (Safe & !(T_min_ <= t && t <= T_max_)) {
      throw std::invalid_argument(
          "can't evaluate cubic hermite spline, out of range");
    }
    if (size_ == 1) {
      return control_points_.front().first;
    } else {
      const bezier_t bezier = buildCurrentBezier(t);
      return bezier(t);
    }
  }
 
  bool isApprox(
      const cubic_hermite_spline_t& other,
      const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
    bool equal = ndcurves::isApprox<num_t>(T_min_, other.min()) &&
                 ndcurves::isApprox<num_t>(T_max_, other.max()) &&
                 dim_ == other.dim() && degree_ == other.degree() &&
                 size_ == other.size() &&
                 time_control_points_ == other.time_control_points_ &&
                 duration_splines_ == other.duration_splines_;
    if (!equal) return false;
    for (std::size_t i = 0; i < size_; ++i) {
      if ((!control_points_[i].first.isApprox(other.control_points_[i].first,
                                              prec)) ||
          (!control_points_[i].second.isApprox(other.control_points_[i].second,
                                               prec)))
        return false;
    }
    return true;
  }
 
  virtual bool isApprox(
      const curve_abc_t* other,
      const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
    const cubic_hermite_spline_t* other_cast =
        dynamic_cast<const cubic_hermite_spline_t*>(other);
    if (other_cast)
      return isApprox(*other_cast, prec);
    else
      return false;
  }
 
  virtual bool operator==(const cubic_hermite_spline_t& other) const {
    return isApprox(other);
  }
 
  virtual bool operator!=(const cubic_hermite_spline_t& other) const {
    return !(*this == other);
  }
 
  virtual Point derivate(const time_t t, const std::size_t order) const {
    check_conditions();
    if (Safe & !(T_min_ <= t && t <= T_max_)) {
      throw std::invalid_argument(
          "can't derivate cubic hermite spline, out of range");
    }
    if (size_ == 1) {
      return control_points_.front().second;
    } else {
      const bezier_t bezier = buildCurrentBezier(t);
      return bezier.derivate(t, order);
    }
  }
 
  piecewise_bezier_t compute_derivate(const std::size_t order) const {
    piecewise_bezier_t res;
    for (size_t i = 0; i < size_ - 1; ++i) {
      const bezier_t curve = buildCurrentBezier(time_control_points_[i]);
      res.add_curve(curve.compute_derivate(order));
    }
    return res;
  }
 
  piecewise_bezier_t* compute_derivate_ptr(const std::size_t order) const {
    return new piecewise_bezier_t(compute_derivate(order));
  }
 
  void setTime(const vector_time_t& time_control_points) {
    time_control_points_ = time_control_points;
    T_min_ = time_control_points_.front();
    T_max_ = time_control_points_.back();
    if (time_control_points.size() != size()) {
      throw std::length_error(
          "size of time control points should be equal to number of control "
          "points");
    }
    computeDurationSplines();
    if (!checkDurationSplines()) {
      throw std::invalid_argument(
          "time_splines not monotonous, all spline duration should be superior "
          "to 0");
    }
  }
 
  t_pair_point_tangent_t getControlPoints() { return control_points_; }
 
  vector_time_t getTime() { return time_control_points_; }
 
  std::size_t size() const { return size_; }
 
  std::size_t numIntervals() const { return size() - 1; }
 
 private:
  std::size_t findInterval(const time_t t) const {
    // time before first control point time.
    if (t <= time_control_points_[0]) {
      return 0;
    }
    // time is after last control point time
    if (t >= time_control_points_[size_ - 1]) {
      return size_ - 2;
    }
    std::size_t left_id = 0;
    std::size_t right_id = size_ - 1;
    while (left_id <= right_id) {
      const std::size_t middle_id = left_id + (right_id - left_id) / 2;
      if (time_control_points_.at(middle_id) < t) {
        left_id = middle_id + 1;
      } else if (time_control_points_.at(middle_id) > t) {
        right_id = middle_id - 1;
      } else {
        return middle_id;
      }
    }
    return left_id - 1;
  }
 
  bezier_t buildCurrentBezier(const time_t t) const {
    size_t id_interval = findInterval(t);
    const pair_point_tangent_t pair0 = control_points_.at(id_interval);
    const pair_point_tangent_t pair1 = control_points_.at(id_interval + 1);
    const Time& t0 = time_control_points_[id_interval];
    const Time& t1 = time_control_points_[id_interval + 1];
    t_point_t control_points;
    control_points.reserve(4);
    control_points.push_back(pair0.first);
    control_points.push_back(pair0.first + pair0.second / 3. * (t1 - t0));
    control_points.push_back(pair1.first - pair1.second / 3. * (t1 - t0));
    control_points.push_back(pair1.first);
    return bezier_t(control_points.begin(), control_points.end(), t0, t1);
  }
 
  void check_conditions() const {
    if (control_points_.size() == 0) {
      throw std::runtime_error(
          "Error in cubic hermite : there is no control points set / did you "
          "use empty constructor ?");
    } else if (dim_ == 0) {
      throw std::runtime_error(
          "Error in cubic hermite : Dimension of points is zero / did you use "
          "empty constructor ?");
    }
  }
 
  void computeDurationSplines() {
    duration_splines_.clear();
    Time actual_time;
    Time prev_time = *(time_control_points_.begin());
    std::size_t i = 0;
    for (i = 0; i < size() - 1; i++) {
      actual_time = time_control_points_.at(i + 1);
      duration_splines_.push_back(actual_time - prev_time);
      prev_time = actual_time;
    }
  }
 
  bool checkDurationSplines() const {
    std::size_t i = 0;
    bool is_positive = true;
    while (is_positive && i < duration_splines_.size()) {
      is_positive = (duration_splines_.at(i) > 0.);
      i++;
    }
    return is_positive;
  }
  /*Operations*/
 
  /*Helpers*/
 public:
  std::size_t virtual dim() const { return dim_; }
  Time virtual min() const { return time_control_points_.front(); }
  Time virtual max() const { return time_control_points_.back(); }
  virtual std::size_t degree() const { return degree_; }
  /*Helpers*/
 
  /*Attributes*/
  std::size_t dim_;
  t_pair_point_tangent_t control_points_;
  vector_time_t time_control_points_;
  vector_time_t duration_splines_;
  /*const*/ Time T_min_;
  /*const*/ Time T_max_;
  std::size_t size_;
  std::size_t degree_;
  /*Attributes*/
 
  // Serialization of the class
  friend class boost::serialization::access;
 
  template <class Archive>
  void serialize(Archive& ar, const unsigned int version) {
    if (version) {
      // Do something depending on version ?
    }
    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
    ar& boost::serialization::make_nvp("dim", dim_);
    ar& boost::serialization::make_nvp("control_points", control_points_);
    ar& boost::serialization::make_nvp("time_control_points",
                                       time_control_points_);
    ar& boost::serialization::make_nvp("duration_splines", duration_splines_);
    ar& boost::serialization::make_nvp("T_min", T_min_);
    ar& boost::serialization::make_nvp("T_max", T_max_);
    ar& boost::serialization::make_nvp("size", size_);
    ar& boost::serialization::make_nvp("degree", degree_);
  }
};  // End struct Cubic hermite spline
}  // namespace ndcurves
 
DEFINE_CLASS_TEMPLATE_VERSION(
    SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point),
    SINGLE_ARG(ndcurves::cubic_hermite_spline<Time, Numeric, Safe, Point>))
#endif  //_CLASS_CUBICHERMITESPLINE