#include "sideparkcalc.h" #include "math.h" #include 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."<(-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. "<= (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"< 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."<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."<=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."<(-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"< 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."<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."< M_PI)fhdg = fhdg - 2.0*M_PI; while(fhdg <= (-M_PI))fhdg = fhdg + 2.0*M_PI; } SideParkType SideParkCalc::GetSolution(std::vector & xvectorpoint,std::vector & xvectorWheel,std::vector & xvectorlen,double & fTotalLen) { if(mSideParkType == SideParkType::NoSolution) { return SideParkType::NoSolution; } xvectorpoint = mvectorpoint; xvectorWheel = mvectorWheel; xvectorlen = mvectorlen; fTotalLen = mfTotalLen; return mSideParkType; }