Commit 4dd18a3a authored by JasonChmn's avatar JasonChmn
Browse files

Add piecewise_curve.h

parent abe6ce3d
Pipeline #4757 passed with stage
in 1 minute and 59 seconds
/**
* \file piecewise_curve.h
* \brief class allowing to create a piecewise curve.
* \author Jason C.
* \date 05/2019
*/
#ifndef _CLASS_PIECEWISE_CURVE
#define _CLASS_PIECEWISE_CURVE
#include "curve_abc.h"
#include "curve_conversion.h"
#include <typeinfo>
namespace curves
{
/// \class PiecewiseCurve.
/// \brief Represent a piecewise curve. We can add some new curve,
/// but the starting time of the curve to add should be equal to the ending time of the actual
/// piecewise_curve.<br>\ Example : A piecewise curve composed of three curves cf0,
/// cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between
/// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
/// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$,
/// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>,
typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >,
typename Curve= curve_abc<Time, Numeric, Dim, Safe, Point>
>
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 Curve curve_t;
typedef typename std::vector < curve_t > t_curve_t;
typedef typename std::vector< Time > t_time_t;
public:
/// \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);
time_curves_.push_back(cf.min());
T_min_ = cf.min();
}
virtual ~piecewise_curve(){}
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");
}
return curves_.at(find_interval(t))(t);
}
/// \brief Evaluate the derivative of order N of curve at time t.
/// \param t : time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
///
virtual Point derivate(const Time t, const std::size_t order) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::invalid_argument("can't evaluate piecewise curve, out of range");
}
return (curves_.at(find_interval(t))).derivate(t, order);
}
/// \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// 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 && cf.min()!=T_max_)
{
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_);
}
/// \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)
{
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.
/// \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])
{
return 0;
}
// time is after last control point time
if(t > time_curves_[size_-1])
{
return size_-1;
}
std::size_t left_id = 0;
std::size_t right_id = size_-1;
while(left_id <= right_id)
{
const std::size_t middle_id = left_id + (right_id - left_id)/2;
if(time_curves_.at(middle_id) < t)
{
left_id = middle_id+1;
}
else if(time_curves_.at(middle_id) > t)
{
right_id = middle_id-1;
}
else
{
return middle_id;
}
}
return left_id-1;
}
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
Time virtual min() const{return 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*/
/* 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_;
};
} // end namespace
#endif // _CLASS_PIECEWISE_CURVE
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment