From d044515ad2782d8bb7df7a476990c18e008e5033 Mon Sep 17 00:00:00 2001
From: JasonChmn <jason.chemin@hotmail.fr>
Date: Thu, 25 Apr 2019 14:23:56 +0200
Subject: [PATCH] Implementation of cubic hermite spline, start to do unit test
---
include/curves/CMakeLists.txt | 1 +
include/curves/cubic_hermite_spline.h | 312 ++++++++++++++++++++++++++
include/curves/cubic_spline.h | 3 +-
tests/Main.cpp | 96 ++++++--
4 files changed, 398 insertions(+), 14 deletions(-)
create mode 100644 include/curves/cubic_hermite_spline.h
diff --git a/include/curves/CMakeLists.txt b/include/curves/CMakeLists.txt
index c0f87f9..21b7a3c 100644
--- a/include/curves/CMakeLists.txt
+++ b/include/curves/CMakeLists.txt
@@ -11,6 +11,7 @@ SET(${PROJECT_NAME}_HEADERS
curve_constraint.h
quintic_spline.h
linear_variable.h
+ cubic_hermite_spline.h
)
INSTALL(FILES
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
new file mode 100644
index 0000000..c953b53
--- /dev/null
+++ b/include/curves/cubic_hermite_spline.h
@@ -0,0 +1,312 @@
+#ifndef _CLASS_CUBICHERMITESPLINE
+#define _CLASS_CUBICHERMITESPLINE
+
+#include "curve_abc.h"
+#include "bernstein.h"
+#include "curve_constraint.h"
+
+#include "MathDefs.h"
+
+#include <vector>
+#include <stdexcept>
+
+#include <iostream>
+
+namespace curves
+{
+/// \class CubicHermiteSpline.
+/// \brief Represents a set of cubic hermite splines defining a continuous function \f$x(t)\f$.
+/// A hermite cubic spline is a minimal degree polynom interpolating a function in two
+/// points \f$P_i\f$ and \f$P_{i+1}\f$ with its tangent \f$m_i\f$ and \f$m_{i+1}\f$.
+/// A hermite cubic spline :
+/// - crosses each of the waypoint given in its initialization (\f$P_0\f$, \f$P_1\f$,...,\f$P_N\f$).
+/// - has its derivate on \f$P_i\f$ and \f$P_{i+1}\f$ are \f$x'(t_{P_i}) = m_i\f$ and \f$x'(t_{P_{i+1}}) = m_{i+1}\f$.
+///
+template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
+, typename Point= Eigen::Matrix<Numeric, Dim, 1>
+, typename Tangent= Eigen::Matrix<Numeric, Dim, 1>
+, typename Pair_point_tangent= std::pair<Point, Tangent>
+, typename Vector_pair= std::vector< Pair_point_tangent ,Eigen::aligned_allocator<Point> >
+>
+struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
+{
+
+ typedef int Index;
+ typedef std::vector<Time> Vector_time;
+
+ /*Attributes*/
+ public:
+ Vector_pair control_points_; // Vector of pair < Point, Tangent > , Dim = Size_.
+ Vector_time time_control_points_; // Time corresponding to each control point, Dim = Size_.
+ Vector_time duration_splines_; // its length should be duration_splines_[0] = t_PO_P1, duration_splines_[1] = t_P1_P2, Dim = Size_-1.
+ /*const*/ Time T_;
+ /*const*/ std::size_t size_;
+ /*Attributes*/
+
+ public:
+ /// \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.
+ ///
+ template<typename In>
+ cubic_hermite_spline(In PairsBegin, In PairsEnd)
+ : T_(1.)
+ , size_(std::distance(PairsBegin, PairsEnd))
+ {
+ // Check size of pairs container.
+ std::size_t const size(std::distance(PairsBegin, PairsEnd));
+ if(Safe && size < 1)
+ {
+ throw std::length_error("can't create cubic_hermite_spline, number of pairs is inferior to 2.");
+ }
+ // Push all pairs in controlPoints
+ In it(PairsBegin);
+ for(; it != PairsEnd; ++it)
+ {
+ control_points_.push_back(*it);
+ }
+ setTimeSplinesDefault();
+ }
+
+ /// \brief Destructor.
+ virtual ~cubic_hermite_spline(){}
+
+ /*Operations*/
+ public:
+ virtual Point operator()(const Time t) const
+ {
+ if(Safe &! (0 <= t && t <= T_))
+ {
+ throw std::out_of_range("can't evaluate bezier curve, out of range");
+ }
+ if (size_ == 1)
+ {
+ return control_points_.front().first;
+ }
+ else
+ {
+ return evalCubicHermiteSpline(t, 0);
+ }
+ }
+
+ /// \brief Evaluate the derivative of order N of spline at time t.
+ /// \param t : time when to evaluate the spline.
+ /// \param order : order of derivative.
+ /// \return \f$\frac{d^Nx(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
+ {
+ return evalCubicHermiteSpline(t, order);
+ }
+
+ /// \brief Set duration of each spline.
+ /// Set duration of each spline, exemple : time_control_points[0] = duration of first spline,
+ /// time_control_points[0] = duration of second spline (...).
+ /// \param time_control_points : Vector containing duration of each spline.
+ void setTimeSplines(const Vector_time & time_control_points) const
+ {
+ time_control_points_ = time_control_points;
+ assert(time_control_points.size() == size());
+ compute_duration_splines();
+ if (!check_duration_splines())
+ {
+ throw std::logic_error("time_splines not monotonous, all spline duration should be superior to 0");
+ }
+ }
+
+ /// \brief Set duration by default of each spline.
+ /// Set duration by default (duration = T / Number of control points) for each spline.
+ ///
+ void setTimeSplinesDefault()
+ {
+ time_control_points_.clear();
+ Time timestep = T_ / (control_points_.size()-1);
+ Time time = 0.;
+ Index i = 0;
+ for (i=0; i<size(); i++)
+ {
+ //time_control_points_.push_back(time);
+ time_control_points_.push_back(time);
+ time += timestep;
+ }
+ compute_duration_splines();
+ }
+
+ /// \brief Returns the index of the interval for the interpolation.
+ ///
+ Index findInterval(const Numeric t) const
+ {
+ // time before first control point time.
+ if(t < time_control_points_[0])
+ {
+ return 0;
+ }
+ // time is after last control point time
+ if(t > time_control_points_[size_-1])
+ {
+ return size_-1;
+ }
+
+ Index left_id = 0;
+ Index right_id = size_-1;
+ while(left_id <= right_id)
+ {
+ const Index middle_id = left_id + (right_id - left_id)/2;
+ if(time_control_points_.at(middle_id) < t)
+ {
+ left_id = middle_id+1;
+ }
+ else if(time_control_points_.at(middle_id) > t)
+ {
+ right_id = middle_id-1;
+ }
+ else
+ {
+ return middle_id;
+ }
+ }
+ return left_id-1;
+ }
+
+ /*
+ Vector_pair control_points_; // Vector of pair < Point, Tangent >
+ Vector_time time_control_points_; // Time corresponding to each control point.
+ Vector_time duration_splines_;
+ */
+ // See how to evaluate cubic hermite spline
+ Point evalCubicHermiteSpline(const Numeric t, std::size_t order_derivated) const
+ {
+ // TO DO
+ const Index id = findInterval(t);
+
+ // ID is on the last control point
+ if(id == size_-1)
+ {
+ if (order_derivated==0)
+ {
+ return control_points_.back().first;
+ }
+ else if (order_derivated==1)
+ {
+ return control_points_.back().second;
+ }
+ else
+ {
+ 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 Time & t0 = time_control_points_[id];
+ const Time & t1 = time_control_points_[id+1];
+
+ const Time dt = (t1-t0);
+ const Time alpha = (t - t0)/dt;
+
+ assert(0. <= alpha <= 1. && "alpha must be in [0,1]");
+
+ Numeric h00, h10, h01, h11;
+ evalCoeffs(alpha,h00,h10,h01,h11,order_derivated);
+ //std::cout << "for val t="<<t<<" coef : h00="<<h00<<" h01="<<h01<<" h10="<<h10<<" h11="<<h11<<std::endl;
+ h10 *= dt; h11 *= dt;
+
+ Point p_ = (h00 * Pair0.first + h10 * Pair0.second + h01 * Pair1.first + h11 * Pair1.second);
+ return p_;
+ }
+
+
+ /// \brief Returns the number of points contained in the trajectory.
+ ///
+ Index size() const { return size_; }
+
+ /// \brief Returns the number of intervals (splines) contained in the trajectory.
+ ///
+ Index numIntervals() const { return size()-1; }
+
+
+ private:
+
+ /// \brief compute duration of each spline, ex : \f$t_{P_0\_P_1}=t_{P_1}-t_{P_0}f$.
+ void compute_duration_splines() {
+ duration_splines_.clear();
+ Time actual_time;
+ Time prev_time = *(time_control_points_.begin());
+ Index i = 0;
+ for (i=0; i<size()-1; i++)
+ {
+ actual_time = time_control_points_.at(i+1);
+ duration_splines_.push_back(actual_time-prev_time);
+ prev_time = actual_time;
+ }
+ }
+
+ /// \brief Check if the absicca
+ bool check_duration_splines() const
+ {
+ if (duration_splines_.size()>0) {
+ return false;
+ }else{
+ return (duration_splines_.array > 0.).all();
+ }
+ }
+
+ static void evalCoeffs(const Numeric t, Numeric & h00, Numeric & h10, Numeric & h01, Numeric & h11, std::size_t order_derivated)
+ {
+ Numeric t_square = t*t;
+ Numeric t_cube = t_square*t;
+ if (order_derivated==0)
+ {
+ h00 = 2*t_cube -3*t_square +1.;
+ h10 = t_cube -2*t_square +t;
+ h01 = -2*t_cube +3*t_square;
+ h11 = t_cube -t_square;
+ }
+ else if (order_derivated==1)
+ {
+ h00 = 6*t_square -6*t;
+ h10 = 3*t_square -4*t +1.;
+ h01 = -6*t_square +6*t;
+ h11 = 3*t_square -2*t;
+ }
+ else if (order_derivated==2)
+ {
+ h00 = 12*t -6.;
+ h10 = 6*t -4.;
+ h01 = -12*t +6.;
+ h11 = 6*t -2.;
+ }
+ else if (order_derivated==3)
+ {
+ h00 = 12.;
+ h10 = 6.;
+ h01 = -12.;
+ h11 = 0.;
+ }
+ else
+ {
+ h00 = 0.;
+ h10 = 0.;
+ h01 = 0.;
+ h11 = 0.;
+ }
+ }
+
+
+ /*Operations*/
+
+ /*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 time_control_points_.front();}
+ /// \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 time_control_points_.back();}
+ /*Helpers*/
+
+};
+
+} // namespace curve
+#endif //_CLASS_CUBICHERMITESPLINE
\ No newline at end of file
diff --git a/include/curves/cubic_spline.h b/include/curves/cubic_spline.h
index 4e2efed..96eba75 100644
--- a/include/curves/cubic_spline.h
+++ b/include/curves/cubic_spline.h
@@ -24,7 +24,8 @@ namespace curves
{
/// \brief Creates coefficient vector of a cubic spline defined on the interval
/// \f$[t_{min}, t_{max}]\f$. It follows the equation :
-/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$.
+/// \f$ x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$ t \in [t_{min}, t_{max}] \f$
+/// with a, b, c and d the control points.
///
template<typename Point, typename T_Point>
T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const &d)
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 162e202..b1b217e 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -6,6 +6,7 @@
#include "curves/helpers/effector_spline.h"
#include "curves/helpers/effector_spline_rotation.h"
#include "curves/bezier_polynom_conversion.h"
+#include "curves/cubic_hermite_spline.h"
#include <string>
#include <iostream>
@@ -16,6 +17,7 @@ using namespace std;
namespace curves
{
typedef Eigen::Vector3d point_t;
+typedef Eigen::Vector3d tangent_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef polynom <double, double, 3, true, point_t, t_point_t> polynom_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
@@ -32,6 +34,9 @@ typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
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;
+
bool QuasiEqual(const double a, const double b, const float margin)
{
if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
@@ -66,7 +71,6 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
}
/*Cubic Function tests*/
-
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
@@ -140,7 +144,6 @@ void CubicFunctionTest(bool& error)
}
/*bezier_curve Function tests*/
-
void BezierCurveTest(bool& error)
{
std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
@@ -240,7 +243,7 @@ void BezierCurveTest(bool& error)
}
#include <ctime>
-void BezierCurveTestCompareHornerAndBernstein(bool& /*error*/)
+void BezierCurveTestCompareHornerAndBernstein(bool&) // error
{
using namespace std;
std::vector<double> values;
@@ -506,7 +509,6 @@ void ExactCubicNoErrorTest(bool& error)
}
}
-
/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error)
{
@@ -544,7 +546,7 @@ void ExactCubicOneDimTest(bool& error)
ComparePoints(one, res1, errmsg, error);
}
-void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error )
+void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&, const exact_cubic_t* curve, bool& error )
{
point_t res1;
for(double i = 0; i <= 1; i = i + step)
@@ -794,18 +796,22 @@ void TestReparametrization(bool& error)
}
}
-point_t randomPoint(const double min, const double max ){
+point_t randomPoint(const double min, const double max )
+{
point_t p;
for(size_t i = 0 ; i < 3 ; ++i)
p[i] = (rand()/(double)RAND_MAX ) * (max-min) + min;
return p;
}
-void BezierEvalDeCasteljau(bool& error){
+void BezierEvalDeCasteljau(bool& error)
+{
using namespace std;
std::vector<double> values;
for (int i =0; i < 100000; ++i)
+ {
values.push_back(rand()/RAND_MAX);
+ }
//first compare regular evaluation (low dim pol)
point_t a(1,2,3);
@@ -828,7 +834,8 @@ void BezierEvalDeCasteljau(bool& error){
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
- if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ if(cf.evalDeCasteljau(*cit) != cf(*cit))
+ {
error = true;
std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
}
@@ -845,7 +852,8 @@ void BezierEvalDeCasteljau(bool& error){
for(std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit)
{
- if(cf.evalDeCasteljau(*cit) != cf(*cit)){
+ if(cf.evalDeCasteljau(*cit) != cf(*cit))
+ {
error = true;
std::cout<<" De Casteljau evaluation did not return the same value as analytical"<<std::endl;
}
@@ -853,22 +861,24 @@ void BezierEvalDeCasteljau(bool& error){
}
-
/**
* @brief BezierSplitCurve test the 'split' method of bezier curve
* @param error
*/
-void BezierSplitCurve(bool& error){
+void BezierSplitCurve(bool& error)
+{
// test for degree 5
int n = 5;
double t_min = 0.2;
double t_max = 10;
- for(size_t i = 0 ; i < 1 ; ++i){
+ for(size_t i = 0 ; i < 1 ; ++i)
+ {
// build a random curve and split it at random time :
//std::cout<<"build a random curve"<<std::endl;
point_t a;
std::vector<point_t> wps;
- for(size_t j = 0 ; j <= n ; ++j){
+ for(size_t j = 0 ; j <= n ; ++j)
+ {
wps.push_back(randomPoint(-10.,10.));
}
double t = (rand()/(double)RAND_MAX )*(t_max-t_min) + t_min;
@@ -930,11 +940,69 @@ void BezierSplitCurve(bool& error){
}
+/* cubic hermite spline function test */
+void CubicHermiteTwoPairsPositionVelocityTest(bool& error)
+{
+ std::string errmsg1("in Cubic Hermite 2 points, 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::vector< Pair_point_tangent > control_points;
+ point_t res1;
+
+ // 1 Dimension
+ control_points.clear();
+ // first point : p0(0.,0.,0.) m0(0.,1.,0.)
+ control_points.push_back(Pair_point_tangent(point_t(0.,0.,0.),tangent_t(0.5,0.,0.)));
+ // second point : p1(1.,0.,0.) m1(1.,1.,1.)
+ control_points.push_back(Pair_point_tangent(point_t(1.,0.,0.),tangent_t(0.1,0.,0.)));
+ // Create cubic hermite spline
+ cubic_hermite_spline_t cubic_hermite_spline1D(control_points.begin(), control_points.end());
+ //Check
+ res1 = cubic_hermite_spline1D(0); // t=0
+ ComparePoints(point_t(0.,0.,0.), res1, errmsg1, error);
+ res1 = cubic_hermite_spline1D(1); // t=1
+ ComparePoints(point_t(1.,0.,0.), res1, errmsg1, error);
+ res1 = cubic_hermite_spline1D(0.5); // t=0.5
+ ComparePoints(point_t(0.55,0.,0.0), res1, errmsg2, error);
+
+ // 2 Dimension
+ control_points.clear();
+ // first point : p0(0.,0.,0.) m0(0.,1.,0.)
+ control_points.push_back(Pair_point_tangent(point_t(0.,0.,0.),tangent_t(0.5,0.5,0.)));
+ // second point : p1(1.,0.,0.) m1(1.,1.,1.)
+ control_points.push_back(Pair_point_tangent(point_t(1.,1.,0.),tangent_t(0.5,0.1,0.)));
+ // Create cubic hermite spline
+ cubic_hermite_spline_t cubic_hermite_spline2D(control_points.begin(), control_points.end());
+ //Check
+ res1 = cubic_hermite_spline2D(0); // t=0
+ ComparePoints(point_t(0.,0.,0.), res1, errmsg1, error);
+ res1 = cubic_hermite_spline2D(1); // t=1
+ ComparePoints(point_t(1.,1.,0.), res1, errmsg1, error);
+ res1 = cubic_hermite_spline2D(0.5); // t=0.5
+ ComparePoints(point_t(0.5,0.55,0.0), res1, errmsg2, error);
+
+ // 2 Dimension
+ control_points.clear();
+ // first point : p0(0.,0.,0.) m0(0.,1.,0.)
+ control_points.push_back(Pair_point_tangent(point_t(0.,0.,0.),tangent_t(0.5,0.5,0.5)));
+ // second point : p1(1.,0.,0.) m1(1.,1.,1.)
+ control_points.push_back(Pair_point_tangent(point_t(1.,1.,1.),tangent_t(0.1,0.1,-0.5)));
+ // Create cubic hermite spline
+ cubic_hermite_spline_t cubic_hermite_spline3D(control_points.begin(), control_points.end());
+ //Check
+ res1 = cubic_hermite_spline3D(0); // t=0
+ ComparePoints(point_t(0.,0.,0.), res1, errmsg1, error);
+ res1 = cubic_hermite_spline3D(1); // t=1
+ ComparePoints(point_t(1.,1.,1.), res1, errmsg1, error);
+ res1 = cubic_hermite_spline3D(0.5); // t=0.5
+ ComparePoints(point_t(0.55,0.55,0.625), res1, errmsg2, error);
+}
+
int main(int /*argc*/, char** /*argv[]*/)
{
std::cout << "performing tests... \n";
bool error = false;
+ /*
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
@@ -954,6 +1022,8 @@ int main(int /*argc*/, char** /*argv[]*/)
BezierToPolynomConversionTest(error);
BezierEvalDeCasteljau(error);
BezierSplitCurve(error);
+ */
+ CubicHermiteTwoPairsPositionVelocityTest(error);
if(error)
{
std::cout << "There were some errors\n";
--
GitLab