modularization/src/tool/map_lanetoxodr/geofit.h

#ifndef GEOFIT_H
#define GEOFIT_H

#include <iostream>
#include <map>
#include <math.h>

#include <vector>
#include <map>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/LU>
#include <Eigen/SVD>

#include <QList>

// inlcude iostream and string libraries
#include <iostream>
#include <fstream>
#include <string>


using namespace std;
using namespace Eigen;


class geobase
{
public:
    double  mfLen;
    double mfX;
    double mfY;
    double mfHdg;
    int  mnType; //0 Line 1 Circle 2 Bezel.
    int mnStartPoint;
    int mnEndPoint;

    double mfA;
    double mfB;   //A*x + B*y + C
    double mfC;

    double mR;
    double mfCenterX;
    double mfCenterY;
    double mfHdgStart;
    double mfHdgEnd;
    double fCurv;
    double mfEndX;
    double mfEndY;

    double mfu[4];
    double mfv[4];
};

Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);

class geofit
{
public:

public:
    geofit();
    inline double GetBazierDis(geobase * parc,double xnow,double ynow);
    QList<double> arcfitanddis(Eigen::VectorXd xvals, Eigen::VectorXd yvals,double & fdismax,double &R,QPointF & sp,QPointF & ep,double & fhdgstart,double & fhdgend,double & flen);
    int CreateBezier(double xstart, double ystart, double hdg_start, double xend, double yend, double hdg_end, double uratio, double vratio,double * u,double * v,double & s,
                     int & ntype,double * fabc,double * fhdg, double * fxy,double * flen);
    static double CalcHdg(double x0,double y0,double x1,double y1);
    int getcrosspoint(double A,double B,double C,double x,double y,double & xc,double & yc);
    std::vector<geobase> getgeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,Eigen::VectorXi etype);
    std::vector<geobase> getlinegeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int nbase);
    std::vector<geobase> getarcgeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int nbase);

    static geofit & Inst();

    static std::vector<geobase> CreateLineGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg);
    static std::vector<geobase> CreateTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double R);
    static std::vector<geobase> CreateUTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double fextend);
    static std::vector<geobase> CreateBezierGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg);

};

#endif // GEOFIT_H