Commit c31bd106 authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[dynamic dim - piecewise curve] Remove dim from template + binding python

parent 005fa588
......@@ -41,7 +41,7 @@ namespace curves
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >,
typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase>
struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase>
{
typedef Point point_t;
typedef T_Point t_point_t;
......@@ -56,7 +56,7 @@ namespace curves
typedef curve_constraints<Point, Dim> spline_constraints;
typedef exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> exact_cubic_t;
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
/* Constructors - destructors */
public:
......
......@@ -22,7 +22,7 @@ namespace curves
/// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$,
/// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >,
typename Curve= curve_abc<Time, Numeric, Safe, Point> >
......@@ -40,7 +40,7 @@ namespace curves
/// \brief Empty constructor. Add at least one curve to call other class functions.
///
piecewise_curve()
: size_(0), T_min_(0), T_max_(0)
: dim_(0), size_(0), T_min_(0), T_max_(0)
{}
/// \brief Constructor.
......@@ -49,12 +49,17 @@ namespace curves
///
piecewise_curve(const curve_t& cf)
{
dim_ = cf(cf.min()).size();
size_ = 0;
add_curve(cf);
}
piecewise_curve(const t_curve_t list_curves)
{
if (list_curves.size()!=0)
{
dim_ = list_curves[0](list_curves[0].min()).size();
}
size_ = 0;
for( std::size_t i=0; i<list_curves.size(); i++ )
{
......@@ -63,7 +68,7 @@ namespace curves
}
piecewise_curve(const piecewise_curve& other)
: curves_(other.curves_), time_curves_(other.time_curves_), size_(other.size_),
: dim_(other.dim_), curves_(other.curves_), time_curves_(other.time_curves_), size_(other.size_),
T_min_(other.T_min_), T_max_(other.T_max_)
{}
......@@ -102,6 +107,10 @@ namespace curves
///
void add_curve(const curve_t& cf)
{
if (size_==0 && dim_==0)
{
dim_ = cf(cf.min()).size();
}
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
if (size_!=0 && !(fabs(cf.min()-T_max_)<MARGIN))
{
......@@ -153,10 +162,10 @@ namespace curves
}
template<typename Bezier>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier()
{
check_if_not_empty();
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Bezier first_curve_output = bezier_from_curve<Bezier, curve_t>(first_curve);
......@@ -171,10 +180,10 @@ namespace curves
}
template<typename Hermite>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite()
{
check_if_not_empty();
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Hermite first_curve_output = hermite_from_curve<Hermite, curve_t>(first_curve);
......@@ -189,10 +198,10 @@ namespace curves
}
template<typename Polynomial>
piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial()
{
check_if_not_empty();
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
// Get first curve (segment)
curve_t first_curve = curves_.at(0);
Polynomial first_curve_output = polynomial_from_curve<Polynomial, curve_t>(first_curve);
......@@ -207,7 +216,7 @@ namespace curves
}
template<typename Polynomial>
static piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial>
static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial>
convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max)
{
if(Safe &! (points.size()>1))
......@@ -215,7 +224,7 @@ namespace curves
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument("piecewise_curve -> convert_discrete_points_to_polynomial, Error, less than 2 discrete points");
}
typedef piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
Time discretization_step = (T_max-T_min)/Time(points.size()-1);
Time time_actual = T_min;
// Initialization at first points
......@@ -302,7 +311,6 @@ namespace curves
}
}
/*Helpers*/
public:
/// \brief Get dimension of curve.
......@@ -343,8 +351,8 @@ namespace curves
}
}; // End struct piecewise curve
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point, typename Curve>
const double piecewise_curve<Time, Numeric, Dim, Safe, Point, T_Point, Curve>::MARGIN(0.001);
template<typename Time, typename Numeric, bool Safe, typename Point, typename T_Point, typename Curve>
const double piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve>::MARGIN(0.001);
} // end namespace
......
......@@ -44,10 +44,6 @@ typedef std::pair<real, point3_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef std::vector<Waypoint6> T_Waypoint6;
// Dynamic dim
typedef curves::cubic_hermite_spline <real, real, true, pointX_t> cubic_hermite_spline_t;
typedef curves::bezier_curve <real, real, true, pointX_t> bezier_t;
// 3D
typedef curves::polynomial <real, real, 3, true, point3_t, t_point3_t> polynomial3_t;
typedef curves::exact_cubic <real, real, 3, true, point3_t, t_point3_t> exact_cubic3_t;
......@@ -55,9 +51,12 @@ typedef polynomial3_t::coeff_t coeff_t;
typedef std::pair<real, point3_t> waypoint3_t;
typedef std::vector<waypoint3_t, Eigen::aligned_allocator<point3_t> > t_waypoint3_t;
typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, polynomial3_t> piecewise_polynomial3_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, bezier_t> piecewise_bezier3_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point3_t, t_point3_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
// Dynamic dim
typedef curves::cubic_hermite_spline <real, real, true, pointX_t> cubic_hermite_spline_t;
typedef curves::bezier_curve <real, real, true, pointX_t> bezier_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, polynomial3_t> piecewise_polynomial_curve_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, bezier_t> piecewise_bezier_curve_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::Bern<double> bernstein_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
......@@ -67,13 +66,13 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_bezier_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_cubic_hermite_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial3_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_bezier3_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_cubic_hermite_curve_t)
namespace curves
{
......@@ -157,21 +156,21 @@ namespace curves
/* End wrap polynomial */
/* Wrap piecewise curve */
piecewise_polynomial3_curve_t* wrapPiecewisePolynomial3CurveConstructor(const polynomial3_t& pol)
piecewise_polynomial_curve_t* wrapPiecewisePolynomial3CurveConstructor(const polynomial3_t& pol)
{
return new piecewise_polynomial3_curve_t(pol);
return new piecewise_polynomial_curve_t(pol);
}
piecewise_polynomial3_curve_t* wrapPiecewisePolynomial3CurveEmptyConstructor()
piecewise_polynomial_curve_t* wrapPiecewisePolynomial3CurveEmptyConstructor()
{
return new piecewise_polynomial3_curve_t();
return new piecewise_polynomial_curve_t();
}
piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier_t& bc)
piecewise_bezier_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier_t& bc)
{
return new piecewise_bezier3_curve_t(bc);
return new piecewise_bezier_curve_t(bc);
}
piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveEmptyConstructor()
piecewise_bezier_curve_t* wrapPiecewiseBezier3CurveEmptyConstructor()
{
return new piecewise_bezier3_curve_t();
return new piecewise_bezier_curve_t();
}
piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermite3CurveConstructor(const cubic_hermite_spline_t& ch)
{
......@@ -382,47 +381,47 @@ namespace curves
/** END polynomial function**/
/** BEGIN piecewise curve function **/
class_<piecewise_polynomial3_curve_t>
("piecewise_polynomial3_curve", init<>())
class_<piecewise_polynomial_curve_t>
("piecewise_polynomial_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewisePolynomial3CurveConstructor),
"Create a peicewise-polynomial curve containing the given polynomial curve.")
.def("min", &piecewise_polynomial3_curve_t::min,"Set the LOWER bound on interval definition of the curve.")
.def("max", &piecewise_polynomial3_curve_t::max,"Set the HIGHER bound on interval definition of the curve.")
.def("dim", &piecewise_polynomial3_curve_t::dim)
.def("__call__", &piecewise_polynomial3_curve_t::operator(),"Evaluate the curve at the given time.")
.def("derivate", &piecewise_polynomial3_curve_t::derivate,"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
.def("add_curve", &piecewise_polynomial3_curve_t::add_curve,
.def("min", &piecewise_polynomial_curve_t::min,"Set the LOWER bound on interval definition of the curve.")
.def("max", &piecewise_polynomial_curve_t::max,"Set the HIGHER bound on interval definition of the curve.")
.def("dim", &piecewise_polynomial_curve_t::dim)
.def("__call__", &piecewise_polynomial_curve_t::operator(),"Evaluate the curve at the given time.")
.def("derivate", &piecewise_polynomial_curve_t::derivate,"Evaluate the derivative of order N of curve at time t.",args("self","t","N"))
.def("add_curve", &piecewise_polynomial_curve_t::add_curve,
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
.def("is_continuous", &piecewise_polynomial3_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
.def("saveAsText", &piecewise_polynomial3_curve_t::saveAsText<piecewise_polynomial3_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_polynomial3_curve_t::loadFromText<piecewise_polynomial3_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_polynomial3_curve_t::saveAsXML<piecewise_polynomial3_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
.def("loadFromXML",&piecewise_polynomial3_curve_t::loadFromXML<piecewise_polynomial3_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
.def("saveAsBinary",&piecewise_polynomial3_curve_t::saveAsBinary<piecewise_polynomial3_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
.def("loadFromBinary",&piecewise_polynomial3_curve_t::loadFromBinary<piecewise_polynomial3_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
//.def(SerializableVisitor<piecewise_polynomial3_curve_t>())
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,"Check if the curve is continuous at the given order.")
.def("saveAsText", &piecewise_polynomial_curve_t::saveAsText<piecewise_polynomial_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_polynomial_curve_t::loadFromText<piecewise_polynomial_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_polynomial_curve_t::saveAsXML<piecewise_polynomial_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
.def("loadFromXML",&piecewise_polynomial_curve_t::loadFromXML<piecewise_polynomial_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
.def("saveAsBinary",&piecewise_polynomial_curve_t::saveAsBinary<piecewise_polynomial_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
.def("loadFromBinary",&piecewise_polynomial_curve_t::loadFromBinary<piecewise_polynomial_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
//.def(SerializableVisitor<piecewise_polynomial_curve_t>())
;
class_<piecewise_bezier3_curve_t>
("piecewise_bezier3_curve", init<>())
class_<piecewise_bezier_curve_t>
("piecewise_bezier_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewiseBezier3CurveConstructor))
.def("min", &piecewise_bezier3_curve_t::min)
.def("max", &piecewise_bezier3_curve_t::max)
.def("dim", &piecewise_bezier3_curve_t::dim)
.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)
.def("saveAsText", &piecewise_bezier3_curve_t::saveAsText<piecewise_bezier3_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_bezier3_curve_t::loadFromText<piecewise_bezier3_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_bezier3_curve_t::saveAsXML<piecewise_bezier3_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
.def("loadFromXML",&piecewise_bezier3_curve_t::loadFromXML<piecewise_bezier3_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
.def("saveAsBinary",&piecewise_bezier3_curve_t::saveAsBinary<piecewise_bezier3_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
.def("loadFromBinary",&piecewise_bezier3_curve_t::loadFromBinary<piecewise_bezier3_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
//.def(SerializableVisitor<piecewise_bezier3_curve_t>())
.def("min", &piecewise_bezier_curve_t::min)
.def("max", &piecewise_bezier_curve_t::max)
.def("dim", &piecewise_bezier_curve_t::dim)
.def("__call__", &piecewise_bezier_curve_t::operator())
.def("derivate", &piecewise_bezier_curve_t::derivate)
.def("add_curve", &piecewise_bezier_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
.def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a text file.")
.def("loadFromText",&piecewise_bezier_curve_t::loadFromText<piecewise_bezier_curve_t>,bp::args("filename"),"Loads *this from a text file.")
.def("saveAsXML",&piecewise_bezier_curve_t::saveAsXML<piecewise_bezier_curve_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
.def("loadFromXML",&piecewise_bezier_curve_t::loadFromXML<piecewise_bezier_curve_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
.def("saveAsBinary",&piecewise_bezier_curve_t::saveAsBinary<piecewise_bezier_curve_t>,bp::args("filename"),"Saves *this inside a binary file.")
.def("loadFromBinary",&piecewise_bezier_curve_t::loadFromBinary<piecewise_bezier_curve_t>,bp::args("filename"),"Loads *this from a binary file.")
//.def(SerializableVisitor<piecewise_bezier_curve_t>())
;
class_<piecewise_cubic_hermite_curve_t>
("piecewise_cubic_hermite3_curve", init<>())
("piecewise_cubic_hermite_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewiseCubicHermite3CurveConstructor))
.def("min", &piecewise_cubic_hermite_curve_t::min)
.def("max", &piecewise_cubic_hermite_curve_t::max)
......@@ -463,7 +462,7 @@ namespace curves
/** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/
class_<cubic_hermite_spline_t>
("cubic_hermite_spline3", init<>())
("cubic_hermite_spline", init<>())
.def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
.def("min", &cubic_hermite_spline_t::min)
.def("max", &cubic_hermite_spline_t::max)
......
......@@ -8,9 +8,9 @@ from numpy.linalg import norm
from curves import (bezier_from_hermite, bezier_from_polynomial, hermite_from_polynomial,
hermite_from_bezier, polynomial_from_hermite, polynomial_from_bezier,
cubic_hermite_spline3, curve_constraints3,curve_constraints, exact_cubic3, bezier,
piecewise_bezier3_curve, piecewise_cubic_hermite3_curve,
piecewise_polynomial3_curve, polynomial3
cubic_hermite_spline, curve_constraints3,curve_constraints, exact_cubic3, bezier,
piecewise_bezier_curve, piecewise_cubic_hermite_curve,
piecewise_polynomial_curve, polynomial3
)
#import curves
......@@ -128,7 +128,7 @@ class TestCurves(unittest.TestCase):
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_points = matrix([0., 1.]).transpose()
a = cubic_hermite_spline3(points, tangents, time_points)
a = cubic_hermite_spline(points, tangents, time_points)
a.min()
a.max()
a(0.4)
......@@ -136,7 +136,7 @@ class TestCurves(unittest.TestCase):
a.derivate(0.4, 2)
# Test serialization
a.saveAsText("serialization_curve.test")
b = cubic_hermite_spline3()
b = cubic_hermite_spline()
b.loadFromText("serialization_curve.test")
self.assertTrue((a(0.4) == b(0.4)).all())
os.remove("serialization_curve.test")
......@@ -144,7 +144,7 @@ class TestCurves(unittest.TestCase):
points = matrix([[1., 2., 3., 4.], [4., 5., 6., 7.]]).transpose()
tangents = matrix([[1., 2., 3., 4.], [4., 5., 6., 7.]]).transpose()
time_points = matrix([0., 1.]).transpose()
a = cubic_hermite_spline3(points, tangents, time_points)
a = cubic_hermite_spline(points, tangents, time_points)
a.min()
a.max()
a(0.4)
......@@ -152,7 +152,7 @@ class TestCurves(unittest.TestCase):
a.derivate(0.4, 2)
# Test serialization
a.saveAsText("serialization_curve.test")
b = cubic_hermite_spline3()
b = cubic_hermite_spline()
b.loadFromText("serialization_curve.test")
self.assertTrue((a(0.4) == b(0.4)).all())
os.remove("serialization_curve.test")
......@@ -167,7 +167,7 @@ class TestCurves(unittest.TestCase):
pol0 = polynomial3(waypoints0, 0., 0.1)
a = polynomial3(waypoints1, 0., 1.)
b = polynomial3(waypoints2, 1., 3.)
pc = piecewise_polynomial3_curve(a)
pc = piecewise_polynomial_curve(a)
pc.add_curve(b)
pc.min()
pc.max()
......@@ -178,7 +178,7 @@ class TestCurves(unittest.TestCase):
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise_polynomial3_curve()
pc_test = piecewise_polynomial_curve()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
......@@ -190,7 +190,7 @@ class TestCurves(unittest.TestCase):
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints, 0., 1.)
b = bezier(waypoints, 1., 2.)
pc = piecewise_bezier3_curve(a)
pc = piecewise_bezier_curve(a)
pc.add_curve(b)
pc.min()
pc.max()
......@@ -201,7 +201,7 @@ class TestCurves(unittest.TestCase):
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise_bezier3_curve()
pc_test = piecewise_bezier_curve()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
......@@ -214,9 +214,9 @@ class TestCurves(unittest.TestCase):
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_points0 = matrix([0., 1.]).transpose()
time_points1 = matrix([1., 2.]).transpose()
a = cubic_hermite_spline3(points, tangents, time_points0)
b = cubic_hermite_spline3(points, tangents, time_points1)
pc = piecewise_cubic_hermite3_curve(a)
a = cubic_hermite_spline(points, tangents, time_points0)
b = cubic_hermite_spline(points, tangents, time_points1)
pc = piecewise_cubic_hermite_curve(a)
pc.add_curve(b)
pc.min()
pc.max()
......@@ -227,7 +227,7 @@ class TestCurves(unittest.TestCase):
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise_cubic_hermite3_curve()
pc_test = piecewise_cubic_hermite_curve()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
......
......@@ -26,9 +26,9 @@ namespace curves
typedef bezier_curve <double, double, true, point_t> bezier_curve_t;
typedef cubic_hermite_spline <double, double, true, point_t> cubic_hermite_spline_t;
typedef exact_cubic <double, double, 1, true, Eigen::Matrix<double,1,1> > exact_cubic_one;
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;
typedef piecewise_curve <double, double, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
typedef piecewise_curve <double, double, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
typedef piecewise_curve <double, double, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment