Commit abe6ce3d authored by JasonChmn's avatar JasonChmn
Browse files

Edit piecewise_polynomial_curve to piecewise_curve (make it more general using...

Edit piecewise_polynomial_curve to piecewise_curve (make it more general using templates) / Add python binding for all types of piecewise curve / Edit std error thrown
parent 89780c8a
......@@ -11,7 +11,7 @@ SET(${PROJECT_NAME}_HEADERS
quintic_spline.h
linear_variable.h
cubic_hermite_spline.h
piecewise_polynomial_curve.h
piecewise_curve.h
)
INSTALL(FILES
......
......@@ -63,7 +63,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
In it(PointsBegin);
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
......@@ -90,7 +90,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound");
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
......@@ -119,7 +119,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
}
if (size_ == 1)
{
......
......@@ -100,7 +100,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate cubic hermite spline, out of range");
throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
}
if (size_ == 1)
{
......@@ -139,7 +139,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
computeDurationSplines();
if (!checkDurationSplines())
{
throw std::logic_error("time_splines not monotonous, all spline duration should be superior to 0");
throw std::invalid_argument("time_splines not monotonous, all spline duration should be superior to 0");
}
}
......
......@@ -38,7 +38,7 @@ Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
}
/// \brief Converts a cubic hermite spline or polynomial to a cubic bezier curve.
/// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve.
template<typename Bezier, typename curveTypeToConvert>
......
/**
* \file piecewise_polynomial_curve.h
* \brief class allowing to create a piecewise polynomial curve.
* \author Jason C.
* \date 05/2019
*/
#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE
#include "curve_abc.h"
#include "polynomial.h"
#include "curve_conversion.h"
namespace curves
{
/// \class PiecewiseCurve.
/// \brief Represent a piecewise polynomial curve. We can add some new polynomials to the curve,
/// but the starting time of the polynomial to add should be equal to the ending time of the
/// piecewise_polynomial_curve.<br>\ Example : A piecewise polynomial curve composed of three polynomials pol_0,
/// pol_1 and pol_2 where pol_0 is defined between \f$[T0_{min},T0_{max}]\f$, pol_1 between
/// \f$[T0_{max},T1_{max}]\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$.
/// On the piecewise polynomial curve, pol_0 is located between \f$[T0_{min},T0_{max}[\f$,
/// pol_1 between \f$[T0_{max},T1_{max}[\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$.
///
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_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
typedef Numeric num_t;
//typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef typename std::vector < polynomial_t > t_polynomial_t;
typedef std::vector<Time> t_vector_time_t;
public:
/// \brief Constructor.
/// Initialize a piecewise polynomial curve by giving the first polynomial curve.
/// \param pol : a polynomial curve.
///
piecewise_polynomial_curve(const polynomial_t& pol)
{
size_ = 0;
add_polynomial_curve(pol);
time_polynomial_curves_.push_back(pol.min());
T_min_ = pol.min();
}
virtual ~piecewise_polynomial_curve(){}
virtual Point operator()(const Time t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
return polynomial_curves_.at(find_interval(t))(t);
}
/// \brief Evaluate the derivative of order N of curve at time t.
/// \param t : time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
///
virtual Point derivate(const Time t, const std::size_t order) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
return (polynomial_curves_.at(find_interval(t))).derivate(t, order);
}
/// \brief Add a new polynomial 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.
/// \param pol : polynomial to add.
///
void add_polynomial_curve(polynomial_t pol)
{
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
if (size_!=0 && pol.min()!=T_max_)
{
throw std::out_of_range("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
}
polynomial_curves_.push_back(pol);
size_ = polynomial_curves_.size();
T_max_ = pol.max();
time_polynomial_curves_.push_back(T_max_);
}
/// \brief Check if the curve is continuous of order given.
/// \param order : order of continuity we want to check.
/// \return True if the curve is continuous of order given.
///
bool is_continuous(const std::size_t order)
{
double margin = 0.001;
bool isContinuous = true;
std::size_t i=0;
point_t value_end, value_start;
while (isContinuous && i<(size_-1))
{
polynomial_t current = polynomial_curves_.at(i);
polynomial_t next = polynomial_curves_.at(i+1);
if (order == 0)
{
value_end = current(current.max());
value_start = next(next.min());
}
else
{
value_end = current.derivate(current.max(),order);
value_start = next.derivate(next.min(),order);
}
if ((value_end-value_start).norm() > margin)
{
isContinuous = false;
}
i++;
}
return isContinuous;
}
private:
/// \brief Get index of the interval corresponding to time t for the interpolation.
/// \param t : time where to look for interval.
/// \return Index of interval for time t.
///
std::size_t find_interval(const Numeric t) const
{
// time before first control point time.
if(t < time_polynomial_curves_[0])
{
return 0;
}
// time is after last control point time
if(t > time_polynomial_curves_[size_-1])
{
return size_-1;
}
std::size_t left_id = 0;
std::size_t right_id = size_-1;
while(left_id <= right_id)
{
const std::size_t middle_id = left_id + (right_id - left_id)/2;
if(time_polynomial_curves_.at(middle_id) < t)
{
left_id = middle_id+1;
}
else if(time_polynomial_curves_.at(middle_id) > t)
{
right_id = middle_id-1;
}
else
{
return middle_id;
}
}
return left_id-1;
}
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
Time virtual min() const{return T_min_;}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
Time virtual max() const{return T_max_;}
/*Helpers*/
/* Variables */
t_polynomial_t polynomial_curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
t_vector_time_t time_polynomial_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
std::size_t size_; // Number of segments in piecewise curve = size of polynomial_curves_
Time T_min_, T_max_;
};
} // end namespace
#endif // _CLASS_PIECEWISE_CURVE
\ No newline at end of file
......@@ -107,7 +107,7 @@ struct polynomial : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if(t_min_ > t_max_)
{
std::out_of_range("Tmin should be inferior to Tmax");
std::invalid_argument("Tmin should be inferior to Tmax");
}
if(coefficients_.size() != int(order_+1))
{
......@@ -142,7 +142,7 @@ struct polynomial : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if((t < t_min_ || t > t_max_) && Safe)
{
throw std::out_of_range("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
throw std::invalid_argument("error in polynomial : time t to evaluate should be in range [Tmin, Tmax] of the curve");
}
time_t const dt (t-t_min_);
point_t h = coefficients_.col(order_);
......@@ -162,7 +162,7 @@ struct polynomial : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if((t < t_min_ || t > t_max_) && Safe)
{
throw std::out_of_range("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
throw std::invalid_argument("error in polynomial : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
}
time_t const dt (t-t_min_);
time_t cdt(1);
......
......@@ -5,7 +5,7 @@
#include "curves/curve_conversion.h"
#include "curves/bernstein.h"
#include "curves/cubic_hermite_spline.h"
#include "curves/piecewise_polynomial_curve.h"
#include "curves/piecewise_curve.h"
#include "python_definitions.h"
#include "python_variables.h"
......@@ -40,13 +40,17 @@ typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tan
typedef curves::bezier_curve <real, real, 3, true, point_t> bezier3_t;
typedef curves::bezier_curve <real, real, 6, true, point6_t> bezier6_t;
typedef curves::polynomial <real, real, 3, true, point_t, t_point_t> polynomial_t;
typedef curves::piecewise_polynomial_curve <real, real, 3, true, point_t, t_point_t> piecewise_polynomial_curve_t;
typedef curves::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
typedef curves::cubic_hermite_spline <real, real, 3, true, point_t> cubic_hermite_spline_t;
typedef polynomial_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t;
typedef curves::piecewise_curve <real, real, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point_t, t_point_t, bezier3_t> piecewise_bezier3_curve_t;
typedef curves::piecewise_curve <real, real, 6, true, point6_t, t_point6_t, bezier6_t> piecewise_bezier6_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::Bern<double> bernstein_t;
typedef curves::curve_constraints<point_t> curve_constraints_t;
......@@ -154,11 +158,23 @@ polynomial_t* wrapSplineConstructor(const coeff_t& array)
}
/* End wrap polynomial */
/* Wrap piecewise polynomial curve */
/* Wrap piecewise curve */
piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
{
return new piecewise_polynomial_curve_t(pol);
}
piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier3_t& bc)
{
return new piecewise_bezier3_curve_t(bc);
}
piecewise_bezier6_curve_t* wrapPiecewiseBezier6CurveConstructor(const bezier6_t& bc)
{
return new piecewise_bezier6_curve_t(bc);
}
piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch)
{
return new piecewise_cubic_hermite_curve_t(ch);
}
/* end wrap piecewise polynomial curve */
/* Wrap exact cubic spline */
......@@ -337,7 +353,7 @@ BOOST_PYTHON_MODULE(curves)
;
/** END polynomial function**/
/** BEGIN piecewise polynomial curve function **/
/** BEGIN piecewise curve function **/
class_<piecewise_polynomial_curve_t>
("piecewise_polynomial_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor))
......@@ -345,10 +361,40 @@ BOOST_PYTHON_MODULE(curves)
.def("max", &piecewise_polynomial_curve_t::max)
.def("__call__", &piecewise_polynomial_curve_t::operator())
.def("derivate", &piecewise_polynomial_curve_t::derivate)
.def("add_polynomial_curve", &piecewise_polynomial_curve_t::add_polynomial_curve)
.def("add_curve", &piecewise_polynomial_curve_t::add_curve)
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous)
;
/** END piecewise polynomial curve function **/
class_<piecewise_bezier3_curve_t>
("piecewise_bezier3_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewiseBezier3CurveConstructor))
.def("min", &piecewise_bezier3_curve_t::min)
.def("max", &piecewise_bezier3_curve_t::max)
.def("__call__", &piecewise_bezier3_curve_t::operator())
.def("derivate", &piecewise_bezier3_curve_t::derivate)
.def("add_curve", &piecewise_bezier3_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier3_curve_t::is_continuous)
;
class_<piecewise_bezier6_curve_t>
("piecewise_bezier6_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewiseBezier6CurveConstructor))
.def("min", &piecewise_bezier6_curve_t::min)
.def("max", &piecewise_bezier6_curve_t::max)
.def("__call__", &piecewise_bezier6_curve_t::operator())
.def("derivate", &piecewise_bezier6_curve_t::derivate)
.def("add_curve", &piecewise_bezier6_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier6_curve_t::is_continuous)
;
class_<piecewise_cubic_hermite_curve_t>
("piecewise_cubic_hermite_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
.def("min", &piecewise_cubic_hermite_curve_t::min)
.def("max", &piecewise_cubic_hermite_curve_t::max)
.def("__call__", &piecewise_cubic_hermite_curve_t::operator())
.def("derivate", &piecewise_cubic_hermite_curve_t::derivate)
.def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
;
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
......
......@@ -10,7 +10,8 @@ from numpy.linalg import norm
import unittest
from curves import bezier3, bezier6
from curves import curve_constraints, polynomial, piecewise_polynomial_curve, exact_cubic, cubic_hermite_spline
from curves import curve_constraints, polynomial, exact_cubic, cubic_hermite_spline
from curves import piecewise_polynomial_curve, piecewise_bezier3_curve, piecewise_bezier6_curve, piecewise_cubic_hermite_curve
from curves import polynomial_from_bezier, polynomial_from_hermite
from curves import bezier_from_hermite, bezier_from_polynomial
from curves import hermite_from_bezier, hermite_from_polynomial
......@@ -100,13 +101,67 @@ class TestCurves(unittest.TestCase):
def test_piecewise_polynomial_curve(self):
# To test :
# - Functions : constructor, min, max, derivate, add_polynomial_curve, is_continuous
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints1 = matrix([[1., 1., 1.]]).transpose()
waypoints2 = matrix([[1., 1., 1.], [1., 1., 1.]]).transpose()
a = polynomial(waypoints1, 0.,1.)
b = polynomial(waypoints2, 1., 3.)
ppc = piecewise_polynomial_curve(a)
ppc.add_polynomial_curve(b)
ppc.add_curve(b)
ppc.min()
ppc.max()
ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2)
ppc.is_continuous(0)
ppc.is_continuous(1)
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()
a = bezier3(waypoints, 0., 1.)
b = bezier3(waypoints, 1., 2.)
ppc = piecewise_bezier3_curve(a)
ppc.add_curve(b)
ppc.min()
ppc.max()
ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2)
ppc.is_continuous(0)
ppc.is_continuous(1)
return
def test_piecewise_bezier6_curve(self):
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).transpose()
a = bezier6(waypoints, 0., 1.)
b = bezier6(waypoints, 1., 2.)
ppc = piecewise_bezier6_curve(a)
ppc.add_curve(b)
ppc.min()
ppc.max()
ppc(0.4)
self.assertTrue ((ppc.derivate(0.4, 0) == ppc(0.4)).all())
ppc.derivate(0.4, 2)
ppc.is_continuous(0)
ppc.is_continuous(1)
return
def test_piecewise_cubic_hermite_curve(self):
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_points0 = matrix([0., 1.]).transpose()
time_points1 = matrix([1., 2.]).transpose()
a = cubic_hermite_spline(points, tangents, time_points0)
b = cubic_hermite_spline(points, tangents, time_points1)
ppc = piecewise_cubic_hermite_curve(a)
ppc.add_curve(b)
ppc.min()
ppc.max()
ppc(0.4)
......
......@@ -6,7 +6,7 @@
#include "curves/helpers/effector_spline_rotation.h"
#include "curves/curve_conversion.h"
#include "curves/cubic_hermite_spline.h"
#include "curves/piecewise_polynomial_curve.h"
#include "curves/piecewise_curve.h"
#include <string>
#include <iostream>
......@@ -37,7 +37,9 @@ typedef cubic_hermite_spline <double, double, 3, true, point_t> cubic_hermite_sp
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;
typedef piecewise_polynomial_curve <double, double, 3, true, point_t> piecewise_polynomial_curve_t;
typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
typedef piecewise_curve <double, double, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
bool QuasiEqual(const double a, const double b, const float margin)
......@@ -1159,8 +1161,9 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
}
void piecewisePolynomialCurveTest(bool& error)
void piecewiseCurveTest(bool& error)
{
// TEST WITH POLYNOMIALS
std::string errmsg1("in piecewise polynomial curve test, Error While checking value of point on curve : ");
point_t a(1,1,1); // in [0,1[
point_t b(2,1,1); // in [1,2[
......@@ -1176,44 +1179,57 @@ void piecewisePolynomialCurveTest(bool& error)
polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
// 1 polynomial in curve
piecewise_polynomial_curve_t ppc(pol1);
res = ppc(0.5);
piecewise_polynomial_curve_t pc(pol1);
res = pc(0.5);
ComparePoints(a,res,errmsg1,error);
// 3 polynomials in curve
ppc.add_polynomial_curve(pol2);
ppc.add_polynomial_curve(pol3);
pc.add_curve(pol2);
pc.add_curve(pol3);
// Check values on piecewise curve
// t in [0,1[ -> res=a
res = ppc(0.);
res = pc(0.);
ComparePoints(a,res,errmsg1,error);
res = ppc(0.5);
res = pc(0.5);
ComparePoints(a,res,errmsg1,error);
// t in [1,2[ -> res=b
res = ppc(1.0);
res = pc(1.0);
ComparePoints(b,res,errmsg1,error);
res = ppc(1.5);
res = pc(1.5);
ComparePoints(b,res,errmsg1,error);
// t in [2,3] -> res=c
res = ppc(2.0);
res = pc(2.0);
ComparePoints(c,res,errmsg1,error);
res = ppc(3.0);
res = pc(3.0);
ComparePoints(c,res,errmsg1,error);
// Create piecewise curve C0
point_t a1(1,1,1);
t_point_t vec_C0;
vec_C0.push_back(a);
vec_C0.push_back(a1);
polynomial_t pol_a(vec_C0, 1.0, 2.0);
piecewise_polynomial_curve_t ppc_C0(pol1); // for t in [0,1[ : x=1 , y=1 , z=1
ppc_C0.add_polynomial_curve(pol_a); // for t in [1,2] : x=(t-1)+1 , y=(t-1)+1 , z=(t-1)+1
// Create piecewise curve C0 from bezier
point_t a0(1,2,3);
point_t b0(2,3,4);
point_t c0(3,4,5);
point_t d0(4,5,6);
std::vector<point_t> params0;
std::vector<point_t> params1;
params0.push_back(a0); // bezier between [0,1]
params0.push_back(b0);
params0.push_back(c0);
params0.push_back(d0);
params1.push_back(d0); // bezier between [1,2]
params1.push_back(c0);
params1.push_back(b0);
params1.push_back(a0);
bezier_curve_t bc0(params0.begin(), params0.end(), 0., 1.);
bezier_curve_t bc1(params1.begin(), params1.end(), 1., 2.);
piecewise_bezier_curve_t pc_C0(bc0);
pc_C0.add_curve(bc1);
// Check value in t=0.5 and t=1.5
res = ppc_C0(0.5);
ComparePoints(a,res,errmsg1,error);
res = ppc_C0(1.5);
ComparePoints(point_t(1.5,1.5,1.5),res,errmsg1,error);
res = pc_C0(0.0);
ComparePoints(a0,res,errmsg1,error);
res = pc_C0(1.0);
ComparePoints(d0,res,errmsg1,error);
res = pc_C0(2.0);
ComparePoints(a0,res,errmsg1,error);
// Create piecewise curve C1 from Hermite
point_t p0(0.,0.,0.);
......@@ -1235,26 +1251,24 @@ void piecewisePolynomialCurveTest(bool& error)
time_control_points1.push_back(3.); // hermite 1 between [1,3]
cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points0);
cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points1);
// Convert to polynomial
polynomial_t pol_chs0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
polynomial_t pol_chs1 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs1);
piecewise_polynomial_curve_t ppc_C1(pol_chs0);
ppc_C1.add_polynomial_curve(pol_chs1);
piecewise_cubic_hermite_curve_t pc_C1(chs0);
pc_C1.add_curve(chs1);
// Create piecewise curve C2
point_t a0(0,0,0);
point_t b0(1,1,1);
point_t a1(0,0,0);
point_t b1(1,1,1);
t_point_t veca, vecb;
// in [0,1[
veca.push_back(a0);
veca.push_back(b0); // x=t, y=t, z=t