Commit a976e461 authored by t steve's avatar t steve
Browse files

Merge branch 'packaging'

parents d270a50a 950e634c
...@@ -46,21 +46,60 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -46,21 +46,60 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
/// ///
template<typename In> template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1) bezier_curve(In PointsBegin, In PointsEnd)
: minBound_(minBound) : T_(1.)
, maxBound_(maxBound) , mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)) , size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1) , degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_)) , bernstein_(spline::makeBernstein<num_t>(degree_))
{ {
assert(bernstein_.size() == size_); assert(bernstein_.size() == size_);
In it(PointsBegin); In it(PointsBegin);
if(Safe && (size_<1 || minBound >= maxBound)) 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"); // TODO
for(; it != PointsEnd; ++it) for(; it != PointsEnd; ++it)
pts_.push_back(*it); pts_.push_back(*it);
} }
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T)
: T_(T)
, mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
assert(bernstein_.size() == size_);
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
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor
///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
///
template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const time_t T, const time_t mult_T)
: T_(T)
, mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_))
{
assert(bernstein_.size() == size_);
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
for(; it != PointsEnd; ++it)
pts_.push_back(*it);
}
///\brief Constructor ///\brief Constructor
/// This constructor will add 4 points (2 after the first one, 2 before the last one) /// This constructor will add 4 points (2 after the first one, 2 before the last one)
...@@ -69,14 +108,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -69,14 +108,14 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param constraints : constraints applying on start / end velocities and acceleration ///\param constraints : constraints applying on start / end velocities and acceleration
/// ///
template<typename In> template<typename In>
bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound=0, const time_t maxBound=1) bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t T=1.)
: minBound_(minBound) : T_(T)
, maxBound_(maxBound) , mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)+4) , size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1) , degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>(degree_)) , bernstein_(spline::makeBernstein<num_t>(degree_))
{ {
if(Safe && (size_<1 || minBound >= maxBound)) if(Safe && (size_<1 || T_ <= 0.))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); throw std::out_of_range("can't create bezier min bound is higher than max bound");
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints); t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit) for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
...@@ -101,7 +140,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -101,7 +140,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t) /// \param return : the value x(t)
virtual point_t operator()(const time_t t) const virtual point_t operator()(const time_t t) const
{ {
num_t nT = (t - minBound_) / (maxBound_ - minBound_); num_t nT = t / T_;
if(Safe &! (0 <= nT && nT <= 1)) if(Safe &! (0 <= nT && nT <= 1))
{ {
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
...@@ -112,22 +151,22 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -112,22 +151,22 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
switch(size_) switch(size_)
{ {
case 1 : case 1 :
return pts_[0]; return mult_T_ * pts_[0];
case 2 : case 2 :
return pts_[0] * dt + nT * pts_[1]; return mult_T_ * (pts_[0] * dt + nT * pts_[1]);
break; break;
case 3 : case 3 :
return pts_[0] * dt * dt return mult_T_ * (pts_[0] * dt * dt
+ 2 * pts_[1] * nT * dt + 2 * pts_[1] * nT * dt
+ pts_[2] * nT * nT; + pts_[2] * nT * nT);
break; break;
case 4 : case 4 :
return pts_[0] * dt * dt * dt return mult_T_ * (pts_[0] * dt * dt * dt
+ 3 * pts_[1] * nT * dt * dt + 3 * pts_[1] * nT * dt * dt
+ 3 * pts_[2] * nT * nT * dt + 3 * pts_[2] * nT * nT * dt
+ pts_[3] * nT * nT *nT; + pts_[3] * nT * nT *nT);
default : default :
return evalHorner(nT); return mult_T_ * evalHorner(nT);
break; break;
} }
} }
...@@ -144,7 +183,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -144,7 +183,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
derived_wp.push_back(degree_ * (*(pit+1) - (*pit))); derived_wp.push_back(degree_ * (*(pit+1) - (*pit)));
if(derived_wp.empty()) if(derived_wp.empty())
derived_wp.push_back(point_t::Zero()); derived_wp.push_back(point_t::Zero());
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_); bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),T_, mult_T_ * (1./T_) );
return deriv.compute_derivate(order-1); return deriv.compute_derivate(order-1);
} }
...@@ -165,7 +204,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -165,7 +204,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
current_sum += *pit; current_sum += *pit;
n_wp.push_back(current_sum / new_degree); n_wp.push_back(current_sum / new_degree);
} }
bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_); bezier_curve_t integ(n_wp.begin(), n_wp.end(),T_, mult_T_*T_);
return integ.compute_primitive(order-1); return integ.compute_primitive(order-1);
} }
...@@ -183,6 +222,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -183,6 +222,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// ///
/// \brief Evaluates all Bernstein polynomes for a certain degree /// \brief Evaluates all Bernstein polynomes for a certain degree
/// Warning: the horner scheme is about 100 times faster than this method.
/// This method will probably be removed in the future
/// ///
point_t evalBernstein(const Numeric u) const point_t evalBernstein(const Numeric u) const
{ {
...@@ -246,15 +287,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -246,15 +287,16 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/ /*Helpers*/
public: public:
virtual time_t min() const{return minBound_;} virtual time_t min() const{return 0.;}
virtual time_t max() const{return maxBound_;} virtual time_t max() const{return T_;}
/*Helpers*/ /*Helpers*/
public: public:
const time_t minBound_, maxBound_; /*const*/ time_t T_;
const std::size_t size_; /*const*/ time_t mult_T_;
const std::size_t degree_; /*const*/ std::size_t size_;
const std::vector<Bern<Numeric> > bernstein_; /*const*/ std::size_t degree_;
/*const*/ std::vector<Bern<Numeric> > bernstein_;
private: private:
t_point_t pts_; t_point_t pts_;
......
/**
* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
*/
#ifndef _BEZIER_POLY_CONVERSION
#define _BEZIER_POLY_CONVERSION
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept>
#include <iostream>
namespace spline
{
/// \brief Provides methods for converting a curve from a bernstein representation
/// to a polynom representation
///
///
///
///\brief Converts a Bezier curve to a polynom
///\param bezier: the Bezier curve to be converted from
///\return the equivalent polynom
template<typename Bezier, typename Polynom>
Polynom from_bezier(const Bezier& curve)
{
typedef typename Polynom::t_point_t t_point_t;
typedef typename Polynom::num_t num_t;
assert (curve.min() == 0.);
assert (curve.max() == 1.);
t_point_t coefficients;
Bezier current (curve);
coefficients.push_back(curve(0.));
num_t fact = 1;
for(std::size_t i = 1; i<= curve.degree_; ++i)
{
current = current.compute_derivate(1);
fact *= i;
coefficients.push_back(current(0.)/fact);
}
return Polynom(coefficients,curve.min(),curve.max());
}
///\brief Converts a polynom to a Bezier curve
///\param polynom: the polynom to be converted from
///\return the equivalent Bezier curve
/*template<typename Bezier, typename Polynom>
Bezier from_polynom(const Polynom& polynom)
{
typedef Bezier::point_t point_t;
typedef Bezier::time_t time_t;
typedef Bezier::num_t num_t;
typedef Bezier::curve_constraints_t curve_constraints_t;
typedef Bezier::t_point_t t_point_t;
typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t;
}*/
}
#endif //_BEZIER_POLY_CONVERSION
...@@ -35,6 +35,7 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S ...@@ -35,6 +35,7 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point> struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
{ {
typedef Point point_t; typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t; typedef Time time_t;
typedef Numeric num_t; typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t; typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
......
...@@ -3,6 +3,9 @@ ...@@ -3,6 +3,9 @@
#include "spline/exact_cubic.h" #include "spline/exact_cubic.h"
#include "spline/spline_deriv_constraint.h" #include "spline/spline_deriv_constraint.h"
#include "spline/curve_constraint.h" #include "spline/curve_constraint.h"
#include "spline/bezier_polynom_conversion.h"
#include "spline/bernstein.h"
#include <vector> #include <vector>
...@@ -35,12 +38,15 @@ typedef polynom_t::coeff_t coeff_t; ...@@ -35,12 +38,15 @@ typedef polynom_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t; typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t; typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t;
typedef spline::Bern<double> bernstein_t;
typedef spline::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t; typedef spline::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t;
typedef spline::curve_constraints<point_t> curve_constraints_t; typedef spline::curve_constraints<point_t> curve_constraints_t;
typedef spline::curve_constraints<point6_t> curve_constraints6_t; typedef spline::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/ /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t)
...@@ -61,17 +67,17 @@ T_Point vectorFromEigenArray(const PointList& array) ...@@ -61,17 +67,17 @@ T_Point vectorFromEigenArray(const PointList& array)
} }
template <typename Bezier, typename PointList, typename T_Point> template <typename Bezier, typename PointList, typename T_Point>
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb = 0., const real ub =1.) Bezier* wrapBezierConstructorTemplate(const PointList& array, const real ub =1.)
{ {
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array); T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), lb, ub); return new Bezier(asVector.begin(), asVector.end(), ub);
} }
template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints> template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real lb = 0., const real ub =1.) Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real ub =1.)
{ {
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array); T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub); return new Bezier(asVector.begin(), asVector.end(), constraints, ub);
} }
/*3D constructors */ /*3D constructors */
...@@ -79,17 +85,17 @@ bezier_t* wrapBezierConstructor(const point_list_t& array) ...@@ -79,17 +85,17 @@ bezier_t* wrapBezierConstructor(const point_list_t& array)
{ {
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array) ; return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array) ;
} }
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb, const real ub) bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real ub)
{ {
return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, lb, ub) ; return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, ub) ;
} }
bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints) bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints)
{ {
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ; return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
} }
bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real lb, const real ub) bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real ub)
{ {
return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, lb, ub) ; return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, ub) ;
} }
/*END 3D constructors */ /*END 3D constructors */
/*6D constructors */ /*6D constructors */
...@@ -97,17 +103,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array) ...@@ -97,17 +103,17 @@ bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{ {
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ; return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ;
} }
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real lb, const real ub) bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real ub)
{ {
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, lb, ub) ; return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, ub) ;
} }
bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints) bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
{ {
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints) ; return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints) ;
} }
bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real lb, const real ub) bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real ub)
{ {
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, lb, ub) ; return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, ub) ;
} }
/*END 6D constructors */ /*END 6D constructors */
...@@ -300,6 +306,17 @@ BOOST_PYTHON_MODULE(spline) ...@@ -300,6 +306,17 @@ BOOST_PYTHON_MODULE(spline)
; ;
/** END spline_deriv_constraints**/ /** END spline_deriv_constraints**/
/** BEGIN bernstein polynom**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
.def("__call__", &bernstein_t::operator())
;
/** END bernstein polynom**/
/** BEGIN Bezier to polynom conversion**/
def("from_bezier", from_bezier<bezier_t,polynom_t>);
/** END Bezier to polynom conversion**/
} }
......
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
from numpy import matrix from numpy import matrix
from numpy.linalg import norm from numpy.linalg import norm
__EPS = 1e-6
waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose() waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose() waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose()
time_waypoints = matrix([0.,1.]) time_waypoints = matrix([0.,1.])
#testing bezier curve #testing bezier curve
a = bezier6(waypoints6) a = bezier6(waypoints6)
a = bezier(waypoints, -1., 3.) a = bezier(waypoints, 3.)
assert(a.degree == a.nbWaypoints -1) assert(a.degree == a.nbWaypoints -1)
a.min() a.min()
...@@ -21,6 +23,7 @@ a = a.compute_derivate(100) ...@@ -21,6 +23,7 @@ a = a.compute_derivate(100)
prim = a.compute_primitive(1) prim = a.compute_primitive(1)
for i in range(10): for i in range(10):
t = float(i) / 10. t = float(i) / 10.
assert(a(t) == prim.derivate(t,1)).all() assert(a(t) == prim.derivate(t,1)).all()
...@@ -32,6 +35,20 @@ for i in range(10): ...@@ -32,6 +35,20 @@ for i in range(10):
assert(a(t) == prim.derivate(t,2)).all() assert(a(t) == prim.derivate(t,2)).all()
assert(prim(0) == matrix([0.,0.,0.])).all() assert(prim(0) == matrix([0.,0.,0.])).all()
waypoints = matrix([[1.,2.,3.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.],[4.,5.,6.]]).transpose()
a0 = bezier(waypoints)
a1 = bezier(waypoints, 3.)
prim0 = a0.compute_primitive(1)
prim1 = a1.compute_primitive(1)
for i in range(10):
t = float(i) / 10.
assert norm(a0(t) - a1(3*t)) < __EPS
assert norm(a0.derivate(t,1) - a1.derivate(3*t,1) * 3.) < __EPS
assert norm(a0.derivate(t,2) - a1.derivate(3*t,2) * 9.) < __EPS
assert norm(prim0(t) - prim1(t*3) / 3.) < __EPS
assert(prim(0) == matrix([0.,0.,0.])).all()
#testing bezier with constraints #testing bezier with constraints
c = curve_constraints(); c = curve_constraints();
...@@ -40,7 +57,7 @@ c.end_vel = matrix([0.,1.,1.]); ...@@ -40,7 +57,7 @@ c.end_vel = matrix([0.,1.,1.]);
c.init_acc = matrix([0.,1.,-1.]); c.init_acc = matrix([0.,1.,-1.]);
c.end_acc = matrix([0.,100.,1.]); c.end_acc = matrix([0.,100.,1.]);
waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
a = bezier(waypoints,c) a = bezier(waypoints,c)
assert norm(a.derivate(0,1) - c.init_vel) < 1e-10 assert norm(a.derivate(0,1) - c.init_vel) < 1e-10
assert norm(a.derivate(1,2) - c.end_acc) < 1e-10 assert norm(a.derivate(1,2) - c.end_acc) < 1e-10
...@@ -78,3 +95,9 @@ c.end_acc = matrix([0.,1.,1.]); ...@@ -78,3 +95,9 @@ c.end_acc = matrix([0.,1.,1.]);
a = spline_deriv_constraint (waypoints, time_waypoints) a = spline_deriv_constraint (waypoints, time_waypoints)
a = spline_deriv_constraint (waypoints, time_waypoints, c) a = spline_deriv_constraint (waypoints, time_waypoints, c)
#converting bezier to polynom
a = bezier(waypoints)
a_pol = from_bezier(a)
assert norm(a(0.3) - a_pol(0.3)) < __EPS
...@@ -5,6 +5,7 @@ ...@@ -5,6 +5,7 @@
#include "spline/spline_deriv_constraint.h" #include "spline/spline_deriv_constraint.h"
#include "spline/helpers/effector_spline.h" #include "spline/helpers/effector_spline.h"
#include "spline/helpers/effector_spline_rotation.h" #include "spline/helpers/effector_spline_rotation.h"
#include "spline/bezier_polynom_conversion.h"
#include <string> #include <string>
#include <iostream> #include <iostream>
...@@ -174,9 +175,7 @@ void BezierCurveTest(bool& error) ...@@ -174,9 +175,7 @@ void BezierCurveTest(bool& error)
//4d curve //4d curve
params.push_back(d); params.push_back(d);
bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2); bezier_curve_t cf4(params.begin(), params.end(), 2);
res1 = cf4(0.4);
ComparePoints(a, res1, errMsg + "3(0) ", error);
res1 = cf4(2); res1 = cf4(2);
ComparePoints(d, res1, errMsg + "3(1) ", error); ComparePoints(d, res1, errMsg + "3(1) ", error);
...@@ -344,6 +343,32 @@ void BezierDerivativeCurveTest(bool& error) ...@@ -344,6 +343,32 @@ void BezierDerivativeCurveTest(bool& error)
ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error); ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
} }