### [polynomial] implement constructor from boundary condition (C0,C1 and C2)

parent ea0f5767
 ... ... @@ -101,6 +101,113 @@ 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_(Dim), degree_(1), T_min_(min), T_max_(max) { t_point_t coeffs; coeffs.push_back(init); coeffs.push_back((end-init)/(max-min)); coefficients_ = init_coeffs(coeffs.begin(), coeffs.end()); } /// /// \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_(Dim), degree_(3), T_min_(min), T_max_(max) { /* 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 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 m_inv = m.inverse(); Eigen::Matrix 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 = init[i]; bc = end[i]; bc = d_init[i]; bc = d_end[i]; coefficients_.row(i) = (m_inv*bc).transpose(); } } /// /// \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_(Dim), degree_(5), T_min_(min), T_max_(max) { /* 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 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 m_inv = m.inverse(); Eigen::Matrix 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 = init[i]; bc = end[i]; bc = d_init[i]; bc = d_end[i]; bc = dd_init[i]; bc = dd_end[i]; coefficients_.row(i) = (m_inv*bc).transpose(); } } /// \brief Destructor ~polynomial() { ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!