geofit.h 2.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384
  1. #ifndef GEOFIT_H
  2. #define GEOFIT_H
  3. #include <iostream>
  4. #include <map>
  5. #include <math.h>
  6. #include <vector>
  7. #include <map>
  8. #include <Eigen/Core>
  9. #include <Eigen/QR>
  10. #include <Eigen/Dense>
  11. #include <Eigen/Cholesky>
  12. #include <Eigen/LU>
  13. #include <Eigen/SVD>
  14. #include <QList>
  15. // inlcude iostream and string libraries
  16. #include <iostream>
  17. #include <fstream>
  18. #include <string>
  19. using namespace std;
  20. using namespace Eigen;
  21. class geobase
  22. {
  23. public:
  24. double mfLen;
  25. double mfX;
  26. double mfY;
  27. double mfHdg;
  28. int mnType; //0 Line 1 Circle 2 Bezel.
  29. int mnStartPoint;
  30. int mnEndPoint;
  31. double mfA;
  32. double mfB; //A*x + B*y + C
  33. double mfC;
  34. double mR;
  35. double mfCenterX;
  36. double mfCenterY;
  37. double mfHdgStart;
  38. double mfHdgEnd;
  39. double fCurv;
  40. double mfEndX;
  41. double mfEndY;
  42. double mfu[4];
  43. double mfv[4];
  44. };
  45. Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
  46. class geofit
  47. {
  48. public:
  49. public:
  50. geofit();
  51. inline double GetBazierDis(geobase * parc,double xnow,double ynow);
  52. QList<double> arcfitanddis(Eigen::VectorXd xvals, Eigen::VectorXd yvals,double & fdismax,double &R,QPointF & sp,QPointF & ep,double & fhdgstart,double & fhdgend,double & flen);
  53. 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,
  54. int & ntype,double * fabc,double * fhdg, double * fxy,double * flen);
  55. static double CalcHdg(double x0,double y0,double x1,double y1);
  56. int getcrosspoint(double A,double B,double C,double x,double y,double & xc,double & yc);
  57. std::vector<geobase> getgeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,Eigen::VectorXi etype);
  58. std::vector<geobase> getlinegeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int nbase);
  59. std::vector<geobase> getarcgeo(Eigen::VectorXd xvals, Eigen::VectorXd yvals,int nbase);
  60. static geofit & Inst();
  61. static std::vector<geobase> CreateLineGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg);
  62. static std::vector<geobase> CreateTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double R);
  63. static std::vector<geobase> CreateUTurnGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg,double fextend);
  64. static std::vector<geobase> CreateBezierGeo(double startx,double starty,double starthdg,double endx,double endy,double endhdg);
  65. };
  66. #endif // GEOFIT_H