Path.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno    
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------



/* @{ */

#ifndef PATH_H
#define PATH_H

#ifdef MIP_HOST_APPLE
#include <applePatch.h>
#endif

#include <iostream>
#include <math.h>
#include <Spaces.h>
#include <Time.h>


using namespace std;

enum PathType{
 NULL_PATH,
 ASTEROID,
 CARDIOID,
 CLOTHOID,
 ELLIPSE,
 EIGHT,
 SEGMENT,
 PATH_NUM
};

static const char* PathNames[PATH_NUM] = {
 "Null Path(NotInitialized)",
 "Asteroid",
 "Cardioid",
 "Clothoid",
 "Ellipse",
 "Eight",
 "Segment"
};



class AsteroidParam{
 private:
  Decimal _A;
  
 public:
  AsteroidParam(){
   setA(1.0);
  }
  AsteroidParam(Decimal A){
   setA(A);
  }
  AsteroidParam(const AsteroidParam& p){
   _A=p._A;
  }
  AsteroidParam& operator=(const AsteroidParam& p);
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setA(Decimal value){
   _A=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Decimal A(){
   return _A;
  }
  
  string print();
};

class CardioidParam{
 private:
  Decimal _A;
  
 public:
  CardioidParam(){
   setA(1.0);
  }
  CardioidParam(Decimal A){
   setA(A);
  }
  CardioidParam(const CardioidParam& p){
   _A=p._A;
  }
  CardioidParam& operator=(const CardioidParam& p);
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setA(Decimal value){
   _A=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Decimal A(){
   return _A;
  }
  
  string print();
};

class ClothoidParam{
 private:
  Decimal _A;
  Decimal _tau;
  Decimal _sommaParziale;
 public:
  ClothoidParam(){
   setA(1.0);
  }
  ClothoidParam(Decimal A){
   setA(A);
  }
  
  void setA(Decimal value){
   _A=value;
  }
  
  void setTau(Decimal value){
   _tau=value;
  }
  
  void setSommaParziale(Decimal value){
   _sommaParziale=value;
  }
  
  
  Decimal A(){
   return _A;
  }
  
  Decimal tau(){
   return _tau;
  }
  
  Decimal sommaParziale(){
   return _sommaParziale;
  }
};

class EllipseParam{
 private:
  Decimal _R1; 
  Decimal _R2; 
 public:
  EllipseParam(){
   setR1(1);
   setR2(1);
  }
  EllipseParam(Decimal R1, Decimal R2){
   setR1(R1);
   setR2(R2);
  }
  
  EllipseParam(const EllipseParam& p){
   _R1=p._R1;
   _R2=p._R2;
  }
  
