diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index d359550bf587f70d011b69410f3adc928e463805..d26a3cc96e5560bbd6ed5f0dbea004f9537966dd 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -100,6 +100,19 @@ namespace curves
return (curves_.at(find_interval(t))).derivate(t, order);
}
+ /**
+ * @brief compute_derivate return a piecewise_curve which is the derivative of this at given order
+ * @param order order of derivative
+ * @return
+ */
+ piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve> compute_derivate(const std::size_t order) const{
+ piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve> res;
+ for(typename t_curve_t::const_iterator itc = curves_.begin() ; itc < curves_.end() ; ++itc){
+ res.add_curve(itc->compute_derivate(order));
+ }
+ return res;
+ }
+
/// \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.
@@ -179,6 +192,7 @@ namespace curves
return pc_res;
}
+
template<typename Hermite>
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
{
@@ -216,53 +230,75 @@ namespace curves
}
template<typename Polynomial>
- static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
- convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
+ static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
+ convert_discrete_points_to_polynomial(T_Point points, t_time_t time_points)
{
if(Safe &! (points.size()>1))
{
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
- throw std::invalid_argument("piecewise_curve -> convert_discrete_points_to_polynomial, Error, less than 2 discrete points");
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
}
- typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
- Time discretization_step = (T_max-T_min)/Time(points.size()-1);
- Time time_actual = T_min;
- // Initialization at first points
- point_t actual_point = points[0];
- point_t next_point = points[1];
- point_t coeff_order_zero(actual_point);
- point_t coeff_order_one((next_point-actual_point)/discretization_step);
- t_point_t coeffs;
- coeffs.push_back(coeff_order_zero);
- coeffs.push_back(coeff_order_one);
- Polynomial pol(coeffs,time_actual,time_actual+discretization_step);
- piecewise_curve_out_t ppc(pol);
- time_actual += discretization_step;
- // Other points
- for (std::size_t i=1; i<points.size()-2; i++)
+ if(points.size() != time_points.size()){
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same size.");
+ }
+ piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
+
+ for(size_t i = 1 ; i < points.size() ; ++i){
+ piecewise_res.add_curve(Polynomial(points[i-1],points[i],time_points[i-1],time_points[i]));
+ }
+ return piecewise_res;
+ }
+
+ template<typename Polynomial>
+ static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
+ convert_discrete_points_to_polynomial(T_Point points,T_Point points_derivative, t_time_t time_points)
+ {
+ if(Safe &! (points.size()>1))
+ {
+ //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
+ }
+ if(points.size() != time_points.size()){
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same size.");
+ }
+ if(points.size() != points_derivative.size()){
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the same size.");
+ }
+ piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
+
+ for(size_t i = 1 ; i < points.size() ; ++i){
+ piecewise_res.add_curve(Polynomial(points[i-1],points_derivative[i-1],points[i],points_derivative[i],time_points[i-1],time_points[i]));
+ }
+ return piecewise_res;
+ }
+
+ template<typename Polynomial>
+ static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
+ convert_discrete_points_to_polynomial(T_Point points,T_Point points_derivative, T_Point points_second_derivative, t_time_t time_points)
+ {
+ if(Safe &! (points.size()>1))
{
- coeffs.clear();
- actual_point = points[i];
- next_point = points[i+1];
- coeff_order_zero = actual_point;
- coeff_order_one = (next_point-actual_point)/discretization_step;
- coeffs.push_back(coeff_order_zero);
- coeffs.push_back(coeff_order_one);
- ppc.add_curve(Polynomial(coeffs,time_actual,time_actual+discretization_step));
- time_actual += discretization_step;
+ //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points");
+ }
+ if(points.size() != time_points.size()){
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same size.");
+ }
+ if(points.size() != points_derivative.size()){
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the same size.");
}
- // Last points
- coeffs.clear();
- actual_point = points[points.size()-2];
- next_point = points[points.size()-1];
- coeff_order_zero = actual_point;
- coeff_order_one = (next_point-actual_point)/discretization_step;
- coeffs.push_back(coeff_order_zero);
- coeffs.push_back(coeff_order_one);
- ppc.add_curve(Polynomial(coeffs,time_actual,T_max));
- return ppc;
+ if(points.size() != points_second_derivative.size()){
+ throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_second_derivative must have the same size.");
+ }
+ piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
+
+ for(size_t i = 1 ; i < points.size() ; ++i){
+ piecewise_res.add_curve(Polynomial(points[i-1],points_derivative[i-1],points_second_derivative[i-1],points[i],points_derivative[i],points_second_derivative[i],time_points[i-1],time_points[i]));
+ }
+ return piecewise_res;
}
+
private:
/// \brief Get index of the interval corresponding to time t for the interpolation.
@@ -357,4 +393,4 @@ namespace curves
} // end namespace
-#endif // _CLASS_PIECEWISE_CURVE
\ No newline at end of file
+#endif // _CLASS_PIECEWISE_CURVE
diff --git a/include/curves/polynomial.h b/include/curves/polynomial.h
index 6c5c4c2ec9785948e4e3dc6d975d2d09341d98e8..1074542b6e00c578d04a8b303d7d95dbe496b071 100644
--- a/include/curves/polynomial.h
+++ b/include/curves/polynomial.h
@@ -84,6 +84,7 @@ namespace curves
safe_check();
}
+
/// \brief Constructor.
/// \param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients
/// it corresponds to the zero degree coefficient.
@@ -101,6 +102,134 @@ namespace curves
safe_check();
}
+ ///
+ /// \brief Constructor from boundary condition with C0 : create a polynomial that connect exactly init and end (order 1)
+ /// \param init the initial point of the curve
+ /// \param end the final point of the curve
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
+ ///
+ polynomial(const Point& init, const Point& end, const time_t min, const time_t max ):
+ dim_(init.size()), degree_(1),
+ T_min_(min), T_max_(max)
+ {
+ if(init.size() != end.size())
+ throw std::invalid_argument("init and end points must have the same dimensions.");
+ t_point_t coeffs;
+ coeffs.push_back(init);
+ coeffs.push_back((end-init)/(max-min));
+ coefficients_ = init_coeffs(coeffs.begin(), coeffs.end());
+ safe_check();
+ }
+
+ ///
+ /// \brief Constructor from boundary condition with C1 :
+ /// create a polynomial that connect exactly init and end and thier first order derivatives(order 3)
+ /// \param init the initial point of the curve
+ /// \param d_init the initial value of the derivative of the curve
+ /// \param end the final point of the curve
+ /// \param d_end the final value of the derivative of the curve
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
+ ///
+ polynomial(const Point& init,const Point& d_init, const Point& end, const Point& d_end,const time_t min, const time_t max ):
+ dim_(init.size()), degree_(3),
+ T_min_(min), T_max_(max)
+ {
+ if(init.size() != end.size())
+ throw std::invalid_argument("init and end points must have the same dimensions.");
+ if(init.size() != d_init.size())
+ throw std::invalid_argument("init and d_init points must have the same dimensions.");
+ if(init.size() != d_end.size())
+ throw std::invalid_argument("init and d_end points must have the same dimensions.");
+ /* the coefficients [c0 c1 c2 c3] are found by solving the following system of equation
+ (found from the boundary conditions) :
+ [1 0 0 0 ] [c0] [ init ]
+ [1 T T^2 T^3 ] x [c1] = [ end ]
+ [0 1 0 0 ] [c2] [d_init]
+ [0 1 2T 3T^2] [c3] [d_end ]
+ */
+ double T = max-min;
+ Eigen::Matrix<double, 4, 4> m;
+ m << 1.,0,0,0,
+ 1.,T,T*T,T*T*T,
+ 0,1.,0,0,
+ 0,1.,2.*T,3.*T*T;
+ Eigen::Matrix<double, 4, 4> m_inv = m.inverse();
+ Eigen::Matrix<double,4,1> bc; // boundary condition vector
+ coefficients_ = coeff_t::Zero(dim_,degree_+1); // init coefficient matrix with the right size
+ for(size_t i = 0 ;i < dim_ ; ++i){ // for each dimension, solve the boundary condition problem :
+ bc[0] = init[i];
+ bc[1] = end[i];
+ bc[2] = d_init[i];
+ bc[3] = d_end[i];
+ coefficients_.row(i) = (m_inv*bc).transpose();
+ }
+ safe_check();
+ }
+
+ ///
+ /// \brief Constructor from boundary condition with C2 :
+ /// create a polynomial that connect exactly init and end and thier first and second order derivatives(order 5)
+ /// \param init the initial point of the curve
+ /// \param d_init the initial value of the derivative of the curve
+ /// \param d_init the initial value of the second derivative of the curve
+ /// \param end the final point of the curve
+ /// \param d_end the final value of the derivative of the curve
+ /// \param d_end the final value of the second derivative of the curve
+ /// \param min : LOWER bound on interval definition of the spline.
+ /// \param max : UPPER bound on interval definition of the spline.
+ ///
+ polynomial(const Point& init,const Point& d_init,const Point& dd_init, const Point& end, const Point& d_end,const Point& dd_end,const time_t min, const time_t max ):
+ dim_(init.size()), degree_(5),
+ T_min_(min), T_max_(max)
+ {
+ if(init.size() != end.size())
+ throw std::invalid_argument("init and end points must have the same dimensions.");
+ if(init.size() != d_init.size())
+ throw std::invalid_argument("init and d_init points must have the same dimensions.");
+ if(init.size() != d_end.size())
+ throw std::invalid_argument("init and d_end points must have the same dimensions.");
+ if(init.size() != dd_init.size())
+ throw std::invalid_argument("init and dd_init points must have the same dimensions.");
+ if(init.size() != dd_end.size())
+ throw std::invalid_argument("init and dd_end points must have the same dimensions.");
+ /* the coefficients [c0 c1 c2 c3 c4 c5] are found by solving the following system of equation
+ (found from the boundary conditions) :
+ [1 0 0 0 0 0 ] [c0] [ init ]
+ [1 T T^2 T^3 T^4 T^5 ] [c1] [ end ]
+ [0 1 0 0 0 0 ] [c2] [d_init ]
+ [0 1 2T 3T^2 4T^3 5T^4 ] x [c3] = [d_end ]
+ [0 0 2 0 0 0 ] [c4] [dd_init]
+ [0 0 2 6T 12T^2 20T^3] [c5] [dd_end ]
+ */
+ double T = max-min;
+ Eigen::Matrix<double, 6, 6> m;
+ m << 1.,0,0,0,0,0,
+ 1.,T,T*T,pow(T,3),pow(T,4),pow(T,5),
+ 0,1.,0,0,0,0,
+ 0,1.,2.*T,3.*T*T,4.*pow(T,3),5.*pow(T,4),
+ 0,0,2,0,0,0,
+ 0,0,2,6.*T,12.*T*T,20.*pow(T,3);
+ Eigen::Matrix<double, 6, 6> m_inv = m.inverse();
+ Eigen::Matrix<double,6,1> bc; // boundary condition vector
+ coefficients_ = coeff_t::Zero(dim_,degree_+1); // init coefficient matrix with the right size
+ for(size_t i = 0 ;i < dim_ ; ++i){ // for each dimension, solve the boundary condition problem :
+ bc[0] = init[i];
+ bc[1] = end[i];
+ bc[2] = d_init[i];
+ bc[3] = d_end[i];
+ bc[4] = dd_init[i];
+ bc[5] = dd_end[i];
+ coefficients_.row(i) = (m_inv*bc).transpose();
+ }
+ safe_check();
+ }
+
+
+
+
+
/// \brief Destructor
~polynomial()
{
@@ -123,11 +252,11 @@ namespace curves
{
if(T_min_ > T_max_)
{
- std::invalid_argument("Tmin should be inferior to Tmax");
+ throw std::invalid_argument("Tmin should be inferior to Tmax");
}
- if(coefficients_.size() != int(degree_+1))
+ if(coefficients_.cols() != int(degree_+1))
{
- std::runtime_error("Spline order and coefficients do not match");
+ throw std::runtime_error("Spline order and coefficients do not match");
}
}
}
@@ -203,6 +332,8 @@ namespace curves
coeff_t deriv_coeff(coeff_t coeff) const
{
+ if(coeff.cols() == 1) // only the constant part is left, fill with 0
+ return coeff_t::Zero(coeff.rows(),1);
coeff_t coeff_derivated(coeff.rows(), coeff.cols()-1);
for (std::size_t i=0; i<std::size_t(coeff_derivated.cols()); i++) {
coeff_derivated.col(i) = coeff.col(i+1)*(num_t)(i+1);
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 3e922505dc0e1a37a52e867d7ef338dc015f97d7..34e5733c09602d783993813c7ea3f78c147c2b26 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -140,6 +140,18 @@ namespace curves
{
return new polynomial_t(array, 0., 1.);
}
+ polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree1(const pointX_t& init,const pointX_t& end,const real min, const real max)
+ {
+ return new polynomial_t(init,end,min,max);
+ }
+ polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree3(const pointX_t& init,const pointX_t& d_init,const pointX_t& end,const pointX_t& d_end,const real min, const real max)
+ {
+ return new polynomial_t(init,d_init,end,d_end,min,max);
+ }
+ polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree5(const pointX_t& init,const pointX_t& d_init,const pointX_t& dd_init,const pointX_t& end,const point_t& d_end,const point_t& dd_end,const real min, const real max)
+ {
+ return new polynomial_t(init,d_init,dd_init,end,d_end,dd_end,min,max);
+ }
/* End wrap polynomial */
/* Wrap piecewise curve */
@@ -167,6 +179,49 @@ namespace curves
{
return new piecewise_cubic_hermite_curve_t();
}
+ static piecewise_polynomial_curve_t discretPointToPolynomialC0(const pointX_list_t& points, const time_waypoints_t& time_points){
+ t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
+ t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
+ return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
+ }
+ static piecewise_polynomial_curve_t discretPointToPolynomialC1(const pointX_list_t& points,const pointX_list_t& points_derivative, const time_waypoints_t& time_points){
+ t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
+ t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
+ t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
+ return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,time_points_list);
+ }
+ static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list_t& points,const pointX_list_t& points_derivative,const pointX_list_t& points_second_derivative, const time_waypoints_t& time_points){
+ t_pointX_t points_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points);
+ t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_derivative);
+ t_pointX_t points_second_derivative_list = vectorFromEigenArray<pointX_list_t,t_pointX_t>(points_second_derivative);
+
+ t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t,t_time_t>(time_points);
+ return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,points_derivative_list,points_second_derivative_list,time_points_list);
+ }
+ void addFinalPointC0(piecewise_polynomial_curve_t self,const pointX_t& end,const real time){
+ if(self.is_continuous(1))
+ std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only guarantee C0 continuity."<<std::endl;
+ polynomial_t pol(self(self.max()),end,self.max(),time);
+ self.add_curve(pol);
+ }
+ void addFinalPointC1(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const real time){
+ if(self.is_continuous(2))
+ std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C2 continuity and only guarantee C1 continuity."<<std::endl;
+ if(!self.is_continuous(1))
+ std::cout<<"Warning: the current piecewise curve is not C1 continuous."<<std::endl;
+ polynomial_t pol(self(self.max()),self.derivate(self.max(),1),end,d_end,self.max(),time);
+ self.add_curve(pol);
+ }
+ void addFinalPointC2(piecewise_polynomial_curve_t self,const pointX_t& end,const pointX_t& d_end,const pointX_t& dd_end,const real time){
+ if(self.is_continuous(3))
+ std::cout<<"Warning: by adding this final point to the piecewise curve, you loose C3 continuity and only guarantee C2 continuity."<<std::endl;
+ if(!self.is_continuous(2))
+ std::cout<<"Warning: the current piecewise curve is not C2 continuous."<<std::endl;
+ polynomial_t pol(self(self.max()),self.derivate(self.max(),1),self.derivate(self.max(),2),end,d_end,dd_end,self.max(),time);
+ self.add_curve(pol);
+ }
+
+
/* end wrap piecewise polynomial curve */
/* Wrap exact cubic spline */
@@ -325,14 +380,30 @@ namespace curves
/** END variable points bezier curve**/
/** BEGIN polynomial curve function**/
class_<polynomial_t>("polynomial", init<>())
- .def("__init__", make_constructor(&wrapPolynomialConstructor1),
+ .def("__init__", make_constructor(&wrapPolynomialConstructor1,default_call_policies(),args("coeffs","min","max")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t \in [min,max]."
" The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
" Spline order is given by the number of the columns -1.")
- .def("__init__", make_constructor(&wrapPolynomialConstructor2),
+ .def("__init__", make_constructor(&wrapPolynomialConstructor2,default_call_policies(),arg("coeffs")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t \in [0,1]."
" The matrix should contain one coefficient per column, from the zero order coefficient,up to the highest order."
" Spline order is given by the number of the columns -1.")
+ .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree1,
+ default_call_policies(),args("init","end","min","max")),
+ "Create a polynomial of degree 1 defined for t \in [min,max], "
+ "such that c(min) == init and c(max) == end.")
+ .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree3,
+ default_call_policies(),args("init","d_init","end","d_end","min","max")),
+ "Create a polynomial of degree 3 defined for t \in [min,max], "
+ "such that c(min) == init and c(max) == end"
+ " dc(min) == d_init and dc(max) == d_end")
+ .def("__init__", make_constructor(&wrapPolynomialConstructorFromBoundaryConditionsDegree5,
+ default_call_policies(),
+ args("init","d_init","dd_init","end","d_end","dd_end","min","max")),
+ "Create a polynomial of degree 5 defined for t \in [min,max], "
+ "such that c(min) == init and c(max) == end"
+ " dc(min) == d_init and dc(max) == d_end"
+ " ddc(min) == dd_init and ddc(max) == dd_end")
.def("min", &polynomial_t::min, "Get the LOWER bound on interval definition of the curve.")
.def("max", &polynomial_t::max,"Get the HIGHER bound on interval definition of the curve.")
.def("dim", &polynomial_t::dim)
@@ -352,14 +423,28 @@ namespace curves
/** BEGIN piecewise curve function **/
class_<piecewise_polynomial_curve_t>
("piecewise_polynomial_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor),
+ .def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor,default_call_policies(),arg("curve")),
"Create a peicewise-polynomial curve containing the given polynomial curve.")
+ .def("FromPointsList",&discretPointToPolynomialC0,
+ "Create a piecewise-polynomial connecting exactly all the given points at the given time. The created piecewise is C0 continuous.",args("points","time_points"))
+ .def("FromPointsList",&discretPointToPolynomialC1,
+ "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative values. The created piecewise is C1 continuous.",args("points","points_derivative","time_points"))
+ .def("FromPointsList",&discretPointToPolynomialC2,
+ "Create a piecewise-polynomial connecting exactly all the given points at the given time and respect the given points derivative and second derivative values. The created piecewise is C2 continuous.",args("points","points_derivative","points_second_derivative","time_points"))
+ .staticmethod("FromPointsList")
+ .def("append",&addFinalPointC0,
+ "Append a new polynomial curve of degree 1 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end",args("self","end","time"))
+ .def("append",&addFinalPointC1,
+ "Append a new polynomial curve of degree 3 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C1 continuity and guarantee that self.derivate(time,1) == d_end",args("self","end","d_end","time"))
+ .def("append",&addFinalPointC2,
+ "Append a new polynomial curve of degree 5 at the end of the piecewise curve, defined between self.max() and time and connecting exactly self(self.max()) and end. It guarantee C2 continuity and guarantee that self.derivate(time,1) == d_end and self.derivate(time,2) == dd_end",args("self","end","d_end","d_end","time"))
.def("min", &piecewise_polynomial_curve_t::min,"Set the LOWER bound on interval definition of the curve.")
.def("max", &piecewise_polynomial_curve_t::max,"Set the HIGHER bound on interval definition of the curve.")
.def("dim", &piecewise_polynomial_curve_t::dim)
.def("__call__", &piecewise_polynomial_curve_t::operator(),"Evaluate the curve at the given time.")
.def("derivate", &piecewise_polynomial_curve_t::derivate,"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
- .def("add_curve", &piecewise_polynomial_curve_t::add_curve,
+ .def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
+ .def("append", &piecewise_polynomial_curve_t::add_curve,
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
@@ -379,6 +464,7 @@ namespace curves
.def("dim", &piecewise_bezier_curve_t::dim)
.def("__call__", &piecewise_bezier_curve_t::operator())
.def("derivate", &piecewise_bezier_curve_t::derivate)
+ .def("compute_derivate",&piecewise_polynomial_curve_t::compute_derivate,"Return a piecewise_polynomial curve which is the derivate of this.",args("self","order"))
.def("add_curve", &piecewise_bezier_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
.def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
diff --git a/python/python_definitions.h b/python/python_definitions.h
index d4648c2f2d7368d8d347d47368ea8f896b90d977..a7687d96e7bb0a6bb333ce19a39420540e9943c2 100644
--- a/python/python_definitions.h
+++ b/python/python_definitions.h
@@ -19,6 +19,7 @@ namespace curves
typedef Eigen::VectorXd time_waypoints_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
+ typedef std::vector<real> t_time_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef std::vector<point6_t,Eigen::aligned_allocator<point6_t> > t_point6_t;
typedef std::pair<real, point_t> Waypoint;
@@ -36,5 +37,15 @@ namespace curves
}
return res;
}
+ template <typename PointList, typename T_Point>
+ T_Point vectorFromEigenVector(const PointList& vector)
+ {
+ T_Point res;
+ for(int i =0;i<vector.rows();++i)
+ {
+ res.push_back(vector[i]);
+ }
+ return res;
+ }
} //namespace curves
#endif //_DEFINITION_PYTHON_BINDINGS
diff --git a/python/test/test.py b/python/test/test.py
index 1fe23a4bd171de3526c9ad8e3c4f425fe4794ad9..c2a12e01f47ecd70a60e6b3d0c7352d166e0bdb6 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -1,7 +1,7 @@
import unittest
import os
-from numpy import matrix
+from numpy import matrix, array_equal, isclose,random
from numpy.linalg import norm
#from curves import ( serialize_polynomial, deserialize_polynomial, serialize_piecewise_polynomial_curve, deserialize_piecewise_polynomial_curve )
@@ -128,6 +128,58 @@ class TestCurves(unittest.TestCase):
os.remove("serialization_curve.test")
return
+ def test_polynomial_from_boundary_condition(self):
+ p0 = matrix([1.,3.,-2.]).T
+ p1 = matrix([0.6,2.,2.5]).T
+ dp0 = matrix([-6.,2.,-1.]).T
+ dp1 = matrix([10.,10.,10.]).T
+ ddp0 = matrix([1.,-7.,4.5]).T
+ ddp1 = matrix([6.,-1.,-4]).T
+ min = 1.
+ max = 2.5
+ polC0 = polynomial(p0,p1,min,max)
+ self.assertEqual(polC0.min(), min)
+ self.assertEqual(polC0.max(),max)
+ self.assertTrue(array_equal(polC0(min), p0))
+ self.assertTrue(array_equal(polC0(max), p1))
+ self.assertTrue(array_equal(polC0((min+max)/2.),0.5*p0+0.5*p1))
+ polC1 = polynomial(p0,dp0,p1,dp1,min,max)
+ self.assertEqual(polC1.min(), min)
+ self.assertEqual(polC1.max(),max)
+ self.assertTrue(isclose(polC1(min), p0).all())
+ self.assertTrue(isclose(polC1(max), p1).all())
+ self.assertTrue(isclose(polC1.derivate(min,1), dp0).all())
+ self.assertTrue(isclose(polC1.derivate(max,1), dp1).all())
+ polC2 = polynomial(p0,dp0,ddp0,p1,dp1,ddp1,min,max)
+ self.assertEqual(polC2.min(), min)
+ self.assertEqual(polC2.max(),max)
+ self.assertTrue(isclose(polC2(min), p0).all())
+ self.assertTrue(isclose(polC2(max), p1).all())
+ self.assertTrue(isclose(polC2.derivate(min,1), dp0).all())
+ self.assertTrue(isclose(polC2.derivate(max,1), dp1).all())
+ self.assertTrue(isclose(polC2.derivate(min,2), ddp0).all())
+ self.assertTrue(isclose(polC2.derivate(max,2), ddp1).all())
+ # check that the exception are correctly raised :
+ try:
+ polC0 = polynomial(p0,p1,max,min)
+ self.assertTrue(False) # should never get there
+ except ValueError:
+ pass
+
+ try:
+ polC1 = polynomial(p0,dp0,p1,dp1,max,min)
+ self.assertTrue(False) # should never get there
+ except ValueError:
+ pass
+
+ try:
+ polC2 = polynomial(p0,dp0,ddp0,p1,dp1,ddp1,max,min)
+ self.assertTrue(False) # should never get there
+ except ValueError:
+ pass
+
+ return
+
def test_cubic_hermite_spline(self):
print("test_cubic_hermite_spline")
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
@@ -174,7 +226,7 @@ class TestCurves(unittest.TestCase):
a = polynomial(waypoints1, 0., 1.)
b = polynomial(waypoints2, 1., 3.)
pc = piecewise_polynomial_curve(a)
- pc.add_curve(b)
+ pc.append(b)
pc.min()
pc.max()
pc(0.4)
@@ -191,8 +243,64 @@ class TestCurves(unittest.TestCase):
os.remove("serialization_pc.test")
return
- def test_piecewise_bezier_curve(self):
- print("test_piecewise_bezier_curve")
+ def test_piecewise_from_points_list(self):
+ N = 7
+ points = matrix(random.rand(3,N))
+ points_derivative = matrix(random.rand(3,N))
+ points_second_derivative = matrix(random.rand(3,N))
+ time_points = matrix(random.rand(N)).T
+ time_points.sort(0)
+ polC0 =piecewise_polynomial_curve.FromPointsList(points,time_points)
+ self.assertEqual(polC0.min(),time_points[0,0])
+ self.assertEqual(polC0.max(),time_points[-1,0])
+ self.assertTrue(polC0.is_continuous(0))
+ self.assertTrue(not polC0.is_continuous(1))
+ for i in range(N):
+ self.assertTrue(isclose(polC0(time_points[i,0]),points[:,i]).all())
+
+ polC1 =piecewise_polynomial_curve.FromPointsList(points,points_derivative,time_points)
+ self.assertEqual(polC1.min(),time_points[0,0])
+ self.assertEqual(polC1.max(),time_points[-1,0])
+ self.assertTrue(polC1.is_continuous(0))
+ self.assertTrue(polC1.is_continuous(1))
+ self.assertTrue(not polC1.is_continuous(2))
+ for i in range(N):
+ self.assertTrue(isclose(polC1(time_points[i,0]),points[:,i]).all())
+ self.assertTrue(isclose(polC1.derivate(time_points[i,0],1),points_derivative[:,i]).all())
+
+ polC2 =piecewise_polynomial_curve.FromPointsList(points,points_derivative,points_second_derivative,time_points)
+ self.assertEqual(polC2.min(),time_points[0,0])
+ self.assertEqual(polC2.max(),time_points[-1,0])
+ self.assertTrue(polC2.is_continuous(0))
+ self.assertTrue(polC2.is_continuous(1))
+ self.assertTrue(polC2.is_continuous(2))
+ self.assertTrue(not polC2.is_continuous(3))
+ for i in range(N):
+ self.assertTrue(isclose(polC2(time_points[i,0]),points[:,i]).all())
+ self.assertTrue(isclose(polC2.derivate(time_points[i,0],1),points_derivative[:,i]).all())
+ self.assertTrue(isclose(polC2.derivate(time_points[i,0],2),points_second_derivative[:,i]).all())
+
+ # check if exepetion are corectly raised when time_points are not in ascending values
+ time_points[0,0] = 1
+ time_points[1,0] = 0.5
+ try:
+ polC0 =piecewise_polynomial_curve.FromPointsList(points,time_points)
+ self.assertTrue(False) # should not get here
+ except ValueError:
+ pass
+ try:
+ polC1 =piecewise_polynomial_curve.FromPointsList(points,points_derivative,time_points)
+ self.assertTrue(False) # should not get here
+ except ValueError:
+ pass
+ try:
+ polC2 =piecewise_polynomial_curve.FromPointsList(points,points_derivative,points_second_derivative,time_points)
+ self.assertTrue(False) # should not get here
+ except ValueError:
+ pass
+ return
+
+ def test_piecewise_bezier3_curve(self):
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 5f44088a7d1faa0aa5d4afaefedfd2f044780a88..5cf3b912e47f9c69d822d52795cbcd55177c0998 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -19,16 +19,16 @@ namespace curves
{
typedef Eigen::Vector3d point_t;
typedef Eigen::VectorXd pointX_t;
- typedef std::vector<pointX_t,Eigen::aligned_allocator<pointX_t> > t_point_t;
+ typedef std::vector<pointX_t,Eigen::aligned_allocator<pointX_t> > t_pointX_t;
typedef curve_abc <double, double, true, pointX_t> curve_abc_t;
- typedef polynomial <double, double, true, pointX_t, t_point_t> polynomial_t;
+ typedef polynomial <double, double, true, pointX_t, t_pointX_t> polynomial_t;
typedef exact_cubic <double, double, true, pointX_t> exact_cubic_t;
typedef exact_cubic <double, double, true, Eigen::Matrix<double,1,1> > exact_cubic_one;
typedef bezier_curve <double, double, true, pointX_t> bezier_curve_t;
typedef cubic_hermite_spline <double, double, true, pointX_t> cubic_hermite_spline_t;
- typedef piecewise_curve <double, double, true, pointX_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
- typedef piecewise_curve <double, double, true, pointX_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
- typedef piecewise_curve <double, double, true, pointX_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
+ typedef piecewise_curve <double, double, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
+ typedef piecewise_curve <double, double, true, pointX_t, t_pointX_t, bezier_curve_t> piecewise_bezier_curve_t;
+ typedef piecewise_curve <double, double, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
@@ -43,6 +43,14 @@ namespace curves
{
return std::fabs(a-b)<margin;
}
+ bool QuasiEqual(const point_t a, const point_t b)
+ {
+ bool equal = true;
+ for(size_t i = 0 ; i < 3 ; ++i){
+ equal = equal && QuasiEqual(a[i],b[i]);
+ }
+ return equal;
+ }
} // End namespace curves
using namespace curves;
@@ -95,7 +103,7 @@ void PolynomialCubicFunctionTest(bool& error)
point_t b(2,3,4);
point_t c(3,4,5);
point_t d(3,6,7);
- t_point_t vec;
+ t_pointX_t vec;
vec.push_back(a);
vec.push_back(b);
vec.push_back(c);
@@ -1114,7 +1122,7 @@ void piecewiseCurveTest(bool& error)
point_t b(2,1,1); // in [1,2[
point_t c(3,1,1); // in [2,3]
point_t res;
- t_point_t vec1, vec2, vec3;
+ t_pointX_t vec1, vec2, vec3;
vec1.push_back(a); // x=1, y=1, z=1
vec2.push_back(b); // x=2, y=1, z=1
vec3.push_back(c); // x=3, y=1, z=1
@@ -1196,7 +1204,7 @@ void piecewiseCurveTest(bool& error)
// Create piecewise curve C2
point_t a1(0,0,0);
point_t b1(1,1,1);
- t_point_t veca, vecb;
+ t_pointX_t veca, vecb;
// in [0,1[
veca.push_back(a1);
veca.push_back(b1); // x=t, y=t, z=t
@@ -1261,6 +1269,40 @@ void piecewiseCurveTest(bool& error)
CompareCurves<piecewise_polynomial_curve_t, piecewise_cubic_hermite_curve_t>(pc, pc_hermite, errmsg5, error);
piecewise_polynomial_curve_t pc_polynomial_same = pc.convert_piecewise_curve_to_polynomial<polynomial_t>();
CompareCurves<piecewise_polynomial_curve_t, piecewise_polynomial_curve_t>(pc, pc_polynomial_same, errmsg5, error);
+
+ // compare compute_derivate and derivate results :
+
+ piecewise_polynomial_curve_t pc_C2_derivate = pc_C2.compute_derivate(1);
+ piecewise_polynomial_curve_t pc_C2_derivate2 = pc_C2.compute_derivate(2);
+ if(pc_C2.min() != pc_C2_derivate.min()){
+ error = true;
+ std::cout<<"min bounds for curve and it's derivate are not equals."<<std::endl;
+ }
+ if(pc_C2.min() != pc_C2_derivate2.min()){
+ error = true;
+ std::cout<<"min bounds for curve and it's second derivate are not equals."<<std::endl;
+ }
+ if(pc_C2.max() != pc_C2_derivate.max()){
+ error = true;
+ std::cout<<"max bounds for curve and it's derivate are not equals."<<std::endl;
+ }
+ if(pc_C2.max() != pc_C2_derivate2.max()){
+ error = true;
+ std::cout<<"max bounds for curve and it's second derivate are not equals."<<std::endl;
+ }
+ double t = 0.;
+ while(t<pc_C2.max()){
+ if(!QuasiEqual(pc_C2.derivate(t,1),pc_C2_derivate(t))){
+ error = true;
+ std::cout<<"value not equal between derivate and compute_derivate (order 1) at t = "<<t<<std::endl;
+ }
+ if(!QuasiEqual(pc_C2.derivate(t,2),pc_C2_derivate2(t))){
+ error = true;
+ std::cout<<"value not equal between derivate and compute_derivate (order 2) at t = "<<t<<std::endl;
+ }
+ t += 0.01;
+ }
+
}
catch(...)
{
@@ -1281,7 +1323,7 @@ void curveAbcDimDynamicTest(bool& error)
// POLYNOMIAL
point_t a(1,1,1);
point_t b(2,2,2);
- t_point_t vec;
+ t_pointX_t vec;
vec.push_back(a);
vec.push_back(b);
polynomial_test_t pol(vec.begin(), vec.end(), 0, 1);
@@ -1362,40 +1404,38 @@ void curveAbcDimDynamicTest(bool& error)
void piecewiseCurveConversionFromDiscretePointsTest(bool& error)
{
- try
- {
- std::string errMsg("piecewiseCurveConversionFromDiscretePointsTest, Error, value on curve is wrong : ");
- point_t p0(0.,0.,0.);
- point_t p1(1.,2.,3.);
- point_t p2(4.,4.,4.);
- point_t p3(10.,10.,10.);
- point_t p_test_0_5 = (p0+p1)/2.0;
- t_point_t points;
- points.push_back(p0);
- points.push_back(p1);
- points.push_back(p2);
- points.push_back(p3);
- double T_min = 1.0;
- double T_max = 3.0;
- double timestep = (T_max-T_min)/double(points.size()-1);
- piecewise_polynomial_curve_t ppc = piecewise_polynomial_curve_t::
- convert_discrete_points_to_polynomial<polynomial_t>(points,T_min,T_max);
- if (!ppc.is_continuous(0))
- {
- std::cout<<"piecewiseCurveConversionFromDiscretePointsTest, Error, piecewise curve is not C0"<<std::endl;
- error = true;
- }
- ComparePoints(p0, ppc(T_min), errMsg, error);
- ComparePoints(p_test_0_5, ppc(T_min+timestep/2.0), errMsg, error);
- ComparePoints(p1, ppc(T_min+timestep), errMsg, error);
- ComparePoints(p2, ppc(T_min+2*timestep), errMsg, error);
- ComparePoints(p3, ppc(T_max), errMsg, error);
- }
- catch(...)
- {
+ std::string errMsg("piecewiseCurveConversionFromDiscretePointsTest, Error, value on curve is wrong : ");
+ point_t p0(0.,0.,0.);
+ point_t p1(1.,2.,3.);
+ point_t p2(4.,4.,4.);
+ point_t p3(10.,10.,10.);
+ point_t p_test_0_5 = (p0+p1)/2.0;
+ t_pointX_t points;
+ points.push_back(p0);
+ points.push_back(p1);
+ points.push_back(p2);
+ points.push_back(p3);
+ double T_min = 1.0;
+ double T_max = 3.0;
+ double timestep = (T_max-T_min)/double(points.size()-1);
+ std::vector<double> time_points;
+ for(size_t i=0;i<points.size();++i)
+ time_points.push_back(T_min+i*timestep);
+ piecewise_polynomial_curve_t ppc = piecewise_polynomial_curve_t::
+ convert_discrete_points_to_polynomial<polynomial_t>(points,time_points);
+ if (!ppc.is_continuous(0))
+ {
+ std::cout<<"piecewiseCurveConversionFromDiscretePointsTest, Error, piecewise curve is not C0"<<std::endl;
error = true;
std::cout<<"Error in piecewiseCurveConversionFromDiscretePointsTest"<<std::endl;
}
+
+ ComparePoints(p0, ppc(T_min), errMsg, error);
+ ComparePoints(p_test_0_5, ppc(T_min+timestep/2.0), errMsg, error);
+ ComparePoints(p1, ppc(T_min+timestep), errMsg, error);
+ ComparePoints(p2, ppc(T_min+2*timestep), errMsg, error);
+ ComparePoints(p3, ppc(T_max), errMsg, error);
+ //TODO : test with C1 and C2
}
void serializationCurvesTest(bool& error)
@@ -1412,7 +1452,7 @@ void serializationCurvesTest(bool& error)
point_t b(2,1,1); // in [1,2[
point_t c(3,1,1); // in [2,3]
point_t res;
- t_point_t vec1, vec2, vec3;
+ t_pointX_t vec1, vec2, vec3;
vec1.push_back(a); // x=1, y=1, z=1
vec2.push_back(b); // x=2, y=1, z=1
vec3.push_back(c); // x=3, y=1, z=1
@@ -1507,6 +1547,131 @@ void serializationCurvesTest(bool& error)
}
}
+void polynomialFromBoundaryConditions(bool& error){
+ pointX_t zeros = point_t(0.,0.,0.);
+ pointX_t p0 = point_t(0.,1.,0.);
+ pointX_t p1 = point_t(1.,2.,-3.);
+ pointX_t dp0 = point_t(-8.,4.,6.);
+ pointX_t dp1 = point_t(10.,-10.,10.);
+ pointX_t ddp0 = point_t(-1.,7.,4.);
+ pointX_t ddp1 = point_t(12.,-8.,2.5);
+ double min = 0.5;
+ double max = 2.;
+ // C0 : order 1
+ polynomial_t polC0 = polynomial_t(p0,p1,min,max);
+ if(polC0.min() != min){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C0: min interval not respected."<<std::endl;
+ }
+ if(polC0.max() != max){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C0: max interval not respected."<<std::endl;
+ }
+ if(polC0(min) != p0){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C0: initial value not respected"<<std::endl;
+ }
+ if(polC0(max) != p1){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C0: final value not respected"<<std::endl;
+ }
+ if(polC0.degree_ != 1){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C0: curve is not degree 1 "<<std::endl;
+ }
+ if(polC0((max+min)/2.) != (p0*0.5 + p1*0.5)){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C0: middle point doesn't have the right value' "<<std::endl;
+ }
+ //C1 : order 3
+ polynomial_t polC1 = polynomial_t(p0,dp0,p1,dp1,min,max);
+ if(polC1.min() != min){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: min interval not respected."<<std::endl;
+ }
+ if(polC1.max() != max){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: max interval not respected."<<std::endl;
+ }
+ if(polC1(min) != p0){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: initial value not respected"<<std::endl;
+ }
+ if(!QuasiEqual(polC1(max),p1)){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: final value not respected"<<std::endl;
+ std::cout<<"p1 = "<<p1.transpose()<< " curve end = "<<polC1(max).transpose()<<std::endl;
+ }
+ if(polC1.derivate(min,1) != dp0){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: initial derivative value not respected"<<std::endl;
+ }
+ if(!QuasiEqual(polC1.derivate(max,1), dp1)){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: final derivative value not respected"<<std::endl;
+ std::cout<<"dp1 = "<<dp1.transpose()<< " curve end derivative = "<<polC1.derivate(max,1).transpose()<<std::endl;
+ }
+ if(polC1.degree_ != 3){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C1: curve is not degree 3 "<<std::endl;
+ }
+ //C2 : order 5
+ polynomial_t polC2 = polynomial_t(p0,dp0,ddp0,p1,dp1,ddp1,min,max);
+ if(polC2.min() != min){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: min interval not respected."<<std::endl;
+ }
+ if(polC2.max() != max){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: max interval not respected."<<std::endl;
+ }
+ if(polC2(min) != p0){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: initial value not respected"<<std::endl;
+ }
+ if(!QuasiEqual(polC2(max),p1)){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: final value not respected"<<std::endl;
+ }
+ if(polC2.derivate(min,1) != dp0){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: initial derivative value not respected"<<std::endl;
+ }
+ if(!QuasiEqual(polC2.derivate(max,1), dp1)){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: final derivative value not respected"<<std::endl;
+ }
+ if(polC2.derivate(min,2) != ddp0){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: initial second derivative value not respected"<<std::endl;
+ }
+ if(!QuasiEqual(polC2.derivate(max,2), ddp1)){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: final second derivative value not respected"<<std::endl;
+ }
+ if(polC2.degree_ != 5){
+ error=true;
+ std::cout<<"polynomialFromBoundaryConditions C2: curve is not degree 5 "<<std::endl;
+ }
+ // check if the exeptions are correctly raised :
+ try{
+ polynomial_t polC0Err = polynomial_t(p0,p1,max,min);
+ error = true;
+ std::cout<<"Created a polynomial with tMin > tMax without error. "<<std::endl;
+ }catch(invalid_argument e){}
+ try{
+ polynomial_t polC1Err = polynomial_t(p0,dp0,p1,dp1,max,min);
+ error = true;
+ std::cout<<"Created a polynomial with tMin > tMax without error. "<<std::endl;
+ }catch(invalid_argument e){}
+ try{
+ polynomial_t polC2Err = polynomial_t(p0,dp0,ddp0,p1,dp1,ddp1,max,min);
+ error = true;
+ std::cout<<"Created a polynomial with tMin > tMax without error. "<<std::endl;
+ }catch(invalid_argument e){}
+}
+
+
int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
@@ -1536,6 +1701,7 @@ int main(int /*argc*/, char** /*argv[]*/)
cubicConversionTest(error);
curveAbcDimDynamicTest(error);
serializationCurvesTest(error);
+ polynomialFromBoundaryConditions(error);
if(error)
{
std::cout << "There were some errors\n";