123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566 |
- #include "sideparkcalc.h"
- #include "math.h"
- #include <iostream>
- SideParkCalc::SideParkCalc(double x,double y,double hdg)
- {
- mx = x;
- my = y;
- mhdg = hdg;
- normalhdg(mhdg);
- mSideParkType = SideParkType::NoSolution;
- }
- void SideParkCalc::CalcPark()
- {
- if(mx< 0.5)
- {
- return;
- }
- if(fabs(mhdg) > M_PI/6.0)
- {
- return;
- }
- SideParkMode parkmode = CalcParkMode();
- if(parkmode == SideParkMode::ParkAtRight)
- {
- ParkAtRightCalc();
- }
- else
- ParkAtLeftCalc();
- }
- SideParkMode SideParkCalc::CalcParkMode()
- {
- if(my>=0)return SideParkMode::ParkAtRight;
- return SideParkMode::ParkAtLeft;
- }
- void SideParkCalc::ParkAtRightCalc()
- {
- if(mhdg<=0.0)
- {
- ParkAtRightCalc_Model1();
- }
- else
- {
- ParkAtRightCalc_Model2();
- }
- }
- void SideParkCalc::ParkAtRightCalc_Model1()
- {
- double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
- dy6 = 0;
- dx6 = mfLastDirectDis;
- // double fMaxdx5 = mfRaidus * sin(mfMaxAngle );
- double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
- // double fMaxdx3 = fMaxdx5;
- // double fMaxdy3 = fMaxdy5;
- dx2 = mfRaidus * sin(fabs(mhdg));
- dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
- double fang ;//= mfMaxAngle;
- if((my+dy2) >= (2*fMaxdy5) )
- {
- fang = mfMaxAngle;
- }
- else
- {
- fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
- }
- dx5 = mfRaidus * sin(fang);
- dy5 = mfRaidus * (1.0 - cos(fang));
- dx3 = dx5;
- dy3 = dy5;
- double thetax = mx - dx6 - dx3 - dx5 - dx2;
- if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
- {
- std::cout<<" divide error."<<std::endl;
- return;
- }
- dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
- dx1 = thetax - dx4;
- dy1 = dx1 * tan(fabs(mhdg));
- dy4 = my + dy1 +dy2 -dy3-dy5;
- std::cout<<" compute. "<<std::endl;
- if(dx1>(-0.1)&&(dx4>(-0.1)))
- {
- mSideParkType = SideParkType::FiveStep;
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel * (-1.0));
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel);
- mvectorWheel.push_back(0.0);
- double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
- x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
- x3 = x4 + dx5;y3 = y4 + dy5;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
- hdg2 = hdg3;
- if(dx4>0.1)
- {
- x2 = x3 + dx4;y2 = y3 + dy4;
- flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
- }
- else
- {
- x2 = x3;y2 = y3;flen3 = 0;
- }
- flen2 = mfRaidus * (fang - mhdg);
- x1 = mx - dx1;
- y1 = my - dy1;
- hdg1 = mhdg;
- flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
- mvectorlen.push_back(flen1);
- mvectorlen.push_back(flen2);
- mvectorlen.push_back(flen3);
- mvectorlen.push_back(flen4);
- mvectorlen.push_back(flen5);
- mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
- mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
- mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
- mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
- mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
- }
- // if((my+dy2) > (2*fMaxdy3) )
- // {
- // dy3 = fMaxdy3;
- // dx3 = fMaxdx3;
- // dx5 = fMaxdx5;
- // dy5 = fMaxdy5;
- // double thetax = mx - dx6 - fMaxdx3 - fMaxdx5 - dx2;
- // dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - fMaxdy3 - fMaxdy5)/(tan(fabs(mhdg ) + tan(mfMaxAngle )));
- // dx1 = thetax - dx4;
- // dy1 = dx1 * tan(fabs(mhdg));
- // dy4 = my + dy1 + dy2 -dy3-dy5;
- // std::cout<<" compute. "<<std::endl;
- // }
- // else
- // {
- // double fang = acos(1.0 - (my+dy2)/(2.0*mfRaidus)) ;
- // dx5 = mfRaidus * sin(fang);
- // dy5 = mfRaidus * (1.0 - cos(fang));
- // dx3 = dx5;
- // dy3 = dy5;
- // double thetax = mx - dx6 - dx3 - dx5 - dx2;
- // dx4 = (my + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
- // dx1 = thetax - dx4;
- // dy1 = dx1 * tan(fabs(mhdg));
- // dy4 = my + dy1 +dy2 -dy3-dy5;
- // std::cout<<" compute. "<<std::endl;
- // }
- }
- void SideParkCalc::ParkAtRightCalc_Model2()
- {
- double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
- dy5 = 0;
- dx5 = mfLastDirectDis;
- double dx2fix = mfRaidus * sin(mhdg);
- double dy2fix = mfRaidus * (1.0 - cos(mhdg));
- double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
- double fang ;
- if((my+dy2fix) >= (2*fMaxdy4) )
- {
- fang = mfMaxAngle;
- }
- else
- {
- fang = acos(1.0 - (my+dy2fix)/(2.0*mfRaidus)) ;
- }
- dx4 = mfRaidus * sin(fang);
- dy4 = mfRaidus * (1.0 - cos(fang));
- dx2 = dx4 - dx2fix;
- dy2 = dy4 - dy2fix;
- double thetax = mx - dx2 -dx4 -dx5;
- double k1 = tan(mhdg);
- double k3 = tan(fang);
- if(fabs(k1-k3)<0.001)
- {
- dy1 = 0;
- dx1 = 0;
- dy3 = my - dy2 - dy4;
- if(k3 > 0.001)
- {
- dx3 = dy3/k3;
- }
- else
- {
- dx3 = 0;
- }
- if(fabs(mx - dx1 - dx2 -dx3 -dx4 - dx5) > 0.1)
- {
- std::cout<<" no solve"<<std::endl;
- return;
- }
- }
- else
- {
- dx3 = (my - dy2 - dy4 - k1 * thetax)/(k3 - k1);
- dy3 = k3 * dx3;
- dx1 = thetax - dx3;
- dy1 = k1 * dx1;
- }
- if((dx3<-0.1) || (dx1<-0.1))
- {
- if(cos(mhdg)> 0.001)
- {
- double fR = my/(1 - cos(mhdg));
- double Rx = mx - fR*sin(mhdg);
- if((Rx>0.3) && (fR >= mfRaidus))
- {
- mSideParkType = SideParkType::TwoStep;
- mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
- mvectorWheel.push_back(0.0);
- double flen1,flen2;
- flen2 = Rx;
- flen1 = fR*fabs(mhdg);
- mvectorlen.push_back(flen1);
- mvectorlen.push_back(flen2);
- mfTotalLen = flen1 + flen2;
- mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
- std::cout<<" Have One Step soluton."<<std::endl;
- }
- }
- }
- else
- {
- mSideParkType = SideParkType::FiveStep;
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel * (-1.0));
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel);
- mvectorWheel.push_back(0.0);
- double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
- x4 = dx5;y4 = dy5;hdg4 = 0;flen5 = x4;
- x3 = x4 + dx4;y3 = y4 + dy4;hdg3 = fang;flen4 = fabs(fang) *mfRaidus;
- hdg2 = hdg3;
- if(dx3>0.1)
- {
- x2 = x3 + dx3;y2 = y3 + dy3;
- flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
- }
- else
- {
- x2 = x3;y2 = y3;flen3 = 0;
- }
- flen2 = mfRaidus * (fang - mhdg);
- x1 = mx - dx1;
- y1 = my - dy1;
- hdg1 = mhdg;
- flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
- mvectorlen.push_back(flen1);
- mvectorlen.push_back(flen2);
- mvectorlen.push_back(flen3);
- mvectorlen.push_back(flen4);
- mvectorlen.push_back(flen5);
- mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
- mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
- mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
- mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
- mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
- std::cout<<" have compute."<<std::endl;
- }
- std::cout<<" compute. "<<std::endl;
- }
- void SideParkCalc::ParkAtLeftCalc()
- {
- if(mhdg>=0.0)
- {
- ParkAtLeftCalc_Model1();
- }
- else
- {
- ParkAtLeftCalc_Model2();
- }
- }
- void SideParkCalc::ParkAtLeftCalc_Model1()
- {
- double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5,dx6,dy6;
- dy6 = 0;
- dx6 = mfLastDirectDis;
- double fMaxdy5 = mfRaidus *(1.0 - cos(mfMaxAngle));
- dx2 = mfRaidus * sin(fabs(mhdg));
- dy2 = mfRaidus * (1 - cos(fabs(mhdg)));
- // double x = mx;
- double y = fabs(my);
- double fang ;//= mfMaxAngle;
- if((y+dy2) >= (2*fMaxdy5) )
- {
- fang = mfMaxAngle;
- }
- else
- {
- fang = acos(1.0 - (y+dy2)/(2.0*mfRaidus)) ;
- }
- dx5 = mfRaidus * sin(fang);
- dy5 = mfRaidus * (1.0 - cos(fang));
- dx3 = dx5;
- dy3 = dy5;
- double thetax = mx - dx6 - dx3 - dx5 - dx2;
- if(fabs(tan(fabs(mhdg )) + tan(fang ))<0.0001)
- {
- std::cout<<" divide error."<<std::endl;
- return;
- }
- dx4 = (y + thetax * tan(fabs(mhdg)) +dy2 - dy3 - dy5)/(tan(fabs(mhdg ) + tan(fang )));
- dx1 = thetax - dx4;
- dy1 = dx1 * tan(fabs(mhdg));
- dy4 = y + dy1 +dy2 -dy3-dy5;
- std::cout<<" compute. "<<std::endl;
- if(dx1>(-0.1)&&(dx4>(-0.1)))
- {
- mSideParkType = SideParkType::FiveStep;
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel * (1.0));
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel * (-1.0));
- mvectorWheel.push_back(0.0);
- double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
- x4 = dx6;y4 = dy6;hdg4 = 0;flen5 = x4;
- x3 = x4 + dx5;y3 = y4 - dy5;hdg3 = fang * (-1);flen4 = fabs(fang) *mfRaidus;
- hdg2 = hdg3;
- if(dx4>0.1)
- {
- x2 = x3 + dx4;y2 = y3 - dy4;
- flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
- }
- else
- {
- x2 = x3;y2 = y3;flen3 = 0;
- }
- flen2 = mfRaidus * (mhdg + fang);
- x1 = mx - dx1;
- y1 = my + dy1;
- hdg1 = mhdg;
- flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
- mvectorlen.push_back(flen1);
- mvectorlen.push_back(flen2);
- mvectorlen.push_back(flen3);
- mvectorlen.push_back(flen4);
- mvectorlen.push_back(flen5);
- mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
- mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
- mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
- mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
- mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
- }
- }
- void SideParkCalc::ParkAtLeftCalc_Model2()
- {
- double dx1,dy1,dx2,dy2,dx3,dy3,dx4,dy4,dx5,dy5;
- dy5 = 0;
- dx5 = mfLastDirectDis;
- double dx2fix = mfRaidus * sin(fabs(mhdg));
- double dy2fix = mfRaidus * (1.0 - cos(fabs(mhdg)));
- double fMaxdy4 = mfRaidus *(1.0 - cos(mfMaxAngle));
- double fang ;
- double y = fabs(my);
- if((y+dy2fix) >= (2*fMaxdy4) )
- {
- fang = mfMaxAngle;
- }
- else
- {
- fang = acos(1.0 - (y+dy2fix)/(2.0*mfRaidus)) ;
- }
- dx4 = mfRaidus * sin(fang);
- dy4 = mfRaidus * (1.0 - cos(fang));
- dx2 = dx4 - dx2fix;
- dy2 = dy4 - dy2fix;
- double thetax = mx - dx2 -dx4 -dx5;
- double k1 = tan(mhdg);
- double k3 = tan(fang);
- if(fabs(k1-k3)<0.001)
- {
- dy1 = 0;
- dx1 = 0;
- dy3 = y - dy2 - dy4;
- if(k3 > 0.001)
- {
- dx3 = dy3/k3;
- }
- else
- {
- dx3 = 0;
- }
- if(fabs(mx - dx1 - dx2 - dx3 -dx4 - dx5) > 0.1)
- {
- std::cout<<" no solve"<<std::endl;
- return;
- }
- }
- else
- {
- dx3 = (y - dy2 - dy4 - k1 * thetax)/(k3 - k1);
- dy3 = k3 * dx3;
- dx1 = thetax - dx3;
- dy1 = k1 * dx1;
- }
- if((dx3<-0.1) || (dx1<-0.1))
- {
- if(cos(fabs(mhdg))> 0.001)
- {
- double fR = y/(1 - cos(fabs(mhdg)));
- double Rx = mx - fR*sin(fabs(mhdg));
- if((Rx>0.3) && (fR >= mfRaidus))
- {
- std::cout<<" Have One Step soluton."<<std::endl;
- mSideParkType = SideParkType::TwoStep;
- mvectorWheel.push_back(mfMaxWheel * mfRaidus/fR);
- mvectorWheel.push_back(0.0);
- double flen1,flen2;
- flen2 = Rx;
- flen1 = fR*fabs(mhdg);
- mvectorlen.push_back(flen1);
- mvectorlen.push_back(flen2);
- mfTotalLen = flen1 + flen2;
- mvectorpoint.push_back(iv::SideParkPoint(Rx,0,0));
- std::cout<<" Have One Step soluton."<<std::endl;
- }
- }
- }
- else
- {
- mSideParkType = SideParkType::FiveStep;
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel * (1.0));
- mvectorWheel.push_back(0.0);
- mvectorWheel.push_back(mfMaxWheel *(-1.0));
- mvectorWheel.push_back(0.0);
- double x1,x2,y1,y2,x3,y3,x4,y4,hdg1,hdg2,hdg3,hdg4,flen1,flen2,flen3,flen4,flen5;
- x4 = dx5;y4 = dy5 * (-1.0);hdg4 = 0;flen5 = x4;
- x3 = x4 + dx4;y3 = y4 - dy4;hdg3 = fang * (-1.0);flen4 = fabs(fang) *mfRaidus;
- hdg2 = hdg3;
- if(dx3>0.1)
- {
- x2 = x3 + dx3;y2 = y3 - dy3;
- flen3 = sqrt(pow(x2 - x3,2) + pow(y2 - y3,2));
- }
- else
- {
- x2 = x3;y2 = y3;flen3 = 0;
- }
- flen2 = mfRaidus * (fang + mhdg);
- x1 = mx - dx1;
- y1 = my + dy1;
- hdg1 = mhdg;
- flen1 = sqrt(pow(dx1,2) + pow(dy1,2));
- mvectorlen.push_back(flen1);
- mvectorlen.push_back(flen2);
- mvectorlen.push_back(flen3);
- mvectorlen.push_back(flen4);
- mvectorlen.push_back(flen5);
- mfTotalLen = flen1 + flen2 + flen3 + flen4 + flen5;
- mvectorpoint.push_back(iv::SideParkPoint(x1,y1,hdg1));
- mvectorpoint.push_back(iv::SideParkPoint(x2,y2,hdg2));
- mvectorpoint.push_back(iv::SideParkPoint(x3,y3,hdg3));
- mvectorpoint.push_back(iv::SideParkPoint(x4,y4,hdg4));
- std::cout<<" have compute."<<std::endl;
- }
- std::cout<<" compute. "<<std::endl;
- }
- void SideParkCalc::normalhdg(double & fhdg)
- {
- while(fhdg > M_PI)fhdg = fhdg - 2.0*M_PI;
- while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI;
- }
- SideParkType SideParkCalc::GetSolution(std::vector<iv::SideParkPoint> & xvectorpoint,std::vector<double> & xvectorWheel,std::vector<double> & xvectorlen,double & fTotalLen)
- {
- if(mSideParkType == SideParkType::NoSolution)
- {
- return SideParkType::NoSolution;
- }
- xvectorpoint = mvectorpoint;
- xvectorWheel = mvectorWheel;
- xvectorlen = mvectorlen;
- fTotalLen = mfTotalLen;
- return mSideParkType;
- }
|