Commit 408c7e50 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

format

parent 431b3207
Pipeline #18048 failed with stage
in 5 minutes and 22 seconds
This diff is collapsed.
......@@ -17,18 +17,19 @@
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <vector>
#include <utility>
#include <vector>
namespace ndcurves {
/// \brief An inverse kinematics architecture enforcing an arbitrary number of strict priority levels (Reference :
/// Boulic et Al. 2003)
/// \brief An inverse kinematics architecture enforcing an arbitrary number of
/// strict priority levels (Reference : Boulic et Al. 2003)
template <typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat) {
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::JacobiSVD<_Matrix_Type_> svd(
pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler = 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
_Matrix_Type_ m_sigma_inv =
_Matrix_Type_::Zero(pinvmat.cols(), pinvmat.rows());
for (long i = 0; i < m_sigma.rows(); ++i) {
if (m_sigma(i) > pinvtoler) {
m_sigma_inv(i, i) = 1.0 / m_sigma(i);
......
......@@ -9,13 +9,13 @@
#ifndef _CLASS_BERNSTEIN
#define _CLASS_BERNSTEIN
#include "curve_abc.h"
#include "MathDefs.h"
#include <math.h>
#include <vector>
#include <stdexcept>
#include <vector>
#include "MathDefs.h"
#include "curve_abc.h"
namespace ndcurves {
/// \brief Computes a binomial coefficient .
......@@ -24,7 +24,8 @@ namespace ndcurves {
/// \return \f$\binom{n}{k}f$
///
inline unsigned int bin(const unsigned int n, const unsigned int k) {
if (k > n) throw std::runtime_error("binomial coefficient higher than degree");
if (k > n)
throw std::runtime_error("binomial coefficient higher than degree");
if (k == 0) return 1;
if (k > n / 2) return bin(n, n - k);
return n * bin(n - 1, k - 1) / k;
......@@ -36,7 +37,8 @@ inline unsigned int bin(const unsigned int n, const unsigned int k) {
template <typename Numeric = double>
struct Bern {
Bern() {}
Bern(const unsigned int m, const unsigned int i) : m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
Bern(const unsigned int m, const unsigned int i)
: m_minus_i(m - i), i_(i), bin_m_i_(bin(m, i)) {}
virtual ~Bern() {}
......@@ -50,11 +52,12 @@ struct Bern {
return bin_m_i_ * (pow(u, i_)) * pow((1 - u), m_minus_i);
}
/// \brief Check if actual Bernstein polynomial and other are approximately equal.
/// \param other : the other Bernstein polynomial to check.
/// \return true if the two Bernstein polynomials are approximately equals.
/// \brief Check if actual Bernstein polynomial and other are approximately
/// equal. \param other : the other Bernstein polynomial to check. \return
/// true if the two Bernstein polynomials are approximately equals.
virtual bool operator==(const Bern& other) const {
return ndcurves::isApprox<Numeric>(m_minus_i, other.m_minus_i) && ndcurves::isApprox<Numeric>(i_, other.i_) &&
return ndcurves::isApprox<Numeric>(m_minus_i, other.m_minus_i) &&
ndcurves::isApprox<Numeric>(i_, other.i_) &&
ndcurves::isApprox<Numeric>(bin_m_i_, other.bin_m_i_);
}
......
This diff is collapsed.
......@@ -13,22 +13,28 @@
namespace ndcurves {
/// \class constant_curve.
/// \brief Represents a constant_curve curve, always returning the same value and a null derivative
/// \brief Represents a constant_curve curve, always returning the same value
/// and a null derivative
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>, typename Point_derivate = Point>
struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename Point_derivate = Point>
struct constant_curve
: public curve_abc<Time, Numeric, Safe, Point, Point_derivate> {
typedef Point point_t;
typedef Point_derivate point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
typedef constant_curve<Time, Numeric, Safe, Point, Point_derivate> constant_curve_t;
typedef constant_curve<Time, Numeric, Safe, Point, Point_derivate>
constant_curve_t;
typedef constant_curve<Time, Numeric, Safe, Point_derivate> curve_derivate_t;
typedef curve_abc<Time, Numeric, Safe, point_t, Point_derivate> curve_abc_t; // parent class
typedef curve_abc<Time, Numeric, Safe, point_t, Point_derivate>
curve_abc_t; // parent class
/* Constructors - destructors */
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
/// \brief Empty constructor. Curve obtained this way can not perform other
/// class functions.
///
constant_curve() : T_min_(0), T_max_(0), dim_(0) {}
......@@ -37,17 +43,22 @@ struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deriv
/// \param T_min : lower bound of the time interval
/// \param T_max : upper bound of the time interval
///
constant_curve(const Point& value, const time_t T_min = 0., const time_t T_max = std::numeric_limits<time_t>::max())
constant_curve(const Point& value, const time_t T_min = 0.,
const time_t T_max = std::numeric_limits<time_t>::max())
: value_(value), T_min_(T_min), T_max_(T_max), dim_(value.size()) {
if (Safe && T_min_ > T_max_) {
throw std::invalid_argument("can't create constant curve: min bound is higher than max bound");
throw std::invalid_argument(
"can't create constant curve: min bound is higher than max bound");
}
}
/// \brief Copy constructor
/// \param other
constant_curve(const constant_curve_t& other)
: value_(other.value_), T_min_(other.T_min_), T_max_(other.T_max_), dim_(other.dim_) {}
: value_(other.value_),
T_min_(other.T_min_),
T_max_(other.T_max_),
dim_(other.dim_) {}
/// \brief Destructor.
virtual ~constant_curve() {}
......@@ -60,14 +71,15 @@ struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deriv
virtual point_t operator()(const time_t t) const {
if (Safe && (t < T_min_ || t > T_max_)) {
throw std::invalid_argument(
"error in constant curve : time t to evaluate should be in range [Tmin, Tmax] of the curve");
"error in constant curve : time t to evaluate should be in range "
"[Tmin, Tmax] of the curve");
}
return value_;
}
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
/// \param order : order of derivative.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier
/// curve of parametric equation x(t). \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
curve_derivate_t compute_derivate() const {
size_t derivate_size;
......@@ -82,7 +94,8 @@ struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deriv
/// \brief Compute the derived curve at order N.
/// \param order : order of derivative.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
/// curve.
virtual curve_derivate_t* compute_derivate_ptr(const std::size_t) const {
return new curve_derivate_t(compute_derivate());
}
......@@ -90,11 +103,13 @@ struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deriv
/// \brief Evaluate the derivative of order N of curve at time t.
/// \param t : time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative
/// curve of order N at time t.
virtual point_derivate_t derivate(const time_t t, const std::size_t) const {
if (Safe && (t < T_min_ || t > T_max_)) {
throw std::invalid_argument(
"error in constant curve : time t to derivate should be in range [Tmin, Tmax] of the curve");
"error in constant curve : time t to derivate should be in range "
"[Tmin, Tmax] of the curve");
}
size_t derivate_size;
if (point_derivate_t::RowsAtCompileTime == Eigen::Dynamic) {
......@@ -106,31 +121,40 @@ struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deriv
}
/**
* @brief isApprox check if other and *this are approximately equals given a precision treshold
* Only two curves of the same class can be approximately equals,
* for comparison between different type of curves see isEquivalent.
* @brief isApprox check if other and *this are approximately equals given a
* precision treshold Only two curves of the same class can be approximately
* equals, for comparison between different type of curves see isEquivalent.
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @param prec the precision treshold, default
* Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
virtual bool isApprox(const constant_curve_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return ndcurves::isApprox<num_t>(T_min_, other.min()) && ndcurves::isApprox<num_t>(T_max_, other.max()) &&
virtual bool isApprox(
const constant_curve_t& other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
return ndcurves::isApprox<num_t>(T_min_, other.min()) &&
ndcurves::isApprox<num_t>(T_max_, other.max()) &&
dim_ == other.dim() && value_.isApprox(other.value_, prec);
}
virtual bool isApprox(const curve_abc_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
const constant_curve_t* other_cast = dynamic_cast<const constant_curve_t*>(other);
virtual bool isApprox(
const curve_abc_t* other,
const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
const constant_curve_t* other_cast =
dynamic_cast<const constant_curve_t*>(other);
if (other_cast)
return isApprox(*other_cast, prec);
else
return false;
}
virtual bool operator==(const constant_curve_t& other) const { return isApprox(other); }
virtual bool operator==(const constant_curve_t& other) const {
return isApprox(other);
}
virtual bool operator!=(const constant_curve_t& other) const { return !(*this == other); }
virtual bool operator!=(const constant_curve_t& other) const {
return !(*this == other);
}
/*Helpers*/
/// \brief Get dimension of curve.
......@@ -171,8 +195,10 @@ struct constant_curve : public curve_abc<Time, Numeric, Safe, Point, Point_deriv
}; // struct constant_curve
} // namespace ndcurves
DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
typename Point_derivate),
SINGLE_ARG(ndcurves::constant_curve<Time, Numeric, Safe, Point, Point_derivate>))
DEFINE_CLASS_TEMPLATE_VERSION(
SINGLE_ARG(typename Time, typename Numeric, bool Safe, typename Point,
typename Point_derivate),
SINGLE_ARG(
ndcurves::constant_curve<Time, Numeric, Safe, Point, Point_derivate>))
#endif // _CLASS_CONSTANTCURVE
......@@ -11,18 +11,27 @@
#include "ndcurves/fwd.h"
namespace ndcurves {
inline Eigen::Vector3d cross(const Eigen::VectorXd& a, const Eigen::VectorXd& b) {
inline Eigen::Vector3d cross(const Eigen::VectorXd& a,
const Eigen::VectorXd& b) {
Eigen::Vector3d c;
c << a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2], a[0] * b[1] - a[1] * b[0];
c << a[1] * b[2] - a[2] * b[1], a[2] * b[0] - a[0] * b[2],
a[0] * b[1] - a[1] * b[0];
return c;
}
inline Eigen::Vector3d cross(const Eigen::Vector3d& a, const Eigen::Vector3d& b) { return a.cross(b); }
inline Eigen::Vector3d cross(const Eigen::Vector3d& a,
const Eigen::Vector3d& b) {
return a.cross(b);
}
inline Eigen::Vector3f cross(const Eigen::Vector3f& a, const Eigen::Vector3f& b) { return a.cross(b); }
inline Eigen::Vector3f cross(const Eigen::Vector3f& a,
const Eigen::Vector3f& b) {
return a.cross(b);
}
template <typename N, bool S>
linear_variable<N, S> cross(const linear_variable<N, S>& a, const linear_variable<N, S>& b) {
linear_variable<N, S> cross(const linear_variable<N, S>& a,
const linear_variable<N, S>& b) {
return a.cross(b);
}
} // namespace ndcurves
......
/**
* \file cubic_hermite_spline.h
* \brief class allowing to create a cubic hermite spline of any dimension.
* \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin <jchemin@laas.fr>
* \date 05/2019
* \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin
* <jchemin@laas.fr> \date 05/2019
*/
#ifndef _CLASS_CUBICHERMITESPLINE
#define _CLASS_CUBICHERMITESPLINE
#include "curve_abc.h"
#include "bezier_curve.h"
#include "piecewise_curve.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept>
#include <boost/serialization/utility.hpp> // To serialize std::pair
#include <iostream>
#include <stdexcept>
#include <vector>
#include <boost/serialization/utility.hpp> // To serialize std::pair
#include "MathDefs.h"
#include "bezier_curve.h"
#include "curve_abc.h"
#include "piecewise_curve.h"
namespace ndcurves {
/// \class CubicHermiteSpline.
/// \brief Represents a set of cubic hermite splines defining a continuous function \f$p(t)\f$.
/// A hermite cubic spline is a minimal degree polynom interpolating a function in two
/// points \f$P_i\f$ and \f$P_{i+1}\f$ with its tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br>
/// A hermite cubic spline :
/// - crosses each of the waypoint given in its initialization (\f$P_0\f$, \f$P_1\f$,...,\f$P_N\f$).
/// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) = m_i\f$ and \f$p'(t_{P_{i+1}}) =
/// m_{i+1}\f$.
/// \brief Represents a set of cubic hermite splines defining a continuous
/// function \f$p(t)\f$. A hermite cubic spline is a minimal degree polynom
/// interpolating a function in two points \f$P_i\f$ and \f$P_{i+1}\f$ with its
/// tangent \f$m_i\f$ and \f$m_{i+1}\f$.<br> A hermite cubic spline :
/// - crosses each of the waypoint given in its initialization (\f$P_0\f$,
/// \f$P_1\f$,...,\f$P_N\f$).
/// - has its derivatives on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$p'(t_{P_i}) =
/// m_i\f$ and \f$p'(t_{P_{i+1}}) = m_{i+1}\f$.
///
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<Point> > t_pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<Point> >
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 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;
typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_t>
piecewise_bezier_t;
public:
/// \brief Empty constructor. Curve obtained this way can not perform other class functions.
/// \brief Empty constructor. Curve obtained this way can not perform other
/// class functions.
///
cubic_hermite_spline() : dim_(0), T_min_(0), T_max_(0) {}
/// \brief Constructor.
/// \param PairsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param PairsEnd : an iterator pointing to the last element of a pair(position, derivative) container.
/// \param PairsBegin : an iterator pointing to the first element of a
/// pair(position, derivative) container. \param PairsEnd : an iterator
/// pointing to the last element of a pair(position, derivative) container.
/// \param time_control_points : vector containing time for each waypoint.
///
template <typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t& time_control_points)
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.");
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.");
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
......@@ -98,7 +107,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
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");
throw std::invalid_argument(
"can't evaluate cubic hermite spline, out of range");
}
if (size_ == 1) {
return control_points_.front().first;
......@@ -110,48 +120,63 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/**
* @brief isApprox check if other and *this are approximately equals.
* Only two curves of the same class can be approximately equals, for comparison between different type of curves see
* isEquivalent
* Only two curves of the same class can be approximately equals, for
* comparison between different type of curves see isEquivalent
* @param other the other curve to check
* @param prec the precision treshold, default Eigen::NumTraits<Numeric>::dummy_precision()
* @param prec the precision treshold, default
* Eigen::NumTraits<Numeric>::dummy_precision()
* @return true is the two curves are approximately equals
*/
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_;
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)))
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);
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 isApprox(other);
}
virtual bool operator!=(const cubic_hermite_spline_t& other) const { return !(*this == other); }
virtual bool operator!=(const cubic_hermite_spline_t& other) const {
return !(*this == other);
}
/// \brief Evaluate the derivative of order N of spline at time t.
/// \param t : time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
/// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative
/// spline of order N at time t.
///
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");
throw std::invalid_argument(
"can't derivate cubic hermite spline, out of range");
}
if (size_ == 1) {
return control_points_.front().second;
......@@ -172,31 +197,37 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Compute the derived curve at order N.
/// \param order : order of derivative.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
/// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
/// curve.
piecewise_bezier_t* compute_derivate_ptr(const std::size_t order) const {
return new piecewise_bezier_t(compute_derivate(order));
}
/// \brief Set time of each control point of cubic hermite spline.
/// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with
/// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
/// \param time_control_points : Vector containing time for each control point.
/// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$
/// with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$
/// respectively.<br> \param time_control_points : Vector containing time for
/// each control point.
///
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");
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");
throw std::invalid_argument(
"time_splines not monotonous, all spline duration should be superior "
"to 0");
}
}
/// \brief Get vector of pair (positition, derivative) corresponding to control points.
/// \return vector containing control points.
/// \brief Get vector of pair (positition, derivative) corresponding to
/// control points. \return vector containing control points.
///
t_pair_point_tangent_t getControlPoints() { return control_points_; }
......@@ -216,9 +247,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
std::size_t numIntervals() const { return size() - 1; }
private:
/// \brief Get index of the interval (subspline) corresponding to time t for the interpolation.
/// \param t : time where to look for interval.
/// \return Index of interval for time t.
/// \brief Get index of the interval (subspline) corresponding to time t for
/// the interpolation. \param t : time where to look for interval. \return
/// Index of interval for time t.
///
std::size_t findInterval(const time_t t) const {
// time before first control point time.
......@@ -245,8 +276,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
}
/**
* @brief buildCurrentBezier set up the current_bezier_ attribut to represent the curve of the interval that contain
* t. This bezier is defined by the following control points: p0, p0 + m0/3, p1 - m1/3, p1
* @brief buildCurrentBezier set up the current_bezier_ attribut to represent
* the curve of the interval that contain t. This bezier is defined by the
* following control points: p0, p0 + m0/3, p1 - m1/3, p1
* @param t the time for which the bezier is build
* @return the bezier curve
*/
......@@ -265,21 +297,25 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return bezier_t(control_points.begin(), control_points.end(), t0, t1);
}
/// \brief Check if control points list is not empty and dimension of point superior to zero.
/// \brief Check if control points list is not empty and dimension of point
/// superior to zero.
///
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 ?");
"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 ?");
"Error in cubic hermite : Dimension of points is zero / did you use "
"empty constructor ?");
}
}
/// \brief compute duration of each spline.
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// Duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} ).
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ...,
/// T_{P_N}\f$ respectively, Duration of each subspline is : (
/// T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} ).
///
void computeDurationSplines() {
duration_splines_.clear();
......@@ -328,18 +364,22 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
std::size_t dim_;
/// Vector of pair < Point, Tangent >.