autoconnect.cpp 30 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084
  1. #include "autoconnect.h"
  2. #include <vector>
  3. #include <math.h>
  4. #include <iostream>
  5. namespace iv {
  6. struct laneconnect
  7. {
  8. unsigned int nroadid1;
  9. unsigned int nroadid2;
  10. int nlaneid1;
  11. int nlaneid2;
  12. unsigned int nstart1; //1 start 2 end;
  13. unsigned int nstart2; //1 start 2 end;
  14. };
  15. struct prenextlane
  16. {
  17. unsigned int nroadid;
  18. int nlaneidcur;
  19. int nlaneidoth;
  20. int nconnect; //1 start 2 end
  21. };
  22. struct ivjunctionlanelink
  23. {
  24. int incominglane;
  25. int connectlane;
  26. };
  27. struct ivjunctionlink
  28. {
  29. int incommingroad;
  30. int connectroad;
  31. int connectpoint; //1 start 2 end
  32. int nid;
  33. std::vector<ivjunctionlanelink> xvectorlanelink;
  34. };
  35. struct ivjunction
  36. {
  37. std::vector<ivjunctionlink> xvectorjunctionlink;
  38. int nid;
  39. int nopid = -1;
  40. // int nsameid = -1; //is same as from id .used for merge same.
  41. };
  42. struct roadconnect
  43. {
  44. unsigned int nroadid;
  45. std::vector<iv::prenextlane> xvectorpre;
  46. std::vector<iv::prenextlane> xvectornxt;
  47. bool mbJunctionpre = false;
  48. bool mbJunctionnxt = false;
  49. bool mbInJunction = false;
  50. int nprejunctionindex;
  51. int nnxtjunctionindex;
  52. };
  53. struct roadpoint
  54. {
  55. unsigned int nRoadID;
  56. unsigned int nstart; //1 start 2 end;
  57. int nlane;
  58. double x;
  59. double y;
  60. double z;
  61. double mfhdg;
  62. bool bIsEnterRoad = false; //true enter this road, false out this road.
  63. };
  64. }
  65. namespace iv {
  66. struct lanewidthabcd
  67. {
  68. int nlane;
  69. double A;
  70. double B;
  71. double C;
  72. double D;
  73. };
  74. }
  75. double getwidth(Road * pRoad, int nlane,std::vector<iv::lanewidthabcd> xvectorlwa,double s)
  76. {
  77. double frtn = 0;
  78. int i;
  79. int nlanecount = xvectorlwa.size();
  80. for(i=0;i<nlanecount;i++)
  81. {
  82. if(nlane == xvectorlwa.at(i).nlane)
  83. {
  84. iv::lanewidthabcd * plwa = &xvectorlwa.at(i);
  85. frtn = plwa->A + plwa->B*s + plwa->C *s*s + plwa->D *s*s*s;
  86. break;
  87. }
  88. }
  89. return frtn;
  90. }
  91. std::vector<iv::lanewidthabcd> GetLaneWidthABCD(Road * pRoad)
  92. {
  93. std::vector<iv::lanewidthabcd> xvectorlwa;
  94. if(pRoad->GetLaneSectionCount()<1)return xvectorlwa;
  95. int i;
  96. int nlanecount = pRoad->GetLaneSection(0)->GetLaneCount();
  97. for(i=0;i<nlanecount;i++)
  98. {
  99. iv::lanewidthabcd xlwa;
  100. LaneWidth* pLW = pRoad->GetLaneSection(0)->GetLane(i)->GetLaneWidth(0);
  101. xlwa.nlane = pRoad->GetLaneSection(0)->GetLane(i)->GetId();
  102. if(pLW != 0)
  103. {
  104. xlwa.A = pLW->GetA();
  105. xlwa.B = pLW->GetB();
  106. xlwa.C = pLW->GetC();
  107. xlwa.D = pLW->GetD();
  108. }
  109. else
  110. {
  111. xlwa.A = 0;
  112. xlwa.B = 0;
  113. xlwa.C = 0;
  114. xlwa.D = 0;
  115. }
  116. xvectorlwa.push_back(xlwa);
  117. }
  118. return xvectorlwa;
  119. }
  120. inline double getoff(int nlane,double s,std::vector<iv::lanewidthabcd> xvectorLWA,std::vector<int> xvectorIndex)
  121. {
  122. int i;
  123. double off = 0;
  124. double a,b,c,d;
  125. if(xvectorIndex.size() == 0)return off;
  126. for(i=0;i<(xvectorIndex.size()-1);i++)
  127. {
  128. a = xvectorLWA[xvectorIndex[i]].A;
  129. b = xvectorLWA[xvectorIndex[i]].B;
  130. c = xvectorLWA[xvectorIndex[i]].C;
  131. d = xvectorLWA[xvectorIndex[i]].D;
  132. if(xvectorLWA[xvectorIndex[i]].nlane != 0)
  133. {
  134. off = off + a + b*s + c*s*s + d*s*s*s;
  135. }
  136. else
  137. {
  138. off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
  139. }
  140. }
  141. i = xvectorIndex.size()-1;
  142. a = xvectorLWA[xvectorIndex[i]].A;
  143. b = xvectorLWA[xvectorIndex[i]].B;
  144. c = xvectorLWA[xvectorIndex[i]].C;
  145. d = xvectorLWA[xvectorIndex[i]].D;
  146. off = off + (a + b*s + c*s*s + d*s*s*s)/2.0;
  147. if(nlane < 0) off = off*(-1.0);
  148. // std::cout<<"s is "<<s<<std::endl;
  149. return off;
  150. }
  151. std::vector<int> GetLWIndex(std::vector<iv::lanewidthabcd> xvectorLWA,int nlane)
  152. {
  153. std::vector<int> xvectorindex;
  154. int i;
  155. if(nlane >= 0)
  156. {
  157. for(i=0;i<=nlane;i++)
  158. {
  159. int j;
  160. int nsize = xvectorLWA.size();
  161. for(j=0;j<nsize;j++)
  162. {
  163. if(i == xvectorLWA.at(j).nlane )
  164. {
  165. xvectorindex.push_back(j);
  166. break;
  167. }
  168. }
  169. }
  170. }
  171. else
  172. {
  173. for(i=0;i>=nlane;i--)
  174. {
  175. int j;
  176. int nsize = xvectorLWA.size();
  177. for(j=0;j<nsize;j++)
  178. {
  179. if(i == xvectorLWA.at(j).nlane )
  180. {
  181. xvectorindex.push_back(j);
  182. break;
  183. }
  184. }
  185. }
  186. }
  187. return xvectorindex;
  188. }
  189. static double getoff(Road *p, int nlane, bool bstart)
  190. {
  191. double off = 0;
  192. std::vector<iv::lanewidthabcd> xvectorLWA = GetLaneWidthABCD(p);
  193. std::vector<int> xvectorindex1;
  194. xvectorindex1 = GetLWIndex(xvectorLWA,nlane);
  195. if(bstart)
  196. {
  197. off = getoff(nlane,0,xvectorLWA,xvectorindex1);
  198. }
  199. else
  200. {
  201. off = getoff(nlane,p->GetRoadLength(),xvectorLWA,xvectorindex1);
  202. }
  203. return off;
  204. }
  205. static std::vector<iv::roadpoint> getstartpoint(Road * p1)
  206. {
  207. double startx,starty,starthdg,starthgt;
  208. double off1;
  209. int nlane;
  210. std::vector<iv::roadpoint> xvectorroadpoint;
  211. if(p1->GetLaneSectionCount() == 0)return xvectorroadpoint;
  212. int i;
  213. for(i=0;i<p1->GetLaneSection(0)->GetLaneCount();i++)
  214. {
  215. nlane = p1->GetLaneSection(0)->GetLane(i)->GetId();
  216. if(nlane == 0)continue;
  217. starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
  218. off1 = getoff(p1,nlane,true);
  219. startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
  220. starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
  221. LaneOffset * pLO = p1->GetLaneOffset(0);
  222. double fofflane = 0.0;
  223. if(pLO != NULL)
  224. {
  225. double s_tem = 0;
  226. fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
  227. +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
  228. }
  229. startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
  230. starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
  231. starthgt = 0;
  232. if(p1->GetElevationCount()>0)
  233. {
  234. starthgt = p1->GetElevation(0)->GetA();
  235. }
  236. if(nlane<0)
  237. {
  238. starthdg = starthdg + M_PI;
  239. if(starthdg >= 2.0*M_PI)starthdg = starthdg - 2.0*M_PI;
  240. }
  241. iv::roadpoint rp;
  242. rp.nRoadID = atoi(p1->GetRoadId().data());
  243. rp.nlane = nlane;
  244. rp.nstart = 1;
  245. rp.mfhdg = starthdg;
  246. rp.x = startx;
  247. rp.y = starty;
  248. rp.z = starthgt;
  249. if(nlane < 0)rp.bIsEnterRoad = true;
  250. if(nlane > 0) rp.bIsEnterRoad = false;
  251. xvectorroadpoint.push_back(rp);
  252. }
  253. return xvectorroadpoint;
  254. }
  255. int GetEndPoint(Road *proad, double &x, double &y, double &hdg)
  256. {
  257. proad->GetGeometryCoords(proad->GetRoadLength(),x,y,hdg);
  258. return 0;
  259. GeometryBlock * pblock = proad->GetLastGeometryBlock();
  260. RoadGeometry * pgeo = pblock->GetLastGeometry();
  261. //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
  262. switch (pgeo->GetGeomType()) {
  263. case 0:
  264. {
  265. GeometryLine * pline = (GeometryLine *)pgeo;
  266. x = pline->GetX() + pline->GetLength() * cos(pline->GetHdg());
  267. y = pline->GetY() + pline->GetLength() * sin(pline->GetHdg());
  268. hdg = pline->GetHdg();
  269. }
  270. return 0;
  271. break;
  272. case 1:
  273. {
  274. GeometryArc * parc = (GeometryArc *)pgeo;
  275. double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
  276. double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
  277. x = x_center + fabs(1.0/parc->GetCurvature()) * cos(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
  278. y = y_center + fabs(1.0/parc->GetCurvature()) * sin(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
  279. hdg = parc->GetHdg() + parc->GetLength() * parc->GetCurvature();
  280. return 0;
  281. }
  282. break;
  283. case 2:
  284. {
  285. GeometrySpiral * pspiral = (GeometrySpiral *)pgeo;
  286. pspiral->GetCoords(pspiral->GetS()+pspiral->GetLength(),x,y,hdg);
  287. return 0;
  288. }
  289. break;
  290. case 3:
  291. // QMessageBox::warning(this,"warn","type not supported.");
  292. break;
  293. case 4:
  294. {
  295. double xtem,ytem;
  296. double xtem1,ytem1,x1,y1;
  297. GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
  298. double s = ppoly3->GetLength();
  299. if(ppoly3->GetNormal())
  300. {
  301. xtem = ppoly3->GetuA() + ppoly3->GetuB() + ppoly3->GetuC() + ppoly3->GetuD() ;
  302. ytem = ppoly3->GetvA() + ppoly3->GetvB() + ppoly3->GetvC() + ppoly3->GetvD() ;
  303. }
  304. else
  305. {
  306. xtem = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
  307. ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
  308. }
  309. // xtem = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
  310. // ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
  311. x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
  312. y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
  313. s = ppoly3->GetLength()*0.999;
  314. // double fgeolen;
  315. double fr = 1.0;
  316. if(s>0)
  317. {
  318. double frel = 0.99;
  319. if(ppoly3->GetNormal() == false)
  320. {
  321. xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
  322. ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
  323. }
  324. else
  325. {
  326. xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * frel + ppoly3->GetuC() *frel*frel + ppoly3->GetuD()*frel*frel*frel ;
  327. ytem1 = ppoly3->GetvA() + ppoly3->GetvB()*frel + ppoly3->GetvC()*frel*frel + ppoly3->GetvD()*frel*frel*frel ;
  328. }
  329. fr = 1.0;
  330. // xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * fr + ppoly3->GetuC() * fr*fr + ppoly3->GetuD() * fr*fr*fr ;
  331. // ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * fr + ppoly3->GetvC() * fr*fr + ppoly3->GetvD() * fr*fr*fr ;
  332. x1 = xtem1*cos(ppoly3->GetHdg()) - ytem1 * sin(ppoly3->GetHdg()) + ppoly3->GetX();
  333. y1 = xtem1*sin(ppoly3->GetHdg()) + ytem1 * cos(ppoly3->GetHdg()) + ppoly3->GetY();
  334. hdg = geofit::CalcHdg(x1,y1,x,y);
  335. }
  336. else
  337. {
  338. hdg = 0;
  339. }
  340. return 0;
  341. }
  342. break;
  343. default:
  344. // QMessageBox::warning(this,"warn","type not supported.");
  345. break;
  346. }
  347. return -1;
  348. }
  349. static std::vector<iv::roadpoint> getendpoint(Road * p1)
  350. {
  351. double startx,starty,starthdg,starthgt;
  352. double off1;
  353. int nlane;
  354. std::vector<iv::roadpoint> xvectorroadpoint;
  355. if(p1->GetLaneSectionCount() == 0)return xvectorroadpoint;
  356. int i;
  357. for(i=0;i<p1->GetLastLaneSection()->GetLaneCount();i++)
  358. {
  359. nlane = p1->GetLastLaneSection()->GetLane(i)->GetId();
  360. if(nlane == 0)continue;
  361. if(GetEndPoint(p1,startx,starty,starthdg) != 0)
  362. {
  363. continue;
  364. }
  365. off1 = getoff(p1,nlane,false);
  366. LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);
  367. double fofflane = 0.0;
  368. if(pLO!= NULL)
  369. {
  370. double s_tem = p1->GetRoadLength() - pLO->GetS();
  371. fofflane = pLO->Geta() + pLO->Getb()*(s_tem)
  372. +pLO->Getc()*(s_tem*s_tem) + pLO->Getd()*(s_tem*s_tem*s_tem);
  373. }
  374. startx = startx + (off1+fofflane) * cos(starthdg +M_PI/2.0);
  375. starty = starty + (off1+fofflane) * sin(starthdg +M_PI/2.0);
  376. starthgt = 0;
  377. if(p1->GetElevationCount()>0)
  378. {
  379. Elevation * pe = p1->GetLastElevation();
  380. double s = p1->GetRoadLength();
  381. double a, b,c,d;
  382. a = pe->GetA();
  383. b = pe->GetB();
  384. c = pe->GetC();
  385. d = pe->GetD();
  386. starthgt = a + b*s + c*s*s + d*s*s*s;
  387. }
  388. if(nlane<0)
  389. {
  390. starthdg = starthdg + M_PI;
  391. if(starthdg >= 2.0*M_PI)starthdg = starthdg - 2.0*M_PI;
  392. }
  393. iv::roadpoint rp;
  394. rp.nRoadID = atoi(p1->GetRoadId().data());
  395. rp.nlane = nlane;
  396. rp.nstart = 2;
  397. rp.mfhdg = starthdg;
  398. rp.x = startx;
  399. rp.y = starty;
  400. rp.z = starthgt;
  401. if(nlane < 0)rp.bIsEnterRoad = false;
  402. if(nlane > 0) rp.bIsEnterRoad = true;
  403. xvectorroadpoint.push_back(rp);
  404. }
  405. return xvectorroadpoint;
  406. }
  407. static std::vector<iv::roadpoint> getroadpoint(OpenDrive * pxodr)
  408. {
  409. std::vector<iv::roadpoint> xvectorroadpoint;
  410. int i;
  411. for(i=0;i<pxodr->GetRoadCount();i++)
  412. {
  413. Road * p1 = pxodr->GetRoad(i);
  414. std::vector<iv::roadpoint> x1,x2;
  415. x1 = getstartpoint(p1);
  416. x2 = getendpoint(p1);
  417. int j;
  418. for(j=0;j<x1.size();j++)xvectorroadpoint.push_back(x1.at(j));
  419. for(j=0;j<x2.size();j++)xvectorroadpoint.push_back(x2.at(j));
  420. }
  421. for(i=0;i<xvectorroadpoint.size();i++)
  422. {
  423. std::cout<<xvectorroadpoint[i].nRoadID<<" "<<xvectorroadpoint[i].nlane<<" "<<xvectorroadpoint[i].nstart<<" "<<xvectorroadpoint[i].x<<" "<<xvectorroadpoint[i].y<<std::endl;
  424. }
  425. return xvectorroadpoint;
  426. }
  427. static bool IsConn(iv::roadpoint rp1,iv::roadpoint rp2)
  428. {
  429. if(rp1.nRoadID == rp2.nRoadID)
  430. {
  431. return false;
  432. }
  433. if(rp1.nstart == rp2.nstart)
  434. {
  435. if(rp1.nlane * rp2.nlane > 0)
  436. {
  437. return false;
  438. }
  439. }
  440. if(rp1.bIsEnterRoad == rp2.bIsEnterRoad)
  441. {
  442. return false;
  443. }
  444. double fdis;
  445. fdis = sqrt(pow(rp1.x-rp2.x,2)+pow(rp1.y -rp2.y,2));
  446. if(fdis>1.0)return false;
  447. // if(rp1.nRoadID == 10011)
  448. // {
  449. // int x = 1;
  450. // x = 2;
  451. // }
  452. double fhdgdiff = rp1.mfhdg - rp2.mfhdg;
  453. if(fhdgdiff>M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
  454. if(fhdgdiff<(-M_PI))fhdgdiff = fhdgdiff + 2.0*M_PI;
  455. if(fabs(fhdgdiff)>1.0)return false;
  456. return true;
  457. }
  458. static std::vector<iv::laneconnect> getlaneconn(std::vector<iv::roadpoint> xvectorroadpoint)
  459. {
  460. std::vector<iv::laneconnect> xvectorroadconn;
  461. int i;
  462. for(i=0;i<(xvectorroadpoint.size()-1);i++)
  463. {
  464. iv::roadpoint rp1;
  465. rp1 = xvectorroadpoint[i];
  466. int j;
  467. for(j=i+1;j<xvectorroadpoint.size();j++)
  468. {
  469. iv::roadpoint rp2;
  470. rp2 = xvectorroadpoint[j];
  471. if(IsConn(rp1,rp2))
  472. {
  473. iv::laneconnect lc;
  474. lc.nlaneid1 = rp1.nlane;
  475. lc.nlaneid2 = rp2.nlane;
  476. lc.nroadid1 = rp1.nRoadID;
  477. lc.nroadid2 = rp2.nRoadID;
  478. lc.nstart1 = rp1.nstart;
  479. lc.nstart2 = rp2.nstart;
  480. xvectorroadconn.push_back(lc);
  481. }
  482. }
  483. }
  484. return xvectorroadconn;
  485. }
  486. static std::vector<iv::roadconnect> getroadconn(std::vector<iv::laneconnect> xvectorlaneconn)
  487. {
  488. std::vector<iv::roadconnect> xvectorroadconn;
  489. //获得road连接关系
  490. int i;
  491. for(i=0;i<xvectorlaneconn.size();i++)
  492. {
  493. iv::laneconnect xlc = xvectorlaneconn.at(i);
  494. bool bhave1,bhave2;
  495. bhave1 = false;
  496. bhave2 = false;
  497. iv::roadconnect * p1, * p2;
  498. int np1pos,np2pos;
  499. int j;
  500. for(j=0;j<xvectorroadconn.size();j++)
  501. {
  502. if(xvectorroadconn[j].nroadid == xlc.nroadid1)
  503. {
  504. bhave1 = true;
  505. np1pos = j;
  506. p1 = &xvectorroadconn[j];
  507. }
  508. if(xvectorroadconn[j].nroadid == xlc.nroadid2)
  509. {
  510. bhave2 = true;
  511. np2pos = j;
  512. p2 = &xvectorroadconn[j];
  513. }
  514. if(bhave1 && bhave2)break;
  515. }
  516. if(bhave1 == false)
  517. {
  518. iv::roadconnect rc;
  519. rc.nroadid = xlc.nroadid1;
  520. xvectorroadconn.push_back(rc);
  521. np1pos = xvectorroadconn.size() -1;
  522. p1 = &xvectorroadconn[xvectorroadconn.size()-1];
  523. }
  524. if(bhave2 == false)
  525. {
  526. iv::roadconnect rc;
  527. rc.nroadid = xlc.nroadid2;
  528. xvectorroadconn.push_back(rc);
  529. np2pos = xvectorroadconn.size() -1;
  530. p2 = &xvectorroadconn[xvectorroadconn.size()-1];
  531. }
  532. p1 = &xvectorroadconn[np1pos];
  533. p2 = &xvectorroadconn[np2pos];
  534. iv::prenextlane xpx;
  535. xpx.nlaneidcur = xlc.nlaneid1;
  536. xpx.nlaneidoth = xlc.nlaneid2;
  537. xpx.nroadid = xlc.nroadid2;
  538. xpx.nconnect = xlc.nstart2;
  539. if(xlc.nstart1 == 1)
  540. {
  541. p1->xvectorpre.push_back(xpx);
  542. }
  543. else
  544. {
  545. p1->xvectornxt.push_back(xpx);
  546. }
  547. xpx.nlaneidcur = xlc.nlaneid2;
  548. xpx.nlaneidoth = xlc.nlaneid1;
  549. xpx.nroadid = xlc.nroadid1;
  550. xpx.nconnect = xlc.nstart1;
  551. if(xlc.nstart2 == 1)
  552. {
  553. p2->xvectorpre.push_back(xpx);
  554. }
  555. else
  556. {
  557. p2->xvectornxt.push_back(xpx);
  558. }
  559. }
  560. //判断road的连接是否是junction
  561. for(i=0;i<xvectorroadconn.size();i++)
  562. {
  563. iv::roadconnect * pr = &xvectorroadconn[i];
  564. int j;
  565. int nfirstid;
  566. if(pr->xvectorpre.size()>0)nfirstid = pr->xvectorpre[0].nroadid;
  567. for(j=1;j<pr->xvectorpre.size();j++)
  568. {
  569. if(pr->xvectorpre[j].nroadid != nfirstid)
  570. {
  571. pr->mbJunctionpre = true;
  572. break;
  573. }
  574. }
  575. if(pr->xvectornxt.size()>0)nfirstid = pr->xvectornxt[0].nroadid;
  576. for(j=1;j<pr->xvectornxt.size();j++)
  577. {
  578. if(pr->xvectornxt[j].nroadid != nfirstid)
  579. {
  580. pr->mbJunctionnxt = true;
  581. break;
  582. }
  583. }
  584. }
  585. //设置junction区域为juncion
  586. return xvectorroadconn;
  587. }
  588. static std::vector<iv::ivjunction> getjunction(std::vector<iv::roadconnect> * pxvectorroadconn)
  589. {
  590. std::vector<iv::ivjunction> xvectorjunction;
  591. int i;
  592. for(i=0;i<pxvectorroadconn->size();i++)
  593. {
  594. iv::roadconnect * pr = &pxvectorroadconn->at(i);
  595. if(pr->mbJunctionpre)
  596. {
  597. int j;
  598. iv::ivjunction ij;
  599. for(j=0;j<pr->xvectorpre.size();j++)
  600. {
  601. int nroadid = pr->xvectorpre[j].nroadid;
  602. int k;
  603. iv::prenextlane * plc = &pr->xvectorpre[j];
  604. iv::ivjunctionlanelink ijl;
  605. ijl.incominglane = plc->nlaneidcur;
  606. ijl.connectlane = plc->nlaneidoth;
  607. bool bfindjunction = false;
  608. int nx = -1;
  609. for(k=0;k<ij.xvectorjunctionlink.size();k++)
  610. {
  611. if(ij.xvectorjunctionlink[k].connectroad == nroadid)
  612. {
  613. nx = k;
  614. bfindjunction = true;
  615. break;
  616. }
  617. }
  618. if(bfindjunction == false)
  619. {
  620. iv::ivjunctionlink ijk;
  621. ijk.incommingroad = pr->nroadid;
  622. ijk.connectpoint = plc->nconnect;
  623. ijk.connectroad = plc->nroadid;
  624. ij.xvectorjunctionlink.push_back(ijk);
  625. nx = ij.xvectorjunctionlink.size() - 1;
  626. }
  627. iv::ivjunctionlink * pijk = &ij.xvectorjunctionlink[nx];
  628. pijk->xvectorlanelink.push_back(ijl);
  629. }
  630. xvectorjunction.push_back(ij);
  631. pr->nprejunctionindex = xvectorjunction.size()-1;
  632. }
  633. if(pr->mbJunctionnxt)
  634. {
  635. int j;
  636. iv::ivjunction ij;
  637. for(j=0;j<pr->xvectornxt.size();j++)
  638. {
  639. int nroadid = pr->xvectornxt[j].nroadid;
  640. int k;
  641. iv::prenextlane * plc = &pr->xvectornxt[j];
  642. iv::ivjunctionlanelink ijl;
  643. ijl.incominglane = plc->nlaneidcur;
  644. ijl.connectlane = plc->nlaneidoth;
  645. bool bfindjunction = false;
  646. int nx = -1;
  647. for(k=0;k<ij.xvectorjunctionlink.size();k++)
  648. {
  649. if(ij.xvectorjunctionlink[k].connectroad == nroadid)
  650. {
  651. nx = k;
  652. bfindjunction = true;
  653. break;
  654. }
  655. }
  656. if(bfindjunction == false)
  657. {
  658. iv::ivjunctionlink ijk;
  659. ijk.incommingroad = pr->nroadid;
  660. ijk.connectpoint = plc->nconnect;
  661. ijk.connectroad = plc->nroadid;
  662. ij.xvectorjunctionlink.push_back(ijk);
  663. nx = ij.xvectorjunctionlink.size() - 1;
  664. }
  665. iv::ivjunctionlink * pijk = &ij.xvectorjunctionlink[nx];
  666. pijk->xvectorlanelink.push_back(ijl);
  667. }
  668. xvectorjunction.push_back(ij);
  669. pr->nnxtjunctionindex = xvectorjunction.size() -1;
  670. }
  671. }
  672. int idnow = 0;
  673. if(xvectorjunction.size() > 0)
  674. {
  675. xvectorjunction[0].nid = idnow;
  676. idnow++;
  677. }
  678. for(i=1;i<xvectorjunction.size();i++)
  679. {
  680. bool bhavejunction = false;
  681. int noldid = -1;
  682. iv::ivjunction * pij = &xvectorjunction[i];
  683. int j;
  684. for(j=0;j<i;j++)
  685. {
  686. iv::ivjunction * pijpre = &xvectorjunction[j];
  687. int k;
  688. for(k=0;k<pijpre->xvectorjunctionlink.size();k++)
  689. {
  690. iv::ivjunctionlink * pijlinkpre = &pijpre->xvectorjunctionlink[k];
  691. int m;
  692. for(m=0;m<pij->xvectorjunctionlink.size();m++)
  693. {
  694. iv::ivjunctionlink * pijlinknow = &pij->xvectorjunctionlink[m];
  695. if(pijlinknow->connectroad == pijlinkpre->connectroad)
  696. {
  697. noldid = pijpre->nid;
  698. bhavejunction = true;
  699. break;
  700. }
  701. }
  702. if(bhavejunction)break;
  703. }
  704. if(bhavejunction)break;;
  705. }
  706. if(bhavejunction)
  707. {
  708. pij->nid = noldid;
  709. }
  710. else
  711. {
  712. pij->nid = idnow;
  713. idnow++;
  714. }
  715. }
  716. return xvectorjunction;
  717. }
  718. static int CreateJunctionID(OpenDrive * pxodr)
  719. {
  720. int i;
  721. bool bUsed = false;
  722. int njunctionidstart = 800000;
  723. int njunctioncount = pxodr->GetJunctionCount();
  724. if(njunctioncount == 0)return njunctionidstart;
  725. int * pjunctionid = new int[njunctioncount];
  726. for(i=0;i<njunctioncount;i++)
  727. {
  728. pjunctionid[i]=atoi(pxodr->GetJunction(i)->GetId().data());
  729. }
  730. do
  731. {
  732. bUsed = false;
  733. for(i=0;i<njunctioncount;i++)
  734. {
  735. if(pjunctionid[i] == njunctionidstart)
  736. {
  737. njunctionidstart++;
  738. bUsed = true;
  739. }
  740. }
  741. }while(bUsed);
  742. delete pjunctionid;
  743. return njunctionidstart;
  744. }
  745. static Junction * GetJunctioByID(OpenDrive * pxodr,int nopid)
  746. {
  747. int i;
  748. for(i=0;i<pxodr->GetJunctionCount();i++)
  749. {
  750. if(atoi(pxodr->GetJunction(i)->GetId().data()) == nopid)
  751. {
  752. return pxodr->GetJunction(i);
  753. }
  754. }
  755. return 0;
  756. }
  757. static Road * GetRoadByID(OpenDrive * pxodr,int nroadid)
  758. {
  759. int i;
  760. for(i=0;i<pxodr->GetRoadCount();i++)
  761. {
  762. if(atoi(pxodr->GetRoad(i)->GetRoadId().data()) == nroadid)
  763. {
  764. return pxodr->GetRoad(i);
  765. }
  766. }
  767. return 0;
  768. }
  769. static void ChangeOpenDrive(OpenDrive * pxodr,std::vector<iv::ivjunction> * pxvectorivjunction,std::vector<iv::roadconnect> * pxvectorroadconn)
  770. {
  771. unsigned int i;
  772. for(i=0;i<pxvectorivjunction->size();i++)
  773. {
  774. bool bnewjunction = true;
  775. Junction * pj = 0;
  776. iv::ivjunction * pivjunction = &pxvectorivjunction->at(i);
  777. if(i==0)
  778. {
  779. bnewjunction = true;
  780. }
  781. else
  782. {
  783. int j;
  784. for(j=0;j<i;j++)
  785. {
  786. if(pxvectorivjunction->at(j).nid == pivjunction->nid)
  787. {
  788. pivjunction->nopid = pxvectorivjunction->at(j).nopid;
  789. bnewjunction = false;
  790. pj = GetJunctioByID(pxodr,pivjunction->nopid);
  791. break;
  792. }
  793. }
  794. }
  795. if(pj == 0)bnewjunction = true;
  796. if(bnewjunction)
  797. {
  798. int njunctionid = CreateJunctionID(pxodr);
  799. pxodr->AddJunction("",QString::number(njunctionid).toStdString());
  800. pivjunction->nopid = njunctionid;
  801. pj = pxodr->GetJunction(pxodr->GetJunctionCount()-1);
  802. }
  803. int j;
  804. JunctionConnection * pjc = 0;
  805. for(j=0;j<pivjunction->xvectorjunctionlink.size();j++)
  806. {
  807. iv::ivjunctionlink * pivjunctionlink = &pivjunction->xvectorjunctionlink[j];
  808. string id = QString::number(pj->GetJunctionConnectionCount()).toStdString();
  809. string strincomming = QString::number(pivjunctionlink->incommingroad).toStdString();
  810. string strconnecting = QString::number(pivjunctionlink->connectroad).toStdString();
  811. Road * proad = GetRoadByID(pxodr,pivjunctionlink->connectroad);
  812. if(proad != 0)
  813. {
  814. proad->SetRoadJunction(pj->GetId());
  815. }
  816. string strcontactpoint;
  817. if(pivjunctionlink->connectpoint == 1)
  818. {
  819. strcontactpoint = "start";
  820. }
  821. else
  822. {
  823. strcontactpoint = "end";
  824. }
  825. pj->AddJunctionConnection(id,strincomming,strconnecting,strcontactpoint);
  826. pjc = pj->GetJunctionConnection(pj->GetJunctionConnectionCount() - 1);
  827. int k;
  828. for(k=0;k<pivjunctionlink->xvectorlanelink.size();k++)
  829. {
  830. iv::ivjunctionlanelink * pivjunctionlanelink = &pivjunctionlink->xvectorlanelink[k];
  831. pjc->AddJunctionLaneLink(pivjunctionlanelink->incominglane,pivjunctionlanelink->connectlane);
  832. }
  833. }
  834. }
  835. for(i=0;i<pxvectorroadconn->size();i++)
  836. {
  837. iv::roadconnect * pr = &pxvectorroadconn->at(i);
  838. Road * proad = GetRoadByID(pxodr,pr->nroadid);
  839. if(pr->mbJunctionpre)
  840. {
  841. string strpre = QString::number(pxvectorivjunction->at(pr->nprejunctionindex).nopid).toStdString();
  842. proad->SetPredecessor("junction",strpre,"start");
  843. LaneSection * pLS = proad->GetLaneSection(0);
  844. unsigned int k;
  845. for(k=0;k<pLS->GetLaneCount();k++)
  846. {
  847. if(pLS->GetLane(k)->IsPredecessorSet())
  848. {
  849. pLS->GetLane(k)->RemovePredecessor();
  850. }
  851. }
  852. }
  853. else
  854. {
  855. if(pr->xvectorpre.size()>0)
  856. {
  857. string strroadid = QString::number(pr->xvectorpre[0].nroadid).toStdString();
  858. string strcontact = "start";
  859. if(pr->xvectorpre[0].nconnect == 2)strcontact = "end";
  860. proad->SetPredecessor("road",strroadid,strcontact);
  861. int j;
  862. for(j=0;j<pr->xvectorpre.size();j++)
  863. {
  864. int nlane = pr->xvectorpre[j].nlaneidcur;
  865. int nlaneoth = pr->xvectorpre[j].nlaneidoth;
  866. Lane * plane = 0;
  867. proad = GetRoadByID(pxodr,pr->nroadid);
  868. LaneSection * pLS = proad->GetLaneSection(0);
  869. int k;
  870. for(k=0;k<pLS->GetLaneCount();k++)
  871. {
  872. if(pLS->GetLane(k)->GetId() == nlane)
  873. {
  874. plane = pLS->GetLane(k);
  875. break;
  876. }
  877. }
  878. plane->SetPredecessor(nlaneoth);
  879. }
  880. }
  881. }
  882. if(pr->mbJunctionnxt)
  883. {
  884. string strnxt = QString::number(pxvectorivjunction->at(pr->nnxtjunctionindex).nopid).toStdString();
  885. proad->SetSuccessor("junction",strnxt,"end");
  886. LaneSection * pLS = proad->GetLastLaneSection();
  887. unsigned int k;
  888. for(k=0;k<pLS->GetLaneCount();k++)
  889. {
  890. if(pLS->GetLane(k)->IsSuccessorSet())
  891. pLS->GetLane(k)->RemoveSuccessor();
  892. }
  893. }
  894. else
  895. {
  896. if(pr->xvectornxt.size()>0)
  897. {
  898. string strroadid = QString::number(pr->xvectornxt[0].nroadid).toStdString();
  899. string strcontact = "start";
  900. if(pr->xvectornxt[0].nconnect == 2)strcontact = "end";
  901. proad->SetSuccessor("road",strroadid,strcontact);
  902. int j;
  903. for(j=0;j<pr->xvectornxt.size();j++)
  904. {
  905. int nlane = pr->xvectornxt[j].nlaneidcur;
  906. int nlaneoth = pr->xvectornxt[j].nlaneidoth;
  907. Lane * plane = 0;
  908. proad = GetRoadByID(pxodr,pr->nroadid);
  909. LaneSection * pLS = proad->GetLastLaneSection();
  910. int k;
  911. for(k=0;k<pLS->GetLaneCount();k++)
  912. {
  913. if(pLS->GetLane(k)->GetId() == nlane)
  914. {
  915. plane = pLS->GetLane(k);
  916. break;
  917. }
  918. }
  919. plane->SetSuccessor(nlaneoth);
  920. }
  921. }
  922. }
  923. }
  924. }
  925. AutoConnect::AutoConnect(OpenDrive * pxodr)
  926. {
  927. mpxodr = pxodr;
  928. while(mpxodr->GetJunctionCount()>0)mpxodr->DeleteJunction(0);
  929. Road * proad;
  930. int nroadcount = mpxodr->GetRoadCount();
  931. int i;
  932. for(i=0;i<nroadcount;i++)
  933. {
  934. proad = mpxodr->GetRoad(i);
  935. proad->SetRoadJunction("-1");
  936. }
  937. }
  938. void AutoConnect::Connect()
  939. {
  940. std::vector<iv::roadpoint> xvectorroadpoint = getroadpoint(mpxodr);
  941. std::vector<iv::laneconnect> xvectorlaneconn = getlaneconn(xvectorroadpoint);
  942. std::vector<iv::roadconnect> xvectorroadconn = getroadconn(xvectorlaneconn);
  943. std::vector<iv::ivjunction> xvectorivjunction = getjunction(&xvectorroadconn);
  944. ChangeOpenDrive(mpxodr,&xvectorivjunction,&xvectorroadconn);
  945. }