diff --git a/src/spline/API/BezierCurve.h b/src/spline/API/BezierCurve.h
new file mode 100644
index 0000000000000000000000000000000000000000..0d7ae3ce5642aa2660bdadd1550697b3b2f3ee81
--- /dev/null
+++ b/src/spline/API/BezierCurve.h
@@ -0,0 +1,65 @@
+/**
+* \file BezierCurve.h
+* \brief class allowing to create a Bezier curve of dimension 1 <= n <= 3.
+* \author Steve T.
+* \version 0.1
+* \date 06/17/2013
+*/
+
+
+#ifndef _CLASS_BEZIERCURVE
+#define _CLASS_BEZIERCURVE
+
+#include "Curve_ABC.h"
+
+#include "Exports.h"
+#include "MathDefs.h"
+
+#include <memory>
+#include <vector>
+
+namespace spline
+{
+ /// \class BezierCurve
+ /// \brief Represents a curve
+ ///
+ class BezierCurve : public Curve_ABC
+ {
+/* 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 Destructor
+ SPLINE_API ~BezierCurve();
+
+ private:
+ BezierCurve(const BezierCurve&);
+ BezierCurve& operator=(const BezierCurve&);
+/* 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;
+/*Operations*/
+
+SPLINE_API virtual Real MinBound() const;
+SPLINE_API virtual Real MaxBound() const;
+/*Helpers*/
+
+ public:
+ const int size_;
+ const Real minBound_, maxBound_;
+
+ private:
+ const T_Vector pts_;
+ };
+}
+#endif //_CLASS_BEZIERCURVE
+
diff --git a/src/spline/API/CubicFunction.h b/src/spline/API/CubicFunction.h
index 7c1219b188233a3db96d6999df88d0fc013266bf..3f8ecb1f72174a99cea32c1f05f47719321bc089 100644
--- a/src/spline/API/CubicFunction.h
+++ b/src/spline/API/CubicFunction.h
@@ -16,6 +16,7 @@
#include "Exports.h"
#include "MathDefs.h"
+#include "Curve_ABC.h"
namespace spline
{
@@ -26,7 +27,7 @@ namespace spline
/// [tBegin, tEnd]. It follows the equation
/// x(t) = a + b(t - tBegin) + c(t - tBegin)^2 + d(t - tBegin)^3
///
- class CubicFunction
+ class CubicFunction : public Curve_ABC
{
/* Constructors - destructors */
public:
@@ -46,17 +47,13 @@ namespace spline
/// \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 bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
+ SPLINE_API virtual bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
/*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;
+ SPLINE_API Real virtual MinBound() const;
+ SPLINE_API Real virtual MaxBound() const;
/*Helpers*/
/*Attributes*/
@@ -66,4 +63,5 @@ SPLINE_API void Accept(SplineVisitor& /*visitor*/, Real /*dt*/) const;
/*Attributes*/
}; //class CubicFunction
}
-#endif //_CLASS_CUBICFUNCTIONIMP
\ No newline at end of file
+#endif //_CLASS_CUBICFUNCTIONIMP
+
diff --git a/src/spline/API/Curve_ABC.h b/src/spline/API/Curve_ABC.h
new file mode 100644
index 0000000000000000000000000000000000000000..d348afd24854f61675b450233fed183f93ce0b1d
--- /dev/null
+++ b/src/spline/API/Curve_ABC.h
@@ -0,0 +1,75 @@
+/**
+* \file Curve_ABC.h
+* \brief class allowing to create an Exact cubic spline.
+* \author Steve T.
+* \version 0.1
+* \date 06/17/2013
+*
+* Interface for a curve
+*/
+
+
+#ifndef _CLASS_CURVEABC
+#define _CLASS_CURVEABC
+
+#include "SplineVisitor.h"
+
+#include "Exports.h"
+#include "MathDefs.h"
+
+#include <memory>
+#include <vector>
+
+namespace spline
+{
+ /// \class Curve_ABC
+ /// \brief Represents a curve
+ ///
+ class Curve_ABC
+ {
+/* Constructors - destructors */
+ public:
+ ///\brief Constructor
+ SPLINE_API Curve_ABC(){};
+
+ ///\brief Destructor
+ SPLINE_API ~Curve_ABC(){};
+
+ private:
+ Curve_ABC(const Curve_ABC&);
+ Curve_ABC& operator=(const 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;
+/*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;
+/*Helpers*/
+
+ };
+}
+#endif //_CLASS_EXACTCUBIC
+
diff --git a/src/spline/API/ExactCubic.h b/src/spline/API/ExactCubic.h
index 3dcad2cb6c4921124adc282ccaac8bae91b92cd5..22d035ef5cf50721fd655ad5c9724e47b4ec7d82 100644
--- a/src/spline/API/ExactCubic.h
+++ b/src/spline/API/ExactCubic.h
@@ -20,27 +20,22 @@
#ifndef _CLASS_EXACTCUBIC
#define _CLASS_EXACTCUBIC
+#include "Curve_ABC.h"
+
#include "Exports.h"
#include "MathDefs.h"
#include <memory>
+#include <vector>
namespace spline
{
- /** Definition for a waypoint */
- typedef std::pair<const Real, const Vector3> Waypoint;
- typedef std::deque<Waypoint> T_Waypoint;
- typedef T_Waypoint::iterator IT_Waypoint;
- typedef T_Waypoint::const_iterator CIT_Waypoint;
-
- class SplineVisitor;
-
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
+ struct ExactCubic : public Curve_ABC
{
/* Constructors - destructors */
public:
@@ -61,18 +56,14 @@ namespace spline
/// \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 bool Evaluate(const Real /*t*/, Vector3& /*result*/) const;
+ /// \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*/
/*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;
+ SPLINE_API Real virtual MinBound() const;
+ SPLINE_API Real virtual MaxBound() const;
/*Helpers*/
/*Attributes*/
@@ -81,4 +72,5 @@ SPLINE_API void Accept(SplineVisitor& /*visitor*/, Real /*dt*/) const;
/*Attributes*/
};
}
-#endif //_CLASS_EXACTCUBIC
\ No newline at end of file
+#endif //_CLASS_EXACTCUBIC
+
diff --git a/src/spline/API/MathDefs.h b/src/spline/API/MathDefs.h
index a707725a9c74d435c0ca28ae1a8c74faaa30541c..1303da90db9950a7548007fae7d98a5f69497108 100644
--- a/src/spline/API/MathDefs.h
+++ b/src/spline/API/MathDefs.h
@@ -20,7 +20,7 @@
#include <Eigen/Dense>
#include <Eigen/SVD>
-#include <deque>
+#include <vector>
#include <utility>
namespace spline{
@@ -61,5 +61,14 @@ 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
index 9feb8f434b1da7c687640bbffcea382c231d7475..5a0be04248e42fa75391c424676ef07c65394bf1 100644
--- a/src/spline/API/SplineVisitor.h
+++ b/src/spline/API/SplineVisitor.h
@@ -6,6 +6,8 @@
* \date 06/17/2013
*/
+#include "Exports.h"
+#include "MathDefs.h"
#ifndef _CLASS_SPLINE_VISITOR
#define _CLASS_SPLINE_VISITOR
@@ -39,4 +41,5 @@ namespace spline
/*Operations*/
};
}
-#endif //_CLASS_SPLINE_VISITOR
\ No newline at end of file
+#endif //_CLASS_SPLINE_VISITOR
+
diff --git a/src/spline/BezierCurve.cpp b/src/spline/BezierCurve.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e8a7b560c59711238bd926add05372bd593f1c10
--- /dev/null
+++ b/src/spline/BezierCurve.cpp
@@ -0,0 +1,54 @@
+#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/CubicFunction.cpp b/src/spline/CubicFunction.cpp
index b3048f7f43f24013e680a22cffe2252983a1ba0e..3009e97e117526177937dfee1ff0fa4ecd1d8573 100644
--- a/src/spline/CubicFunction.cpp
+++ b/src/spline/CubicFunction.cpp
@@ -1,5 +1,4 @@
#include "API/CubicFunction.h"
-#include "API/SplineVisitor.h"
using namespace spline;
@@ -28,11 +27,12 @@ bool CubicFunction::Evaluate(const Real t, Vector3& result) const
}
}
-void CubicFunction::Accept(SplineVisitor& visitor, Real dt) const
+Real CubicFunction::MinBound() const
{
- for(Real ti = tBegin_; ti <= tEnd_; ti = ti + dt)
- {
- Vector3 res; Evaluate(ti, res);
- visitor.Visit(ti, res);
- }
+ 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 72ab212e1d5c6d13bc4fa75ce675e6f9cd8f05ab..66ccc625adc265c390d21b187269d2674e941cd2 100644
--- a/src/spline/ExactCubic.cpp
+++ b/src/spline/ExactCubic.cpp
@@ -1,6 +1,5 @@
#include "API/ExactCubic.h"
#include "API/CubicFunction.h"
-#include "API/SplineVisitor.h"
#include <vector>
@@ -34,9 +33,10 @@ struct CubicPImpl{
using namespace spline;
ExactCubic::ExactCubic(const T_Waypoint& waypoints)
+ : Curve_ABC()
{
size_t size = waypoints.size();
- assert(size > 2);
+ 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);
@@ -115,7 +115,7 @@ ExactCubic::ExactCubic(const T_Waypoint& waypoints)
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()
@@ -153,11 +153,13 @@ bool ExactCubic::Evaluate(const Real t, Vector3& value) const
}
}
-void ExactCubic::Accept(SplineVisitor& visitor, Real dt) const
+Real ExactCubic::MinBound() const
{
- for(Real ti = pImpl_->subSplines_[0].first; ti <= pImpl_->maxTime_; ti = ti + dt)
- {
- Vector3 res; Evaluate(ti, res);
- visitor.Visit(ti, res);
- }
+ return this->pImpl_->subSplines_[0].first;
+}
+
+Real ExactCubic::MaxBound() const
+{
+ return pImpl_->maxTime_;
}
+
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 35ea38fa6d469f9a4a3ed7a76c24647e638c59d7..dbfbe69f64a28238ba99f3e56030098b35e1033c 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -2,6 +2,7 @@
#include "CubicFunction.h"
#include "ExactCubic.h"
#include "SplineVisitor.h"
+#include "BezierCurve.h"
#include <string>
#include <iostream>
@@ -87,6 +88,61 @@ void CubicFunctionTest(bool& error)
}
}
+/*BezierCurve 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);
+
+ spline::T_Vector params;
+ params.push_back(a);
+ params.push_back(b);
+
+ // 2d curve
+ BezierCurve cf(params);
+ Vector3 res1;
+ cf.Evaluate(0, res1);
+ Vector3 x20 = a ;
+ ComparePoints(x20, res1, errMsg + "2(0) ", error);
+
+ Vector3 x21 = b;
+ cf.Evaluate(1, res1);
+ ComparePoints(x21, res1, errMsg + "2(1) ", error);
+
+ //3d curve
+ params.push_back(c);
+ BezierCurve cf3(params);
+ cf3.Evaluate(0, res1);
+ ComparePoints(a, res1, errMsg + "3(0) ", error);
+
+ cf3.Evaluate(1, res1);
+ ComparePoints(c, res1, errMsg + "3(1) ", error);
+
+ //4d curve
+ params.push_back(d);
+ BezierCurve cf4(params, 0.4, 2);
+ cf4.Evaluate(0.4, res1);
+ ComparePoints(a, res1, errMsg + "3(0) ", error);
+
+ cf4.Evaluate(2, res1);
+ ComparePoints(d, res1, errMsg + "3(1) ", error);
+
+ if(cf.Evaluate(-0.4, res1))
+ {
+ error = true;
+ std::cout << "Evaluation of cubic cf2 error, 0.4 should be an out of range value\n";
+ }
+ if(cf.Evaluate(1.1, res1))
+ {
+ error = true;
+ std::cout << "Evaluation of cubic cf2 error, 1.1 should be an out of range value\n";
+ }
+}
+
/*Exact Cubic Function tests*/
void ExactCubicNoErrorTest(bool& error)
{
@@ -112,6 +168,16 @@ void ExactCubicNoErrorTest(bool& error)
error = true;
std::cout << "Evaluation of exactCubic error, 1.2 should be an out of range value\n";
}
+ if(exactCubic.MaxBound() != 1)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 1\n";
+ }
+ if(exactCubic.MinBound() != 0)
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 1\n";
+ }
}
void ExactCubicPointsCrossedTest(bool& error)
@@ -134,8 +200,6 @@ void ExactCubicPointsCrossedTest(bool& error)
/*Cubic Visitor tests*/
#include <vector>
-#include<Eigen/StdVector>
-
namespace spline
{
typedef std::vector<Vector3,Eigen::aligned_allocator<Vector3> > T_Vector;
@@ -201,6 +265,7 @@ int main(int argc, char *argv[])
ExactCubicNoErrorTest(error);
ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
SplineVisitorTestFunction(error); // checks that given wayPoints are crossed
+ BezierCurveTest(error);
if(error)
{
std::cout << "There were some errors\n";