12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182 |
- #ifndef SIDEPARKCALC_H
- #define SIDEPARKCALC_H
- #include <math.h>
- #include <vector>
- enum SideParkMode
- {
- ParkAtLeft =1,
- ParkAtRight =2
- };
- enum SideParkType
- {
- FiveStep = 1, //Wh
- TwoStep = 2, //When hdg is big, y is small, use 2 steps is ok. 1 point
- NoSolution = 3
- };
- namespace iv {
- struct SideParkPoint
- {
- SideParkPoint(double x,double y,double fhdg)
- {
- mx = x;
- my = y;
- mhdg = fhdg;
- }
- double mx;
- double my;
- double mhdg;
- };
- }
- class SideParkCalc
- {
- public:
- SideParkCalc(double x,double y,double hdg,double fRadius = 5.0,double MaxWheel = 430.0,double MaxAngle = 45.0,double fLastDirectDis = 0.3);
- public:
- void CalcPark();
- SideParkType GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen);
- private:
- double mfLastDirectDis = 0.3;
- double mfRaidus = 5.0;
- double mfMaxAngle = 45.0 *M_PI/180.0;
- double mfMaxWheel = 430.0;
- std::vector<iv::SideParkPoint> mvectorpoint;
- SideParkType mSideParkType; //five steps / 2 steps / no solution
- std::vector<double> mvectorWheel; // 5 values / 2 values /no value
- std::vector<double> mvectorlen; //5 values / 2 values /no value
- double mfTotalLen = 0.0;
- private:
- double mx,my,mhdg;
- bool mbPark = false;
- private:
- SideParkMode CalcParkMode();
- void ParkAtRightCalc();
- void ParkAtRightCalc_Model1();
- void ParkAtRightCalc_Model2();
- void ParkAtLeftCalc();
- void ParkAtLeftCalc_Model1();
- void ParkAtLeftCalc_Model2();
- void normalhdg(double & fhdg);
- };
- #endif // SIDEPARKCALC_H
|