diff --git a/include/curves/MathDefs.h b/include/curves/MathDefs.h
index 18cced66cf22e1f7c39943e695a388b693daa5a7..251daa63e98d40f0582743cfe67c76c2379b8255 100644
--- a/include/curves/MathDefs.h
+++ b/include/curves/MathDefs.h
@@ -26,20 +26,20 @@ namespace curves{
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
- Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
- _Matrix_Type_ m_sigma = svd.singularValues();
+ Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
+ _Matrix_Type_ m_sigma = svd.singularValues();
- double pinvtoler= 1.e-6; // choose your tolerance widely!
+ double pinvtoler= 1.e-6; // choose your tolerance widely!
- _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)
- {
- m_sigma_inv(i,i)=1.0/m_sigma(i);
- }
- }
- pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
+ _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)
+ {
+ m_sigma_inv(i,i)=1.0/m_sigma(i);
+ }
+ }
+ pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
} // namespace curves
diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index cb8597886131ed69417aa9b65d409e8b4fd228dd..42fd8b11cafac14b3adb5c1231fe805f1048c72b 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -32,16 +32,16 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, 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;
+ typedef Point point_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
typedef curve_constraints<point_t> curve_constraints_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
- public:
+ public:
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
@@ -99,22 +99,22 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
- ///\brief Destructor
- ~bezier_curve()
- {
- // NOTHING
- }
+ ///\brief Destructor
+ ~bezier_curve()
+ {
+ // NOTHING
+ }
- private:
-// bezier_curve(const bezier_curve&);
+ private:
+// bezier_curve(const bezier_curve&);
// bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
- public:
- /// \brief Evaluation of the bezier curve at time t.
- /// \param t : time when to evaluate the curve.
- /// \return \f$x(t)\f$ point corresponding on curve at time t.
+ public:
+ /// \brief Evaluation of the bezier curve at time t.
+ /// \param t : time when to evaluate the curve.
+ /// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
@@ -128,7 +128,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
return evalHorner(t);
}
- }
+ }
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
@@ -384,7 +384,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
virtual time_t max() const{return T_max_;}
/*Helpers*/
- public:
+ public:
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
/*const*/ time_t T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index 1eeda8dafd7bb1e657ce0adb0dfd4809addc5bc5..687d06e7c2f4e49e3c79e28b2b1b6fc804eb4a6a 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -63,15 +63,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
- /// \brief Constructor.
- /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
- /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
+ /// \brief Constructor.
+ /// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
+ /// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
/// \param time_control_points : vector containing time for each waypoint.
///
- template<typename In>
- cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
- {
- // Check size of pairs container.
+ template<typename In>
+ 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));
size_ = size;
if(Safe && size < 1)
@@ -85,30 +85,30 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
control_points_.push_back(*it);
}
setTime(time_control_points);
- }
+ }
- /// \brief Destructor.
+ /// \brief Destructor.
virtual ~cubic_hermite_spline(){}
/*Operations*/
- public:
+ public:
/// \brief Evaluation of the cubic hermite spline at time t.
/// \param t : time when to evaluate the spline.
/// \return \f$p(t)\f$ point corresponding on spline at time t.
///
- virtual Point operator()(const Time t) const
+ virtual Point operator()(const Time t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
+ throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
}
if (size_ == 1)
{
- return control_points_.front().first;
+ return control_points_.front().first;
}
else
{
- return evalCubicHermiteSpline(t, 0);
+ return evalCubicHermiteSpline(t, 0);
}
}
@@ -353,17 +353,17 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
return is_positive;
}
- /*Operations*/
+ /*Operations*/
/*Helpers*/
- public:
+ 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*/
+ /*Helpers*/
};
diff --git a/include/curves/curve_abc.h b/include/curves/curve_abc.h
index d6afacb9b3c342e037434eff4a07ec94779e4671..2624d1a8b0ec3ba7c564f75f892bf19d8a0a2633 100644
--- a/include/curves/curve_abc.h
+++ b/include/curves/curve_abc.h
@@ -25,23 +25,23 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct curve_abc : std::unary_function<Time, Point>
{
- typedef Point point_t;
- typedef Time time_t;
+ typedef Point point_t;
+ typedef Time time_t;
/* Constructors - destructors */
- public:
- /// \brief Constructor.
+ public:
+ /// \brief Constructor.
curve_abc(){}
- /// \brief Destructor.
+ /// \brief Destructor.
virtual ~curve_abc(){}
/* Constructors - destructors */
/*Operations*/
- public:
- /// \brief Evaluation of the cubic spline at time t.
- /// \param t : time when to evaluate the spine
- /// \return \f$x(t)\f$, point corresponding on curve at time t.
+ public:
+ /// \brief Evaluation of the cubic spline at time t.
+ /// \param t : time when to evaluate the spine
+ /// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const = 0;
@@ -53,18 +53,18 @@ struct curve_abc : std::unary_function<Time, Point>
/*Operations*/
/*Helpers*/
- public:
- /// \brief Get the minimum time for which the curve is defined.
- /// \return \f$t_{min}\f$, lower bound of time range.
- virtual time_t min() const = 0;
- /// \brief Get the maximum time for which the curve is defined.
- /// \return \f$t_{max}\f$, upper bound of time range.
- virtual time_t max() const = 0;
+ public:
+ /// \brief Get the minimum time for which the curve is defined.
+ /// \return \f$t_{min}\f$, lower bound of time range.
+ virtual time_t min() const = 0;
+ /// \brief Get the maximum time for which the curve is defined.
+ /// \return \f$t_{max}\f$, upper bound of time range.
+ virtual time_t max() const = 0;
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
/*Helpers*/
- };
+ };
} // namespace curves
#endif //_STRUCT_CURVE_ABC
diff --git a/include/curves/curve_constraint.h b/include/curves/curve_constraint.h
index eb88d1cd7026a9ea65aead04ddc442ecd932a911..11426967bc5a1235b293c1a8b38537454d6ae19b 100644
--- a/include/curves/curve_constraint.h
+++ b/include/curves/curve_constraint.h
@@ -22,16 +22,16 @@ namespace curves
template <typename Point>
struct curve_constraints
{
- typedef Point point_t;
+ typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
- ~curve_constraints(){}
+ ~curve_constraints(){}
- point_t init_vel;
- point_t init_acc;
- point_t end_vel;
- point_t end_acc;
+ point_t init_vel;
+ point_t init_acc;
+ point_t end_vel;
+ point_t end_acc;
};
} // namespace curves
#endif //_CLASS_CUBICZEROVELACC
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index cbbcbf52ed934813c02792c4bcf87ecce7322e40..24a872079ba0737b330c79db1c4add1d3ca47fab 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -41,12 +41,12 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
- typedef Point point_t;
+ 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 Time time_t;
+ typedef Numeric num_t;
typedef SplineBase spline_t;
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
@@ -54,14 +54,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
typedef curve_constraints<point_t> spline_constraints;
- /* Constructors - destructors */
- 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.
+ /* Constructors - destructors */
+ 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>
- exact_cubic(In wayPointsBegin, In wayPointsEnd)
+ template<typename In>
+ exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
/// \brief Constructor.
@@ -82,7 +82,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic(const exact_cubic& other)
: curve_abc_t(), subSplines_(other.subSplines_) {}
- /// \brief Destructor.
+ /// \brief Destructor.
virtual ~exact_cubic(){}
std::size_t getNumberSplines()
@@ -249,13 +249,13 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
private:
//exact_cubic& operator=(const exact_cubic&);
- /* Constructors - destructors */
+ /* Constructors - destructors */
- /*Operations*/
- public:
- /// \brief Evaluation of the cubic spline at time t.
+ /*Operations*/
+ public:
+ /// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spline
- /// \return \f$x(t)\f$ point corresponding on spline at time t.
+ /// \return \f$x(t)\f$ point corresponding on spline at time t.
///
virtual point_t operator()(const time_t t) const
{
@@ -295,22 +295,22 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
}
- /*Operations*/
+ /*Operations*/
- /*Helpers*/
- public:
+ /*Helpers*/
+ public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$ lower bound of time range.
num_t virtual min() const{return subSplines_.front().min();}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$ upper bound of time range.
num_t virtual max() const{return subSplines_.back().max();}
- /*Helpers*/
+ /*Helpers*/
- /*Attributes*/
+ /*Attributes*/
public:
t_spline_t subSplines_; // const
- /*Attributes*/
+ /*Attributes*/
};
} // namespace curves
#endif //_CLASS_EXACTCUBIC
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index 1481e83aa12b53450f374294d4bbb0fafc829f97..fe99f43736a54a2663d51c022ae4b8b9d63f43bc 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -31,36 +31,36 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
>
struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
- typedef Point point_t;
- typedef T_Point t_point_t;
- typedef Time time_t;
- typedef Numeric num_t;
+ typedef Point point_t;
+ typedef T_Point t_point_t;
+ typedef Time time_t;
+ typedef Numeric num_t;
typedef Curve curve_t;
typedef typename std::vector < curve_t > t_curve_t;
typedef typename std::vector< Time > t_time_t;
- public:
+ public:
- /// \brief Constructor.
+ /// \brief Constructor.
/// Initialize a piecewise curve by giving the first curve.
/// \param pol : a polynomial curve.
///
- piecewise_curve(const curve_t& cf)
- {
- size_ = 0;
- add_curve(cf);
+ piecewise_curve(const curve_t& cf)
+ {
+ size_ = 0;
+ add_curve(cf);
time_curves_.push_back(cf.min());
T_min_ = cf.min();
- }
+ }
- virtual ~piecewise_curve(){}
+ virtual ~piecewise_curve(){}
- virtual Point operator()(const Time t) const
+ virtual Point operator()(const Time t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
- throw std::out_of_range("can't evaluate piecewise curve, out of range");
+ //std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
+ throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
return curves_.at(find_interval(t))(t);
}
@@ -74,7 +74,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
- throw std::invalid_argument("can't evaluate piecewise curve, out of range");
+ throw std::invalid_argument("can't evaluate piecewise curve, out of range");
}
return (curves_.at(find_interval(t))).derivate(t, order);
}
@@ -83,66 +83,66 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// is equal to \f$T_{max}\f$ of the actual piecewise curve.
/// \param cf : curve to add.
///
- void add_curve(const curve_t& cf)
- {
- // Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
- if (size_!=0)
- {
+ void add_curve(const curve_t& cf)
+ {
+ // Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
+ if (size_!=0)
+ {
if (!(fabs(cf.min()-T_max_)<std::numeric_limits<Time>::epsilon()))
{
throw std::invalid_argument("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
}
- }
- curves_.push_back(cf);
- size_ = curves_.size();
- T_max_ = cf.max();
- time_curves_.push_back(T_max_);
- }
+ }
+ curves_.push_back(cf);
+ size_ = curves_.size();
+ T_max_ = cf.max();
+ time_curves_.push_back(T_max_);
+ }
/// \brief Check if the curve is continuous of order given.
/// \param order : order of continuity we want to check.
/// \return True if the curve is continuous of order given.
///
- bool is_continuous(const std::size_t order)
- {
+ bool is_continuous(const std::size_t order)
+ {
double margin = 0.001;
- bool isContinuous = true;
- std::size_t i=0;
- point_t value_end, value_start;
- while (isContinuous && i<(size_-1))
- {
- curve_t current = curves_.at(i);
- curve_t next = curves_.at(i+1);
-
- if (order == 0)
- {
- value_end = current(current.max());
- value_start = next(next.min());
- }
- else
- {
- value_end = current.derivate(current.max(),order);
- value_start = next.derivate(next.min(),order);
- }
-
- if ((value_end-value_start).norm() > margin)
- {
- isContinuous = false;
- }
- i++;
- }
- return isContinuous;
- }
-
-
- private:
-
- /// \brief Get index of the interval corresponding to time t for the interpolation.
+ bool isContinuous = true;
+ std::size_t i=0;
+ point_t value_end, value_start;
+ while (isContinuous && i<(size_-1))
+ {
+ curve_t current = curves_.at(i);
+ curve_t next = curves_.at(i+1);
+
+ if (order == 0)
+ {
+ value_end = current(current.max());
+ value_start = next(next.min());
+ }
+ else
+ {
+ value_end = current.derivate(current.max(),order);
+ value_start = next.derivate(next.min(),order);
+ }
+
+ if ((value_end-value_start).norm() > margin)
+ {
+ isContinuous = false;
+ }
+ i++;
+ }
+ return isContinuous;
+ }
+
+
+ private:
+
+ /// \brief Get index of the interval corresponding to time t for the interpolation.
/// \param t : time where to look for interval.
/// \return Index of interval for time t.
///
std::size_t find_interval(const Numeric t) const
- {
+ {
// time before first control point time.
if(t < time_curves_[0])
{
@@ -176,20 +176,20 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
/*Helpers*/
- public:
+ 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 T_min_;}
/// \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 T_max_;}
- /*Helpers*/
+ /*Helpers*/
/* Variables */
- t_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
- t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
- std::size_t size_; // Number of segments in piecewise curve = size of curves_
- Time T_min_, T_max_;
+ t_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
+ t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
+ std::size_t size_; // Number of segments in piecewise curve = size of curves_
+ Time T_min_, T_max_;
};
} // end namespace
diff --git a/tests/Main.cpp b/tests/Main.cpp
index bd1cfbb7f1c8f0fd86a4d6d9eb98b9e6b33d76b1..252405f5ab359e1cedb1d655adf476688be2de2f 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -61,10 +61,10 @@ ostream& operator<<(ostream& os, const point_t& pt)
void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const std::string& errmsg, bool& error, bool notequal = false)
{
if(!QuasiEqual((pt1-pt2).norm(), 0.0) && !notequal)
- {
- error = true;
+ {
+ error = true;
std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
- }
+ }
}
template<typename curve1, typename curve2>
@@ -96,9 +96,9 @@ void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error)
void CubicFunctionTest(bool& error)
{
std::string errMsg("In test CubicFunctionTest ; unexpected result for x ");
- point_t a(1,2,3);
- point_t b(2,3,4);
- point_t c(3,4,5);
+ point_t a(1,2,3);
+ point_t b(2,3,4);
+ point_t c(3,4,5);
point_t d(3,6,7);
t_point_t vec;
vec.push_back(a);
@@ -223,42 +223,42 @@ void BezierCurveTest(bool& error)
bool error_in(true);
- try
- {
- cf(-0.4);
- } catch(...)
- {
+ try
+ {
+ cf(-0.4);
+ } catch(...)
+ {
error_in = false;
- }
+ }
if(error_in)
- {
- std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
+ {
+ std::cout << "Evaluation of bezier cf error, -0.4 should be an out of range value\n";
error = true;
- }
+ }
error_in = true;
- try
- {
- cf(1.1);
- } catch(...)
- {
+ try
+ {
+ cf(1.1);
+ } catch(...)
+ {
error_in = false;
- }
+ }
if(error_in)
- {
- std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
+ {
+ std::cout << "Evaluation of bezier cf error, 1.1 should be an out of range value\n";
error = true;
- }
+ }
if (!QuasiEqual(cf.max(),1.0))
- {
- error = true;
- std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
- }
+ {
+ error = true;
+ std::cout << "Evaluation of bezier cf error, MaxBound should be equal to 1\n";
+ }
if (!QuasiEqual(cf.min(),0.0))
- {
- error = true;
- std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
- }
+ {
+ error = true;
+ std::cout << "Evaluation of bezier cf error, MinBound should be equal to 1\n";
+ }
}
#include <ctime>
@@ -557,12 +557,12 @@ void cubicConversionTest(bool& error)
void ExactCubicNoErrorTest(bool& error)
{
// Create an exact cubic spline with 7 waypoints => 6 polynomials defined in [0.0,3.0]
- curves::T_Waypoint waypoints;
- for(double i = 0.0; i <= 3.0; i = i + 0.5)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ curves::T_Waypoint waypoints;
+ for(double i = 0.0; i <= 3.0; i = i + 0.5)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
// Test number of polynomials in exact cubic
std::size_t numberSegments = exactCubic.getNumberSplines();
@@ -580,59 +580,59 @@ void ExactCubicNoErrorTest(bool& error)
}
// Other tests
- try
- {
- exactCubic(0.0);
- exactCubic(3.0);
- }
- catch(...)
- {
- error = true;
- std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
- }
- error = true;
- try
- {
- exactCubic(3.2);
- }
- catch(...)
- {
- error = false;
- }
- if(error)
- {
- std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
- }
+ try
+ {
+ exactCubic(0.0);
+ exactCubic(3.0);
+ }
+ catch(...)
+ {
+ error = true;
+ std::cout << "Evaluation of ExactCubicNoErrorTest error when testing value on bounds\n";
+ }
+ error = true;
+ try
+ {
+ exactCubic(3.2);
+ }
+ catch(...)
+ {
+ error = false;
+ }
+ if(error)
+ {
+ std::cout << "Evaluation of exactCubic cf error, 3.2 should be an out of range value\n";
+ }
if (!QuasiEqual(exactCubic.max(),3.0))
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3\n";
- }
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MaxBound should be equal to 3\n";
+ }
if (!QuasiEqual(exactCubic.min(),0.0))
- {
- error = true;
- std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0\n";
- }
+ {
+ error = true;
+ std::cout << "Evaluation of exactCubic error, MinBound should be equal to 0\n";
+ }
}
/*Exact Cubic Function tests*/
void ExactCubicTwoPointsTest(bool& error)
{
// Create an exact cubic spline with 2 waypoints => 1 polynomial defined in [0.0,1.0]
- curves::T_Waypoint waypoints;
- for(double i = 0.0; i < 2.0; ++i)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
- exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+ curves::T_Waypoint waypoints;
+ for(double i = 0.0; i < 2.0; ++i)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
+ exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
- point_t res1 = exactCubic(0);
- std::string errmsg0("in ExactCubicTwoPointsTest, Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(point_t(0,0,0), res1, errmsg0, error);
+ point_t res1 = exactCubic(0);
+ std::string errmsg0("in ExactCubicTwoPointsTest, Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(point_t(0,0,0), res1, errmsg0, error);
- res1 = exactCubic(1);
- ComparePoints(point_t(1,1,1), res1, errmsg0, error);
+ res1 = exactCubic(1);
+ ComparePoints(point_t(1,1,1), res1, errmsg0, error);
// Test number of polynomials in exact cubic
std::size_t numberSegments = exactCubic.getNumberSplines();
@@ -650,21 +650,21 @@ void ExactCubicTwoPointsTest(bool& error)
void ExactCubicOneDimTest(bool& error)
{
- curves::T_WaypointOne waypoints;
- point_one zero; zero(0,0) = 9;
- point_one one; one(0,0) = 14;
- point_one two; two(0,0) = 25;
- waypoints.push_back(std::make_pair(0., zero));
- waypoints.push_back(std::make_pair(1., one));
- waypoints.push_back(std::make_pair(2., two));
- exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
-
- point_one res1 = exactCubic(0);
- std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)");
- ComparePoints(zero, res1, errmsg, error);
-
- res1 = exactCubic(1);
- ComparePoints(one, res1, errmsg, error);
+ curves::T_WaypointOne waypoints;
+ point_one zero; zero(0,0) = 9;
+ point_one one; one(0,0) = 14;
+ point_one two; two(0,0) = 25;
+ waypoints.push_back(std::make_pair(0., zero));
+ waypoints.push_back(std::make_pair(1., one));
+ waypoints.push_back(std::make_pair(2., two));
+ exact_cubic_one exactCubic(waypoints.begin(), waypoints.end());
+
+ point_one res1 = exactCubic(0);
+ std::string errmsg("in ExactCubicOneDim Error While checking that given wayPoints are crossed (expected / obtained)");
+ ComparePoints(zero, res1, errmsg, error);
+
+ res1 = exactCubic(1);
+ ComparePoints(one, res1, errmsg, error);
}
void CheckWayPointConstraint(const std::string& errmsg, const double step, const curves::T_Waypoint&, const exact_cubic_t* curve, bool& error )
@@ -686,11 +686,11 @@ void CheckDerivative(const std::string& errmsg, const double eval_point, const s
void ExactCubicPointsCrossedTest(bool& error)
{
- curves::T_Waypoint waypoints;
- for(double i = 0; i <= 1; i = i + 0.2)
- {
- waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
- }
+ curves::T_Waypoint waypoints;
+ for(double i = 0; i <= 1; i = i + 0.2)
+ {
+ waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+ }
exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
@@ -1338,12 +1338,12 @@ int main(int /*argc*/, char** /*argv[]*/)
cubicConversionTest(error);
if(error)
- {
+ {
std::cout << "There were some errors\n";
- return -1;
- } else
- {
- std::cout << "no errors found \n";
- return 0;
- }
+ return -1;
+ } else
+ {
+ std::cout << "no errors found \n";
+ return 0;
+ }
}