Commit 67acc1d4 authored by JasonChmn's avatar JasonChmn
Browse files

Add conversion cubic hermite to cubic bezier,test ok

parent 3bb91b85
SET(${PROJECT_NAME}_HEADERS
bernstein.h
bezier_polynomial_conversion.h
curve_conversion.h
curve_abc.h
exact_cubic.h
MathDefs.h
polynomial.h
spline_deriv_constraint.h
bezier_curve.h
cubic_spline.h
curve_constraint.h
......
......@@ -50,10 +50,11 @@ Polynomial from_bezier(const Bezier& curve)
return Polynomial(coefficients,curve.min(),curve.max());
}
///\brief Converts a polynomial to a Bezier curve.
///\param polynomial : the polynomial to convert.
///\return the equivalent Bezier curve.
/*template<typename Bezier, typename Polynomial>
/*
/// \brief Converts a polynomial to a Bezier curve.
/// \param polynomial : the polynomial to convert.
/// \return the equivalent Bezier curve.
template<typename Bezier, typename Polynomial>
Bezier from_polynomial(const Polynomial& polynomial)
{
typedef Bezier::point_t point_t;
......@@ -63,7 +64,8 @@ Bezier from_polynomial(const Polynomial& polynomial)
typedef Bezier::t_point_t t_point_t;
typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t;
}*/
}
*/
} // namespace curves
#endif //_BEZIER_POLY_CONVERSION
/**
* \file cubic_hermite_spline.h
* \brief class allowing to create a cubic hermite spline of any dimension.
* \author Justin Carpentier <jcarpent@laas.fr> modified by Jason Chemin <jchemin@laas.fr>
* \date 05/2019
*/
#ifndef _CLASS_CUBICHERMITESPLINE
#define _CLASS_CUBICHERMITESPLINE
......@@ -28,25 +35,25 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
>
struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef std::pair<Point, Tangent> Pair_point_tangent;
typedef std::vector< Pair_point_tangent ,Eigen::aligned_allocator<Point> > Vector_pair;
typedef std::vector<Time> Vector_time;
typedef std::pair<Point, Tangent> pair_point_tangent_t;
typedef std::vector< pair_point_tangent_t ,Eigen::aligned_allocator<Point> > t_pair_point_tangent_t;
typedef std::vector<Time> vector_time_t;
typedef int Index;
/*Attributes*/
public:
/// Vector of pair < Point, Tangent >.
Vector_pair control_points_;
t_pair_point_tangent_t control_points_;
/// Vector of Time corresponding to time of each N control points : time at \f$P_0, P_1, P_2, ..., P_N\f$.
/// Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.
Vector_time time_control_points_;
vector_time_t time_control_points_;
private:
/// Vector of Time corresponding to time duration of each subspline.<br>
/// For N control points with time \f$T_{P_0}, T_{P_1}, T_{P_2}, ..., T_{P_N}\f$ respectively,
/// duration of each subspline is : ( T_{P_1}-T_{P_0}, T_{P_2}-T_{P_1}, ..., T_{P_N}-T_{P_{N-1} )<br>
/// It contains \f$N-1\f$ durations.
Vector_time duration_splines_;
vector_time_t duration_splines_;
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
Time T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
......@@ -62,7 +69,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param time_control_points : vector containing time for each waypoint.
///
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd, const Vector_time & time_control_points)
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
{
// Check size of pairs container.
std::size_t const size(std::distance(PairsBegin, PairsEnd));
......@@ -120,7 +127,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
/// \param time_control_points : Vector containing time for each control point.
///
void setTimeSplines(const Vector_time & time_control_points)
void setTimeSplines(const vector_time_t & time_control_points)
{
time_control_points_ = time_control_points;
T_min_ = time_control_points_.front();
......@@ -162,7 +169,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Get vector of pair (positition, derivative) corresponding to control points.
/// \return vector containing control points.
///
Vector_pair getControlPoints()
t_pair_point_tangent_t getControlPoints()
{
return control_points_;
}
......@@ -170,7 +177,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \brief Get vector of Time corresponding to Time for each control point.
/// \return vector containing time of each control point.
///
Vector_time getTimeSplines()
vector_time_t getTimeSplines()
{
return time_control_points_;
}
......@@ -217,8 +224,8 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
return control_points_.back().first*0.; // To modify, create a new Tangent ininitialized with 0.
}
}
const Pair_point_tangent Pair0 = control_points_.at(id);
const Pair_point_tangent Pair1 = control_points_.at(id+1);
const pair_point_tangent_t Pair0 = control_points_.at(id);
const pair_point_tangent_t Pair1 = control_points_.at(id+1);
const Time & t0 = time_control_points_[id];
const Time & t1 = time_control_points_[id+1];
// Polynom for a cubic hermite spline defined on [0., 1.] is :
......
#ifndef _CLASS_CURVE_CONVERSION
#define _CLASS_CURVE_CONVERSION
#include "curve_abc.h"
#include "bernstein.h"
#include "curve_constraint.h"
#include "MathDefs.h"
#include <vector>
#include <stdexcept>
#include <iostream>
namespace curves
{
/// \brief Converts a Bezier curve to a polynomial.
/// \param curve : the Bezier curve defined between [0,1] to convert.
/// \return the equivalent polynomial.
template<typename Bezier, typename Polynomial>
Polynomial polynom_from_bezier(const Bezier& curve)
{
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::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 *= (num_t)i;
coefficients.push_back(current(0.)/fact);
}
return Polynomial(coefficients,curve.min(),curve.max());
}
/// \brief Converts a Cubic Hermite curve to a cubic bezier.
/// \param curve : the cubic hermite curve defined between [0,1] to convert.
/// \return the equivalent cubic bezier curve.
template<typename Hermite, typename Bezier>
Bezier bezier_from_hermite(const Hermite& curve)
{
typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t;
typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t;
assert (curve.min() == 0.);
assert (curve.max() == 1.);
Hermite current (curve);
assert(current.control_points_.size() >= 2);
pair_point_tangent_t pair0 = current.control_points_.at(0);
pair_point_tangent_t pair1 = current.control_points_.at(1);
// Positions/Velocities of hermite curve
point_t h_p0 = pair0.first;
point_t h_m0 = pair0.second;
point_t h_p1 = pair1.first;
point_t h_m1 = pair1.second;
// Convert to bezier control points
// for t in [0,1] : x'(0)=3(b_p1-b_p0) and x'(1)=3(b_p3-b_p2)
// so : h_m0=3(b_p1-b_p0) and h_m1=3(b_p3-b_p2)
// <=> b_p1=(h_m0/3)+b_p0 and b_p2=(h_m1/3)+b_p3
point_t b_p0 = h_p0;
point_t b_p3 = h_p1;
point_t b_p1 = (h_m0/3)+b_p0;
point_t b_p2 = -(h_m1/3)+b_p3;
t_point_t control_points;
control_points.push_back(b_p0);
control_points.push_back(b_p1);
control_points.push_back(b_p2);
control_points.push_back(b_p3);
return Bezier(control_points.begin(), control_points.end());
}
} // namespace curve
#endif //_CLASS_CURVE_CONVERSION
\ No newline at end of file
......@@ -23,6 +23,7 @@
#include "curve_abc.h"
#include "cubic_spline.h"
#include "quintic_spline.h"
#include "curve_constraint.h"
#include "MathDefs.h"
......@@ -43,6 +44,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, 3, 3> Matrix3;
typedef Time time_t;
typedef Numeric num_t;
typedef SplineBase spline_t;
......@@ -50,6 +52,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
typedef curve_constraints<point_t> spline_constraints;
/* Constructors - destructors */
public:
......@@ -61,6 +64,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
/// \param constraints : constraints on the init and end velocity / accelerations of the spline.
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
/// \brief Constructor.
/// \param subSplines: vector of subsplines.
......@@ -159,6 +170,71 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
return subSplines;
}
template<typename In>
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
throw std::length_error("number of waypoints should be superior to one"); // TODO
}
t_spline_t subSplines;
subSplines.reserve(size-1);
spline_constraints cons = constraints;
In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd-1);
++next;
for(std::size_t i(0); next != end; ++next, ++it, ++i)
{
compute_one_spline<In>(it, next, cons, subSplines);
}
compute_end_spline<In>(it, next,cons, subSplines);
return subSplines;
}
template<typename In>
void compute_one_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
{
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
const point_t& b0 = constraints.init_vel , c0 = constraints.init_acc / 2.;
const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
subSplines.push_back(create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>
(a0,b0,c0,d0,init_t, end_t));
constraints.init_vel = subSplines.back().derivate(end_t, 1);
constraints.init_acc = subSplines.back().derivate(end_t, 2);
}
template<typename In>
void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
{
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
//solving a system of four linear eq with 4 unknows: d0, e0
const point_t alpha_0 = a1 - a0 - b0 *dt - c0 * dt_2;
const point_t alpha_1 = b1 - b0 - 2 *c0 * dt;
const point_t alpha_2 = c1 - 2 *c0;
const num_t x_d_0 = dt_3, x_d_1 = 3*dt_2, x_d_2 = 6*dt;
const num_t x_e_0 = dt_4, x_e_1 = 4*dt_3, x_e_2 = 12*dt_2;
const num_t x_f_0 = dt_5, x_f_1 = 5*dt_4, x_f_2 = 20*dt_3;
point_t d, e, f;
MatrixX rhs = MatrixX::Zero(3,Dim);
rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
Matrix3 eq = Matrix3::Zero(3,3);
eq(0,0) = x_d_0; eq(0,1) = x_e_0; eq(0,2) = x_f_0;
eq(1,0) = x_d_1; eq(1,1) = x_e_1; eq(1,2) = x_f_1;
eq(2,0) = x_d_2; eq(2,1) = x_e_2; eq(2,2) = x_f_2;
rhs = eq.inverse().eval() * rhs;
d = rhs.row(0); e = rhs.row(1); f = rhs.row(2);
subSplines.push_back(create_quintic<Time,Numeric,Dim,Safe,Point,T_Point>
(a0,b0,c0,d,e,f, init_t, end_t));
}
private:
//exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
......
#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE
#include "curve_abc.h"
#include "cubic_hermite_spline.h"
#include "bezier_curve"
#include "polynomial.h"
namespace curves
{
/// \class PiecewiseCurve.
///
///
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 piecewise_curve
{
}
} // end namespace
#endif // _CLASS_PIECEWISE_CURVE
\ No newline at end of file
#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
#include "curves/exact_cubic.h"
#include "curves/spline_deriv_constraint.h"
#include "curves/curve_constraint.h"
#include "curves/bezier_polynomial_conversion.h"
#include "curves/curve_conversion.h"
#include "curves/bernstein.h"
#include "curves/cubic_hermite_spline.h"
......@@ -48,8 +47,6 @@ typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t
typedef curves::Bern<double> bernstein_t;
typedef curves::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t;
typedef curves::curve_constraints<point_t> curve_constraints_t;
typedef curves::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
......@@ -61,7 +58,6 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace curves
{
......@@ -185,19 +181,11 @@ exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoi
t_waypoint_t wps = getWayPoints(array, time_wp);
return new exact_cubic_t(wps.begin(), wps.end());
}
/* End wrap exact cubic spline */
/* Wrap deriv constraint */
spline_deriv_constraint_t* wrapSplineDerivConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
return new spline_deriv_constraint_t(wps.begin(), wps.end(),constraints);
}
spline_deriv_constraint_t* wrapSplineDerivConstraintNoConstraints(const coeff_t& array, const time_waypoints_t& time_wp)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
return new spline_deriv_constraint_t(wps.begin(), wps.end());
return new exact_cubic_t(wps.begin(), wps.end(), constraints);
}
point_t get_init_vel(const curve_constraints_t& c)
......@@ -239,7 +227,7 @@ void set_end_acc(curve_constraints_t& c, const point_t& val)
{
c.end_acc = val;
}
/* End wrap deriv constraint */
/* End wrap exact cubic spline */
BOOST_PYTHON_MODULE(curves)
......@@ -343,6 +331,7 @@ BOOST_PYTHON_MODULE(curves)
class_<exact_cubic_t>
("exact_cubic", no_init)
.def("__init__", make_constructor(&wrapExactCubicConstructor))
.def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
.def("min", &exact_cubic_t::min)
.def("max", &exact_cubic_t::max)
.def("__call__", &exact_cubic_t::operator())
......@@ -373,19 +362,6 @@ BOOST_PYTHON_MODULE(curves)
;
/** END curve constraints**/
/** BEGIN spline_deriv_constraints**/
class_<spline_deriv_constraint_t>
("spline_deriv_constraint", no_init)
.def("__init__", make_constructor(&wrapSplineDerivConstraint))
.def("__init__", make_constructor(&wrapSplineDerivConstraintNoConstraints))
.def("min", &exact_cubic_t::min)
.def("max", &exact_cubic_t::max)
.def("__call__", &exact_cubic_t::operator())
.def("derivate", &exact_cubic_t::derivate)
;
/** END spline_deriv_constraints**/
/** BEGIN bernstein polynomial**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
......@@ -394,7 +370,7 @@ BOOST_PYTHON_MODULE(curves)
/** END bernstein polynomial**/
/** BEGIN Bezier to polynomial conversion**/
def("from_bezier", from_bezier<bezier3_t,polynomial_t>);
def("polynom_from_bezier", polynom_from_bezier<bezier3_t,polynomial_t>);
/** END Bezier to polynomial conversion**/
......
......@@ -9,7 +9,7 @@ from numpy.linalg import norm
import unittest
from curves import bezier3, bezier6, curve_constraints, exact_cubic, cubic_hermite_spline, from_bezier, polynomial, spline_deriv_constraint
from curves import bezier3, bezier6, curve_constraints, exact_cubic, cubic_hermite_spline, polynom_from_bezier, polynomial
class TestCurves(unittest.TestCase):
......@@ -108,7 +108,7 @@ class TestCurves(unittest.TestCase):
a.derivate(0.4, 2)
return
def test_spline_deriv_constraint(self):
def test_exact_cubic_constraint(self):
# To test :
# - Functions : constructor, min, max, derivate
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
......@@ -122,16 +122,16 @@ class TestCurves(unittest.TestCase):
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., 1.]).transpose()
c.end_acc = matrix([0., 1., 1.]).transpose()
a = spline_deriv_constraint(waypoints, time_waypoints)
a = spline_deriv_constraint(waypoints, time_waypoints, c)
a = exact_cubic(waypoints, time_waypoints)
a = exact_cubic(waypoints, time_waypoints, c)
return
def test_from_bezier(self):
def test_polynom_from_bezier(self):
# converting bezier to polynomial
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier3(waypoints)
a_pol = from_bezier(a)
a_pol = polynom_from_bezier(a)
self.assertTrue (norm(a(0.3) - a_pol(0.3)) < __EPS)
return
......
......@@ -2,10 +2,9 @@
#include "curves/exact_cubic.h"
#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
#include "curves/spline_deriv_constraint.h"
#include "curves/helpers/effector_spline.h"
#include "curves/helpers/effector_spline_rotation.h"
#include "curves/bezier_polynomial_conversion.h"
#include "curves/curve_conversion.h"
#include "curves/cubic_hermite_spline.h"
#include <string>
......@@ -21,9 +20,8 @@ typedef Eigen::Vector3d tangent_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
......@@ -35,7 +33,8 @@ typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_spline_t;
typedef std::pair<point_t, tangent_t> Pair_point_tangent;
typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
bool QuasiEqual(const double a, const double b, const float margin)
{
......@@ -343,7 +342,6 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
std::cout << "time for bernstein eval " << double(e1 - s1) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for horner eval " << double(e2 - s2) / CLOCKS_PER_SEC << std::endl;
std::cout << "time for deCasteljau eval " << double(e3 - s3) / CLOCKS_PER_SEC << std::endl;
}
void BezierDerivativeCurveTest(bool& error)
......@@ -426,7 +424,7 @@ void BezierDerivativeCurveConstraintTest(bool& error)
void BezierToPolynomialConversionTest(bool& error)
{
std::string errMsg("In test BezierToPolynomialConversionTest ; unexpected result for x ");
std::string errMsg("In test BezierToPolynomialConversionTest ; unexpected result for x => ");
point_t a(1,2,3);
point_t b(2,3,4);
point_t c(3,4,5);
......@@ -437,19 +435,19 @@ void BezierToPolynomialConversionTest(bool& error)
point_t h(43,6,7);
point_t i(3,6,77);
std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
params.push_back(c);
params.push_back(d);
params.push_back(e);
params.push_back(f);
params.push_back(g);
params.push_back(h);
params.push_back(i);
bezier_curve_t cf(params.begin(), params.end());
polynomial_t pol =from_bezier<bezier_curve_t, polynomial_t>(cf);
std::vector<point_t> control_points;
control_points.push_back(a);
control_points.push_back(b);
control_points.push_back(c);
control_points.push_back(d);
control_points.push_back(e);
control_points.push_back(f);
control_points.push_back(g);
control_points.push_back(h);
control_points.push_back(i);
bezier_curve_t cf(control_points.begin(), control_points.end());
polynomial_t pol =polynom_from_bezier<bezier_curve_t, polynomial_t>(cf);
for(double i =0.; i<1.; i+=0.01)
{
ComparePoints(cf(i),pol(i),errMsg, error, true);
......@@ -457,6 +455,39 @@ void BezierToPolynomialConversionTest(bool& error)
}
}
void CubicHermiteToCubicBezierConversionTest(bool& error)
{
std::string errMsg("In test CubicHermiteToCubicBezierConversionTest ; unexpected result for x => ");
point_t p0(1,2,3);
point_t m0(2,3,4);
point_t p1(3,4,5);
point_t m1(3,6,7);
std::vector< double > time_control_points;
time_control_points.push_back(0.);
time_control_points.push_back(1.);
pair_point_tangent_t pair0(p0,m0);
pair_point_tangent_t pair1(p1,m1);
t_pair_point_tangent_t control_points;
control_points.push_back(pair0);
control_points.push_back(pair1);
cubic_hermite_spline_t ch(control_points.begin(), control_points.end(), time_control_points);
bezier_curve_t bc = bezier_from_hermite<cubic_hermite_spline_t, bezier_curve_t>(ch);
// Test derivative in t=0 and t=1
ComparePoints(ch.derivate(0.,1),bc.derivate(0.,1),errMsg, error, true);
ComparePoints(ch.derivate(1.,1),bc.derivate(1.,1),errMsg, error, true);
for(double i =0.; i<1.; i+=0.01)
{
ComparePoints(ch(i),bc(i),errMsg, error, true);
ComparePoints(ch(i),bc(i),errMsg, error, false);
}
}
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
......@@ -578,7 +609,11 @@ void ExactCubicVelocityConstraintsTest(bool& error)
}
std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
spline_constraints_t constraints;
spline_deriv_constraint_t exactCubic(waypoints.begin(), waypoints.end());