diff --git a/include/curves/CMakeLists.txt b/include/curves/CMakeLists.txt
index 6ecf6771ded2dd81519641e09aeb8d3afb3db2a6..a4beb8a5df60991a625d801535d17f483d2545d1 100644
--- a/include/curves/CMakeLists.txt
+++ b/include/curves/CMakeLists.txt
@@ -1,11 +1,10 @@
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
diff --git a/include/curves/bezier_polynomial_conversion.h b/include/curves/bezier_polynomial_conversion.h
index 516ef772a324e78401253b982323c032c114ce8c..c45461dc98555c2f7e7ffca8f07fc97455da273a 100644
--- a/include/curves/bezier_polynomial_conversion.h
+++ b/include/curves/bezier_polynomial_conversion.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
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index 5735133d06ae6fd97e45976ce0db6c97df0b02e6..ddf274771968c264b63c07a9a96eb6f6bab9d507 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -1,3 +1,10 @@
+/**
+* \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 :
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
new file mode 100644
index 0000000000000000000000000000000000000000..d042b3558f9e9a74064afe6e5c59617806598ff7
--- /dev/null
+++ b/include/curves/curve_conversion.h
@@ -0,0 +1,84 @@
+#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
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index 154df466838d09f88e1b54bd3bb42ff0b128c702..3e23758420dad4afb3e9ed2daddee01efd0b2f1a 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -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 */
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
new file mode 100644
index 0000000000000000000000000000000000000000..066df07e2a69694f15ce16385d52310a12ccd033
--- /dev/null
+++ b/include/curves/piecewise_curve.h
@@ -0,0 +1,28 @@
+#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
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 146df0cef609073b315e224bb8dea345c80d1bfd..f1e216d66e2d93bc3d990597d5dde381838f86c1 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -1,9 +1,8 @@
#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**/
diff --git a/python/test/test.py b/python/test/test.py
index 3cd54bc22e9342d4ba00c4eae96e90c03764851f..d44c4a6592a592ee4fcd53f882b9593c6b53273b 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -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
diff --git a/tests/Main.cpp b/tests/Main.cpp
index be4f44988ff37b63eb2f7601aa07f2066000980c..2d908cf1fa72c3234a5411b4f257a1f8e27f1918 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -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());
+ constraints.end_vel = point_t(0,0,0);
+ constraints.init_vel = point_t(0,0,0);
+ constraints.end_acc = point_t(0,0,0);
+ constraints.init_acc = point_t(0,0,0);
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end(), constraints);
// now check that init and end velocity are 0
CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
@@ -593,7 +628,7 @@ void ExactCubicVelocityConstraintsTest(bool& error)
constraints.end_acc = point_t(4,5,6);
constraints.init_acc = point_t(-4,-4,-6);
std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
- spline_deriv_constraint_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
+ exact_cubic_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
@@ -950,7 +985,7 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
- std::vector< Pair_point_tangent > control_points;
+ std::vector< pair_point_tangent_t > control_points;
point_t res1;
point_t P0(0.,0.,0.);
@@ -965,9 +1000,9 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
// Two pairs
control_points.clear();
- control_points.push_back(Pair_point_tangent(P0,T0));
+ control_points.push_back(pair_point_tangent_t(P0,T0));
time_control_points.push_back(0.); // Time at P0
- control_points.push_back(Pair_point_tangent(P1,T1));
+ control_points.push_back(pair_point_tangent_t(P1,T1));
time_control_points.push_back(1.); // Time at P1
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
@@ -988,7 +1023,7 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
ComparePoints(T1, res1, errmsg3, error);
// Three pairs
- control_points.push_back(Pair_point_tangent(P2,T2));
+ control_points.push_back(pair_point_tangent_t(P2,T2));
time_control_points.clear();
time_control_points.push_back(0.); // Time at P0
time_control_points.push_back(2.); // Time at P1
@@ -1050,6 +1085,7 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierCurveTestCompareHornerAndBernstein(error);
BezierDerivativeCurveTimeReparametrizationTest(error);
BezierToPolynomialConversionTest(error);
+ CubicHermiteToCubicBezierConversionTest(error);
BezierEvalDeCasteljau(error);
BezierSplitCurve(error);
CubicHermitePairsPositionDerivativeTest(error);