diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4cf5b6411dc5ae98094a5eed1955d368a85c7f8c..571175132aa2995b6cceecfcf5ea6ee9a0ade444 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,12 +3,10 @@ project(spline)
set(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/build/")
-set(LIBRARY_OUTPUT_PATH "${PROJECT_SOURCE_DIR}/lib/")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${PROJECT_SOURCE_DIR}/bin/")
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
-add_subdirectory (src/spline)
add_subdirectory (src/tests/spline_test)
diff --git a/src/spline/API/BezierCurve.h b/src/spline/API/BezierCurve.h
index 0d7ae3ce5642aa2660bdadd1550697b3b2f3ee81..88843912b2a7e0d8b834a5739ac11f70073d9dfa 100644
--- a/src/spline/API/BezierCurve.h
+++ b/src/spline/API/BezierCurve.h
@@ -1,5 +1,5 @@
/**
-* \file BezierCurve.h
+* \file bezier_curve.h
* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
* \author Steve T.
* \version 0.1
@@ -15,51 +15,104 @@
#include "Exports.h"
#include "MathDefs.h"
-#include <memory>
#include <vector>
namespace spline
{
- /// \class BezierCurve
- /// \brief Represents a curve
- ///
- class BezierCurve : public Curve_ABC
- {
+/// \class BezierCurve
+/// \brief Represents a curve
+///
+template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
+, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
+{
+ typedef Point point_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
+
/* Constructors - destructors */
public:
- ///\brief Constructor
- ///\param points: the points parametering the Bezier curve
- ///\TODO : sor far size above 3 is ignored
- SPLINE_API BezierCurve(const T_Vector& /*points*/, const Real minBound=0, const Real maxBound=1);
+ ///\brief Constructor
+ ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
+ ///\TODO : so far size above 3 is ignored
+ template<typename In>
+ SPLINE_API bezier_curve(In PointsBegin, In PointsEnd, const time_t minBound=0, const time_t maxBound=1)
+ : minBound_(minBound)
+ , maxBound_(maxBound)
+ , size_(std::distance(PointsBegin, PointsEnd))
+ {
+ In it(PointsBegin);
+ if(Safe && (size_<=1 || minBound == maxBound))
+ {
+ throw; // TODO
+ }
+ for(; it != PointsEnd; ++it)
+ {
+ pts_.push_back(*it);
+ }
+ }
- ///\brief Destructor
- SPLINE_API ~BezierCurve();
+ ///\brief Destructor
+ SPLINE_API ~bezier_curve()
+ {
+ // NOTHING
+ }
private:
- BezierCurve(const BezierCurve&);
- BezierCurve& operator=(const BezierCurve&);
+ bezier_curve(const bezier_curve&);
+ bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
+ public:
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
- /// \param result : a reference to the Point set to the x(t)
- /// \param return : true if evaluation is successful, false if t is out of range
- SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
+ /// \param return : the value x(t)
+ SPLINE_API virtual point_t operator()(time_t t) const
+ {
+ num_t nT = (t - minBound_) / (maxBound_ - minBound_);
+ if(Safe &! (0 <= nT && nT <= 1))
+ {
+ throw; // TODO
+ }
+ else
+ {
+ num_t dt = (1 - nT);
+ switch(size_)
+ {
+ case 2 :
+ return pts_[0] * dt + nT * pts_[1];
+ break;
+ case 3 :
+ return pts_[0] * dt * dt
+ + 2 * pts_[1] * nT * dt
+ + pts_[2] * nT * nT;
+ break;
+ default :
+ return pts_[0] * dt * dt * dt
+ + 3 * pts_[1] * nT * dt * dt
+ + 3 * pts_[2] * nT * nT * dt
+ + pts_[3] * nT * nT *nT;
+ break;
+ }
+ }
+ }
/*Operations*/
-
-SPLINE_API virtual Real MinBound() const;
-SPLINE_API virtual Real MaxBound() const;
+
+/*Helpers*/
+ SPLINE_API virtual time_t MinBound() const{return minBound_;}
+ SPLINE_API virtual time_t MaxBound() const{return minBound_;}
/*Helpers*/
public:
- const int size_;
- const Real minBound_, maxBound_;
+ const int size_;
+ const time_t minBound_, maxBound_;
private:
- const T_Vector pts_;
- };
+ typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Vector;
+ T_Vector pts_;
+};
}
#endif //_CLASS_BEZIERCURVE
diff --git a/src/spline/API/CubicFunction.h b/src/spline/API/CubicFunction.h
index 3f8ecb1f72174a99cea32c1f05f47719321bc089..81776f5e099f317803451d8248e1f4396f671d55 100644
--- a/src/spline/API/CubicFunction.h
+++ b/src/spline/API/CubicFunction.h
@@ -5,63 +5,84 @@
* \version 0.1
* \date 06/17/2013
*
-* This file contains definitions for the CubicFunction class.
-* It allows the creation and evaluation of natural 3D
-* smooth cubic splines
+* This file contains definitions for the CubicFunction struct.
+* It allows the creation and evaluation of natural
+* smooth cubic splines of arbitrary dimension
*/
-#ifndef _CLASS_CUBICFUNCTIONIMP
-#define _CLASS_CUBICFUNCTIONIMP
+#ifndef _STRUCT_CUBICFUNCTION
+#define _STRUCT_CUBICFUNCTION
#include "Exports.h"
#include "MathDefs.h"
+
#include "Curve_ABC.h"
+#include <stdexcept>
+
namespace spline
{
- class SplineVisitor;
-
/// \class CubicFunction
/// \brief Represents a cubic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation
- /// x(t) = a + b(t - tBegin) + c(t - tBegin)^2 + d(t - tBegin)^3
+ /// x(t) = a + b(t - t_min_) + c(t - t_min_)^2 + d(t - t_min_)^3
///
- class CubicFunction : public Curve_ABC
+ template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
+ , typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+ struct cubic_function : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
+ typedef Point point_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
/* Constructors - destructors */
public:
///\brief Constructor
- SPLINE_API CubicFunction(const Vector3& /*a*/, const Vector3& /*b*/, const Vector3& /*c*/, const Vector3& /*d*/, const Real /*tBegin*/, const Real /*tEnd*/);
+ SPLINE_API cubic_function(point_t const& a, point_t const& b, point_t const& c, point_t const &d, time_t min, time_t max)
+ :a_(a), b_(b), c_(c), d_(d), t_min_(min), t_max_(max)
+ {
+ if(t_min_ >= t_max_ & Safe)
+ {
+ std::out_of_range("TODO");
+ }
+ }
///\brief Destructor
- SPLINE_API ~CubicFunction();
+ SPLINE_API ~cubic_function()
+ {
+ // NOTHING
+ }
private:
- CubicFunction(const CubicFunction&);
- CubicFunction& operator=(const CubicFunction&);
+ //cubic_function(const cubic_function&);
+ //cubic_function& operator=(const cubic_function&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
- /// \param result : a reference to the Point set to the x(t)
- SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
+ /// \param return : the value x(t)
+ SPLINE_API virtual point_t operator()(time_t t) const
+ {
+ if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+ time_t const dt (t-t_min_);
+ return a_+ b_ * dt + c_ * dt*dt + d_ * dt*dt*dt;
+ }
/*Operations*/
/*Helpers*/
public:
- SPLINE_API Real virtual MinBound() const;
- SPLINE_API Real virtual MaxBound() const;
+ SPLINE_API num_t virtual MinBound() const {return t_min_;}
+ SPLINE_API num_t virtual MaxBound() const {return t_max_;}
/*Helpers*/
/*Attributes*/
- private:
- const Vector3 a_, b_, c_ ,d_;
- const Real tBegin_, tEnd_;
+ public:
+ const point_t a_, b_, c_ ,d_;
+ const time_t t_min_, t_max_;
/*Attributes*/
}; //class CubicFunction
}
-#endif //_CLASS_CUBICFUNCTIONIMP
+#endif //_STRUCT_CUBICFUNCTION
diff --git a/src/spline/API/Curve_ABC.h b/src/spline/API/Curve_ABC.h
index d348afd24854f61675b450233fed183f93ce0b1d..6507f297055b0f26e78bf2a15f15ee06976e6416 100644
--- a/src/spline/API/Curve_ABC.h
+++ b/src/spline/API/Curve_ABC.h
@@ -1,6 +1,6 @@
/**
-* \file Curve_ABC.h
-* \brief class allowing to create an Exact cubic spline.
+* \file curve_abc.h
+* \brief interface for a Curve of arbitrary dimension.
* \author Steve T.
* \version 0.1
* \date 06/17/2013
@@ -9,67 +9,49 @@
*/
-#ifndef _CLASS_CURVEABC
-#define _CLASS_CURVEABC
-
-#include "SplineVisitor.h"
+#ifndef _STRUCT_CURVE_ABC
+#define _STRUCT_CURVE_ABC
#include "Exports.h"
#include "MathDefs.h"
-#include <memory>
-#include <vector>
+#include <functional>
namespace spline
{
- /// \class Curve_ABC
- /// \brief Represents a curve
- ///
- class Curve_ABC
- {
+/// \struct curve_abc
+/// \brief Represents a curve of dimension Dim
+/// is Safe is false, no verification is made on the evaluation of the curve.
+template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
+, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+struct curve_abc : std::unary_function<Time, Point>
+{
+ typedef Point point_t;
+ typedef Time time_t;
+
/* Constructors - destructors */
public:
- ///\brief Constructor
- SPLINE_API Curve_ABC(){};
-
- ///\brief Destructor
- SPLINE_API ~Curve_ABC(){};
+ ///\brief Constructor
+ SPLINE_API curve_abc(){};
- private:
- Curve_ABC(const Curve_ABC&);
- Curve_ABC& operator=(const Curve_ABC&);
+ ///\brief Destructor
+ SPLINE_API ~curve_abc(){};
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
- /// \param result : a reference to the Point set to the x(t)
- /// \param return : true if evaluation is successful, false if t is out of range
- SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const = 0;
+ /// \param return : the value x(t)
+ SPLINE_API virtual point_t operator()(time_t t) const = 0;
/*Operations*/
/*Helpers*/
- public:
-/// \brief Given a timestep dt, returns a set of values for the exact spline
-/// separated by this timestep
-/// \param visitor : an object called for each timestep in the spline interval.
-/// \param dt : the timestep
-/// \param result : a reference to the Point set to the x(t)
-SPLINE_API void Accept(SplineVisitor& visitor, Real dt) const
-{
- for(Real ti = MinBound(); ti <= MaxBound(); ti = ti + dt)
- {
- Vector3 res; Evaluate(ti, res);
- visitor.Visit(ti, res);
- }
-}
-
-SPLINE_API virtual Real MinBound() const = 0;
-SPLINE_API virtual Real MaxBound() const = 0;
+ SPLINE_API virtual time_t MinBound() const = 0;
+ SPLINE_API virtual time_t MaxBound() const = 0;
/*Helpers*/
};
}
-#endif //_CLASS_EXACTCUBIC
+#endif //_STRUCT_CURVE_ABC
diff --git a/src/spline/API/ExactCubic.h b/src/spline/API/ExactCubic.h
index 22d035ef5cf50721fd655ad5c9724e47b4ec7d82..058a0a58806fef2fa5772906133e0e1d7b704058 100644
--- a/src/spline/API/ExactCubic.h
+++ b/src/spline/API/ExactCubic.h
@@ -1,5 +1,5 @@
/**
-* \file ExactCubic.h
+* \file exact_cubic.h
* \brief class allowing to create an Exact cubic spline.
* \author Steve T.
* \version 0.1
@@ -21,56 +21,157 @@
#define _CLASS_EXACTCUBIC
#include "Curve_ABC.h"
+#include "CubicFunction.h"
#include "Exports.h"
#include "MathDefs.h"
-#include <memory>
+#include <functional>
#include <vector>
+#include <Eigen/StdVector>
+
+#include <iostream>
namespace spline
{
- struct CubicPImpl; // private implementation
- /// \class ExactCubic
- /// \brief Represents a set of cubic splines defining a continuous function
- /// crossing each of the waypoint given in its initialization
- ///
- struct ExactCubic : public Curve_ABC
- {
-/* Constructors - destructors */
+/// \class ExactCubic
+/// \brief Represents a set of cubic splines defining a continuous function
+/// crossing each of the waypoint given in its initialization
+///
+template<typename Time= double, typename Numeric=Time, int Dim=3, bool Safe=false
+, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
+struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
+{
+ typedef Point point_t;
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
+ typedef Time time_t;
+ typedef Numeric num_t;
+ typedef cubic_function<time_t, Numeric, Dim, Safe, Point> cubic_function_t;
+ typedef typename std::vector<cubic_function_t*> T_cubic;
+ typedef typename T_cubic::iterator IT_cubic;
+ typedef typename T_cubic::const_iterator CIT_cubic;
+
+ /* Constructors - destructors */
public:
- ///\brief Constructor
- ///\param waypoints : a list comprising at least 2 waypoints in ascending time order
- SPLINE_API ExactCubic(const T_Waypoint& /*waypoints*/);
+ ///\brief Constructor
+ ///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
+ ///\param wayPointsEns : an iterator pointing to the end of a waypoint container
+ template<typename In>
+ SPLINE_API exact_cubic(In wayPointsBegin, In wayPointsEnd)
+ {
+ std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ if(Safe && size < 1)
+ {
+ throw; // TODO
+ }
+
+ // refer to the paper to understand all this.
+ MatrixX h1 = MatrixX::Zero(size, size);
+ MatrixX h2 = MatrixX::Zero(size, size);
+ MatrixX h3 = MatrixX::Zero(size, size);
+ MatrixX h4 = MatrixX::Zero(size, size);
+ MatrixX h5 = MatrixX::Zero(size, size);
+ MatrixX h6 = MatrixX::Zero(size, size);
- ///\brief Destructor
- SPLINE_API ~ExactCubic();
+ MatrixX a = MatrixX::Zero(size, Dim);
+ MatrixX b = MatrixX::Zero(size, Dim);
+ MatrixX c = MatrixX::Zero(size, Dim);
+ MatrixX d = MatrixX::Zero(size, Dim);
+ MatrixX x = MatrixX::Zero(size, Dim);
+
+
+ In it(wayPointsBegin), next(wayPointsBegin);
+ ++next;
+ Numeric t_previous((*it).first);
+
+ for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
+ {
+ num_t const dTi((*next).first - (*it).first);
+ num_t const dTi_sqr(dTi * dTi);
+ num_t const dTi_cube(dTi_sqr * dTi);
+ // filling matrices values
+ h3(i,i) = -3 / dTi_sqr;
+ h3(i,i+1) = 3 / dTi_sqr;
+ h4(i,i) = -2 / dTi;
+ h4(i,i+1) = -1 / dTi;
+ h5(i,i) = 2 / dTi_cube;
+ h5(i,i+1) = -2 / dTi_cube;
+ h6(i,i) = 1 / dTi_sqr;
+ h6(i,i+1) = 1 / dTi_sqr;
+ if( i+2 < size)
+ {
+ In it2(next); ++ it2;
+ num_t const dTi_1(1/((*it2).first - (*next).first));
+ num_t const dTi_1sqr(dTi_1 * dTi_1);
+ // this can be optimized but let's focus on clarity as long as not needed
+ h1(i+1, i) = 2 / dTi;
+ h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
+ h1(i+1, i+2) = 2 / dTi_1;
+ h2(i+1, i) = -6 / dTi_sqr;
+ h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
+ h2(i+1, i+2) = 6 / dTi_1sqr;
+ }
+ x.row(i)= (*it).second.transpose();
+ }
+ // adding last x
+ x.row(size-1)= (*it).second.transpose();
+ a= x;
+ PseudoInverse(h1);
+ b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+ c = h3 * x + h4 * b;
+ d = h5 * x + h6 * b;
+ it= wayPointsBegin, next=wayPointsBegin; ++ next;
+ for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
+ {
+ subSplines_.push_back(new cubic_function_t(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
+ }
+ subSplines_.push_back(new cubic_function_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
+ }
+
+ ///\brief Destructor
+ SPLINE_API ~exact_cubic()
+ {
+ for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+ {
+ delete(*it);
+ }
+ }
private:
- ExactCubic(const ExactCubic&);
- ExactCubic& operator=(const ExactCubic&);
-/* Constructors - destructors */
+ exact_cubic(const exact_cubic&);
+ exact_cubic& operator=(const exact_cubic&);
+ /* Constructors - destructors */
-/*Operations*/
+ /*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
- /// \param result : a reference to the Point set to the x(t)
- /// \return : true if evaluation is successful, false if t is out of range
- SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
-/*Operations*/
+ /// \param return : the value x(t)
+ SPLINE_API virtual point_t operator()(time_t t) const
+ {
+
+ if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");}
+ for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
+ {
+ if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
+ {
+ return (*it)->operator()(t);
+ }
+ }
+ }
+ /*Operations*/
-/*Helpers*/
+ /*Helpers*/
public:
- SPLINE_API Real virtual MinBound() const;
- SPLINE_API Real virtual MaxBound() const;
-/*Helpers*/
+ SPLINE_API num_t virtual MinBound() const{return subSplines_.front()->t_min_;}
+ SPLINE_API num_t virtual MaxBound() const{return subSplines_.back()->t_max_;}
+ /*Helpers*/
-/*Attributes*/
+ /*Attributes*/
private:
- std::auto_ptr<CubicPImpl> pImpl_;
-/*Attributes*/
- };
+ T_cubic subSplines_;
+ /*Attributes*/
+};
}
#endif //_CLASS_EXACTCUBIC
diff --git a/src/spline/API/MathDefs.h b/src/spline/API/MathDefs.h
index 1303da90db9950a7548007fae7d98a5f69497108..8c2f660e7b27581880774b411989162e735b2716 100644
--- a/src/spline/API/MathDefs.h
+++ b/src/spline/API/MathDefs.h
@@ -24,35 +24,17 @@
#include <utility>
namespace spline{
-
-#if (USEFLOAT)
- typedef float Real;
- typedef Eigen::Vector3f Vector3;
- typedef Eigen::Vector2f Vector2;
- typedef Eigen::VectorXf VectorX;
- typedef Eigen::MatrixXf MatrixX;
- typedef Eigen::Matrix4f Matrix4;
- typedef Eigen::Matrix3f Matrix3;
-#else
- typedef double Real;
- typedef Eigen::Vector3d Vector3;
- typedef Eigen::Vector2d Vector2;
- typedef Eigen::VectorXd VectorX;
- typedef Eigen::MatrixXd MatrixX;
- typedef Eigen::Matrix4d Matrix4;
- typedef Eigen::Matrix3d Matrix3;
-#endif
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
- VectorX m_sigma = svd.singularValues();
+ _Matrix_Type_ m_sigma = svd.singularValues();
- Real pinvtoler= 1.e-6; // choose your tolerance widely!
+ double pinvtoler= 1.e-6; // choose your tolerance widely!
- MatrixX m_sigma_inv = MatrixX::Zero(pinvmat.cols(),pinvmat.rows());
+ _Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
@@ -61,14 +43,6 @@ void PseudoInverse(_Matrix_Type_& pinvmat)
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
-typedef std::vector<Vector3,Eigen::aligned_allocator<Vector3> > T_Vector;
-
-/** Definition for a waypoint */
-typedef std::pair<Real, Vector3> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef T_Waypoint::iterator IT_Waypoint;
-typedef T_Waypoint::const_iterator CIT_Waypoint;
-
} // namespace spline
#endif //_SPLINEMATH
diff --git a/src/spline/API/SplineVisitor.h b/src/spline/API/SplineVisitor.h
deleted file mode 100644
index 5a0be04248e42fa75391c424676ef07c65394bf1..0000000000000000000000000000000000000000
--- a/src/spline/API/SplineVisitor.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/**
-* \file SplineVisitor.h
-* \brief Visitor for the Spline classes
-* \author Steve T.
-* \version 0.1
-* \date 06/17/2013
-*/
-
-#include "Exports.h"
-#include "MathDefs.h"
-
-#ifndef _CLASS_SPLINE_VISITOR
-#define _CLASS_SPLINE_VISITOR
-
-namespace spline
-{
- /// \class SplineVisitor
- /// \brief Represents a generic visitor for any Spline in the library.
- ///
- class SPLINE_API SplineVisitor
- {
-/* Constructors - destructors */
- public:
- ///\brief Constructor
- SplineVisitor(){}
-
- ///\brief Destructor
- ~SplineVisitor(){}
-
- private:
- SplineVisitor(const SplineVisitor&);
- SplineVisitor& operator=(const SplineVisitor&);
-/* Constructors - destructors */
-
-/*Operations*/
- public:
- /// \brief Evaluation of the cubic spline at time t.
- /// \param time : the time associated to the given value
- /// \param value : the value x(time)
- virtual void Visit(const Real /*time*/, const Vector3& /*value*/) = 0;
-/*Operations*/
- };
-}
-#endif //_CLASS_SPLINE_VISITOR
-
diff --git a/src/spline/BezierCurve.cpp b/src/spline/BezierCurve.cpp
index e8a7b560c59711238bd926add05372bd593f1c10..c73475b946443efcdbac522886c11a371ea73968 100644
--- a/src/spline/BezierCurve.cpp
+++ b/src/spline/BezierCurve.cpp
@@ -1,54 +1,2 @@
#include "API/BezierCurve.h"
-using namespace spline;
-
-BezierCurve::BezierCurve(const T_Vector& points, const Real minBound, const Real maxBound)
- : size_ ((int)points.size())
- , pts_(points)
- , minBound_(minBound)
- , maxBound_(maxBound)
-{
- assert(size_>1 && minBound < maxBound);
- // NOTHING
-}
-
-BezierCurve::~BezierCurve()
-{
- // NOTHING
-}
-
-bool BezierCurve::Evaluate(const Real t, Vector3& result) const
-{
- Real nT = (t - minBound_) / (maxBound_ - minBound_);
- if(0 <= nT && nT <= 1)
- {
- Real dt = (1 - nT);
- switch(size_)
- {
- case 2 :
- result = pts_[0] * dt + nT * pts_[1];
- break;
- case 3 :
- result = pts_[0] * dt * dt + 2 * pts_[1] * nT * dt + pts_[2] * nT * nT ;
- break;
- default :
- result = pts_[0] * dt * dt * dt + 3 * pts_[1] * nT * dt * dt + 3 * pts_[2] * nT * nT * dt + pts_[3] * nT * nT *nT ;
- break;
- }
- return true;
- }
- else // t out of bounds
- {
- return false;
- }
-}
-
-Real BezierCurve::MinBound() const
-{
- return minBound_;
-}
-
-Real BezierCurve::MaxBound() const
-{
- return maxBound_;
-}
\ No newline at end of file
diff --git a/src/spline/CMakeLists.txt b/src/spline/CMakeLists.txt
deleted file mode 100644
index ae2df2c394f808870fb483d4e2dd3c096d54e3d1..0000000000000000000000000000000000000000
--- a/src/spline/CMakeLists.txt
+++ /dev/null
@@ -1,16 +0,0 @@
-cmake_minimum_required(VERSION 2.6)
-
-add_definitions(-DSPLINE_DLLEXPORT)
-
-file(
- GLOB_RECURSE
- source_files
- ./*
- ./API/*
-)
-
-add_library(
- spline
- SHARED
- ${source_files}
-)
diff --git a/src/spline/CubicFunction.cpp b/src/spline/CubicFunction.cpp
index 3009e97e117526177937dfee1ff0fa4ecd1d8573..9c7c812c9231256965c64472da41736d17fadb94 100644
--- a/src/spline/CubicFunction.cpp
+++ b/src/spline/CubicFunction.cpp
@@ -1,38 +1,2 @@
#include "API/CubicFunction.h"
-using namespace spline;
-
-CubicFunction::CubicFunction(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, const Real tBegin, const Real tEnd)
- : a_(a), b_(b), c_(c), d_(d), tBegin_(tBegin), tEnd_(tEnd)
-{
- // NOTHING
-}
-
-CubicFunction::~CubicFunction()
-{
- // NOTHING
-}
-
-bool CubicFunction::Evaluate(const Real t, Vector3& result) const
-{
- if(tBegin_ <= t && t <= tEnd_)
- {
- Real dt = (t - tBegin_);
- result = a_ + b_ * dt + c_ * dt * dt + d_ * dt * dt * dt;
- return true;
- }
- else // t out of bounds
- {
- return false;
- }
-}
-
-Real CubicFunction::MinBound() const
-{
- return tBegin_;
-}
-
-Real CubicFunction::MaxBound() const
-{
- return tEnd_;
-}
\ No newline at end of file
diff --git a/src/spline/ExactCubic.cpp b/src/spline/ExactCubic.cpp
index 66ccc625adc265c390d21b187269d2674e941cd2..36438419754057bca796704eb0e498c0ef339434 100644
--- a/src/spline/ExactCubic.cpp
+++ b/src/spline/ExactCubic.cpp
@@ -1,165 +1 @@
#include "API/ExactCubic.h"
-#include "API/CubicFunction.h"
-
-#include <vector>
-
-namespace spline
-{
-typedef std::pair<Real, CubicFunction*> SubSpline;
-typedef std::vector<SubSpline> T_SubSpline;
-typedef T_SubSpline::iterator IT_SubSpline;
-typedef T_SubSpline::const_iterator CIT_SubSpline;
-
-struct CubicPImpl{
- CubicPImpl(Real maxTime)
- : maxTime_(maxTime)
- {
- // NOTHING
- }
-
- ~CubicPImpl()
- {
- for(IT_SubSpline it = subSplines_.begin(); it != subSplines_.end(); ++ it)
- {
- delete it->second;
- }
- }
-
- T_SubSpline subSplines_;
- Real maxTime_; // max bound on which spline is defined.
-};
-} // namespace spline
-
-using namespace spline;
-
-ExactCubic::ExactCubic(const T_Waypoint& waypoints)
- : Curve_ABC()
-{
- size_t size = waypoints.size();
- assert(size > 1);
- pImpl_.reset(new CubicPImpl(waypoints[size-1].first)); // setting max boundary
- // refer to the paper to understand all this.
- MatrixX h1 = MatrixX::Zero(size, size);
- MatrixX h2 = MatrixX::Zero(size, size);
- MatrixX h3 = MatrixX::Zero(size, size);
- MatrixX h4 = MatrixX::Zero(size, size);
- MatrixX h5 = MatrixX::Zero(size, size);
- MatrixX h6 = MatrixX::Zero(size, size);
-
- MatrixX a = MatrixX::Zero(size, 3);
- MatrixX b = MatrixX::Zero(size, 3);
- MatrixX c = MatrixX::Zero(size, 3);
- MatrixX d = MatrixX::Zero(size, 3);
- MatrixX x = MatrixX::Zero(size, 3);
-
- Real dTi, dTi_1, dTi_sqr, dTi_1sqr, dTi_cube;
- Real t_previous = waypoints[0].first;
-
- // I think using numbers instead of iterators will be clearer on this case
- for(int i=0; i< size - 1; ++i)
- {
- dTi = waypoints[i + 1].first - waypoints[i].first;
- dTi_sqr = dTi * dTi;
- dTi_cube = dTi_sqr * dTi;
-
- assert(dTi > 0); //make sure of the ascendant order
-
- // filling matrices values
- h3(i,i) = -3 / dTi_sqr;
- h3(i,i+1) = 3 / dTi_sqr;
-
- h4(i,i) = -2 / dTi;
- h4(i,i+1) = -1 / dTi;
-
- h5(i,i) = 2 / dTi_cube;
- h5(i,i+1) = -2 / dTi_cube;
-
- h6(i,i) = 1 / dTi_sqr;
- h6(i,i+1) = 1 / dTi_sqr;
-
- // we stop one step earlier for matrices h1 and h2
- if(i + 2 < size)
- {
- // this can be optimized but let's focus on clarity as long as not needed
- dTi_1 = waypoints[i + 2].first - waypoints[i + 1].first;
- dTi_1sqr = dTi_1 * dTi_1;
-
- h1(i+1, i) = 2 / dTi;
- h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
- h1(i+1, i+2) = 2 / dTi_1;
-
- h2(i+1, i) = - 6 / dTi_sqr;
- h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
- h2(i+1, i+2) = 6 / dTi_1sqr;
- }
-
- //computing x = [x0* x1* ... xT*]^T
- x.row(i) = waypoints[i].second.transpose();
- }
- // adding last x
- x.row(size-1) = waypoints[size-1] .second.transpose();
-
- // now we just have to apply the linear relations:
-
- a = x;
- // should I pseudo inverse this?
- PseudoInverse(h1);
- b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
- c = h3 * x + h4 * b;
- d = h5 * x + h6 * b;
-
- // Now each line i of the a, b, c and d matrices correponds to the coefficient of spline i
- // Let's create those, and initialiaze our pImpl_
- for(int i=0; i< size - 1; ++i) // last spline never evaluated, that's ok because xi(ti_1) = x_i_1*
- {
- CubicFunction* subSpline = new CubicFunction(a.row(i), b.row(i), c.row(i), d.row(i), waypoints[i].first, waypoints[i+1].first);
- pImpl_->subSplines_.push_back(std::make_pair(waypoints[i].first, subSpline));
- }
- //}
-}
-
-ExactCubic::~ExactCubic()
-{
- // NOTHING
-}
-
-
-bool ExactCubic::Evaluate(const Real t, Vector3& value) const
-{
- CIT_SubSpline it = pImpl_->subSplines_.begin();
- CIT_SubSpline it2 = it;
- ++it2;
- for(; it2!= pImpl_->subSplines_.end(); ++it2)
- {
- // t out of min bound
- if(t < it->first)
- {
- return false;
- }
- else if(t <= it2->first)
- {
- return it->second->Evaluate(t, value);
- }
- ++it;
- }
- // handling max Bound
- if(t <= pImpl_->maxTime_)
- {
- return it->second->Evaluate(t, value);
- }
- else
- {
- return false; // t out of max bound
- }
-}
-
-Real ExactCubic::MinBound() const
-{
- return this->pImpl_->subSplines_[0].first;
-}
-
-Real ExactCubic::MaxBound() const
-{
- return pImpl_->maxTime_;
-}
-
diff --git a/src/tests/spline_test/CMakeLists.txt b/src/tests/spline_test/CMakeLists.txt
index 773062aef43829c2969c7b59e31004f2d269f7ad..39f96aa35bb4eae712174b302efb1dc51fdce7c9 100644
--- a/src/tests/spline_test/CMakeLists.txt
+++ b/src/tests/spline_test/CMakeLists.txt
@@ -6,4 +6,3 @@ add_executable(
spline_tests Main.cpp
)
-target_link_libraries (spline_tests spline)
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index dbfbe69f64a28238ba99f3e56030098b35e1033c..7260644f7cff057948c1a89a001abdb255b07bbd 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -1,7 +1,6 @@
#include "CubicFunction.h"
#include "ExactCubic.h"
-#include "SplineVisitor.h"
#include "BezierCurve.h"
#include <string>
@@ -12,7 +11,14 @@ using namespace std;
namespace spline
{
-bool QuasiEqual(const Real a, const Real b, const float margin)
+typedef Eigen::Vector3d point_t;
+typedef cubic_function<double, double, 3, true, point_t> cubic_function_t;
+typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
+typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
+typedef typename std::pair<double, point_t> Waypoint;
+typedef typename std::vector<Waypoint> T_Waypoint;
+
+bool QuasiEqual(const double a, const double b, const float margin)
{
if ((a <= 0 && b <= 0) || (a >= 0 && b>= 0))
{
@@ -26,21 +32,22 @@ bool QuasiEqual(const Real a, const Real b, const float margin)
const float margin = 0.01f;
-bool operator ==(const Vector3& a, const Vector3& b)
+bool operator ==(const point_t& a, const point_t& b)
{
return QuasiEqual(a.x(), b.x(), margin) && QuasiEqual(a.y(), b.y(), margin) && QuasiEqual(a.z(), b.z(), margin);
}
+
} // namespace spline
using namespace spline;
-ostream& operator<<(ostream& os, const Vector3& pt)
+ostream& operator<<(ostream& os, const point_t& pt)
{
os << "(" << pt.x() << ", " << pt.y() << ", " << pt.z() << ")";
return os;
}
-void ComparePoints(const Vector3& pt1, const Vector3& pt2, const std::string& errmsg, bool& error)
+void ComparePoints(const point_t& pt1, const point_t& pt2, const std::string& errmsg, bool& error)
{
if(!(pt1 == pt2))
{
@@ -54,92 +61,124 @@ void ComparePoints(const Vector3& pt1, const Vector3& pt2, const std::string& er
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
- Vector3 a(1,2,3);
- Vector3 b(2,3,4);
- Vector3 c(3,4,5);
- Vector3 d(3,6,7);
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
- CubicFunction cf(a, b, c, d, 0, 1);
- Vector3 res1;
- cf.Evaluate(0, res1);
- Vector3 x0(1,2,3);
+ cubic_function_t cf(a, b, c, d, 0, 1);
+ point_t res1;
+ res1 =cf(0);
+ point_t x0(1,2,3);
ComparePoints(x0, res1, errMsg + "(0) ", error);
- Vector3 x1(9,15,19);
- cf.Evaluate(1, res1);
+ point_t x1(9,15,19);
+
+ res1 =cf(1);
ComparePoints(x1, res1, errMsg + "(1) ", error);
- Vector3 x2(3.125,5.25,7.125);
- cf.Evaluate(0.5, res1);
+ point_t x2(3.125,5.25,7.125);
+ res1 =cf(0.5);
ComparePoints(x2, res1, errMsg + "(0.5) ", error);
- CubicFunction cf2(a, b, c, d, 0.5, 1);
- cf2.Evaluate(0.5, res1);
+ cubic_function_t cf2(a, b, c, d, 0.5, 1);
+ res1 = cf2(0.5);
ComparePoints(x0, res1, errMsg + "x3 ", error);
- if(cf2.Evaluate(0.4, res1))
+ error = true;
+ try
+ {
+ cf2(0.4);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
{
- error = true;
std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
}
- if(cf2.Evaluate(1.1, res1))
+ error = true;
+ try
+ {
+ cf2(1.1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
{
- error = true;
std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
}
}
-/*BezierCurve Function tests*/
+/*bezier_curve Function tests*/
void BezierCurveTest(bool& error)
{
std::string errMsg("In test BezierCurveTest ; unexpected result for x ");
- Vector3 a(1,2,3);
- Vector3 b(2,3,4);
- Vector3 c(3,4,5);
- Vector3 d(3,6,7);
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
+ point_t d(3,6,7);
- spline::T_Vector params;
+ std::vector<point_t> params;
params.push_back(a);
params.push_back(b);
// 2d curve
- BezierCurve cf(params);
- Vector3 res1;
- cf.Evaluate(0, res1);
- Vector3 x20 = a ;
+ bezier_curve_t cf(params.begin(), params.end());
+ point_t res1;
+ res1 = cf(0);
+ point_t x20 = a ;
ComparePoints(x20, res1, errMsg + "2(0) ", error);
- Vector3 x21 = b;
- cf.Evaluate(1, res1);
+ point_t x21 = b;
+ res1 = cf(1);
ComparePoints(x21, res1, errMsg + "2(1) ", error);
//3d curve
params.push_back(c);
- BezierCurve cf3(params);
- cf3.Evaluate(0, res1);
+ bezier_curve_t cf3(params.begin(), params.end());
+ res1 = cf3(0);
ComparePoints(a, res1, errMsg + "3(0) ", error);
- cf3.Evaluate(1, res1);
+ res1 = cf3(1);
ComparePoints(c, res1, errMsg + "3(1) ", error);
//4d curve
params.push_back(d);
- BezierCurve cf4(params, 0.4, 2);
- cf4.Evaluate(0.4, res1);
+ bezier_curve_t cf4(params.begin(), params.end(), 0.4, 2);
+ res1 = cf4(0.4);
ComparePoints(a, res1, errMsg + "3(0) ", error);
-
- cf4.Evaluate(2, res1);
+
+ res1 = cf4(2);
ComparePoints(d, res1, errMsg + "3(1) ", error);
- if(cf.Evaluate(-0.4, res1))
+ try
{
- error = true;
- std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
+ cf(-0.4);
}
- if(cf.Evaluate(1.1, res1))
+ catch(...)
{
- error = true;
- std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
+ }
+ error = true;
+ try
+ {
+ cf(1.1);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
}
}
@@ -147,26 +186,34 @@ void BezierCurveTest(bool& error)
void ExactCubicNoErrorTest(bool& error)
{
spline::T_Waypoint waypoints;
- for(Real i = 0; i <= 1; i = i + 0.2)
+ for(double i = 0; i <= 1; i = i + 0.2)
{
- waypoints.push_back(std::make_pair(i,Vector3(i,i,i)));
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
}
- ExactCubic exactCubic(waypoints);
- Vector3 res1;
- if(!exactCubic.Evaluate(0, res1))
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ point_t res1;
+ try
{
- error = true;
- std::cout << "Evaluation of exactCubic error, 0 should be in range value\n";
+ exactCubic(0);
+ exactCubic(1);
}
- if(!exactCubic.Evaluate(1, res1))
+ catch(...)
{
error = true;
- std::cout << "Evaluation of exactCubic error, 1 should be in range value\n";
+ std::cout << "Evaluation of ExactCubicNoErrorTest error\n";
}
- if(exactCubic.Evaluate(1.2, res1))
+ error = true;
+ try
{
- error = true;
- std::cout << "Evaluation of exactCubic error, 1.2 should be an out of range value\n";
+ exactCubic(1.2);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of exactCubic cf error, 1.2 should be an out of range value\n";
}
if(exactCubic.MaxBound() != 1)
{
@@ -183,78 +230,18 @@ void ExactCubicNoErrorTest(bool& error)
void ExactCubicPointsCrossedTest(bool& error)
{
spline::T_Waypoint waypoints;
- for(Real i = 0; i <= 1; i = i + 0.2)
+ for(double i = 0; i <= 1; i = i + 0.2)
{
- waypoints.push_back(std::make_pair(i,Vector3(i,i,i)));
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
}
- ExactCubic exactCubic(waypoints);
- Vector3 res1;
- for(Real i = 0; i <= 1; i = i + 0.2)
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ point_t res1;
+ for(double i = 0; i <= 1; i = i + 0.2)
{
- exactCubic.Evaluate(i, res1);
+ res1 = exactCubic(i);
std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(Vector3(i,i,i), res1, errmsg, error);
- }
-}
-
-/*Cubic Visitor tests*/
-#include <vector>
-
-namespace spline
-{
- typedef std::vector<Vector3,Eigen::aligned_allocator<Vector3> > T_Vector;
- typedef T_Vector::const_iterator CIT_Vector;
- struct SplineVisitorTest : public SplineVisitor
- {
- SplineVisitorTest()
- : previousTime_(-1)
- {
- // NOTHING
- }
-
- ~SplineVisitorTest()
- {
- // NOTHING
- }
-
- virtual void Visit(const Real time, const Vector3& value)
- {
- if(previousTime_ >= time)
- {
- std::cout << "Error : Visit method called with non sequential time values" << std::endl;
- }
- previousTime_ = time;
- values_.push_back(value);
- }
-
- Real previousTime_;
- T_Vector values_;
- };
-}
-
-void SplineVisitorTestFunction(bool& error)
-{
- spline::T_Waypoint waypoints;
- for(Real i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,Vector3(i,i,i)));
+ ComparePoints(point_t(i,i,i), res1, errmsg, error);
}
- ExactCubic exactCubic(waypoints);
- Vector3 res1;
- SplineVisitorTest visitor;
- exactCubic.Accept(visitor, 0.2);
- CIT_Vector it = visitor.values_.begin();
- for(Real i = 0; i <= 1; i = i + 0.2)
- {
- assert(it != visitor.values_.end());
- exactCubic.Evaluate(i, res1);
- std::string errmsg("Error While testing SplineVisitor at timestep (expected / obtained)");
- ComparePoints(*it, res1, errmsg, error);
- ++it;
- }
- error = error & exactCubic.Evaluate(0.9, res1);
- std::string errmsg("Error While testing SplineVisitor at timestep (expected / obtained)");
- ComparePoints(Vector3(0.923, 0.923 ,0.923), res1, errmsg, error);
}
int main(int argc, char *argv[])
@@ -264,8 +251,7 @@ int main(int argc, char *argv[])
CubicFunctionTest(error);
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
- SplineVisitorTestFunction(error); // checks that given wayPoints are crossed
- BezierCurveTest(error);
+ //BezierCurveTest(error);
if(error)
{
std::cout << "There were some errors\n";
@@ -277,3 +263,4 @@ int main(int argc, char *argv[])
return 0;
}
}
+