1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084 |
- #include "autoconnect.h"
- #include <vector>
- #include <math.h>
- #include <iostream>
- namespace iv {
- struct laneconnect
- {
- unsigned int nroadid1;
- unsigned int nroadid2;
- int nlaneid1;
- int nlaneid2;
- unsigned int nstart1; //1 start 2 end;
- unsigned int nstart2; //1 start 2 end;
- };
- struct prenextlane
- {
- unsigned int nroadid;
- int nlaneidcur;
- int nlaneidoth;
- int nconnect; //1 start 2 end
- };
- struct ivjunctionlanelink
- {
- int incominglane;
- int connectlane;
- };
- struct ivjunctionlink
- {
- int incommingroad;
- int connectroad;
- int connectpoint; //1 start 2 end
- int nid;
- std::vector<ivjunctionlanelink> xvectorlanelink;
- };
- struct ivjunction
- {
- std::vector<ivjunctionlink> xvectorjunctionlink;
- int nid;
- int nopid = -1;
- // int nsameid = -1; //is same as from id .used for merge same.
- };
- struct roadconnect
- {
- unsigned int nroadid;
- std::vector<iv::prenextlane> xvectorpre;
- std::vector<iv::prenextlane> xvectornxt;
- bool mbJunctionpre = false;
- bool mbJunctionnxt = false;
- bool mbInJunction = false;
- int nprejunctionindex;
- int nnxtjunctionindex;
- };
- struct roadpoint
- {
- unsigned int nRoadID;
- unsigned int nstart; //1 start 2 end;
- int nlane;
- double x;
- double y;
- double z;
- double mfhdg;
- bool bIsEnterRoad = false; //true enter this road, false out this road.
- };
- }
- namespace iv {
- struct lanewidthabcd
- {
- int nlane;
- double A;
- double B;
- double C;
- double D;
- };
- }
- double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
- {
- double frtn = 0;
- int i;
- int nlanecount = xvectorlwa.size();
- for(i=0;i<nlanecount;i++)
- {
- if(nlane == xvectorlwa.at(i).nlane)
- {
- iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
- frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
- break;
- }
- }
- return frtn;
- }
- std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
- {
- std::vector<iv::lanewidthabcd> xvectorlwa;
- if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
- int i;
- int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- iv::lanewidthabcd xlwa;
- LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
- xlwa.nlane = pRoad->GetLaneSection(0)->GetLane(i)->GetId();
- if(pLW != 0)
- {
- xlwa.A = pLW->GetA();
- xlwa.B = pLW->GetB();
- xlwa.C = pLW->GetC();
- xlwa.D = pLW->GetD();
- }
- else
- {
- xlwa.A = 0;
- xlwa.B = 0;
- xlwa.C = 0;
- xlwa.D = 0;
- }
- xvectorlwa.push_back(xlwa);
- }
- return xvectorlwa;
- }
- inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
- {
- int i;
- double off = 0;
- double a,b,c,d;
- if(xvectorIndex.size() == 0)return off;
- for(i=0;i<(xvectorIndex.size()-1);i++)
- {
- a = xvectorLWA[xvectorIndex[i]].A;
- b = xvectorLWA[xvectorIndex[i]].B;
- c = xvectorLWA[xvectorIndex[i]].C;
- d = xvectorLWA[xvectorIndex[i]].D;
- if(xvectorLWA[xvectorIndex[i]].nlane != 0)
- {
- off = off + a + b*s + c*s*s + d*s*s*s;
- }
- else
- {
- off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
- }
- }
- i = xvectorIndex.size()-1;
- a = xvectorLWA[xvectorIndex[i]].A;
- b = xvectorLWA[xvectorIndex[i]].B;
- c = xvectorLWA[xvectorIndex[i]].C;
- d = xvectorLWA[xvectorIndex[i]].D;
- off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
- if(nlane < 0) off = off*(-1.0);
- // std::cout<<"s is "<<s<<std::endl;
- return off;
- }
- std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
- {
- std::vector<int> xvectorindex;
- int i;
- if(nlane >= 0)
- {
- for(i=0;i<=nlane;i++)
- {
- int j;
- int nsize = xvectorLWA.size();
- for(j=0;j<nsize;j++)
- {
- if(i == xvectorLWA.at(j).nlane )
- {
- xvectorindex.push_back(j);
- break;
- }
- }
- }
- }
- else
- {
- for(i=0;i>=nlane;i--)
- {
- int j;
- int nsize = xvectorLWA.size();
- for(j=0;j<nsize;j++)
- {
- if(i == xvectorLWA.at(j).nlane )
- {
- xvectorindex.push_back(j);
- break;
- }
- }
- }
- }
- return xvectorindex;
- }
- static double getoff(Road *p, int nlane, bool bstart)
- {
- double off = 0;
- std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(p);
- std::vector<int> xvectorindex1;
- xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
- if(bstart)
- {
- off = getoff(nlane,0,xvectorLWA,xvectorindex1);
- }
- else
- {
- off = getoff(nlane,p->GetRoadLength(),xvectorLWA,xvectorindex1);
- }
- return off;
- }
- static std::vector<iv::roadpoint> getstartpoint(Road * p1)
- {
- double startx,starty,starthdg,starthgt;
- double off1;
- int nlane;
- std::vector<iv::roadpoint> xvectorroadpoint;
- if(p1->GetLaneSectionCount() == 0)return xvectorroadpoint;
- int i;
- for(i=0;i<p1->GetLaneSection(0)->GetLaneCount();i++)
- {
- nlane = p1->GetLaneSection(0)->GetLane(i)->GetId();
- if(nlane == 0)continue;
- starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
- off1 = getoff(p1,nlane,true);
- startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
- starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
- LaneOffset * pLO = p1->GetLaneOffset(0);
- double fofflane = 0.0;
- if(pLO != NULL)
- {
- double s_tem = 0;
- fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
- +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
- }
- startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
- starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
- starthgt = 0;
- if(p1->GetElevationCount()>0)
- {
- starthgt = p1->GetElevation(0)->GetA();
- }
- if(nlane<0)
- {
- starthdg = starthdg + M_PI;
- if(starthdg >= 2.0*M_PI)starthdg = starthdg - 2.0*M_PI;
- }
- iv::roadpoint rp;
- rp.nRoadID = atoi(p1->GetRoadId().data());
- rp.nlane = nlane;
- rp.nstart = 1;
- rp.mfhdg = starthdg;
- rp.x = startx;
- rp.y = starty;
- rp.z = starthgt;
- if(nlane < 0)rp.bIsEnterRoad = true;
- if(nlane > 0) rp.bIsEnterRoad = false;
- xvectorroadpoint.push_back(rp);
- }
- return xvectorroadpoint;
- }
- int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
- {
- proad->GetGeometryCoords(proad->GetRoadLength(),x,y,hdg);
- return 0;
- GeometryBlock * pblock = proad->GetLastGeometryBlock();
- RoadGeometry * pgeo = pblock->GetLastGeometry();
- //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
- switch (pgeo->GetGeomType()) {
- case 0:
- {
- GeometryLine * pline = (GeometryLine *)pgeo;
- x = pline->GetX() + pline->GetLength() * cos(pline->GetHdg());
- y = pline->GetY() + pline->GetLength() * sin(pline->GetHdg());
- hdg = pline->GetHdg();
- }
- return 0;
- break;
- case 1:
- {
- GeometryArc * parc = (GeometryArc *)pgeo;
- double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
- double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
- x = x_center + fabs(1.0/parc->GetCurvature()) * cos(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
- y = y_center + fabs(1.0/parc->GetCurvature()) * sin(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
- hdg = parc->GetHdg() + parc->GetLength() * parc->GetCurvature();
- return 0;
- }
- break;
- case 2:
- {
- GeometrySpiral * pspiral = (GeometrySpiral *)pgeo;
- pspiral->GetCoords(pspiral->GetS()+pspiral->GetLength(),x,y,hdg);
- return 0;
- }
- break;
- case 3:
- // QMessageBox::warning(this,"warn","type not supported.");
- break;
- case 4:
- {
- double xtem,ytem;
- double xtem1,ytem1,x1,y1;
- GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
- double s = ppoly3->GetLength();
- if(ppoly3->GetNormal())
- {
- xtem = ppoly3->GetuA() + ppoly3->GetuB() + ppoly3->GetuC() + ppoly3->GetuD() ;
- ytem = ppoly3->GetvA() + ppoly3->GetvB() + ppoly3->GetvC() + ppoly3->GetvD() ;
- }
- else
- {
- xtem = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
- ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
- }
- // xtem = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
- // ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
- x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
- y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
- s = ppoly3->GetLength()*0.999;
- // double fgeolen;
- double fr = 1.0;
- if(s>0)
- {
- double frel = 0.99;
- if(ppoly3->GetNormal() == false)
- {
- xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
- ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
- }
- else
- {
- xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * frel + ppoly3->GetuC() *frel*frel + ppoly3->GetuD()*frel*frel*frel ;
- ytem1 = ppoly3->GetvA() + ppoly3->GetvB()*frel + ppoly3->GetvC()*frel*frel + ppoly3->GetvD()*frel*frel*frel ;
- }
- fr = 1.0;
- // xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * fr + ppoly3->GetuC() * fr*fr + ppoly3->GetuD() * fr*fr*fr ;
- // ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * fr + ppoly3->GetvC() * fr*fr + ppoly3->GetvD() * fr*fr*fr ;
- x1 = xtem1*cos(ppoly3->GetHdg()) - ytem1 * sin(ppoly3->GetHdg()) + ppoly3->GetX();
- y1 = xtem1*sin(ppoly3->GetHdg()) + ytem1 * cos(ppoly3->GetHdg()) + ppoly3->GetY();
- hdg = geofit::CalcHdg(x1,y1,x,y);
- }
- else
- {
- hdg = 0;
- }
- return 0;
- }
- break;
- default:
- // QMessageBox::warning(this,"warn","type not supported.");
- break;
- }
- return -1;
- }
- static std::vector<iv::roadpoint> getendpoint(Road * p1)
- {
- double startx,starty,starthdg,starthgt;
- double off1;
- int nlane;
- std::vector<iv::roadpoint> xvectorroadpoint;
- if(p1->GetLaneSectionCount() == 0)return xvectorroadpoint;
- int i;
- for(i=0;i<p1->GetLastLaneSection()->GetLaneCount();i++)
- {
- nlane = p1->GetLastLaneSection()->GetLane(i)->GetId();
- if(nlane == 0)continue;
- if(GetEndPoint(p1,startx,starty,starthdg) != 0)
- {
- continue;
- }
- off1 = getoff(p1,nlane,false);
- LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);
- double fofflane = 0.0;
- if(pLO!= NULL)
- {
- double s_tem = p1->GetRoadLength() - pLO->GetS();
- fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
- +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
- }
- startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
- starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
- starthgt = 0;
- if(p1->GetElevationCount()>0)
- {
- Elevation * pe = p1->GetLastElevation();
- double s = p1->GetRoadLength();
- double a, b,c,d;
- a = pe->GetA();
- b = pe->GetB();
- c = pe->GetC();
- d = pe->GetD();
- starthgt = a + b*s + c*s*s + d*s*s*s;
- }
- if(nlane<0)
- {
- starthdg = starthdg + M_PI;
- if(starthdg >= 2.0*M_PI)starthdg = starthdg - 2.0*M_PI;
- }
- iv::roadpoint rp;
- rp.nRoadID = atoi(p1->GetRoadId().data());
- rp.nlane = nlane;
- rp.nstart = 2;
- rp.mfhdg = starthdg;
- rp.x = startx;
- rp.y = starty;
- rp.z = starthgt;
- if(nlane < 0)rp.bIsEnterRoad = false;
- if(nlane > 0) rp.bIsEnterRoad = true;
- xvectorroadpoint.push_back(rp);
- }
- return xvectorroadpoint;
- }
- static std::vector<iv::roadpoint> getroadpoint(OpenDrive * pxodr)
- {
- std::vector<iv::roadpoint> xvectorroadpoint;
- int i;
- for(i=0;i<pxodr->GetRoadCount();i++)
- {
- Road * p1 = pxodr->GetRoad(i);
- std::vector<iv::roadpoint> x1,x2;
- x1 = getstartpoint(p1);
- x2 = getendpoint(p1);
- int j;
- for(j=0;j<x1.size();j++)xvectorroadpoint.push_back(x1.at(j));
- for(j=0;j<x2.size();j++)xvectorroadpoint.push_back(x2.at(j));
- }
- for(i=0;i<xvectorroadpoint.size();i++)
- {
- std::cout<<xvectorroadpoint[i].nRoadID<<" "<<xvectorroadpoint[i].nlane<<" "<<xvectorroadpoint[i].nstart<<" "<<xvectorroadpoint[i].x<<" "<<xvectorroadpoint[i].y<<std::endl;
- }
- return xvectorroadpoint;
- }
- static bool IsConn(iv::roadpoint rp1,iv::roadpoint rp2)
- {
- if(rp1.nRoadID == rp2.nRoadID)
- {
- return false;
- }
- if(rp1.nstart == rp2.nstart)
- {
- if(rp1.nlane * rp2.nlane > 0)
- {
- return false;
- }
- }
- if(rp1.bIsEnterRoad == rp2.bIsEnterRoad)
- {
- return false;
- }
- double fdis;
- fdis = sqrt(pow(rp1.x-rp2.x,2)+pow(rp1.y -rp2.y,2));
- if(fdis>1.0)return false;
- // if(rp1.nRoadID == 10011)
- // {
- // int x = 1;
- // x = 2;
- // }
- double fhdgdiff = rp1.mfhdg - rp2.mfhdg;
- if(fhdgdiff>M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
- if(fhdgdiff<(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI;
- if(fabs(fhdgdiff)>1.0)return false;
- return true;
- }
- static std::vector<iv::laneconnect> getlaneconn(std::vector<iv::roadpoint> xvectorroadpoint)
- {
- std::vector<iv::laneconnect> xvectorroadconn;
- int i;
- for(i=0;i<(xvectorroadpoint.size()-1);i++)
- {
- iv::roadpoint rp1;
- rp1 = xvectorroadpoint[i];
- int j;
- for(j=i+1;j<xvectorroadpoint.size();j++)
- {
- iv::roadpoint rp2;
- rp2 = xvectorroadpoint[j];
- if(IsConn(rp1,rp2))
- {
- iv::laneconnect lc;
- lc.nlaneid1 = rp1.nlane;
- lc.nlaneid2 = rp2.nlane;
- lc.nroadid1 = rp1.nRoadID;
- lc.nroadid2 = rp2.nRoadID;
- lc.nstart1 = rp1.nstart;
- lc.nstart2 = rp2.nstart;
- xvectorroadconn.push_back(lc);
- }
- }
- }
- return xvectorroadconn;
- }
- static std::vector<iv::roadconnect> getroadconn(std::vector<iv::laneconnect> xvectorlaneconn)
- {
- std::vector<iv::roadconnect> xvectorroadconn;
- //获得road连接关系
- int i;
- for(i=0;i<xvectorlaneconn.size();i++)
- {
- iv::laneconnect xlc = xvectorlaneconn.at(i);
- bool bhave1,bhave2;
- bhave1 = false;
- bhave2 = false;
- iv::roadconnect * p1, * p2;
- int np1pos,np2pos;
- int j;
- for(j=0;j<xvectorroadconn.size();j++)
- {
- if(xvectorroadconn[j].nroadid == xlc.nroadid1)
- {
- bhave1 = true;
- np1pos = j;
- p1 = &xvectorroadconn[j];
- }
- if(xvectorroadconn[j].nroadid == xlc.nroadid2)
- {
- bhave2 = true;
- np2pos = j;
- p2 = &xvectorroadconn[j];
- }
- if(bhave1 && bhave2)break;
- }
- if(bhave1 == false)
- {
- iv::roadconnect rc;
- rc.nroadid = xlc.nroadid1;
- xvectorroadconn.push_back(rc);
- np1pos = xvectorroadconn.size() -1;
- p1 = &xvectorroadconn[xvectorroadconn.size()-1];
- }
- if(bhave2 == false)
- {
- iv::roadconnect rc;
- rc.nroadid = xlc.nroadid2;
- xvectorroadconn.push_back(rc);
- np2pos = xvectorroadconn.size() -1;
- p2 = &xvectorroadconn[xvectorroadconn.size()-1];
- }
- p1 = &xvectorroadconn[np1pos];
- p2 = &xvectorroadconn[np2pos];
- iv::prenextlane xpx;
- xpx.nlaneidcur = xlc.nlaneid1;
- xpx.nlaneidoth = xlc.nlaneid2;
- xpx.nroadid = xlc.nroadid2;
- xpx.nconnect = xlc.nstart2;
- if(xlc.nstart1 == 1)
- {
- p1->xvectorpre.push_back(xpx);
- }
- else
- {
- p1->xvectornxt.push_back(xpx);
- }
- xpx.nlaneidcur = xlc.nlaneid2;
- xpx.nlaneidoth = xlc.nlaneid1;
- xpx.nroadid = xlc.nroadid1;
- xpx.nconnect = xlc.nstart1;
- if(xlc.nstart2 == 1)
- {
- p2->xvectorpre.push_back(xpx);
- }
- else
- {
- p2->xvectornxt.push_back(xpx);
- }
- }
- //判断road的连接是否是junction
- for(i=0;i<xvectorroadconn.size();i++)
- {
- iv::roadconnect * pr = &xvectorroadconn[i];
- int j;
- int nfirstid;
- if(pr->xvectorpre.size()>0)nfirstid = pr->xvectorpre[0].nroadid;
- for(j=1;j<pr->xvectorpre.size();j++)
- {
- if(pr->xvectorpre[j].nroadid != nfirstid)
- {
- pr->mbJunctionpre = true;
- break;
- }
- }
- if(pr->xvectornxt.size()>0)nfirstid = pr->xvectornxt[0].nroadid;
- for(j=1;j<pr->xvectornxt.size();j++)
- {
- if(pr->xvectornxt[j].nroadid != nfirstid)
- {
- pr->mbJunctionnxt = true;
- break;
- }
- }
- }
- //设置junction区域为juncion
- return xvectorroadconn;
- }
- static std::vector<iv::ivjunction> getjunction(std::vector<iv::roadconnect> * pxvectorroadconn)
- {
- std::vector<iv::ivjunction> xvectorjunction;
- int i;
- for(i=0;i<pxvectorroadconn->size();i++)
- {
- iv::roadconnect * pr = &pxvectorroadconn->at(i);
- if(pr->mbJunctionpre)
- {
- int j;
- iv::ivjunction ij;
- for(j=0;j<pr->xvectorpre.size();j++)
- {
- int nroadid = pr->xvectorpre[j].nroadid;
- int k;
- iv::prenextlane * plc = &pr->xvectorpre[j];
- iv::ivjunctionlanelink ijl;
- ijl.incominglane = plc->nlaneidcur;
- ijl.connectlane = plc->nlaneidoth;
- bool bfindjunction = false;
- int nx = -1;
- for(k=0;k<ij.xvectorjunctionlink.size();k++)
- {
- if(ij.xvectorjunctionlink[k].connectroad == nroadid)
- {
- nx = k;
- bfindjunction = true;
- break;
- }
- }
- if(bfindjunction == false)
- {
- iv::ivjunctionlink ijk;
- ijk.incommingroad = pr->nroadid;
- ijk.connectpoint = plc->nconnect;
- ijk.connectroad = plc->nroadid;
- ij.xvectorjunctionlink.push_back(ijk);
- nx = ij.xvectorjunctionlink.size() - 1;
- }
- iv::ivjunctionlink * pijk = &ij.xvectorjunctionlink[nx];
- pijk->xvectorlanelink.push_back(ijl);
- }
- xvectorjunction.push_back(ij);
- pr->nprejunctionindex = xvectorjunction.size()-1;
- }
- if(pr->mbJunctionnxt)
- {
- int j;
- iv::ivjunction ij;
- for(j=0;j<pr->xvectornxt.size();j++)
- {
- int nroadid = pr->xvectornxt[j].nroadid;
- int k;
- iv::prenextlane * plc = &pr->xvectornxt[j];
- iv::ivjunctionlanelink ijl;
- ijl.incominglane = plc->nlaneidcur;
- ijl.connectlane = plc->nlaneidoth;
- bool bfindjunction = false;
- int nx = -1;
- for(k=0;k<ij.xvectorjunctionlink.size();k++)
- {
- if(ij.xvectorjunctionlink[k].connectroad == nroadid)
- {
- nx = k;
- bfindjunction = true;
- break;
- }
- }
- if(bfindjunction == false)
- {
- iv::ivjunctionlink ijk;
- ijk.incommingroad = pr->nroadid;
- ijk.connectpoint = plc->nconnect;
- ijk.connectroad = plc->nroadid;
- ij.xvectorjunctionlink.push_back(ijk);
- nx = ij.xvectorjunctionlink.size() - 1;
- }
- iv::ivjunctionlink * pijk = &ij.xvectorjunctionlink[nx];
- pijk->xvectorlanelink.push_back(ijl);
- }
- xvectorjunction.push_back(ij);
- pr->nnxtjunctionindex = xvectorjunction.size() -1;
- }
- }
- int idnow = 0;
- if(xvectorjunction.size() > 0)
- {
- xvectorjunction[0].nid = idnow;
- idnow++;
- }
- for(i=1;i<xvectorjunction.size();i++)
- {
- bool bhavejunction = false;
- int noldid = -1;
- iv::ivjunction * pij = &xvectorjunction[i];
- int j;
- for(j=0;j<i;j++)
- {
- iv::ivjunction * pijpre = &xvectorjunction[j];
- int k;
- for(k=0;k<pijpre->xvectorjunctionlink.size();k++)
- {
- iv::ivjunctionlink * pijlinkpre = &pijpre->xvectorjunctionlink[k];
- int m;
- for(m=0;m<pij->xvectorjunctionlink.size();m++)
- {
- iv::ivjunctionlink * pijlinknow = &pij->xvectorjunctionlink[m];
- if(pijlinknow->connectroad == pijlinkpre->connectroad)
- {
- noldid = pijpre->nid;
- bhavejunction = true;
- break;
- }
- }
- if(bhavejunction)break;
- }
- if(bhavejunction)break;;
- }
- if(bhavejunction)
- {
- pij->nid = noldid;
- }
- else
- {
- pij->nid = idnow;
- idnow++;
- }
- }
- return xvectorjunction;
- }
- static int CreateJunctionID(OpenDrive * pxodr)
- {
- int i;
- bool bUsed = false;
- int njunctionidstart = 800000;
- int njunctioncount = pxodr->GetJunctionCount();
- if(njunctioncount == 0)return njunctionidstart;
- int * pjunctionid = new int[njunctioncount];
- for(i=0;i<njunctioncount;i++)
- {
- pjunctionid[i]=atoi(pxodr->GetJunction(i)->GetId().data());
- }
- do
- {
- bUsed = false;
- for(i=0;i<njunctioncount;i++)
- {
- if(pjunctionid[i] == njunctionidstart)
- {
- njunctionidstart++;
- bUsed = true;
- }
- }
- }while(bUsed);
- delete pjunctionid;
- return njunctionidstart;
- }
- static Junction * GetJunctioByID(OpenDrive * pxodr,int nopid)
- {
- int i;
- for(i=0;i<pxodr->GetJunctionCount();i++)
- {
- if(atoi(pxodr->GetJunction(i)->GetId().data()) == nopid)
- {
- return pxodr->GetJunction(i);
- }
- }
- return 0;
- }
- static Road * GetRoadByID(OpenDrive * pxodr,int nroadid)
- {
- int i;
- for(i=0;i<pxodr->GetRoadCount();i++)
- {
- if(atoi(pxodr->GetRoad(i)->GetRoadId().data()) == nroadid)
- {
- return pxodr->GetRoad(i);
- }
- }
- return 0;
- }
- static void ChangeOpenDrive(OpenDrive * pxodr,std::vector<iv::ivjunction> * pxvectorivjunction,std::vector<iv::roadconnect> * pxvectorroadconn)
- {
- unsigned int i;
- for(i=0;i<pxvectorivjunction->size();i++)
- {
- bool bnewjunction = true;
- Junction * pj = 0;
- iv::ivjunction * pivjunction = &pxvectorivjunction->at(i);
- if(i==0)
- {
- bnewjunction = true;
- }
- else
- {
- int j;
- for(j=0;j<i;j++)
- {
- if(pxvectorivjunction->at(j).nid == pivjunction->nid)
- {
- pivjunction->nopid = pxvectorivjunction->at(j).nopid;
- bnewjunction = false;
- pj = GetJunctioByID(pxodr,pivjunction->nopid);
- break;
- }
- }
- }
- if(pj == 0)bnewjunction = true;
- if(bnewjunction)
- {
- int njunctionid = CreateJunctionID(pxodr);
- pxodr->AddJunction("",QString::number(njunctionid).toStdString());
- pivjunction->nopid = njunctionid;
- pj = pxodr->GetJunction(pxodr->GetJunctionCount()-1);
- }
- int j;
- JunctionConnection * pjc = 0;
- for(j=0;j<pivjunction->xvectorjunctionlink.size();j++)
- {
- iv::ivjunctionlink * pivjunctionlink = &pivjunction->xvectorjunctionlink[j];
- string id = QString::number(pj->GetJunctionConnectionCount()).toStdString();
- string strincomming = QString::number(pivjunctionlink->incommingroad).toStdString();
- string strconnecting = QString::number(pivjunctionlink->connectroad).toStdString();
- Road * proad = GetRoadByID(pxodr,pivjunctionlink->connectroad);
- if(proad != 0)
- {
- proad->SetRoadJunction(pj->GetId());
- }
- string strcontactpoint;
- if(pivjunctionlink->connectpoint == 1)
- {
- strcontactpoint = "start";
- }
- else
- {
- strcontactpoint = "end";
- }
- pj->AddJunctionConnection(id,strincomming,strconnecting,strcontactpoint);
- pjc = pj->GetJunctionConnection(pj->GetJunctionConnectionCount() - 1);
- int k;
- for(k=0;k<pivjunctionlink->xvectorlanelink.size();k++)
- {
- iv::ivjunctionlanelink * pivjunctionlanelink = &pivjunctionlink->xvectorlanelink[k];
- pjc->AddJunctionLaneLink(pivjunctionlanelink->incominglane,pivjunctionlanelink->connectlane);
- }
- }
- }
- for(i=0;i<pxvectorroadconn->size();i++)
- {
- iv::roadconnect * pr = &pxvectorroadconn->at(i);
- Road * proad = GetRoadByID(pxodr,pr->nroadid);
- if(pr->mbJunctionpre)
- {
- string strpre = QString::number(pxvectorivjunction->at(pr->nprejunctionindex).nopid).toStdString();
- proad->SetPredecessor("junction",strpre,"start");
- LaneSection * pLS = proad->GetLaneSection(0);
- unsigned int k;
- for(k=0;k<pLS->GetLaneCount();k++)
- {
- if(pLS->GetLane(k)->IsPredecessorSet())
- {
- pLS->GetLane(k)->RemovePredecessor();
- }
- }
- }
- else
- {
- if(pr->xvectorpre.size()>0)
- {
- string strroadid = QString::number(pr->xvectorpre[0].nroadid).toStdString();
- string strcontact = "start";
- if(pr->xvectorpre[0].nconnect == 2)strcontact = "end";
- proad->SetPredecessor("road",strroadid,strcontact);
- int j;
- for(j=0;j<pr->xvectorpre.size();j++)
- {
- int nlane = pr->xvectorpre[j].nlaneidcur;
- int nlaneoth = pr->xvectorpre[j].nlaneidoth;
- Lane * plane = 0;
- proad = GetRoadByID(pxodr,pr->nroadid);
- LaneSection * pLS = proad->GetLaneSection(0);
- int k;
- for(k=0;k<pLS->GetLaneCount();k++)
- {
- if(pLS->GetLane(k)->GetId() == nlane)
- {
- plane = pLS->GetLane(k);
- break;
- }
- }
- plane->SetPredecessor(nlaneoth);
- }
- }
- }
- if(pr->mbJunctionnxt)
- {
- string strnxt = QString::number(pxvectorivjunction->at(pr->nnxtjunctionindex).nopid).toStdString();
- proad->SetSuccessor("junction",strnxt,"end");
- LaneSection * pLS = proad->GetLastLaneSection();
- unsigned int k;
- for(k=0;k<pLS->GetLaneCount();k++)
- {
- if(pLS->GetLane(k)->IsSuccessorSet())
- pLS->GetLane(k)->RemoveSuccessor();
- }
- }
- else
- {
- if(pr->xvectornxt.size()>0)
- {
- string strroadid = QString::number(pr->xvectornxt[0].nroadid).toStdString();
- string strcontact = "start";
- if(pr->xvectornxt[0].nconnect == 2)strcontact = "end";
- proad->SetSuccessor("road",strroadid,strcontact);
- int j;
- for(j=0;j<pr->xvectornxt.size();j++)
- {
- int nlane = pr->xvectornxt[j].nlaneidcur;
- int nlaneoth = pr->xvectornxt[j].nlaneidoth;
- Lane * plane = 0;
- proad = GetRoadByID(pxodr,pr->nroadid);
- LaneSection * pLS = proad->GetLastLaneSection();
- int k;
- for(k=0;k<pLS->GetLaneCount();k++)
- {
- if(pLS->GetLane(k)->GetId() == nlane)
- {
- plane = pLS->GetLane(k);
- break;
- }
- }
- plane->SetSuccessor(nlaneoth);
- }
- }
- }
- }
- }
- AutoConnect::AutoConnect(OpenDrive * pxodr)
- {
- mpxodr = pxodr;
- while(mpxodr->GetJunctionCount()>0)mpxodr->DeleteJunction(0);
- Road * proad;
- int nroadcount = mpxodr->GetRoadCount();
- int i;
- for(i=0;i<nroadcount;i++)
- {
- proad = mpxodr->GetRoad(i);
- proad->SetRoadJunction("-1");
- }
- }
- void AutoConnect::Connect()
- {
- std::vector<iv::roadpoint> xvectorroadpoint = getroadpoint(mpxodr);
- std::vector<iv::laneconnect> xvectorlaneconn = getlaneconn(xvectorroadpoint);
- std::vector<iv::roadconnect> xvectorroadconn = getroadconn(xvectorlaneconn);
- std::vector<iv::ivjunction> xvectorivjunction = getjunction(&xvectorroadconn);
- ChangeOpenDrive(mpxodr,&xvectorivjunction,&xvectorroadconn);
- }
|