Skip to content
GitLab
Commit 395f9991 authored by JasonChmn's avatar JasonChmn
Browse files

[All] Set indentation as spaces

parent 83feb504
Hide whitespace changes
Inline Side-by-side
include/curves/MathDefs.h
View file @ 395f9991
......@@ -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
......
include/curves/bezier_curve.h
View file @ 395f9991
......@@ -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.
......
include/curves/cubic_hermite_spline.h
View file @ 395f9991
......@@ -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*/
};
......
include/curves/curve_abc.h
View file @ 395f9991
......@@ -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
include/curves/curve_constraint.h
View file @ 395f9991
......@@ -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
include/curves/exact_cubic.h
View file @ 395f9991
......@@ -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
......
include/curves/piecewise_curve.h
View file @ 395f9991
......@@ -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
......
tests/Main.cpp
View file @ 395f9991
......@@ -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;