Unverified Commit c76e5052 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #14 from pFernbach/topic/piecewise_generic

Topic/piecewise generic
parents 596a5d2c 6b64aaf9
......@@ -39,8 +39,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
typedef piecewise_curve<Time, Numeric, Safe, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
/* Constructors - destructors */
public:
......@@ -158,6 +160,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return deriv.compute_derivate(order - 1);
}
/// \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.
bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
return new bezier_curve_t(compute_derivate(order));
}
/// \brief Compute the primitive of the curve at order N.
/// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
/// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
......@@ -190,8 +199,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual point_t derivate(const time_t t, const std::size_t order) const {
bezier_curve_t deriv = compute_derivate(order);
return deriv(t);
return compute_derivate(order)(t);
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
......@@ -325,12 +333,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
/// \brief Split the bezier curve in several curves, all accessible
/// within a piecewise_bezier_curve_t.
/// within a piecewise_curve_t.
/// \param times : list of times of size n.
/// \return a piecewise_bezier_curve_t comprising n+1 curves
/// \return a piecewise_curve_t comprising n+1 curves
///
piecewise_bezier_curve_t split(const vector_x_t& times) const {
typename piecewise_bezier_curve_t::t_curve_t curves;
piecewise_curve_t split(const vector_x_t& times) const {
std::vector<bezier_curve_t> curves;
bezier_curve_t current = *this;
for (int i = 0; i < times.rows(); ++i) {
std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
......@@ -338,7 +346,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
current = pairsplit.second;
}
curves.push_back(current);
return piecewise_bezier_curve_t(curves);
piecewise_curve_t res;
for(typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end() ; ++cit){
typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
res.add_curve_ptr(ptr);
}
return res;
}
/// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve
......@@ -414,6 +427,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const { return T_max_; }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const {return degree_;}
/*Helpers*/
/* Attributes */
......
......@@ -39,6 +39,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
typedef std::vector<Time> vector_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;
public:
......@@ -112,6 +113,19 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return evalCubicHermiteSpline(t, order);
}
cubic_hermite_spline_t compute_derivate(const std::size_t /*order*/) const {
throw std::logic_error("Compute derivate for cubic hermite spline is not implemented yet.");
}
/// \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.
cubic_hermite_spline_t* compute_derivate_ptr(const std::size_t order) const {
return new cubic_hermite_spline_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>
......@@ -325,6 +339,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
Time virtual max() const { return time_control_points_.back(); }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const {return degree_;}
/*Helpers*/
/*Attributes*/
......
......@@ -15,6 +15,7 @@
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"
#include <boost/serialization/shared_ptr.hpp>
#include <boost/smart_ptr/shared_ptr.hpp>
#include <functional>
......@@ -28,6 +29,8 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
typedef Point point_t;
typedef Point_derivate point_derivate_t;
typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
typedef boost::shared_ptr<curve_t> curve_ptr_t;
/* Constructors - destructors */
public:
......@@ -44,12 +47,19 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const = 0;
/// \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.
virtual curve_t* compute_derivate_ptr(const std::size_t order) const = 0;
/// \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.
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const = 0;
/*Operations*/
/*Helpers*/
......@@ -62,6 +72,10 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const = 0;
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const =0;
std::pair<time_t, time_t> timeRange() { return std::make_pair(min(), max()); }
/*Helpers*/
......
......@@ -16,38 +16,38 @@ namespace curves {
/// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
/// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent polynomial.
template <typename Polynomial, typename curveTypeToConvert>
Polynomial polynomial_from_curve(const curveTypeToConvert& curve) {
template <typename Polynomial>
Polynomial polynomial_from_curve(const typename Polynomial::curve_abc_t& curve) {
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
t_point_t coefficients;
curveTypeToConvert current(curve);
coefficients.push_back(curve(curve.min()));
num_t fact = 1;
for (std::size_t i = 1; i <= curve.degree_; ++i) {
for (std::size_t i = 1; i <= curve.degree(); ++i) {
fact *= (num_t)i;
coefficients.push_back(current.derivate(current.min(), i) / fact);
coefficients.push_back(curve.derivate(curve.min(), i) / fact);
}
return Polynomial(coefficients, curve.min(), curve.max());
}
/// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve.
template <typename Bezier, typename curveTypeToConvert>
Bezier bezier_from_curve(const curveTypeToConvert& curve) {
template <typename Bezier>
Bezier bezier_from_curve(const typename Bezier::curve_abc_t& curve) {
typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t;
curveTypeToConvert current(curve);
num_t T_min = current.min();
num_t T_max = current.max();
num_t T_min = curve.min();
num_t T_max = curve.max();
num_t T = T_max - T_min;
// Positions and derivatives
point_t p0 = current(T_min);
point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min, 1);
point_t m1 = current.derivate(T_max, 1);
point_t p0 = curve(T_min);
point_t p1 = curve(T_max);
point_t m0 = curve.derivate(T_min, 1);
point_t m1 = curve.derivate(T_max, 1);
// Convert to bezier control points
// for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T
// so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T
......@@ -61,26 +61,25 @@ Bezier bezier_from_curve(const curveTypeToConvert& curve) {
control_points.push_back(b_p1);
control_points.push_back(b_p2);
control_points.push_back(b_p3);
return Bezier(control_points.begin(), control_points.end(), current.min(), current.max());
return Bezier(control_points.begin(), control_points.end(), curve.min(), curve.max());
}
/// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
/// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic hermite spline.
template <typename Hermite, typename curveTypeToConvert>
Hermite hermite_from_curve(const curveTypeToConvert& curve) {
template <typename Hermite>
Hermite hermite_from_curve(const typename Hermite::curve_abc_t& curve) {
typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t;
typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t;
typedef typename Hermite::point_t point_t;
typedef typename Hermite::num_t num_t;
curveTypeToConvert current(curve);
num_t T_min = current.min();
num_t T_max = current.max();
num_t T_min = curve.min();
num_t T_max = curve.max();
// Positions and derivatives
point_t p0 = current(T_min);
point_t p1 = current(T_max);
point_t m0 = current.derivate(T_min, 1);
point_t m1 = current.derivate(T_max, 1);
point_t p0 = curve(T_min);
point_t p1 = curve(T_max);
point_t m0 = curve.derivate(T_min, 1);
point_t m1 = curve.derivate(T_max, 1);
// Create pairs pos/vel
pair_point_tangent_t pair0(p0, m0);
pair_point_tangent_t pair1(p1, m1);
......
......@@ -39,7 +39,7 @@ template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> {
struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
......@@ -53,8 +53,9 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
typedef curve_constraints<Point> spline_constraints;
typedef exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase> exact_cubic_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
typedef piecewise_curve<Time, Numeric, Safe, Point> piecewise_curve_t;
typedef typename piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t;
/* Constructors - destructors */
public:
......@@ -68,7 +69,13 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
///
template <typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
: piecewise_curve_t()
{
t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
this->add_curve(*it);
}
}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
......@@ -77,11 +84,24 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
///
template <typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
: piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
: piecewise_curve_t()
{
t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd,constraints);
for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
this->add_curve(*it);
}
}
/// \brief Constructor.
/// \param subSplines: vector of subSplines.
exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t(subSplines) {}
exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t()
{
for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
this->add_curve(*it);
}
}
exact_cubic(const t_curve_ptr_t& subSplines) : piecewise_curve_t(subSplines) { }
/// \brief Copy Constructor.
exact_cubic(const exact_cubic& other) : piecewise_curve_t(other) {}
......@@ -91,7 +111,13 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
std::size_t getNumberSplines() { return this->getNumberCurves(); }
spline_t getSplineAt(std::size_t index) { return this->curves_.at(index); }
spline_t getSplineAt(std::size_t index) {
boost::shared_ptr<spline_t> s_ptr = boost::dynamic_pointer_cast<spline_t>(this->curves_.at(index));
if(s_ptr)
return *s_ptr;
else
throw std::runtime_error("Parent piecewise curve do not contain only curves created from exact_cubic class methods");
}
private:
/// \brief Compute polynom of exact cubic spline from waypoints.
......
......@@ -9,6 +9,9 @@
#ifndef CURVES_FWD_H
#define CURVES_FWD_H
#include <Eigen/Dense>
#include <vector>
#include <boost/smart_ptr/shared_ptr.hpp>
namespace curves {
......@@ -21,12 +24,12 @@ template <typename Time, typename Numeric, bool Safe,typename Point >
template <typename Time, typename Numeric, bool Safe,typename Point >
struct cubic_hermite_spline;
template <typename Time, typename Numeric, bool Safe, typename Point ,
typename T_Point , typename SplineBase >
template <typename Time, typename Numeric, bool Safe, typename Point,
typename T_Point,typename SplineBase >
struct exact_cubic;
template <typename Time, typename Numeric, bool Safe, typename Point,
typename T_Point, typename Curve, typename Point_derivate>
typename Point_derivate>
struct piecewise_curve;
template <typename Time, typename Numeric, bool Safe,typename Point, typename T_Point>
......@@ -49,6 +52,50 @@ template <typename Numeric, bool Safe>
template <typename Numeric>
struct quadratic_variable;
// typedef of the commonly used templates arguments :
// eigen types :
typedef Eigen::Vector3d point3_t;
typedef Eigen::Matrix<double, 6, 1> point6_t;
typedef Eigen::VectorXd pointX_t;
typedef Eigen::Matrix<double, 3, 3> matrix3_t;
typedef Eigen::Matrix<double, 4, 4> matrix4_t;
typedef Eigen::Quaternion<double> quaternion_t;
typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
typedef std::vector<pointX_t, Eigen::aligned_allocator<point3_t> > t_point3_t;
typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
// abstract curves types:
typedef curve_abc<double, double, true, pointX_t,pointX_t> curve_abc_t; // base abstract class
typedef curve_abc<double, double, true, point3_t,point3_t> curve_3_t; // generic class of curve of size 3
typedef curve_abc<double, double, true, matrix3_t, point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
typedef curve_abc<double, double, true, transform_t, point6_t> curve_SE3_t; // templated abstract class used for all the se3 curves (return dimension are fixed)
// shared pointer to abstract types:
typedef boost::shared_ptr<curve_abc_t> curve_ptr_t;
typedef boost::shared_ptr<curve_3_t> curve3_ptr_t;
typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
// definition of all curves class with pointX as return type:
typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic<double, double, true, pointX_t,t_pointX_t, polynomial_t> exact_cubic_t;
typedef bezier_curve<double, double, true, pointX_t> bezier_t;
typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
typedef piecewise_curve <double, double, true, pointX_t,pointX_t> piecewise_t;
// definition of all curves class with point3 as return type:
typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
typedef exact_cubic<double, double, true, point3_t,t_point3_t, polynomial_t> exact_cubic3_t;
typedef bezier_curve<double, double, true, point3_t> bezier3_t;
typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
typedef piecewise_curve <double, double, true, point3_t,point3_t> piecewise3_t;
// special curves with return type fixed:
typedef SO3Linear<double, double, true> SO3Linear_t;
typedef SE3Curve<double, double, true> SE3Curve_t;
typedef piecewise_curve <double, double, true, transform_t, point6_t> piecewise_SE3_t;
}
#endif // CURVES_FWD_H
......@@ -102,9 +102,8 @@ exact_cubic_t* effector_spline(In wayPointsBegin, In wayPointsEnd, const Point&
spline_t end_spline =
make_end_spline(land_normal, landWaypoint.second, land_offset, landWaypoint.first, land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline, land_offset_duration);
exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(), constraints);
t_spline_t splines = all_but_end.curves_;
splines.push_back(end_spline);
exact_cubic_t splines(waypoints.begin(), waypoints.end(), constraints);
splines.add_curve(end_spline);
return new exact_cubic_t(splines);
}
} // namespace helpers
......
......@@ -76,6 +76,14 @@ class rotation_spline : public curve_abc_quat_t {
throw std::runtime_error("TODO quaternion spline does not implement derivate");
}
/// \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.
curve_abc_quat_t* compute_derivate_ptr(const std::size_t /*order*/) const {
throw std::logic_error("Compute derivate for quaternion spline is not implemented yet.");
}
/// \brief Initialize time reparametrization for spline.
exact_cubic_constraint_one_dim computeWayPoints() const {
t_waypoint_one_dim_t waypoints;
......@@ -87,6 +95,9 @@ class rotation_spline : public curve_abc_quat_t {
virtual std::size_t dim() const { return dim_; }
virtual time_t min() const { return min_; }
virtual time_t max() const { return max_; }
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const {return 1;}
/*Attributes*/
Eigen::Quaterniond quat_from_; // const
......
......@@ -34,7 +34,8 @@ quadratic_variable<Numeric> compute_integral_cost_internal(const problem_data<Po
typedef typename t_point_t::const_iterator cit_point_t;
bezier_t acc = pData.bezier->compute_derivate(num_derivate);
const t_point_t& wps = acc.waypoints();
return bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_);
quadratic_variable<Numeric> res(bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_));
return res;
}
template <typename Point, typename Numeric>
......
......@@ -10,6 +10,8 @@
#include "curve_abc.h"
#include "curve_conversion.h"
#include <boost/smart_ptr/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
namespace curves {
/// \class PiecewiseCurve.
......@@ -23,21 +25,19 @@ namespace curves {
///
template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
typename Curve = curve_abc<Time, Numeric, Safe, Point>,
typename Point_derivate = Point >
struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
typedef Point point_t;
typedef T_Point t_point_t;
typedef Point_derivate point_derivate_t;
typedef Point_derivate point_derivate_t;
typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> > t_point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
typedef Curve curve_t;
typedef typename std::vector<curve_t> t_curve_t;
typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
typedef boost::shared_ptr<curve_t> curve_ptr_t;
typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
typedef typename std::vector<Time> t_time_t;
typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_abc_t; // parent class
typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate> piecewise_curve_t;
public:
/// \brief Empty constructor. Add at least one curve to call other class functions.
///
......@@ -47,19 +47,17 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
/// Initialize a piecewise curve by giving the first curve.
/// \param cf : a curve.
///
piecewise_curve(const curve_t& cf) {
dim_ = cf.dim();
size_ = 0;
add_curve(cf);
piecewise_curve(const curve_ptr_t& cf):
dim_(0), size_(0), T_min_(0), T_max_(0)
{
add_curve_ptr(cf);
}
piecewise_curve(const t_curve_t list_curves) {
if (list_curves.size() != 0) {
dim_ = list_curves[0].dim();
}
size_ = 0;
for (std::size_t i = 0; i < list_curves.size(); i++) {
add_curve(list_curves[i]);
piecewise_curve(const t_curve_ptr_t& curves_list):
dim_(0), size_(0), T_min_(0), T_max_(0)
{
for(typename t_curve_ptr_t::const_iterator it = curves_list.begin() ; it != curves_list.end() ; ++it){
add_curve_ptr(*it);
}
}
......@@ -73,13 +71,13 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
virtual ~piecewise_curve() {}
virtual Point operator()(const Time t) const {
virtual point_t operator()(const Time t) const {
check_if_not_empty();
if (Safe & !(T_min_ <= t && t <= T_max_)) {
// std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
return curves_.at(find_interval(t))(t);
return (*curves_.at(find_interval(t)))(t);
}
/// \brief Evaluate the derivative of order N of curve at time t.
......@@ -92,7 +90,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
if (Safe & !(T_min_ <= t && t <= T_max_)) {
throw std::invalid_argument("can't evaluate piecewise curve, out of range");
}
return (curves_.at(find_interval(t))).derivate(t, order);
return (*curves_.at(find_interval(t))).derivate(t, order);
}
/**
......@@ -100,44 +98,54 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
* @param order order of derivative
* @return
*/
piecewise_curve<Time, Numeric, Safe, point_derivate_t, std::vector<point_derivate_t, Eigen::aligned_allocator<Point> >, Curve> compute_derivate(const std::size_t order) const {
piecewise_curve<Time, Numeric, Safe, point_derivate_t, std::vector<point_derivate_t, Eigen::aligned_allocator<Point> >, Curve> res;
for (typename t_curve_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
res.add_curve(itc->compute_derivate(order));
piecewise_curve_t* compute_derivate_ptr(const std::size_t order) const {
piecewise_curve_t* res(new piecewise_curve_t());
for (typename t_curve_ptr_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
curve_ptr_t ptr((*itc)->compute_derivate_ptr(order));
res->add_curve_ptr(ptr);
}
return res;
}
template <typename Curve>
void add_curve(const Curve& curve) {
curve_ptr_t curve_ptr = boost::make_shared<Curve>(curve);
add_curve_ptr(curve_ptr);
}
/// \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where
/// \f$T_{min}\f$
/// is equal to \f$T_{max}\f$ of the actual piecewise curve. The curve added should be of type Curve as
/// defined in the template.
/// \param cf : curve to add.
///
void add_curve(const curve_t& cf) {
if (size_ == 0 && dim_ == 0) { // first curve added
dim_ = cf.dim();
void add_curve_ptr(const curve_ptr_t& cf) {
if (size_ == 0) { // first curve added
dim_ = cf->dim();
}
// Check time continuity : Beginning time of cf must be equal to T_max_ of actual piecewise curve.
if (size_ != 0 && !(fabs(cf.min() - T_max_) < MARGIN)) {
throw std::invalid_argument(
"Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
if (size_ != 0 && !(fabs(cf->min() - T_max_) < MARGIN)) {
std::stringstream ss; ss << "Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min(). Current T_max is "<<T_max_<<" new curve min is "<<cf->min();
throw std::invalid_argument(ss.str().c_str());
}
if(cf.dim() != dim_){
throw std::invalid_argument(
"All the curves in a piecewiseCurve should have the same dimension");
if(cf->dim() != dim_){
std::stringstream ss; ss << "All the curves in a piecewiseCurve should have the same dimension. Current dim is "<<dim_<<" dim of the new curve is "<<cf->dim();
throw std::invalid_argument(ss.str().c_str</