TimeScaling.h

#ifndef TIME_SCALING_H
#define TIME_SCALING_H

#include <KrisLibrary/planning/GeneralizedBezierCurve.h>
#include <KrisLibrary/spline/TimeSegmentation.h>
#include <KrisLibrary/spline/PiecewisePolynomial.h>
#include <KrisLibrary/math/vector.h>
#include <vector>
#include <utility>
using namespace Math;
using namespace std;

class TimeScaling
{
public:
  int TimeToSegment(Real t) const;
  Real TimeToParam(Real t) const;
  Real TimeToParam(int segment,Real t) const;
  Real TimeToParamDeriv(int segment,Real t) const;
  Real TimeToParamAccel(int segment,Real t) const;
  int ParamToSegment(Real s) const;
  Real ParamToTime(Real s) const;
  Real ParamToTime(int segment,Real s) const;

  bool SolveMinTime(const Vector& vmin,const Vector& vmax,
                    const Vector& amin,const Vector& amax,
                    const vector<Real>& paramdivs,
                    const vector<Vector>& dxMins,const vector<Vector>& dxMaxs,
                    const vector<Vector>& ddxMins,const vector<Vector>& ddxMaxs,
                    Real ds0=-1,Real dsEnd=-1,
        vector<pair<int,int> >* velocityLimitedVariables=NULL,
        vector<pair<int,int> >* accelerationLimitedSegments=NULL);

  bool SolveMinTime(const Vector& vmin,const Vector& vmax,
                    const Vector& amin,const Vector& amax,
                    const GeneralizedCubicBezierSpline& path,
                    Real ds0=-1,Real dsEnd=-1);

  bool SolveMinTime(const Vector& vmin,const Vector& vmax,
                    const Vector& amin,const Vector& amax,
                    const vector<Real>& paramdivs,
                    const vector<Vector>& dxs,
                    Real ds0=-1,Real dsEnd=-1);

  void ConditionMinTime(vector<Real>& paramdivs,vector<Vector>& dxs,
                        vector<Vector>& dxMins,vector<Vector>& dxMaxs,
                        vector<Vector>& ddxMins,vector<Vector>& ddxMaxs);

  bool SolveMinTimeArcLength(const Vector& vmin,const Vector& vmax,
                             const Vector& amin,const Vector& amax,
                             const vector<Real>& paramdivs,
                             const vector<Vector>& dxMins,const vector<Vector>& dxMaxs,
                             const vector<Vector>& ddxMins,const vector<Vector>& ddxMaxs,
                             Real ds0=-1,Real dsEnd=-1);
  
  bool SolveMinTimeArcLength(const Vector& vmin,const Vector& vmax,
                             const Vector& amin,const Vector& amax,
                             const vector<Real>& paramdivs,
                             const vector<Vector>& dxs,
                             Real ds0=-1,Real dsEnd=-1);

  void GetTimeToParam(Spline::PiecewisePolynomial& poly) const;

  Spline::TimeSegmentation params,times;
  vector<Real> ds;
};



class TimeScaledBezierCurve
{
public:
  bool OptimizeTimeScaling(const Vector& vmin,const Vector& vmax,const Vector& amin,const Vector& amax);
  void GetPiecewiseLinear(std::vector<Real>& times,std::vector<Config>& milestones) const;
  void GetDiscretizedPath(Real dt,std::vector<Config>& milestones) const;
  Real EndTime() const;
  void Eval(Real t,Vector& x) const;
  void Deriv(Real t,Vector& dx) const;
  void Accel(Real t,Vector& ddx) const;

  void Plot(const char* fn,const Vector& vmin,const Vector& vmax,const Vector& amin,const Vector& amax,Real res=1e-3);

  GeneralizedCubicBezierSpline path;
  Spline::TimeSegmentation pathSegments;  //optionally set to the partial sums of path.durations
  TimeScaling timeScaling;
};

bool OptimizeTimeScaling(const GeneralizedCubicBezierSpline& path,
                         const Vector& vmin,const Vector& vmax,
                         const Vector& amin,const Vector& amax,
                         TimeScaling& scaling);

#endif