zero init velocity

include/spline/exact_cubic.h

@@ -41,7 +41,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 {
 	typedef Point 	point_t;
     typedef T_Point t_point_t;
-	typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+    typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
 	typedef Time 	time_t;
 	typedef Numeric	num_t;
     typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
@@ -57,9 +57,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	///\param wayPointsEns   : an iterator pointing to the end           of a waypoint container
 	template<typename In>
 	exact_cubic(In wayPointsBegin, In wayPointsEnd)
-        : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
-    {
-	}
+        : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
 
 	///\brief Destructor
     ~exact_cubic(){}

include/spline/exact_cubic_vel_acc_cons.h

@@ -39,7 +39,7 @@ namespace spline
 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> > >
-struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
+struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
 {
     typedef Point 	point_t;
     typedef T_Point t_point_t;
@@ -47,7 +47,7 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
     typedef Time 	time_t;
     typedef Numeric	num_t;
     typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
-    typedef exact_cubic<Time, Numeric, Dim, Safe, Point> exact_cubic_t;
+    typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
     typedef typename std::vector<spline_t> t_spline_t;
     typedef typename t_spline_t::iterator it_spline_t;
     typedef typename t_spline_t::const_iterator cit_spline_t;
@@ -59,18 +59,129 @@ struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Po
 	///\param wayPointsEns   : an iterator pointing to the end           of a waypoint container
 	template<typename In>
     cubic_zero_vel(In wayPointsBegin, In wayPointsEnd)
-        : exact_cubic_t(wayPointsBegin, wayPointsEnd)
-	{
-        // NOTHING
-	}
+        : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
 
 	///\brief Destructor
     ~cubic_zero_vel(){}
 
+    protected:
+    template<typename In>
+    t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
+    {
+        std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+        if(Safe && size < 1)
+        {
+            throw; // TODO
+        }
+        t_spline_t subSplines; subSplines.reserve(size);
+
+        // refer to the paper to understand all this.
+        MatrixX h1 = MatrixX::Zero(size, size);
+        MatrixX h2 = MatrixX::Zero(size, size);
+        MatrixX h3 = MatrixX::Zero(size, size);
+        MatrixX h4 = MatrixX::Zero(size, size);
+        MatrixX h5 = MatrixX::Zero(size, size);
+        MatrixX h6 = MatrixX::Zero(size, size);
+
+        MatrixX a =  MatrixX::Zero(size, Dim);
+        MatrixX b =  MatrixX::Zero(size, Dim);
+        MatrixX c =  MatrixX::Zero(size, Dim);
+        MatrixX d =  MatrixX::Zero(size, Dim);
+        MatrixX x =  MatrixX::Zero(size, Dim);
+
+
+        In it(wayPointsBegin), next(wayPointsBegin);
+        ++next;
+        for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
+        {
+            num_t const dTi((*next).first  - (*it).first);
+            num_t const dTi_sqr(dTi * dTi);
+            num_t const dTi_cube(dTi_sqr * dTi);
+            // filling matrices values
+            h3(i,i)   = -3 / dTi_sqr;
+            h3(i,i+1) =  3 / dTi_sqr;
+            h4(i,i)   = -2 / dTi;
+            h4(i,i+1) = -1 / dTi;
+            num_t coeff_h5(-2);
+            if(i == 0)
+            {
+                coeff_h5 = 1;
+            }
+            h5(i,i)   = -coeff_h5 / dTi_cube;
+            h5(i,i+1) =  coeff_h5 / dTi_cube;
+            h6(i,i)   =  1 / dTi_sqr;
+            h6(i,i+1) =  1 / dTi_sqr;
+            if( i+2 < size)
+            {
+                In it2(next); ++ it2;
+                num_t const dTi_1((*it2).first - (*next).first);
+                num_t const dTi_1sqr(dTi_1 * dTi_1);
+                // this can be optimized but let's focus on clarity as long as not needed
+                h1(i+1, i)   =  2 / dTi;
+                h1(i+1, i+1) =  4 / dTi + 4 / dTi_1;
+                h1(i+1, i+2) =  2 / dTi_1;
+                h2(i+1, i)   = -6 / dTi_sqr;
+                h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
+                h2(i+1, i+2) =  6 / dTi_1sqr;
+            }
+            x.row(i)= (*it).second.transpose();
+        }
+        // adding last x
+        x.row(size-1)= (*it).second.transpose();
+        a= x;
+        h1(0,0)= 1;
+        h3(0,0)= 0;h3(0,1)= 0;
+        h4(0,0)= 0;h4(0,1)= 0;
+        h6(0,0)= 0;h6(0,1)= 0;
+        PseudoInverse(h1);
+        b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+        c = h3 * x + h4 * b;
+        d = h5 * x + h6 * b;
+        it= wayPointsBegin, next=wayPointsBegin; ++ next;
+        for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
+        {
+            subSplines.push_back(
+                create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first));
+        }
+        subSplines.push_back(
+                create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
+        return subSplines;
+    }
+
+
 	private:
     cubic_zero_vel(const cubic_zero_vel&);
     cubic_zero_vel& operator=(const cubic_zero_vel&);
     /* Constructors - destructors */
+
+    /*Operations*/
+    public:
+    ///  \brief Evaluation of the cubic spline at time t.
+    ///  \param t : the time when to evaluate the spine
+    ///  \param return : the value x(t)
+    virtual point_t operator()(time_t t) const
+    {
+        if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
+        for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+        {
+            if(t >= (it->min()) && t <= (it->max()))
+            {
+                return it->operator()(t);
+            }
+        }
+    }
+    /*Operations*/
+
+    /*Helpers*/
+    public:
+    num_t virtual min() const{return subSplines_.front().min();}
+    num_t virtual max() const{return subSplines_.back().max();}
+    /*Helpers*/
+
+    /*Attributes*/
+    public:
+    const t_spline_t subSplines_;
+    /*Attributes*/
 };
 }
 #endif //_CLASS_CUBICZEROVELACC

include/spline/spline_curve.h

@@ -44,12 +44,12 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 /* Constructors - destructors */
     public:
     ///\brief Constructor
-    ///\param coefficients : a container containing all coefficients of the spline, starting
-    /// with the zero order coefficient, up to the highest order. Spline order is given
-    /// by the size of the coefficients
+    ///\param coefficients : a reference to an Eigen matrix where each column is a coefficient,
+    /// from the zero order coefficient, up to the highest order. Spline order is given
+    /// by the number of the columns -1.
     ///\param min: LOWER bound on interval definition of the spline
     ///\param max: UPPER bound on interval definition of the spline
-    spline_curve(const coeff_t_ref& coefficients, const time_t min, const time_t max)
+    spline_curve(const coeff_t_ref coefficients, const time_t min, const time_t max)
         : curve_abc_t(),
           coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
     {