123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765 |
- #include "collectconvert.h"
- #include "gnss_coordinate_convert.h"
- #include "geofit.h"
- #include "autoconnect.h"
- int groadid = 10001;
- CollectConvert::CollectConvert(iv::map::collectveh & xcollectveh,OpenDrive * pxodr)
- {
- mcollectveh.CopyFrom(xcollectveh);
- mpxodr = pxodr;
- }
- void CollectConvert::StartConvert()
- {
- threadconvert();
- }
- void CollectConvert::threadconvert()
- {
- OpenDrive * pxodr = mpxodr;
- pxodr->SetHeader(1,1,"adc",1.1,"2022",0,0,0,0,mcollectveh.mlat0(),mcollectveh.mlon0(),360);
- int i;
- for(i=0;i<mcollectveh.mroads_size();i++)
- {
- iv::map::collectvehroad * proad = mcollectveh.mutable_mroads(i);
- ConvertRoad(proad,pxodr);
- }
- for(i=0;i<mcollectveh.mconnects_size();i++)
- {
- iv::map::roadconnect * pconnect = mcollectveh.mutable_mconnects(i);
- convertconnect(pconnect,pxodr);
- }
- AutoConnect pAC(pxodr);
- pAC.Connect();
- }
- void CollectConvert::ConvertRoad(iv::map::collectvehroad *proad, OpenDrive *pxodr)
- {
- if(proad->mpoints_size() == 0)return;
- double flon0,flat0;
- pxodr->GetHeader()->GetLat0Lon0(flat0,flon0);
- // pxodr->GetHeader()->GetLat0Lon0(flon0,flat0);
- double x0,y0;
- GaussProjCal(flon0,flat0,&x0,&y0);
- int i;
- std::vector<iv::collectxy> xvectorcollectxy;
- for(i=0;i<proad->mpoints_size();i++)
- {
- iv::map::collectvehroadpoint * ppoint = proad->mutable_mpoints(i);
- double x,y;
- // double fl1 = ppoint->mflon();
- // double fl2 = ppoint->mflat();
- GaussProjCal(ppoint->mflon(),ppoint->mflat(),&x,&y);
- iv::collectxy xc;
- xc.mLaneColor_Left = ppoint->mlanecolor_left();
- xc.mLaneColor_Right = ppoint->mlanecolor_right();
- xc.mLaneType_Left = ppoint->mlanetype_left();
- xc.mLaneType_Right = ppoint->mlanetype_right();
- xc.height = 0;
- xc.width = ppoint->mflanewidth();
- xc.fhdg = (90 -ppoint->mfheading())*M_PI/180.0;
- xc.x = x-x0 + ppoint->mfdis_left()*cos(xc.fhdg +M_PI/2.0);
- xc.y = y-y0 + ppoint->mfdis_left()*sin(xc.fhdg +M_PI/2.0);
- xvectorcollectxy.push_back(xc);
- if(i>0)
- {
- double fdis =sqrt(pow(xvectorcollectxy[i].x - xvectorcollectxy[i-1].x,2)+pow(xvectorcollectxy[i].y-xvectorcollectxy[i-1].y,2));
- xvectorcollectxy[i].fdistolast = fdis;
- }
- }
- double LINE_ERROR = 0.20;
- std::vector<geobase> xvectorgeo;
- bool bComplete = false;
- int j;
- int ncurpos = 0;
- int nrange = xvectorcollectxy.size();
- bool bHaveLastxyhdg = false;
- double fLastX,fLastY,fLastHdg;
- double fEndX,fEndY,fEndHdg;
- double fStartX,fStartY,fStartHdg;
- while(bComplete == false)
- {
- VectorXd x_veh(nrange);
- VectorXd y_veh(nrange);
- for(j=0;j<nrange;j++)
- {
- x_veh[j] =xvectorcollectxy[j+ncurpos].x;// xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fx;//mvectorrtkdata.at(j+ncurpos).mfrelx;
- y_veh[j] = xvectorcollectxy[j+ncurpos].y;//xvectorvehicle[xvectorvl[j+ncurpos].mvehindex].fy;//mvectorrtkdata.at(j+ncurpos).mfrely;
- }
- bool bArcOk = false;
- bool bLineOk = false;
- double dismax = 0;
- QPointF sp,ep;
- double fR,flen;
- double fhdgstart,fhdgend;
- if(nrange >= 3)
- {
- geofit::Inst().arcfitanddis(x_veh,y_veh,dismax,fR,sp,ep,fhdgstart,fhdgend,flen);
- if(dismax < LINE_ERROR)
- {
- bArcOk = true;
- // std::cout<<" a arc is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
- }
- }
- auto coeffs = polyfit(x_veh, y_veh, 1);
- dismax = 0;
- for(j=0;j<nrange;j++)
- {
- double A = coeffs[1];
- double B = -1;
- double C = coeffs[0];
- double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
- if(dis>dismax)dismax = dis;
- }
- if(dismax < LINE_ERROR)
- {
- bLineOk = true;
- // std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
- }
- if((nrange <= 2)||bLineOk||bArcOk)
- {
- if((bLineOk)||(nrange<=2))
- {
- // std::cout<<"use line"<<std::endl;
- geobase xgeo;
- xgeo.mnStartPoint = ncurpos;
- xgeo.mnEndPoint = ncurpos + nrange -1 ;
- double x0,y0,x1,y1;
- geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[0],
- y_veh[0],x0,y0);
- geofit::Inst().getcrosspoint(coeffs[1],-1,coeffs[0],x_veh[nrange -1],
- y_veh[nrange - 1],x1,y1);
- xgeo.mfA = coeffs[1];
- xgeo.mfB = -1;
- xgeo.mfC = coeffs[0];
- xgeo.mfHdg = geofit::Inst().CalcHdg(x0,y0,x1,y1);
- xgeo.mfX = x0;
- xgeo.mfY = y0;
- xgeo.mfLen = sqrt(pow(x1-x0,2)+pow(y1-y0,2));
- xgeo.mnType = 0;
- fStartX = x0;
- fStartY = y0;
- fStartHdg = xgeo.mfHdg;
- fEndX = x1;
- fEndY = y1;
- fEndHdg = xgeo.mfHdg;
- xvectorgeo.push_back(xgeo);
- }
- else
- {
- if(bArcOk)
- {
- std::cout<<"use arc"<<std::endl;
- geobase xgeo;
- xgeo.mnStartPoint = ncurpos;
- xgeo.mnEndPoint = ncurpos + nrange -1;
- xgeo.mfHdg = fhdgstart;
- xgeo.mfHdgStart = fhdgstart;
- xgeo.mfHdgEnd = fhdgend;
- xgeo.mfX = sp.x();
- xgeo.mfY = sp.y();
- xgeo.mfLen = flen;
- xgeo.mnType = 1;
- xgeo.mfEndX = ep.x();
- xgeo.mfEndY = ep.y();
- xgeo.mR = fR;
- fStartX = xgeo.mfX;
- fStartY = xgeo.mfY;
- fStartHdg = xgeo.mfHdgStart;
- fEndX = xgeo.mfEndX;
- fEndY = xgeo.mfEndY;
- fEndHdg = xgeo.mfHdgEnd;
- xvectorgeo.push_back(xgeo);
- }
- }
- // std::cout<<" a line is ok "<<"pos is "<<ncurpos<<" range is "<<nrange<<" error is "<<dismax<<std::endl;
- if(bHaveLastxyhdg)
- {
- std::vector<geobase> xvectorgeobe = geofit::CreateBezierGeo(fLastX,fLastY,
- fLastHdg,fStartX,fStartY,fStartHdg);
- if(xvectorgeobe.size()>0)
- {
- xvectorgeobe[0].mnStartPoint = ncurpos-1;
- xvectorgeobe[0].mnEndPoint = ncurpos;
- xvectorgeo.insert(xvectorgeo.begin()+(xvectorgeo.size()-1),xvectorgeobe[0]);
- }
- }
- bHaveLastxyhdg = true;
- fLastX = fEndX;
- fLastY = fEndY;
- fLastHdg = fEndHdg;
- // ncurpos = ncurpos + nrange -1;
- ncurpos = ncurpos + nrange; //Skip 1 point, use bezier
- nrange = xvectorcollectxy.size() - ncurpos;
- }
- else
- {
- if(nrange > 30)
- nrange = nrange/2;
- else
- nrange = nrange -1;
- if(nrange<2)nrange = 2;
- }
- if(ncurpos>=(int)(xvectorcollectxy.size()-1))
- {
- if(ncurpos == (int)(xvectorcollectxy.size()-1))
- {
- // std::cout<<"Last section, 2 Points, use line."<<std::endl;
- VectorXd x_veh1(2);
- VectorXd y_veh1(2);
- x_veh1[0] = fLastX;
- y_veh1[0] = fLastY;
- x_veh1[1] = xvectorcollectxy[ncurpos].x;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fx;
- y_veh1[1] = xvectorcollectxy[ncurpos].y;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].fy;
- auto coeffs = polyfit(x_veh1, y_veh1, 1);
- geobase xgeo;
- xgeo.mnStartPoint = ncurpos-1;
- xgeo.mnEndPoint = ncurpos;
- xgeo.mfA = coeffs[1];
- xgeo.mfB = -1;
- xgeo.mfC = coeffs[0];
- xgeo.mfHdg = geofit::Inst().CalcHdg(x_veh1[0],y_veh1[0],x_veh1[1],y_veh1[1]);
- xgeo.mfX = x_veh1[0];
- xgeo.mfY = y_veh1[0];
- xgeo.mfLen = sqrt(pow(x_veh1[1]-x_veh1[0],2)+pow(y_veh1[1]-y_veh1[0],2));
- xgeo.mnType = 0;
- xvectorgeo.push_back(xgeo);
- }
- bComplete = true;
- }
- }
- double snow = 0;
- double froadlen = 0;
- for(i=0;i<(int)xvectorgeo.size();i++)
- {
- geobase * pgeo = &xvectorgeo[i];
- froadlen = froadlen + xvectorgeo[i].mfLen;
- // std::cout<<"geo len: "<<pgeo->mfLen<<" type"<<pgeo->mnType<<std::endl;
- double tems = 0;
- xvectorcollectxy[pgeo->mnStartPoint].s = snow;
- for(j=(pgeo->mnStartPoint+1);j<=pgeo->mnEndPoint;j++)
- {
- xvectorcollectxy[j].s = snow + tems+xvectorcollectxy[j].fdistolast;
- // xvectorvehicle[xvectorvl[j].mvehindex].s = snow + tems + xvectorvehicle[xvectorvl[j].mvehindex].fdistolast;
- tems = tems + xvectorcollectxy[j].fdistolast;
- }
- snow = snow + pgeo->mfLen;
- }
- std::vector<iv::widthabcd> xvectorwidthabcd;
- bComplete = false;
- ncurpos = 0;
- nrange = xvectorcollectxy.size();
- double flanewidtherror = 0.1; //1cm
- while(bComplete == false)
- {
- double ele_coff[4];
- for(j=0;j<4;j++)ele_coff[j] = 0;
- if(nrange>0)ele_coff[0] = xvectorcollectxy[ncurpos].width;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].flanewidth;
- int M = nrange;
- VectorXd x_vehhg(M);
- VectorXd y_vehhg(M);
- for(j=0;j<M;j++)
- {
- x_vehhg[j] = xvectorcollectxy[ncurpos+j].s - xvectorcollectxy[ncurpos].s;// xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].s - xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
- y_vehhg[j] = xvectorcollectxy[ncurpos+j].width;// xvectorvehicle[xvectorvl[ncurpos+j].mvehindex].flanewidth;
- }
- int MX = 3;
- if(M<4)MX = M -1;
- if(MX>3)MX = 3;
- if(MX>0)
- {
- VectorXd coeffs = polyfit(x_vehhg, y_vehhg, MX);
- for(j=0;j<=MX;j++)
- {
- ele_coff[j] = coeffs[j];
- }
- }
- double ferror = 0;
- for(j=0;j<M;j++)
- {
- double s = x_vehhg[j];
- double fvalue = fabs(ele_coff[0] + ele_coff[1]*s + ele_coff[2]*s*s+ele_coff[3]*s*s*s - y_vehhg[j]);
- if(fvalue>ferror)ferror = fvalue;
- }
- if(ferror>flanewidtherror)
- {
- if(nrange>10)nrange = nrange/2;
- else nrange = nrange -1;
- }
- else
- {
- iv::widthabcd xabcd;
- xabcd.s = xvectorcollectxy[ncurpos].s;// xvectorvehicle[xvectorvl[ncurpos].mvehindex].s;
- xabcd.A = ele_coff[0];
- xabcd.B = ele_coff[1];
- xabcd.C = ele_coff[2];
- xabcd.D = ele_coff[3];
- xvectorwidthabcd.push_back(xabcd);
- ncurpos = ncurpos + nrange-1;
- nrange = xvectorcollectxy.size() - ncurpos;
- }
- if(ncurpos>=(int)(xvectorcollectxy.size()-1))
- {
- bComplete = true;
- }
- }
- pxodr->AddRoad(proad->strroadname(),froadlen, QString::number(groadid).toStdString(),"-1");
- Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
- groadid++;
- // p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
- double s = 0;
- j= 0;
- // for(j=0;j<4;j++)
- for(j=0;j<(int)xvectorgeo.size();j++)
- {
- p->AddGeometryBlock();
- GeometryBlock * pgb = p->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo[j].mnType)
- {
- case 0:
- pline = &xvectorgeo[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo[j];
- // std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
- break;
- }
- s = s + xvectorgeo[j].mfLen;
- }
- p->AddLaneSection(0);
- LaneSection * pLS = p->GetLaneSection(0);
- pLS->AddLane(0,0,"none",false);
- Lane * pCenterLane = pLS->GetLastAddedLane();
- iv::map::collectvehroadpoint_LaneType xLastType;
- if(xvectorcollectxy.size()>0)
- {
- xLastType = xvectorcollectxy[0].mLaneType_Left;
- std::string strlanecolor = "white";
- if(xvectorcollectxy[0].mLaneColor_Left == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
- pCenterLane->AddRoadMarkRecord(xvectorcollectxy[0].s,LaneTypeToStr(xLastType),
- "standard",strlanecolor,0.15,"none");
- }
- for(i=1;i<(int)xvectorcollectxy.size();i++)
- {
- if(xvectorcollectxy[i].mLaneType_Left != xLastType)
- {
- xLastType = xvectorcollectxy[i].mLaneType_Left;
- std::string strlanecolor = "white";
- if(xvectorcollectxy[i].mLaneColor_Left == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
- pCenterLane->AddRoadMarkRecord(xvectorcollectxy[i].s,LaneTypeToStr(xLastType),
- "standard",strlanecolor,0.15,"none");
- }
- }
- pLS->AddLane(-1,(-1)*(pLS->GetRightLaneCount()+1),"driving",false);
- Lane * pLane = pLS->GetLastRightLane();
- for(j=0;j<(int)xvectorwidthabcd.size();j++)
- {
- iv::widthabcd * pwa = &xvectorwidthabcd[j];
- pLane->AddWidthRecord(pwa->s,pwa->A,pwa->B,pwa->C,pwa->D);
- }
- if(xvectorcollectxy.size()>0)
- {
- xLastType = xvectorcollectxy[0].mLaneType_Right;
- std::string strlanecolor = "white";
- if(xvectorcollectxy[0].mLaneColor_Right == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
- pLane->AddRoadMarkRecord(xvectorcollectxy[0].s,LaneTypeToStr(xLastType),
- "standard",strlanecolor,0.15,"none");
- }
- for(i=1;i<(int)xvectorcollectxy.size();i++)
- {
- if(xvectorcollectxy[i].mLaneType_Right != xLastType)
- {
- xLastType = xvectorcollectxy[i].mLaneType_Right;
- std::string strlanecolor = "white";
- if(xvectorcollectxy[i].mLaneColor_Right == iv::map::collectvehroadpoint_LaneColor_YELLOW)strlanecolor = "yellow";
- pLane->AddRoadMarkRecord(xvectorcollectxy[i].s,LaneTypeToStr(xLastType),
- "standard",strlanecolor,0.15,"none");
- }
- }
- }
- std::string CollectConvert::LaneTypeToStr(iv::map::collectvehroadpoint_LaneType xType)
- {
- switch (xType) {
- case iv::map::collectvehroadpoint_LaneType_NONE:
- return "none";
- break;
- case iv::map::collectvehroadpoint_LaneType_SOLID:
- return "solid";
- break;
- case iv::map::collectvehroadpoint_LaneType_DASH:
- return "broken";
- break;
- case iv::map::collectvehroadpoint_LaneType_SOLID_SOLID:
- return "solid solid";
- break;
- case iv::map::collectvehroadpoint_LaneType_SOLID_DASH:
- return "solid broken";
- break;
- case iv::map::collectvehroadpoint_LaneType_DASH_SOLID:
- return "broken solid";
- break;
- case iv::map::collectvehroadpoint_LaneType_DASH_DASH:
- return "broken broken";
- break;
- default:
- return "none";
- break;
- }
- }
- void CollectConvert::convertconnect(iv::map::roadconnect *pconnect, OpenDrive *pxodr)
- {
- Road * p1, * p2;
- std::string strroad1name = pconnect->strroad1();
- std::string strroad2name = pconnect->strroad2();
- int i;
- p1 = NULL;
- p2 = NULL;
- double fwidth1,fwidth2;
- for(i=0;i<(int)pxodr->GetRoadCount();i++)
- {
- if(pxodr->GetRoad(i)->GetRoadName() == strroad1name)
- {
- p1 = pxodr->GetRoad(i);
- break;
- }
- }
- for(i=0;i<(int)pxodr->GetRoadCount();i++)
- {
- if(pxodr->GetRoad(i)->GetRoadName() == strroad2name)
- {
- p2 = pxodr->GetRoad(i);
- break;
- }
- }
- if((p1 == NULL)||(p2 == NULL))
- {
- std::cout<<"convertconnect can not find road. "<<std::endl;
- return;
- }
- double startx,starty,starthdg;
- double endx,endy,endhdg;
- p1->GetGeometryCoords(p1->GetRoadLength() - 0.001,startx,starty,starthdg);
- p2->GetGeometryCoords(0,endx,endy,endhdg);
- // std::cout<<"start x : "<<startx<<" start y: "<<starty<<std::endl;
- LaneSection * pLS1 = p1->GetLastLaneSection();
- if(pLS1 == NULL)
- {
- std::cout<<" road p1 no lanesection."<<std::endl;
- return;
- }
- Lane * pLane = NULL;
- for(i=0;i<(int)pLS1->GetLaneCount();i++)
- {
- Lane * pLaneX = pLS1->GetLane(i);
- if(pLaneX->GetId() == -1)
- {
- pLane = pLaneX;
- break;
- }
- }
- if(pLane == NULL)
- {
- std::cout<<"can't get road1 lane."<<std::endl;
- return;
- }
- fwidth1 = pLane->GetWidthValue(p1->GetRoadLength() - 0.001);
- pLS1 = p2->GetLaneSection(0);
- if(pLS1 == NULL)
- {
- std::cout<<" road p2 no lanesection."<<std::endl;
- return;
- }
- pLane = NULL;
- for(i=0;i<(int)pLS1->GetLaneCount();i++)
- {
- Lane * pLaneX = pLS1->GetLane(i);
- if(pLaneX->GetId() == -1)
- {
- pLane = pLaneX;
- break;
- }
- }
- if(pLane == NULL)
- {
- std::cout<<"can't get road2 lane."<<std::endl;
- return;
- }
- fwidth2 = pLane->GetWidthValue(0.001);
- //Create Geo
- double R = 6.0;
- std::vector<geobase> xvectorgeo;
- std::vector<geobase> xvectorgeo1,xvectorgeo2;
- switch(pconnect->mmode())
- {
- case iv::map::roadconnect_ConnectMode_TURN:
- xvectorgeo = geofit::CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
- break;
- case iv::map::roadconnect_ConnectMode_STRAIGHT:
- {
- double fdis = sqrt(pow(startx - endx,2) +pow(starty -endy,2));
- if((fdis<3) || (starthdg == endhdg) )
- {
- xvectorgeo = geofit::CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
- }
- else
- {
- xvectorgeo = geofit::CreateBezierGeo(startx,starty,starthdg,endx,endy,endhdg);
- }
- //
- }
- break;
- case iv::map::roadconnect_ConnectMode_UTURN:
- xvectorgeo = geofit::CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
- break;
- default:
- break;
- }
- if(pconnect->mmode() == iv::map::roadconnect_ConnectMode_UTURN)
- {
- for(i=0;i<(int)xvectorgeo.size()/2;i++)
- {
- xvectorgeo1.push_back(xvectorgeo.at(i));
- }
- for(i=(int)xvectorgeo.size()/2;i<(int)xvectorgeo.size();i++)
- {
- xvectorgeo2.push_back(xvectorgeo.at(i));
- }
- }
- if(xvectorgeo.size() == 0)
- {
- std::cout<<" CollectConvert::convertconnect Create Road Fail."<<std::endl;
- // QMessageBox::warning(this,"warn","Create Road Fail.");
- return;
- }
- double xroadlen = 0;
- if(pconnect->mmode() != iv::map::roadconnect_ConnectMode_UTURN)
- {
- for(i=0;i<(int)xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
- pxodr->AddRoad("",xroadlen, QString::number(groadid).toStdString(),"-1");
- groadid++;
- Road * p = pxodr->GetRoad(pxodr->GetRoadCount() - 1);
- // p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);
- double s = 0;
- int j;
- j= 0;
- for(j=0;j<(int)xvectorgeo.size();j++)
- {
- p->AddGeometryBlock();
- GeometryBlock * pgb = p->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo[j].mnType)
- {
- case 0:
- pline = &xvectorgeo[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo[j];
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
- break;
- }
- s = s + xvectorgeo[j].mfLen;
- }
- p->AddLaneSection(0);
- LaneSection * pLS = p->GetLaneSection(0);
- pLS->SetS(0);
- pLS->AddLane(0,0,"none",false);
- double b = (fwidth2 - fwidth1)/xroadlen;
- pLS->AddLane(-1,-1,"driving",false);
- pLane = pLS->GetLastAddedLane();
- pLane->AddWidthRecord(0,fwidth1,b,0,0);
- }
- else
- {
- double xroadlen1 = 0;
- double xroadlen2 = 0;
- for(i=0;i<(int)xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
- for(i=0;i<(int)xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
- int index1 = pxodr->AddRoad("",xroadlen1, QString::number(groadid).toStdString(),"-1");groadid++;
- int index2 = pxodr->AddRoad("",xroadlen2, QString::number(groadid).toStdString(),"-1");groadid++;
- Road * proad2 = pxodr->GetRoad(index2);
- Road * proad1 = pxodr->GetRoad(index1);
- // OpenDrive * px = &mxodr;
- double s = 0;
- int j;
- j= 0;
- for(j=0;j<(int)xvectorgeo1.size();j++)
- {
- proad1->AddGeometryBlock();
- GeometryBlock * pgb = proad1->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo1[j].mnType)
- {
- case 0:
- pline = &xvectorgeo1[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo1[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo1[j];
- std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3],false);
- break;
- }
- s = s + xvectorgeo1[j].mfLen;
- }
- s=0.0;
- for(j=0;j<(int)xvectorgeo2.size();j++)
- {
- proad2->AddGeometryBlock();
- GeometryBlock * pgb = proad2->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo2[j].mnType)
- {
- case 0:
- pline = &xvectorgeo2[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo2[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo2[j];
- std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
- break;
- }
- s = s + xvectorgeo2[j].mfLen;
- }
- proad1->AddLaneSection(0);
- LaneSection * pLS1 = proad1->GetLaneSection(0);
- pLS1->SetS(0);
- pLS1->AddLane(0,0,"none",false);
- proad2->AddLaneSection(0);
- LaneSection * pLS2 = proad2->GetLaneSection(0);
- pLS2->SetS(0);
- pLS2->AddLane(0,0,"none",false);
- double b1,b2;
- double fwidth3;
- fwidth3 = fwidth1 + (fwidth2 - fwidth1)*(xroadlen1)/(xroadlen1+xroadlen2);
- b1 = (fwidth3 - fwidth1)/xroadlen1;
- b2 = (fwidth2 - fwidth3)/xroadlen2;
- pLS1->AddLane(-1,-1,"driving",false);
- pLane = pLS1->GetLastAddedLane();
- pLane->AddWidthRecord(0,fwidth1,b1,0,0);
- pLS2->AddLane(-1,-1,"driving",false);
- pLane = pLS2->GetLastAddedLane();
- pLane->AddWidthRecord(0,fwidth3,b2,0,0);
- }
- }
|