Merge pull request #14 from pFernbach/topic/piecewise_generic

Topic/piecewise generic

include/curves/bezier_curve.h

@@ -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 */

include/curves/cubic_hermite_spline.h

@@ -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*/

include/curves/curve_abc.h

@@ -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*/
 

include/curves/curve_conversion.h

@@ -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);

include/curves/exact_cubic.h

@@ -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.

include/curves/fwd.h

@@ -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

include/curves/helpers/effector_spline.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

include/curves/helpers/effector_spline_rotation.h

@@ -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

include/curves/optimization/integral_cost.h

@@ -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>

include/curves/piecewise_curve.h

@@ -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