Merge remote-tracking branch 'jchemin/topic/add_piecewise_curve_final' into HEAD
| /** | ||
| * \file piecewise_polynomial_curve.h | ||
| * \brief class allowing to create a piecewise polynomial curve. | ||
| * \file piecewise_curve.h | ||
| * \brief class allowing to create a piecewise curve. | ||
| * \author Jason C. | ||
| * \date 05/2019 | ||
| */ | ||
| ... | ... | @@ -9,61 +9,60 @@ |
| #define _CLASS_PIECEWISE_CURVE | ||
| #include "curve_abc.h" | ||
| #include "polynomial.h" | ||
| #include "curve_conversion.h" | ||
| #include <typeinfo> | ||
| namespace curves | ||
| { | ||
| /// \class PiecewiseCurve. | ||
| /// \brief Represent a piecewise polynomial curve. We can add some new polynomials to the curve, | ||
| /// but the starting time of the polynomial to add should be equal to the ending time of the | ||
| /// piecewise_polynomial_curve.<br>\ Example : A piecewise polynomial curve composed of three polynomials pol_0, | ||
| /// pol_1 and pol_2 where pol_0 is defined between \f$[T0_{min},T0_{max}]\f$, pol_1 between | ||
| /// \f$[T0_{max},T1_{max}]\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$. | ||
| /// On the piecewise polynomial curve, pol_0 is located between \f$[T0_{min},T0_{max}[\f$, | ||
| /// pol_1 between \f$[T0_{max},T1_{max}[\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$. | ||
| /// \brief Represent a piecewise curve. We can add some new curve, | ||
| /// but the starting time of the curve to add should be equal to the ending time of the actual | ||
| /// piecewise_curve.<br>\ Example : A piecewise curve composed of three curves cf0, | ||
| /// cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between | ||
| /// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$. | ||
| /// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$, | ||
| /// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$. | ||
| /// | ||
| template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false, | ||
| typename Point= Eigen::Matrix<Numeric, Dim, 1>, | ||
| typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> > | ||
| typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >, | ||
| typename Curve= curve_abc<Time, Numeric, Dim, Safe, Point> | ||
| > | ||
| struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> | ||
| struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> | ||
| { | ||
| typedef Point point_t; | ||
| typedef T_Point t_point_t; | ||
| typedef Time time_t; | ||
| typedef Numeric num_t; | ||
| //typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t; | ||
| typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t; | ||
| typedef typename std::vector < polynomial_t > t_polynomial_t; | ||
| typedef std::vector<Time> t_vector_time_t; | ||
| public: | ||
| /// \brief Constructor. | ||
| /// Initialize a piecewise polynomial curve by giving the first polynomial curve. | ||
| typedef Point point_t; | ||
| typedef T_Point t_point_t; | ||
| typedef Time time_t; | ||
| typedef Numeric num_t; | ||
| typedef Curve curve_t; | ||
| typedef typename std::vector < curve_t > t_curve_t; | ||
| typedef typename std::vector< Time > t_time_t; | ||
| public: | ||
| /// \brief Constructor. | ||
| /// Initialize a piecewise curve by giving the first curve. | ||
| /// \param pol : a polynomial curve. | ||
| /// | ||
| piecewise_polynomial_curve(const polynomial_t& pol) | ||
| { | ||
| size_ = 0; | ||
| add_polynomial_curve(pol); | ||
| time_polynomial_curves_.push_back(pol.min()); | ||
| T_min_ = pol.min(); | ||
| } | ||
| piecewise_curve(const curve_t& cf) | ||
| { | ||
| size_ = 0; | ||
| add_curve(cf); | ||
| time_curves_.push_back(cf.min()); | ||
| T_min_ = cf.min(); | ||
| } | ||
| virtual ~piecewise_polynomial_curve(){} | ||
| virtual ~piecewise_curve(){} | ||
| virtual Point operator()(const Time t) const | ||
| virtual Point operator()(const Time t) const | ||
| { | ||
| 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"); | ||
| //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 polynomial_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. | ||
| ... | ... | @@ -75,78 +74,78 @@ struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, P |
| { | ||
| if(Safe &! (T_min_ <= t && t <= T_max_)) | ||
| { | ||
| throw std::out_of_range("can't evaluate piecewise curve, out of range"); | ||
| throw std::invalid_argument("can't evaluate piecewise curve, out of range"); | ||
| } | ||
| return (polynomial_curves_.at(find_interval(t))).derivate(t, order); | ||
| return (curves_.at(find_interval(t))).derivate(t, order); | ||
| } | ||
| /// \brief Add a new polynomial to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$ | ||
| /// \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. | ||
| /// \param pol : polynomial to add. | ||
| /// \param cf : curve to add. | ||
| /// | ||
| void add_polynomial_curve(polynomial_t pol) | ||
| { | ||
| // Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve. | ||
| if (size_!=0 && pol.min()!=T_max_) | ||
| { | ||
| throw std::out_of_range("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()"); | ||
| </ |