diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h index 87362eaa7c48d9d2e8a4e07cb34ab05427ff3bf6..5c3cd83e7b90d2ba6b3495f1f5ef08ccf2ad52e4 100644 --- a/include/curves/bezier_curve.h +++ b/include/curves/bezier_curve.h @@ -60,7 +60,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> In it(PointsBegin); if(Safe && (size_<1 || T_ <= 0.)) { - throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO + throw std::out_of_range("can't create bezier min bound is higher than max bound"); } for(; it != PointsEnd; ++it) { diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_polynomial_curve.h similarity index 61% rename from include/curves/piecewise_curve.h rename to include/curves/piecewise_polynomial_curve.h index 2d21931f5c1dfcfe9cca2f45bcbd2a8d6399ff41..f1bbbf1ad04b2088d9241a5f909979c3276e79dc 100644 --- a/include/curves/piecewise_curve.h +++ b/include/curves/piecewise_polynomial_curve.h @@ -19,7 +19,7 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S typename Point= Eigen::Matrix<Numeric, Dim, 1>, typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point>, typename Polynomial= polynomial <double, double, 3, true, point_t, t_point_t> > > -struct piecewise_curve +struct piecewise_polynomial_curve { typedef Point point_t; @@ -28,17 +28,37 @@ struct piecewise_curve typedef Numeric num_t; //typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t; + typedef Polynomial polynomial_t; typedef std::vector < Polynomial > t_polynomial_t; typedef std::vector<Time> t_vector_time_t; public: - piecewise_curve + piecewise_polynomial_curve(polynomial_t pol) + { + size_ = 0; + T_min_ = pol.min(); + add_polynomial_curve(pol); + } + + void add_polynomial_curve(polynomial_t pol) + { + // Check time continuity : Begin 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()"); + } + polynomial_curves_.push_back(pol); + size_ = polynomial_curves_.size(); + T_max_ = pol.max(); + time_polynomial_curves_.push_back(T_max_); + } private: t_polynomial_t polynomial_curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ] - t_vector_time_t time_polynomial_curves_; // for curves 0/1/2 : [ (Tmin0,Tmax0),(Tmin1,Tmax1),(Tmin2,Tmax2) ] + t_vector_time_t time_polynomial_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ] Numeric size_; + Time T_min_, T_max_; } } // end namespace