  EllipseParam& operator=(const EllipseParam& p){
   if (this != &p){
    _R1=p._R1;
    _R2=p._R2;
   }
   return *this;
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setR1(Decimal value){
   _R1=value;
  };
  
  void setR2(Decimal value){
   _R2=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Decimal R1(){
   return _R1;
  }
  
  Decimal R2(){
   return _R2;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<"Horizontal axis\t"<<_R1<<endl;
   s<<"Vertical axis\t"<<_R2<<endl;
   return s.str();
  }
};

class EightParam{
 private:
  Decimal _R1; 
  Decimal _R2; 
 public:
  EightParam(){
   setR1(1);
   setR2(1);
  }
  EightParam(Decimal R1, Decimal R2){
   setR1(R1);
   setR2(R2);
  }
  
  EightParam(const EightParam& p){
   _R1=p._R1;
   _R2=p._R2;
  }
  
  EightParam& operator=(const EightParam& p){
   if (this != &p){
    _R1=p._R1;
    _R2=p._R2;
   }
   return *this;
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setR1(Decimal value){
   _R1=value;
  }
  
  void setR2(Decimal value){
   _R2=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Decimal R1(){
   return _R1;
  }
  
  Decimal R2(){
   return _R2;
  };
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<"Horizontal axis\t"<<_R1<<endl;
   s<<"Vertical axis\t"<<_R2<<endl;
   return s.str();
  }
};

class SegmentParam{
 private:
  Position _A; 
  Position _B; 
 public:
  SegmentParam(){
   Position A=Position(0.0,0.0);
   Position B=Position(1.0,1.0);
   setA(A);
   setB(B);
  }
  SegmentParam(Position A,Position B){
   setA(A);
   setB(B);
  }
  
  SegmentParam(const SegmentParam& p){
   _A=p._A;
   _B=p._B;
  }
  
  SegmentParam& operator=(const SegmentParam& p){
   if (this != &p){
    _A=p._A;
    _B=p._B;
   }
   return *this;
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setA(Position value){
   _A=value;
  }
     
  void setB(Position value){
   _B=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Position A(){
   return _A;
  }
  
  Position B(){
   return _B;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<"Starting point\t"<<_A.print()<<endl;
   s<<"Ending point\t"<<_B.print()<<endl;
   return s.str();
  }
};

//Path = partialX(s), partialY(s), theta(s) e derivate
class Path{
 private:
  Position _center;    
  Position _centredPos;  
  Position _centredPosp; 
  Position _centredPospp; 
  Position _pos;     
  Position _posp;     
  Position _pospp;    
  Angle _theta;
  Angle _thetap;
  Angle _thetapp;
  Angle _rotation;
  bool _clockwise;
  PathType _pathType;
   
 public:
  Path(){
   Position origin=Position(0.0,0.0);
   setCenter(origin);
   setPos(origin);
   setPosp(origin);
   setPospp(origin);
   setTheta(0.0);
   setThetap(0.0);
   setThetapp(0.0);
   setRotation(0.0);
   unsetClockwise();
   setPathType(NULL_PATH);
  };
  
  Path(const Path& path){
   _center=path._center;
   _centredPos=path._centredPos;
   _centredPosp=path._centredPosp;
   _centredPospp=path._centredPospp;
   _pos=path._pos;
   _posp=path._posp;
   _pospp=path._pospp;
   _theta=path._theta;
   _thetap=path._thetap;
   _thetapp=path._thetapp;
   _rotation=path._rotation;
   _clockwise=path._clockwise;
   _pathType=path._pathType;
   
  }
  
  Path& operator=(const Path& path){
   if (this != &path){
    _center=path._center;
    _centredPos=path._centredPos;
    _centredPosp=path._centredPosp;
    _centredPospp=path._centredPospp;
    _pos=path._pos;
    _posp=path._posp;
    _pospp=path._pospp;
    _theta=path._theta;
    _thetap=path._thetap;
    _thetapp=path._thetapp;
    _rotation=path._rotation;
    _clockwise=path._clockwise;
    _pathType=path._pathType;
   }
   return *this;
  }
  
  bool operator!=(const Path& path){
   return( _center!=path._center       ||
       _rotation!=path._rotation     ||
       _clockwise!=path._clockwise    ||
       _pathType!=path._pathType);
  }
  
  virtual Position evalCentredPos(Decimal s)=0;
  
  virtual Position evalCentredPosp(Decimal s)=0;
  
  virtual Position evalCentredPospp(Decimal s)=0;
  
  virtual Angle evalTheta(Decimal s)=0;

  virtual Angle evalThetap(Decimal s, Decimal sp)=0;

  virtual Angle evalThetapp(Decimal s, Decimal sp,Decimal spp)=0;
  
  Position rotate(Position p, Angle theta){
   Position rotP = Position(p.x()*cos(theta.dCast2Pi())-p.y()*sin(theta.dCast2Pi()),
                p.x()*sin(theta.dCast2Pi())+p.y()*cos(theta.dCast2Pi()));
   return rotP;
  }
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setCenter(Position value){
   _center=value;
  }
  
  void setCentredPos(Position value){
   _centredPos=value;
  }
  
  void setCentredPosp(Position value){
   _centredPosp=value;
  }
  
  void setCentredPospp(Position value){
   _centredPospp=value;
  }
  
  void setPos(Position value){
   _pos=value;
  }
  
  void setPosp(Position value){
   _posp=value;
  }
  
  void setPospp(Position value){
   _pospp=value;
  }
  
  void setTheta(Angle value){
   _theta=value;
  }
  
  void setThetap(Angle value){
   _thetap=value;
  }
  
  void setThetapp(Angle value){
   _thetapp=value;
  }
  
  void evalPos(Decimal s){
   evalCentredPos(s);
   Position rotatedCentredPos=(Position)rotate(centredPos(),rotation());
   setCentredPos(rotatedCentredPos);
   _pos=centredPos()+center();
  }
  
  void evalPosp(Decimal s, Decimal sp){
   evalCentredPosp(s);
   Position rotatedCentredPosp=(Position)rotate(centredPosp(),rotation());
   setCentredPosp(rotatedCentredPosp);
   _posp=centredPosp()*sp;
  }
  
  void evalPospp(Decimal s, Decimal sp,Decimal spp){
   evalCentredPospp(s);
   Position rotatedCentredPospp=(Position)rotate(centredPospp(),rotation());
   setCentredPospp(rotatedCentredPospp);
   _pospp=centredPospp()*(sp*sp)+centredPosp()*spp;
  }
  
  void setRotation(Angle value){
   _rotation=value;
  }
  
  void setClockwise(){
   _clockwise=true;
  }
  
  void unsetClockwise(){
   _clockwise=false;
  }
  
  void setPathType(PathType value){
   _pathType=value;
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Position center(){
   return _center;
  }
  
  Position centredPos(){
   return _centredPos;
  }
  
  Position centredPosp(){
   return _centredPosp;
  }
  
  Position centredPospp(){
   return _centredPospp;
  }
  
  Position pos(){
   return _pos;
  }
  
  Position posp(){
   return _posp;
  }
  
  Position pospp(){
   return _pospp;
  }
  
  Angle theta(){
   return _theta;
  }
  
  Angle thetap(){
   return _thetap;
  }
  
  Angle thetapp(){
   return _thetapp;
  }
  
  
  Angle rotation(){
   return _rotation;
  }
  
  bool clockwise(){
   return _clockwise;
  }
  
  PathType pathType(){
   return _pathType;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<"Path Type\t"<<PathNames[_pathType]<<endl;
   s<<"Center\t\t"<<_center.print()<<endl;
   if(_clockwise)
    s<<"Clockwise\tYES"<<endl;
   else
    s<<"Clockwise\tNO"<<endl;
   s<<"Rotation\t"<<_rotation.dCast2Pi()<<endl;
   return s.str();
  }
};

class Asteroid : public Path{
 private:
  AsteroidParam _param; 
 public:
  Asteroid(AsteroidParam param){
   Position origin=Position(0.0,0.0);
   setCenter(origin);
   setRotation(0.0);
   _param.setA(param.A());
   setPathType(ASTEROID);
  }
  Asteroid(Position center,AsteroidParam param){
   setCenter(center);
   _param.setA(param.A());
   setPathType(ASTEROID);
  }
  
  Asteroid(Position center,Angle rotation,AsteroidParam param){
   setCenter(center);
   setRotation(rotation);
   _param.setA(param.A());
   setPathType(ASTEROID);
  }
  
  Asteroid(const Asteroid& asteroid):Path(asteroid){
   _param=asteroid._param;
  }
  
  Asteroid& operator=(const Asteroid& asteroid){
   if (this != &asteroid){
    Path::operator=(asteroid);
    _param=asteroid._param;
   }
   return *this;
  }
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s){}

  Angle evalThetap(Decimal s, Decimal sp){}

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(AsteroidParam param){
   _param.setA(param.A());
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  AsteroidParam param(){
   return _param;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<Path::print();
   s<<_param.print()<<endl;
   return s.str();
  }
};


class Cardioid : public Path{
 private:
  CardioidParam _param; 
 public:
  Cardioid(CardioidParam param){
   Position origin=Position(0.0,0.0);
   setCenter(origin);
   setRotation(0.0);
   _param.setA(param.A());
   setPathType(CARDIOID);
  }
  Cardioid(Position center,CardioidParam param){
   setCenter(center);
   _param.setA(param.A());
   setPathType(CARDIOID);
  }
  
  Cardioid(Position center,Angle rotation,CardioidParam param){
   setCenter(center);
   setRotation(rotation);
   _param.setA(param.A());
   setPathType(CARDIOID);
  }
  
  Cardioid(const Cardioid& cardioid):Path(cardioid){
   _param=cardioid._param;
  }
  
  Cardioid& operator=(const Cardioid& cardioid){
   if (this != &cardioid){
    Path::operator=(cardioid);
    _param=cardioid._param;
   }
   return *this;
  }
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s){}

  Angle evalThetap(Decimal s, Decimal sp){}

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(CardioidParam param){
   _param.setA(param.A());
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  CardioidParam param(){
   return _param;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<Path::print();
   s<<_param.print()<<endl;
   return s.str();
  }
};


class Clothoid : public Path{
 private:
 public:
  Decimal _A;
  Decimal _tauPrev;
  Decimal parsumX;
  Decimal parsumY;
  Decimal prevS;
  Decimal r;
//   Position oscCenter();
  Angle _alfa;
  Decimal _Amin;
  Decimal _tauMax;
  
//   Decimal epsilon;
//   Pose cPose;
//   Decimal Kc;
//   Decimal e;
  
  
  
//  public:
  Clothoid(){
   _A=0.5;
   parsumX=0.0;
   parsumY=0.0;
   _tauPrev=0.0;
   prevS=0.0;
   setRotation(0.0);
//    epsilon=0.1;
//    Kc=1/pow(_A,2);
//    e=1.0;
  }
  Clothoid(Decimal A){
   _A=A;
   parsumX=0.0;
   parsumY=0.0;
   _tauPrev=0.0;
   prevS=0.0;
//    epsilon=0.1;
//    Kc=1/pow(_A,2);
//    e=1.0;
  }
  
  Clothoid(Position center,Decimal A){
   setCenter(center);
   _A=A;
   parsumX=0.0;
   parsumY=0.0;
   _tauPrev=0.0;
   prevS=0.0;
//    epsilon=0.1;
//    Kc=1/pow(_A,2);
//    e=1.0;
  }
  
  Clothoid(Position center,Angle rotation,Decimal A){
   setCenter(center);
   setRotation(rotation);
   _A=A;
   parsumX=0.0;
   parsumY=0.0;
   _tauPrev=0.0;
   prevS=0.0;
//    epsilon=0.1;
//    Kc=1/pow(_A,2);
//    e=1.0;
  }
  
  Decimal evalTau(Decimal s){
   return (pow(s,2)/(pow(_A,2)*2));
  }
  
  void evalAmin(Decimal r){
   _Amin=r*sqrt(0.4*_alfa.dCast2Pi());
  }
  
  void evalTauMax(Angle alfa){
   _tauMax=alfa.dCast2Pi()/3.0;
  }
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s){}
  
  Angle evalThetap(Decimal s, Decimal sp){}
  
  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
};


class Ellipse : public Path{
 private:
  EllipseParam _param; 
 public:
  Ellipse(){
   setPathType(ELLIPSE);
  };
  Ellipse(EllipseParam param){
   Position origin=Position(0.0,0.0);
   setCenter(origin);
   setRotation(0.0);
   _param.setR1(param.R1());
   _param.setR2(param.R2());
   setPathType(ELLIPSE);
  }
  Ellipse(Position center,EllipseParam param){
   setCenter(center);
   _param.setR1(param.R1());
   _param.setR2(param.R2());
   setPathType(ELLIPSE);
  }
  
  Ellipse(Position center,Angle rotation,EllipseParam param){
   setCenter(center);
   setRotation(rotation);
   _param.setR1(param.R1());
   _param.setR2(param.R2());
   setPathType(ELLIPSE);
  }
  
  Ellipse(const Ellipse& ellipse):Path(ellipse){
   _param=ellipse._param;
  }
  
  Ellipse& operator=(const Ellipse& ellipse){
   if (this != &ellipse){
    Path::operator=(ellipse);
    _param=ellipse._param;
   }
   return *this;
  }
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s);

  Angle evalThetap(Decimal s, Decimal sp);

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp);
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(EllipseParam param){
   _param.setR1(param.R1());
   _param.setR2(param.R2());
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  EllipseParam param(){
   return _param;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<Path::print();
   s<<_param.print()<<endl;
   return s.str();
  }
};

class Eight : public Path{
 private:
  EightParam _param; 
 public:
  Eight(EightParam param){
   Position origin=Position(0.0,0.0);
   setCenter(origin);
   setRotation(0.0);
   _param.setR1(param.R1());
   _param.setR2(param.R2());
   setPathType(EIGHT);
  }
  Eight(Position center,EightParam param){
   setCenter(center);
   _param.setR1(param.R1());
   _param.setR2(param.R2());
   setPathType(EIGHT);
  }
  
  Eight(Position center,Angle rotation,EightParam param){
   setCenter(center);
   setRotation(rotation);
   _param.setR1(param.R1());
   _param.setR2(param.R2());
   setPathType(EIGHT);
  }
  
  Eight(const Eight& eight):Path(eight){
   _param=eight._param;
  }
  
  Eight& operator=(const Eight& eight){
   if (this != &eight){
    Path::operator=(eight);
    _param=eight._param;
   }
   return *this;
  }
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s){}

  Angle evalThetap(Decimal s, Decimal sp){}

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(EightParam param){
   _param.setR1(param.R1());
   _param.setR2(param.R2());
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  EightParam param(){
   return _param;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<Path::print();
   s<<_param.print()<<endl;
   return s.str();
  }
};

class Segment : public Path{
 private:
  SegmentParam _param; 
 public:
  Segment(){
   setPathType(SEGMENT);
  };
  Segment(SegmentParam param){
   Position origin=Position(0.0,0.0);
   setCenter(origin);
   setRotation(0.0);
   _param.setA(param.A());
   _param.setB(param.B());
   setPathType(SEGMENT);
  };
  
  Segment(const Segment& segment):Path(segment){
   _param=segment._param;
  }
  
  Segment& operator=(const Segment& segment){
   if (this != &segment){
    Path::operator=(segment);
    _param=segment._param;
   }
   return *this;
  }
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s);

  Angle evalThetap(Decimal s, Decimal sp);

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp);
  
/***************************************************************************************************
***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(SegmentParam param){
   _param.setA(param.A());
   _param.setB(param.B());
  }
/***************************************************************************************************
***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  SegmentParam param(){
   return _param;
  }
  
  string print(){
   stringstream s;
   s.precision(3);
   s.setf(ios::fixed,ios::floatfield);
   s<<Path::print();
   s<<_param.print()<<endl;
   return s.str();
  }
};


class Clothoid2Param{
 private:
  Position _A; 
  Position _B; 
  Position _C; 
  Decimal _eps; 
  Decimal _a;  
  Decimal _d;  
 public:
  Clothoid2Param(){
   Position A=Position(0.0,0.0);
   Position B=Position(1.0,1.0); 
   Position C=Position(2.0,2.0);
   setA(A);
   setB(B);
   setB(C);
   setEps(1.0);
   setA(1.0);
   setD(1.0);
  }
  Clothoid2Param(Position A,Position B,Position C,Decimal eps,Decimal a,Decimal d){
   setA(A);
   setB(B);
   setC(C);
   setEps(eps);
   setA(a);
   setD(d);
  }
  
/***************************************************************************************************
  ***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setA(Position value){
   _A=value;
  }
     
  void setB(Position value){
   _B=value;
  }
  
  void setC(Position value){
   _C=value;
  }
  
  void setEps(Decimal value){
   _eps=value;
  };
     
  void setA(Decimal value){
   _a=value;
  };
  
  void setD(Decimal value){
   _d=value;
  };
  
/***************************************************************************************************
  ***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Position A(){
   return _A;
  }
  
  Position B(){
   return _B;
  }
  
  Position C(){
   return _C;
  }
  
  Decimal eps(){
   return _eps;
  };
     
  Decimal a(){
   return _a;
  };
  
  Decimal d(){
   return _d;
  };

};



class Clothoid2 : public Path{
 private:
  Clothoid2Param _param;    
  Decimal _X0,_Xc,_Yc,_Kc,_e; 
  Angle _alfa,_thetac;    
  Pose _ref;         
  Position _start;      
  Position _end;       
  bool _ribalta;//TODO nome
  
  Position Base2RegNormale(Position punto, Pose reg);//TODO verify
  
  Position Reg2BaseRibaltato(Position punto, Pose reg);//TODO verify
  
  Position Reg2BaseNormale(Position punto, Pose reg);//TODO verify
  
  Position Reg2Base(Position punto, Pose reg);//TODO verify
  
  Position Base2Reg(Position punto, Pose reg);//TODO verify
  
  Decimal SF(Decimal z, Decimal coef);//TODO verify
  
  Decimal CF(Decimal z, Decimal coef);//TODO verify
  
  void Assesta();//TODO nome
 public:
  Clothoid2(Clothoid2Param param);
  
  Position evalCentredPos(Decimal s);
  
  Position evalCentredPosp(Decimal s);
  
  Position evalCentredPospp(Decimal s);
  
  Angle evalTheta(Decimal s){}

  Angle evalThetap(Decimal s, Decimal sp){}

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
  
/***************************************************************************************************
  ***********************************        SET FUNCTIONS         ***********************************
***************************************************************************************************/
  void setParam(Clothoid2Param param){
   _param.setA(param.A());
   _param.setB(param.B());
   _param.setC(param.C());
   _param.setEps(_param.eps());
   _param.setA(_param.a());
   _param.setD(_param.d());
  }
  
  void setRibalta(){
   _ribalta=true;
  }
  
  void unsetRibalta(){
   _ribalta=false;
  }
  
  void setX0(Decimal value){
   _X0=value;
  }
  
  void setXc(Decimal value){
   _Xc=value;
  }
 
  void setYc(Decimal value){
   _Yc=value;
  }
  
  void setKc(Decimal value){
   _Kc=value;
  }
  
  void setE(Decimal value){
   _e=value;
  }
  
  void setAlfa (Angle value){
   _alfa=value;
  }
  
  void setThetac (Angle value){
   _thetac=value;
  }
  
  void setStart (Position value){
   _start=value;
  }
  
  void setEnd (Position value){
   _end=value;
  }
 
  void setRiferimento (Pose value){
   _ref=value;
  }
  
/***************************************************************************************************
  ***********************************        GET FUNCTIONS         ***********************************
***************************************************************************************************/
  Clothoid2Param param(){
   return _param;
  }
  
  bool ribalta(){
   return _ribalta;
  }
  
  Decimal X0(){
   return _X0;
  }
  
  Decimal Xc(){
   return _Xc;
  }
  
  Decimal Yc(){
   return _Yc;
  }
  
  Decimal Kc(){
   return _Kc;
  }
  
  Decimal e(){
   return _e;
  }
  
  Angle alfa (){
   return _alfa;
  }
  
  Angle thetac (){
   return _thetac;
  }
  
  Position start (){
   return _start;
  }
  
  Position end (){
   return _end;
  }
  
  Pose riferimento (){
   return _ref;
  }
};


class Bezier : public Path{
 private:
 public:
//   Position _P0;
//   Position _P1;
//   Position _P2;
//   Position _P3;
//   Position _P4;
//   Position _P5;
//   
  vector<Decimal> _weightAtt;
  vector<Decimal> _weightRep;
//   Decimal _w2;
//   Decimal _w3;
//   Decimal _w4;
  vector<Decimal> _xAtt;
  vector<Decimal> _yAtt;
  vector<Decimal> _xRep;
  vector<Decimal> _yRep;
  vector<int> _binomialAtt;
  vector<int> _binomialRep;
  
//  public:
  int factorial(int n){
   int fact=1;
  for (unsigned int i=1;i<=n;i++){
   fact*=i;
  }
  return fact;
  }
  
  
  
  
//   Bezier(Position P0, Position P1, Position P2, Position P3, Position P4, Position P5, Decimal w1, Decimal w2, Decimal w3, Decimal w4){
//    _P0=P0;
//    _P1=P1;
//    _P2=P2;
//    _P3=P3;
//    _P4=P4;
//    _P5=P5;
//    _w1=w1;
//    _w2=w2;
//    _w3=w3;
//    _w4=w4;
//   };
  
  Bezier(vector<Position> pointsAtt){
   unsigned int sizeAtt=pointsAtt.size()-1;
   for (unsigned int i=0;i<=sizeAtt;i++){
    _binomialAtt.push_back(factorial(sizeAtt)/(factorial(i)*factorial(sizeAtt-i)));
    _xAtt.push_back(pointsAtt[i].x());
    _yAtt.push_back(pointsAtt[i].y());
    _weightAtt.push_back(1.0);
   }
   
   
//    _weight[3]=0.0;
//    _weight[2]=0.0;
//     _weight[5]=0.0;
  }
  Position evalCentredPos(Decimal s){
   unsigned int size=_xAtt.size()-1;
   Decimal xAtt=0.0;
   Decimal yAtt=0.0;
   
   Decimal normAtt=0.0;
   Decimal normRep=0.0;
   Decimal x;
   Decimal y;
   
   for (unsigned int i=0;i<=size;i++){//se metto < anziché <= fa un loop
    xAtt+=_weightAtt[i]*_binomialAtt[i]*pow(1.0-s, int(size-i))*pow(s,int(i))*_xAtt[i];
    yAtt+=_weightAtt[i]*_binomialAtt[i]*pow(1.0-s, int(size-i))*pow(s,int(i))*_yAtt[i];
    normAtt+=_weightAtt[i]*_binomialAtt[i]*pow(1.0-s,int(size-i))*pow(s,int(i));
//     cout<<"massa "<<i<<"=\t"<<_weight[i]*_binomial[i]*pow(1.0-s,size-i)*pow(s,i)<<endl;
   }
   xAtt=xAtt/normAtt;
   yAtt=yAtt/normAtt;
   
   

   
//    xRep=_weightRep[0]*s*(s-1.0)*_xRep[0]-_weightRep[1]*s*(s-1.0)*_xRep[1];
//    yRep=_weightRep[0]*s*(s-1.0)*_yRep[0]-_weightRep[1]*s*(s-1.0)*_yRep[1];
   
   cout<<"normAtt\t"<<normAtt<<endl;
/*   
   cout<<"normAtt\t"<<normAtt<<endl;
   cout<<"normRep\t"<<normRep<<endl;*/
   
   x=xAtt;
   y=yAtt;
   
   Position p=Position(x,y);
   setCentredPosp(p);
   return p;
  }
  
  Position evalCentredPosp(Decimal s){};
  
  Position evalCentredPospp(Decimal s){};
  
  Angle evalTheta(Decimal s){}

  Angle evalThetap(Decimal s, Decimal sp){}

  Angle evalThetapp(Decimal s, Decimal sp,Decimal spp){}
};



#endif

/* @} */