mainwindow.cpp 135 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156315731583159316031613162316331643165316631673168316931703171317231733174317531763177317831793180318131823183318431853186318731883189319031913192319331943195319631973198319932003201320232033204320532063207320832093210321132123213321432153216321732183219322032213222322332243225322632273228322932303231323232333234323532363237323832393240324132423243324432453246324732483249325032513252325332543255325632573258325932603261326232633264326532663267326832693270327132723273327432753276327732783279328032813282328332843285328632873288328932903291329232933294329532963297329832993300330133023303330433053306330733083309331033113312331333143315331633173318331933203321332233233324332533263327332833293330333133323333333433353336333733383339334033413342334333443345334633473348334933503351335233533354335533563357335833593360336133623363336433653366336733683369337033713372337333743375337633773378337933803381338233833384338533863387338833893390339133923393339433953396339733983399340034013402340334043405340634073408340934103411341234133414341534163417341834193420342134223423342434253426342734283429343034313432343334343435343634373438343934403441344234433444344534463447344834493450345134523453345434553456345734583459346034613462346334643465346634673468346934703471347234733474347534763477347834793480348134823483348434853486348734883489349034913492349334943495349634973498349935003501350235033504350535063507350835093510351135123513351435153516351735183519352035213522352335243525352635273528352935303531353235333534353535363537353835393540354135423543354435453546354735483549355035513552355335543555355635573558355935603561356235633564356535663567356835693570357135723573357435753576357735783579358035813582358335843585358635873588358935903591359235933594359535963597359835993600360136023603360436053606360736083609361036113612361336143615361636173618361936203621362236233624362536263627362836293630363136323633363436353636363736383639364036413642364336443645364636473648364936503651365236533654365536563657365836593660366136623663366436653666366736683669367036713672367336743675367636773678367936803681368236833684368536863687368836893690369136923693369436953696369736983699370037013702370337043705370637073708370937103711371237133714371537163717371837193720372137223723372437253726372737283729373037313732373337343735373637373738373937403741374237433744374537463747374837493750375137523753375437553756375737583759376037613762376337643765376637673768376937703771377237733774377537763777377837793780378137823783378437853786378737883789379037913792379337943795379637973798379938003801380238033804380538063807380838093810381138123813381438153816381738183819382038213822382338243825382638273828382938303831383238333834383538363837383838393840384138423843384438453846384738483849385038513852385338543855385638573858385938603861386238633864386538663867386838693870387138723873387438753876387738783879388038813882388338843885388638873888388938903891389238933894389538963897389838993900390139023903390439053906390739083909391039113912391339143915391639173918391939203921392239233924392539263927392839293930393139323933393439353936393739383939394039413942394339443945394639473948394939503951395239533954395539563957395839593960396139623963396439653966396739683969397039713972397339743975397639773978397939803981398239833984398539863987398839893990399139923993399439953996399739983999400040014002400340044005400640074008400940104011401240134014401540164017401840194020402140224023402440254026402740284029403040314032403340344035403640374038403940404041404240434044404540464047404840494050405140524053405440554056405740584059406040614062406340644065406640674068406940704071407240734074407540764077407840794080408140824083408440854086408740884089409040914092409340944095409640974098409941004101410241034104410541064107410841094110411141124113411441154116411741184119412041214122412341244125412641274128412941304131413241334134413541364137413841394140414141424143414441454146414741484149415041514152415341544155415641574158415941604161416241634164416541664167416841694170417141724173417441754176417741784179418041814182418341844185418641874188418941904191419241934194419541964197419841994200420142024203420442054206420742084209421042114212421342144215421642174218421942204221422242234224422542264227422842294230423142324233423442354236423742384239424042414242424342444245424642474248424942504251425242534254425542564257425842594260426142624263426442654266426742684269427042714272427342744275427642774278427942804281428242834284428542864287428842894290429142924293429442954296429742984299430043014302430343044305430643074308430943104311431243134314431543164317431843194320432143224323432443254326432743284329433043314332433343344335433643374338433943404341434243434344434543464347434843494350435143524353435443554356435743584359436043614362436343644365436643674368436943704371437243734374437543764377437843794380438143824383438443854386438743884389439043914392439343944395439643974398439944004401440244034404440544064407440844094410441144124413441444154416441744184419442044214422442344244425442644274428442944304431443244334434443544364437443844394440444144424443444444454446444744484449445044514452445344544455445644574458445944604461446244634464446544664467446844694470447144724473447444754476447744784479448044814482448344844485448644874488448944904491449244934494449544964497449844994500450145024503450445054506450745084509451045114512451345144515451645174518451945204521452245234524452545264527452845294530453145324533453445354536453745384539454045414542454345444545454645474548454945504551455245534554455545564557455845594560456145624563456445654566456745684569457045714572457345744575457645774578457945804581458245834584458545864587458845894590459145924593459445954596459745984599460046014602460346044605460646074608460946104611461246134614461546164617461846194620462146224623462446254626462746284629463046314632463346344635463646374638463946404641464246434644464546464647464846494650465146524653465446554656465746584659466046614662466346644665466646674668466946704671467246734674467546764677467846794680468146824683468446854686468746884689469046914692469346944695469646974698469947004701470247034704470547064707470847094710471147124713471447154716471747184719472047214722472347244725472647274728472947304731473247334734473547364737473847394740474147424743474447454746474747484749475047514752475347544755475647574758475947604761476247634764476547664767
  1. #include "mainwindow.h"
  2. #include "ui_mainwindow.h"
  3. #include <iomanip>//不要忘记包含此头文件
  4. #include <QScrollBar>
  5. #include "gpsimu.pb.h"
  6. #include "gnss_coordinate_convert.h"
  7. #define VIEW_WIDTH 10000
  8. #define VIEW_HEIGHT 10000
  9. double glon0 = 117.0866293;
  10. double glat0 = 39.1364713;
  11. //double glon0 = 117;
  12. //double glat0 = 39;
  13. double ghdg0 = 360;
  14. MainWindow::MainWindow(QWidget *parent) :
  15. QMainWindow(parent),
  16. ui(new Ui::MainWindow)
  17. {
  18. ui->setupUi(this);
  19. mnfac = 1;
  20. // int aaa[10000000];
  21. mnMoveX = VIEW_WIDTH/2;
  22. mnMoveY = VIEW_HEIGHT/2;
  23. mnDefmnfac = mnfac;
  24. mnDefMoveX = mnMoveX;
  25. mnDefMoveY = mnMoveY;
  26. myview = new MyView(this);
  27. myview->setObjectName(QStringLiteral("graphicsView"));
  28. myview->setGeometry(QRect(30, 30, 600, 600));
  29. connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));
  30. image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300,使用32位颜色
  31. myview->setCacheMode(myview->CacheBackground);
  32. painter = new QPainter(image);
  33. painter->end();
  34. scene = new QGraphicsScene;
  35. // painter->begin(image);
  36. QTabWidget * p = new QTabWidget(ui->centralWidget);
  37. p->setGeometry(30,30,300,300);
  38. mnFontHeight = ui->centralWidget->fontMetrics().height();
  39. CreateTab1View(p);
  40. mTabMain = p;
  41. QTimer * timer = new QTimer();
  42. connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
  43. timer->start(1000);
  44. setWindowTitle("Create Map From Lane Info");
  45. }
  46. MainWindow::~MainWindow()
  47. {
  48. delete ui;
  49. }
  50. void MainWindow::resizeEvent(QResizeEvent *event)
  51. {
  52. qDebug("resize");
  53. QSize sizemain = ui->centralWidget->size();
  54. qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());
  55. AdjustWPos(sizemain);
  56. }
  57. void MainWindow::AdjustWPos(QSize sizemain)
  58. {
  59. myview->setGeometry(0,30,sizemain.width()-mnFontHeight * 22 - 30,sizemain.height());
  60. mTabMain->setGeometry(sizemain.width()-mnFontHeight * 22,30,mnFontHeight * 22,sizemain.height()-50);
  61. // mgplidar->setGeometry(sizemain.width()-280,30,260,200);
  62. }
  63. void MainWindow::ExecPainter()
  64. {
  65. QTime x;
  66. x.start();
  67. // qDebug("painter.");
  68. painter->begin(image);
  69. qDebug("time is %d",x.elapsed());
  70. image->fill(QColor(255, 255, 255));//对画布进行填充
  71. // std::vector<iv::GPSData> navigation_data = brain->navigation_data;
  72. painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式,好看一点
  73. painter->translate(mnMoveX,mnMoveY);
  74. painter->setPen(Qt::black);
  75. painter->drawLine(VIEW_WIDTH/(-2),0,VIEW_WIDTH/2,0);
  76. painter->drawLine(0,VIEW_HEIGHT/(-2),0,VIEW_HEIGHT/2);
  77. int i;
  78. // int nfac = 5;;
  79. painter->setPen(Qt::blue);
  80. int nfac = mnfac;
  81. if(mbClick)
  82. {
  83. painter->setPen(Qt::red);
  84. painter->drawEllipse(QPoint(mClickX ,mClickY),mnMarkSize,mnMarkSize);
  85. painter->setPen(Qt::black);
  86. }
  87. if(mbSetObj)
  88. {
  89. painter->setPen(Qt::green);
  90. painter->drawRect(mfObjX*mnfac-mnMarkSize,mfObjY*mnfac*(-1)-mnMarkSize,mnMarkSize*2,mnMarkSize*2);
  91. painter->setPen(Qt::black);
  92. }
  93. painter->setPen(Qt::green);
  94. double x0,y0;
  95. GaussProjCal(glon0,glat0,&x0,&y0);
  96. painter->setPen(Qt::blue);
  97. // int nfac = mnfac;
  98. int selid = mpCBRoad->currentText().toInt();
  99. for(i=0;i<mxodr.GetRoadCount();i++)
  100. {
  101. // continue;
  102. int j;
  103. Road * pRoad = mxodr.GetRoad(i);
  104. painter->setPen(Qt::blue);
  105. if(selid == atoi(pRoad->GetRoadId().data()))
  106. {
  107. painter->setPen(Qt::red);
  108. }
  109. if(mxodr.GetRoad(i)->GetGeometryBlockCount()>0)
  110. {
  111. GeometryBlock * pgeob = pRoad->GetGeometryBlock(0);
  112. double x,y;
  113. RoadGeometry * pg;
  114. pg = pgeob->GetGeometryAt(0);
  115. x = pg->GetX();
  116. y = pg->GetY();
  117. double endx,endy,endhdg;
  118. GetEndPoint(pRoad,endx,endy,endhdg);
  119. x = (x+endx)/2;
  120. y = (y+endy)/2;
  121. painter->drawText(x*mnfac,y*mnfac*(-1),mxodr.GetRoad(i)->GetRoadId().data());
  122. }
  123. for(j=0;j<mxodr.GetRoad(i)->GetGeometryBlockCount();j++)
  124. {
  125. GeometryBlock * pgeob = pRoad->GetGeometryBlock(j);
  126. double x,y;
  127. double x_center,y_center;
  128. double R;
  129. RoadGeometry * pg;
  130. GeometryArc * parc;
  131. GeometryParamPoly3 * ppp3;
  132. GeometrySpiral *pSpiral;
  133. double rel_x,rel_y,rel_hdg;
  134. pg = pgeob->GetGeometryAt(0);
  135. x = pg->GetX();
  136. y = pg->GetY();
  137. if(j== 0)
  138. {
  139. if(selid == atoi(pRoad->GetRoadId().data()))
  140. {
  141. painter->setPen(Qt::green);
  142. painter->drawEllipse(x*mnfac-5,y*mnfac*(-1)-5,10,10);
  143. painter->setPen(Qt::red);
  144. }
  145. }
  146. switch (pg->GetGeomType()) {
  147. case 0:
  148. painter->drawLine(QPoint(x*mnfac,y*mnfac*(-1)),
  149. QPoint((x + pg->GetLength() * cos(pg->GetHdg()))*mnfac,(y + pg->GetLength() * sin(pg->GetHdg()))*mnfac*(-1)));
  150. break;
  151. case 1:
  152. pSpiral = (GeometrySpiral * )pg;
  153. {
  154. int ncount = pSpiral->GetLength() * mnfac;
  155. double sstep = pSpiral->GetLength()/((double)ncount);
  156. int k;
  157. double x0,y0,hdg0,s0;
  158. x0 = pSpiral->GetX();
  159. y0 = pSpiral->GetY();
  160. s0 = pSpiral->GetS();
  161. hdg0 = pSpiral->GetHdg() ;
  162. painter->setPen(Qt::red);
  163. for(k=0;k<ncount;k++)
  164. {
  165. pSpiral->GetCoords(s0+sstep*k,rel_x,rel_y,rel_hdg);
  166. x = rel_x;
  167. y = rel_y;
  168. painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
  169. }
  170. painter->setPen(Qt::blue);
  171. }
  172. // qDebug("spi");
  173. break;
  174. case 2:
  175. {
  176. parc = (GeometryArc *)pg;
  177. R = abs(1.0/parc->GetCurvature());
  178. if(parc->GetCurvature() > 0)
  179. {
  180. x_center = pg->GetX() + R *cos(pg->GetHdg() + M_PI/2.0);
  181. y_center = pg->GetY() + R * sin(pg->GetHdg() + M_PI/2.0);
  182. }
  183. else
  184. {
  185. x_center = pg->GetX() + R *cos(pg->GetHdg() -M_PI/2.0);
  186. y_center = pg->GetY() + R * sin(pg->GetHdg() - M_PI/2.0);
  187. }
  188. int k;
  189. int ncount = parc->GetLength() * mnfac ;
  190. double curv = parc->GetCurvature();
  191. double hdgstep;
  192. double hdg0 = parc->GetHdg();
  193. double hdgnow = parc->GetHdg();
  194. if(ncount > 0) hdgstep= (parc->GetLength()/R)/ncount;
  195. for(k=0;k<ncount;k++)
  196. {
  197. double x_draw,y_draw;
  198. if(curv > 0)
  199. {
  200. hdgnow = hdg0 + k*hdgstep;
  201. x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
  202. y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
  203. }
  204. else
  205. {
  206. hdgnow = hdg0 - k * hdgstep;
  207. x_draw = x_center + R *cos(hdgnow + M_PI/2.0);
  208. y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
  209. }
  210. painter->drawPoint(x_draw * mnfac ,y_draw * mnfac *(-1));
  211. }
  212. }
  213. break;
  214. case 4:
  215. {
  216. ppp3 = (GeometryParamPoly3 * )pg;
  217. int ncount = ppp3->GetLength()* mnfac;
  218. double sstep;
  219. if(ncount > 0)sstep = ppp3->GetLength()/ncount;
  220. else sstep = 10000.0;
  221. double s = 0;
  222. while(s < ppp3->GetLength())
  223. {
  224. double xtem,ytem;
  225. xtem = ppp3->GetuA() + ppp3->GetuB() * s + ppp3->GetuC() * s*s + ppp3->GetuD() * s*s*s;
  226. ytem = ppp3->GetvA() + ppp3->GetvB() * s + ppp3->GetvC() * s*s + ppp3->GetvD() * s*s*s;
  227. x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
  228. y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
  229. painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
  230. s = s+ sstep;
  231. }
  232. }
  233. break;
  234. default:
  235. break;
  236. }
  237. // painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
  238. }
  239. }
  240. for(i=0;i<mvectorlp.size();i++)
  241. {
  242. if(!mbShowLane)continue;
  243. if(i == mnMarkLane)
  244. {
  245. painter->setPen(Qt::red);
  246. }
  247. else
  248. {
  249. painter->setPen(Qt::blue);
  250. }
  251. int npsize = mvectorlp[i].mvectorlpleft.size();
  252. int j;
  253. for(j=0;j<npsize;j++)
  254. {
  255. double x,y;
  256. double lon,lat;
  257. lon = mvectorlp[i].mvectorlpleft[j].mfLon;
  258. lat = mvectorlp[i].mvectorlpleft[j].mfLat;
  259. GaussProjCal(lon,lat,&x,&y);
  260. x = x-x0;
  261. y= y-y0;
  262. painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
  263. lon = mvectorlp[i].mvectorlpright[j].mfLon;
  264. lat = mvectorlp[i].mvectorlpright[j].mfLat;
  265. GaussProjCal(lon,lat,&x,&y);
  266. x = x-x0;
  267. y= y-y0;
  268. painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
  269. }
  270. }
  271. painter->setPen(Qt::green);
  272. painter->end();
  273. }
  274. //刷新
  275. void MainWindow::paintEvent(QPaintEvent *)
  276. {
  277. if(mbRefresh)
  278. {
  279. ExecPainter();
  280. // qDebug(" time is %d ",x.elapsed());
  281. mbRefresh = false;
  282. }
  283. scene->clear();
  284. scene->addPixmap(QPixmap::fromImage(*image));
  285. myview->setScene(scene);
  286. myview->show();
  287. if(mbInit == false)
  288. {
  289. myview->horizontalScrollBar()->setValue((mnMoveX - 800));
  290. myview->verticalScrollBar()->setValue((mnMoveY - 500));
  291. mbInit = true;
  292. }
  293. // qDebug(" time 2 is %d ",x.elapsed());
  294. }
  295. void MainWindow::onTimer()
  296. {
  297. }
  298. void MainWindow::CreateTab1View(QTabWidget * p)
  299. {
  300. QGroupBox * pGroup = new QGroupBox();
  301. pGroup->setGeometry(0,0,mnFontHeight * 21,mnFontHeight * 110);
  302. QLabel * pLabel;
  303. QLineEdit * pLE;
  304. QPushButton * pPB;
  305. QSlider * pSlider;
  306. QComboBox * pCB;
  307. int nXPos = 10;
  308. int nYPos = 30;
  309. int i;
  310. int nSpace = mnFontHeight * 65/10;
  311. int nLEWidth = mnFontHeight * 6;
  312. int nLEHeight = mnFontHeight * 3/2;
  313. pLabel = new QLabel(pGroup);
  314. pLabel->setText("Lat0");
  315. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  316. nXPos = nXPos + nSpace;
  317. pLabel = new QLabel(pGroup);
  318. pLabel->setText("Lon0");
  319. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  320. nXPos = nXPos + nSpace;
  321. pLabel = new QLabel(pGroup);
  322. pLabel->setText("Head0");
  323. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  324. nXPos = nXPos + nSpace;
  325. nXPos = 10;
  326. nYPos = nYPos + mnFontHeight * 2;
  327. pLE = new QLineEdit(pGroup);
  328. pLE->setText(QString::number(glat0,'f',7));
  329. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  330. mpLELat0 = pLE;
  331. nXPos = nXPos + nSpace;
  332. pLE = new QLineEdit(pGroup);
  333. pLE->setText(QString::number(glon0,'f',7));
  334. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  335. mpLELon0 = pLE;
  336. nXPos = nXPos + nSpace;
  337. pLE = new QLineEdit(pGroup);
  338. pLE->setText(QString::number(ghdg0,'f',3));
  339. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  340. mpLEHead0 = pLE;
  341. nXPos = nXPos + nSpace;
  342. nXPos = 10;
  343. nYPos = nYPos + mnFontHeight * 4;
  344. pPB = new QPushButton(pGroup);
  345. pPB->setText("Restore Default View");
  346. pPB->setGeometry(nXPos,nYPos,mnFontHeight*10,nLEHeight);
  347. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDefView()));
  348. nXPos = nXPos + mnFontHeight * 11;
  349. pPB = new QPushButton(pGroup);
  350. pPB->setText("Zoom One");
  351. pPB->setGeometry(nXPos,nYPos,mnFontHeight*6,nLEHeight);
  352. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickZoomOne()));
  353. nXPos = 10;
  354. nYPos = nYPos + mnFontHeight * 4;
  355. pLabel = new QLabel(pGroup);
  356. pLabel->setText("Scale");
  357. pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  358. nXPos = nXPos + mnFontHeight * 4;
  359. pSlider = new QSlider(pGroup);
  360. pSlider->setOrientation(Qt::Horizontal);
  361. pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
  362. pSlider->setRange(1,100);
  363. pSlider->setValue(mnfac);
  364. connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeScale(int)));
  365. mpSlider_Scale = pSlider;
  366. nXPos = nXPos + mnFontHeight*11;
  367. pLE = new QLineEdit(pGroup);
  368. pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  369. pLE->setText(QString::number(pSlider->value()));
  370. mpLE_Scale = pLE;
  371. nXPos = 10;
  372. nYPos = nYPos + mnFontHeight * 2;
  373. pLabel = new QLabel(pGroup);
  374. pLabel->setText("MoveX");
  375. pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  376. nXPos = nXPos + mnFontHeight * 4;
  377. pSlider = new QSlider(pGroup);
  378. pSlider->setOrientation(Qt::Horizontal);
  379. pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
  380. pSlider->setRange(0,100);
  381. pSlider->setValue(mnMoveX*100/VIEW_WIDTH);
  382. connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMoveX(int)));
  383. mpSlider_MoveX = pSlider;
  384. nXPos = nXPos + mnFontHeight*11;
  385. pLE = new QLineEdit(pGroup);
  386. pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  387. pLE->setText(QString::number(pSlider->value()));
  388. mpLE_MoveX = pLE;
  389. nXPos = 10;
  390. nYPos = nYPos + mnFontHeight * 2;
  391. pLabel = new QLabel(pGroup);
  392. pLabel->setText("MoveY");
  393. pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  394. nXPos = nXPos + mnFontHeight * 4;
  395. pSlider = new QSlider(pGroup);
  396. pSlider->setOrientation(Qt::Horizontal);
  397. pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
  398. pSlider->setRange(0,100);
  399. pSlider->setValue(mnMoveY*100/VIEW_HEIGHT);
  400. connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMoveY(int)));
  401. mpSlider_MoveY = pSlider;
  402. nXPos = nXPos + mnFontHeight*11;
  403. pLE = new QLineEdit(pGroup);
  404. pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  405. pLE->setText(QString::number(pSlider->value()));
  406. mpLE_MoveY = pLE;
  407. nXPos = 10;
  408. nYPos = nYPos + mnFontHeight * 4;
  409. pLabel = new QLabel(pGroup);
  410. pLabel->setText("Mark");
  411. pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  412. nXPos = nXPos + mnFontHeight * 4;
  413. pSlider = new QSlider(pGroup);
  414. pSlider->setOrientation(Qt::Horizontal);
  415. pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
  416. pSlider->setRange(5,100);
  417. pSlider->setValue(mnMarkSize);
  418. connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMark(int)));
  419. mpSlider_Mark = pSlider;
  420. nXPos = nXPos + mnFontHeight*11;
  421. pLE = new QLineEdit(pGroup);
  422. pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
  423. pLE->setText(QString::number(pSlider->value()));
  424. mpLE_Mark = pLE;
  425. nXPos = 10;
  426. nYPos = nYPos + mnFontHeight * 4;
  427. pLabel = new QLabel(pGroup);
  428. pLabel->setText("SelLon");
  429. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  430. nXPos = nXPos + nSpace;
  431. pLabel = new QLabel(pGroup);
  432. pLabel->setText("SelLat");
  433. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  434. nXPos = nXPos + nSpace;
  435. nXPos = 10;
  436. nYPos = nYPos + mnFontHeight * 2;
  437. pLE = new QLineEdit(pGroup);
  438. pLE->setText("");
  439. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  440. mpLE_SelX = pLE;
  441. nXPos = nXPos + nSpace;
  442. pLE = new QLineEdit(pGroup);
  443. pLE->setText("");
  444. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  445. mpLE_SelY = pLE;
  446. nXPos = nXPos + nSpace;
  447. nXPos = 10;
  448. nYPos = nYPos + mnFontHeight * 2;
  449. pLE = new QLineEdit(pGroup);
  450. pLE->setText("");
  451. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  452. mpLE_SelLon = pLE;
  453. nXPos = nXPos + nSpace;
  454. pLE = new QLineEdit(pGroup);
  455. pLE->setText("");
  456. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  457. mpLE_SelLat = pLE;
  458. nXPos = nXPos + nSpace;
  459. pLE = new QLineEdit(pGroup);
  460. pLE->setText("");
  461. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  462. pLE->setText("360");
  463. mpLE_StartHeading = pLE;
  464. nXPos = 10;
  465. nYPos = nYPos + mnFontHeight * 4;
  466. pPB = new QPushButton(pGroup);
  467. pPB->setText("Load Lane");
  468. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  469. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLoadLane()));
  470. nXPos = nXPos + nSpace;
  471. pCB = new QComboBox(pGroup);
  472. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  473. nXPos = nXPos + nSpace;
  474. mpCBLane = pCB;
  475. nXPos = 10;
  476. nYPos = nYPos + mnFontHeight * 2;
  477. pPB = new QPushButton(pGroup);
  478. pPB->setText("Remove");
  479. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  480. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRemoveLane()));
  481. nXPos = nXPos + nSpace;
  482. pPB = new QPushButton(pGroup);
  483. pPB->setText("Mark");
  484. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  485. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickMarkLane()));
  486. nXPos = nXPos + nSpace;
  487. pPB = new QPushButton(pGroup);
  488. pPB->setText("Clear Road Lane");
  489. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  490. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickClearRoadLane()));
  491. nXPos = nXPos + nSpace;
  492. nXPos = 10;
  493. nYPos = nYPos + mnFontHeight * 2;
  494. pPB = new QPushButton(pGroup);
  495. pPB->setText("To Road");
  496. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  497. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickToRoad()));
  498. nXPos = nXPos + nSpace;
  499. pPB = new QPushButton(pGroup);
  500. pPB->setText("To Opposite");
  501. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  502. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickToOpposite()));
  503. nXPos = nXPos + nSpace;
  504. pLE = new QLineEdit(pGroup);
  505. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  506. nXPos = nXPos + nSpace;
  507. mpLE_RoadName = pLE;
  508. nXPos = 10;
  509. nYPos = nYPos + mnFontHeight * 2;
  510. pCB = new QComboBox(pGroup);
  511. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  512. nXPos = nXPos + nSpace;
  513. mpCBSelLane = pCB;
  514. pCB = new QComboBox(pGroup);
  515. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  516. nXPos = nXPos + nSpace;
  517. mpCBSelOpLane = pCB;
  518. pPB = new QPushButton(pGroup);
  519. pPB->setText("Add Road");
  520. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  521. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickAddRoad()));
  522. nXPos = nXPos + nSpace;
  523. nXPos = 10;
  524. nYPos = nYPos + mnFontHeight * 4;
  525. pCB = new QComboBox(pGroup);
  526. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  527. nXPos = nXPos + nSpace;
  528. mpCBRoad = pCB;
  529. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBRoadChange(int)));
  530. pPB = new QPushButton(pGroup);
  531. pPB->setText("Mark");
  532. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  533. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadMark()));
  534. nXPos = nXPos + nSpace;
  535. pPB = new QPushButton(pGroup);
  536. pPB->setText("Del");
  537. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  538. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadDel()));
  539. nXPos = nXPos + nSpace;
  540. nXPos = 10;
  541. nYPos = nYPos + mnFontHeight * 2;
  542. pLabel = new QLabel(pGroup);
  543. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  544. pLabel->setText("Predecessor:");
  545. nXPos = nXPos + nSpace;
  546. pLabel = new QLabel(pGroup);
  547. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  548. pLabel->setText("road");
  549. mpLabelRoadShowPreType1 = pLabel;
  550. nXPos = nXPos + nSpace;
  551. pLabel = new QLabel(pGroup);
  552. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  553. pLabel->setText("10010");
  554. mpLabelRoadShowPreID = pLabel;
  555. nXPos = nXPos + nSpace;
  556. nXPos = 10;
  557. nYPos = nYPos + mnFontHeight * 2;
  558. pLabel = new QLabel(pGroup);
  559. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  560. pLabel->setText("start");
  561. mpLabelRoadShowPreType2 = pLabel;
  562. nXPos = nXPos + nSpace;
  563. pCB = new QComboBox(pGroup);
  564. pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
  565. mpCBRoadShowPre = pCB;
  566. nXPos = 10;
  567. nYPos = nYPos + mnFontHeight * 2;
  568. pLabel = new QLabel(pGroup);
  569. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  570. pLabel->setText("Successor:");
  571. nXPos = nXPos + nSpace;
  572. pLabel = new QLabel(pGroup);
  573. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  574. pLabel->setText("road");
  575. mpLabelRoadShowNxtType1 = pLabel;
  576. nXPos = nXPos + nSpace;
  577. pLabel = new QLabel(pGroup);
  578. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  579. pLabel->setText("10010");
  580. mpLabelRoadShowNxtID = pLabel;
  581. nXPos = nXPos + nSpace;
  582. nXPos = 10;
  583. nYPos = nYPos + mnFontHeight * 2;
  584. pLabel = new QLabel(pGroup);
  585. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  586. pLabel->setText("start");
  587. mpLabelRoadShowNxtType2 = pLabel;
  588. nXPos = nXPos + nSpace;
  589. pCB = new QComboBox(pGroup);
  590. pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
  591. mpCBRoadShowNext = pCB;
  592. // nXPos = 10;
  593. // nYPos = nYPos + mnFontHeight * 2;
  594. // pCB = new QComboBox(pGroup);
  595. // pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  596. // nXPos = nXPos + nSpace;
  597. // mpCBPreNxtCurLane = pCB;
  598. nXPos = 10;
  599. nYPos = nYPos + mnFontHeight * 2;
  600. pCB = new QComboBox(pGroup);
  601. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  602. nXPos = nXPos + nSpace;
  603. mpCBPreNxtRoad = pCB;
  604. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickPreNxtRoadChange(int)));
  605. pCB = new QComboBox(pGroup);
  606. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  607. nXPos = nXPos + nSpace;
  608. mpCBPreNxtRelLane = pCB;
  609. pCB = new QComboBox(pGroup);
  610. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  611. nXPos = nXPos + nSpace;
  612. mpCBPreNxtConatact = pCB;
  613. pCB->addItem("start");
  614. pCB->addItem("end");
  615. nXPos = 10;
  616. nYPos = nYPos + mnFontHeight * 2;
  617. pPB = new QPushButton(pGroup);
  618. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  619. nXPos = nXPos + nSpace*15/10;
  620. pPB->setText("Set Predecessor");
  621. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetRoadPredecessor()));
  622. pPB = new QPushButton(pGroup);
  623. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  624. nXPos = nXPos + nSpace*15/10;
  625. pPB->setText("Set Successor");
  626. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetRoadSuccessor()));
  627. nXPos = 10;
  628. nYPos = nYPos + mnFontHeight * 2;
  629. pPB = new QPushButton(pGroup);
  630. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  631. nXPos = nXPos + nSpace*15/10;
  632. pPB->setText("Del Predecessor");
  633. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDelRoadPredecessor()));
  634. pPB = new QPushButton(pGroup);
  635. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  636. nXPos = nXPos + nSpace*15/10;
  637. pPB->setText("Del Successor");
  638. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDelRoadSuccessor()));
  639. nXPos = 10;
  640. nYPos = nYPos + mnFontHeight * 4;
  641. pCB = new QComboBox(pGroup);
  642. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  643. nXPos = nXPos + nSpace;
  644. mpCBRoad1 = pCB;
  645. pCB = new QComboBox(pGroup);
  646. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  647. nXPos = nXPos + nSpace;
  648. pCB->addItem("start");
  649. pCB->addItem("end");
  650. mpCBRC1 = pCB;
  651. nXPos = 10;
  652. nYPos = nYPos + mnFontHeight * 2;
  653. pCB = new QComboBox(pGroup);
  654. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  655. nXPos = nXPos + nSpace;
  656. mpCBRoad2 = pCB;
  657. pCB = new QComboBox(pGroup);
  658. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  659. nXPos = nXPos + nSpace;
  660. pCB->addItem("start");
  661. pCB->addItem("end");
  662. mpCBRC2 = pCB;
  663. pPB = new QPushButton(pGroup);
  664. pPB->setText("Road Contact");
  665. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  666. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadContact()));
  667. nXPos = 10;
  668. nYPos = nYPos + mnFontHeight * 4;
  669. pCB = new QComboBox(pGroup);
  670. pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
  671. nXPos = nXPos + nSpace;
  672. mpCBRoadCon = pCB;
  673. nXPos = 10;
  674. nYPos = nYPos + mnFontHeight * 2;
  675. pCB = new QComboBox(pGroup);
  676. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  677. nXPos = nXPos + nSpace;
  678. mpCBLane1 = pCB;
  679. pCB = new QComboBox(pGroup);
  680. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  681. nXPos = nXPos + nSpace;
  682. mpCBLane2 = pCB;
  683. nXPos = 10;
  684. nYPos = nYPos + mnFontHeight * 2;
  685. pPB = new QPushButton(pGroup);
  686. pPB->setText("Lane Contact");
  687. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  688. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLaneContact()));
  689. nXPos = nXPos + nSpace*15/10;
  690. pCB = new QComboBox(pGroup);
  691. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  692. nXPos = nXPos + nSpace;
  693. mpCBLane1Lane2 = pCB;
  694. nXPos = 10;
  695. nYPos = nYPos + mnFontHeight * 2;
  696. pPB = new QPushButton(pGroup);
  697. pPB->setText("OpLane Contact");
  698. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  699. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickOpLaneContact()));
  700. nXPos = nXPos + nSpace*15/10;
  701. pCB = new QComboBox(pGroup);
  702. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  703. nXPos = nXPos + nSpace;
  704. mpCBLane1Lane2op = pCB;
  705. nXPos = 10;
  706. nYPos = nYPos + mnFontHeight * 2;
  707. pPB = new QPushButton(pGroup);
  708. pPB->setText("Clear");
  709. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  710. nXPos = nXPos + nSpace;
  711. pPB = new QPushButton(pGroup);
  712. pPB->setText("Create Road");
  713. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  714. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateRoad()));
  715. nXPos = 10;
  716. nYPos = nYPos + mnFontHeight * 2;
  717. pCB = new QComboBox(pGroup);
  718. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  719. nXPos = nXPos + nSpace;
  720. mpCBRoadType = pCB;
  721. pCB->addItem("Turn");
  722. pCB->addItem("Straight");
  723. pCB->addItem("U-Turn");
  724. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onChangeRoadType(int)));
  725. pLabel = new QLabel(pGroup);
  726. pLabel->setText("Radius:");
  727. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  728. nXPos = nXPos + nSpace;
  729. mpLBRoadType = pLabel;
  730. pLE = new QLineEdit(pGroup);
  731. pLE->setText("6.0");
  732. pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  733. mpLERoadType = pLE;
  734. mpCBRoadType->setCurrentIndex(0);
  735. mpLBRoadType->setVisible(true);
  736. mpLERoadType->setVisible(true);
  737. nXPos = 10;
  738. nYPos = nYPos + mnFontHeight * 4;
  739. pPB = new QPushButton(pGroup);
  740. pPB->setText("Create Junction");
  741. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  742. nXPos = nXPos + nSpace*15/10;
  743. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateJunction()));
  744. pCB = new QComboBox(pGroup);
  745. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  746. nXPos = nXPos + nSpace;
  747. mpCBJunction = pCB;
  748. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionChange(int)));
  749. pCB = new QComboBox(pGroup);
  750. pCB->setGeometry(nXPos,nYPos,nLEWidth*5/10,nLEHeight);
  751. mpCBJunctionConnection = pCB;
  752. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionConnectionChange(int)));
  753. nXPos = 10;
  754. nYPos = nYPos + mnFontHeight * 2;
  755. pLabel = new QLabel(pGroup);
  756. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  757. nXPos = nXPos + nSpace;
  758. mpLabelJunctionIncommingRoad = pLabel;
  759. pLabel = new QLabel(pGroup);
  760. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  761. nXPos = nXPos + nSpace;
  762. mpLabelJunctionContactPoint = pLabel;
  763. pLabel = new QLabel(pGroup);
  764. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  765. nXPos = nXPos + nSpace;
  766. mpLabelJunctionConnectingRoad = pLabel;
  767. nXPos = 10;
  768. nYPos = nYPos + mnFontHeight * 2;
  769. pCB = new QComboBox(pGroup);
  770. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  771. mpCBJunctionFromTo = pCB;
  772. nXPos = 10;
  773. nYPos = nYPos + mnFontHeight * 2;
  774. pLabel = new QLabel(pGroup);
  775. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  776. nXPos = nXPos + nSpace;
  777. pLabel->setText("Incomming");
  778. pCB = new QComboBox(pGroup);
  779. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  780. nXPos = nXPos + nSpace;
  781. mpCBJunctionIncommingRoad = pCB;
  782. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionIncommingChange(int)));
  783. nXPos = 10;
  784. nYPos = nYPos + mnFontHeight * 2;
  785. pLabel = new QLabel(pGroup);
  786. pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  787. nXPos = nXPos + nSpace;
  788. pLabel->setText("Connecting");
  789. pCB = new QComboBox(pGroup);
  790. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  791. nXPos = nXPos + nSpace;
  792. mpCBJunctionConnectingRoad= pCB;
  793. connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionConnectionroadChange(int)));
  794. pCB = new QComboBox(pGroup);
  795. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  796. nXPos = nXPos + nSpace;
  797. pCB->addItem("start");
  798. pCB->addItem("end");
  799. mpCBJunctionContactPoint = pCB;
  800. nXPos = 10;
  801. nYPos = nYPos + mnFontHeight * 2;
  802. pCB = new QComboBox(pGroup);
  803. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  804. nXPos = nXPos + nSpace;
  805. mpCBJunctionFromLane = pCB;
  806. pCB = new QComboBox(pGroup);
  807. pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  808. nXPos = nXPos + nSpace;
  809. mpCBJunctionToLane = pCB;
  810. nXPos = 10;
  811. nYPos = nYPos + mnFontHeight * 2;
  812. pPB = new QPushButton(pGroup);
  813. pPB->setText("Create Lane Link");
  814. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  815. nXPos = nXPos + nSpace*15/10;
  816. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateJunctionLaneLink()));
  817. pPB = new QPushButton(pGroup);
  818. pPB->setText("Delete Lane Link");
  819. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  820. nXPos = nXPos + nSpace*15/10;
  821. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDeleteJunctionLaneLink()));
  822. nXPos = 10;
  823. nYPos = nYPos + mnFontHeight * 4;
  824. pPB = new QPushButton(pGroup);
  825. pPB->setText("Auto Connect Road");
  826. pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
  827. nXPos = nXPos + nSpace*15/10;
  828. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickAutoConnect()));
  829. nXPos = 10;
  830. nYPos = nYPos + mnFontHeight * 4;
  831. pPB = new QPushButton(pGroup);
  832. pPB->setText("Save");
  833. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  834. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSave()));
  835. nXPos = nXPos + nSpace;
  836. pPB = new QPushButton(pGroup);
  837. pPB->setText("Load");
  838. pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
  839. connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLoad()));
  840. nXPos = nXPos + nSpace;
  841. QScrollArea * pScroll = new QScrollArea();
  842. pScroll->setWidget(pGroup);
  843. p->addTab(pScroll,"Calculate");
  844. }
  845. void MainWindow::onClickXY(double x, double y)
  846. {
  847. mClickX = x - mnMoveX;
  848. mClickY = y - mnMoveY;
  849. mbClick = true;
  850. mbRefresh = true;
  851. double selx,sely;
  852. double lon,lat;
  853. selx = mClickX;
  854. sely = mClickY * (-1);
  855. selx = selx/((double )mnfac);
  856. sely = sely/((double)mnfac);
  857. mpLE_SelX->setText(QString::number(selx,'f',3));
  858. mpLE_SelY->setText(QString::number(sely,'f',3));
  859. double x0,y0;
  860. GaussProjCal(glon0,glat0,&x0,&y0);
  861. GaussProjInvCal(x0+selx,y0+sely,&lon,&lat);
  862. mpLE_SelLon->setText(QString::number(lon,'f',7));
  863. mpLE_SelLat->setText(QString::number(lat,'f',7));
  864. update();
  865. }
  866. void MainWindow::onChangeScale(int scale)
  867. {
  868. mnfac = scale;
  869. mpLE_Scale->setText(QString::number(scale));
  870. mbRefresh = true;
  871. update();
  872. }
  873. void MainWindow::onChangeMoveX(int scale)
  874. {
  875. mnMoveX = VIEW_WIDTH * scale/100;
  876. mpLE_MoveX->setText(QString::number(scale));
  877. mbRefresh = true;
  878. update();
  879. }
  880. void MainWindow::onChangeMoveY(int scale)
  881. {
  882. mnMoveY = VIEW_HEIGHT * scale/100;
  883. mpLE_MoveY->setText(QString::number(scale));
  884. mbRefresh = true;
  885. update();
  886. }
  887. void MainWindow::onChangeMark(int scale)
  888. {
  889. mnMarkSize = scale;
  890. mpLE_Mark->setText(QString::number(scale));
  891. mbRefresh = true;
  892. update();
  893. }
  894. void MainWindow::onClickDefView()
  895. {
  896. mnfac = mnDefmnfac;
  897. mnMoveX = mnDefMoveX;
  898. mnMoveY = mnDefMoveY;
  899. mpSlider_Scale->setValue(mnfac);
  900. mpSlider_MoveX->setValue(mnMoveX*100/VIEW_WIDTH);
  901. mpSlider_MoveY->setValue(mnMoveY*100/VIEW_HEIGHT);
  902. // mbRefresh = true;
  903. update();
  904. }
  905. void MainWindow::onClickZoomOne()
  906. {
  907. myview->zoomone();
  908. }
  909. void MainWindow::onClickAsStart()
  910. {
  911. }
  912. void MainWindow::onClickAsDst()
  913. {
  914. double lon,lat;
  915. lon = mpLE_SelLon->text().toDouble();
  916. lat = mpLE_SelLat->text().toDouble();
  917. mpLE_DstLon->setText(QString::number(lon,'f',7));
  918. mpLE_DstLat->setText(QString::number(lat,'f',7));
  919. double x,y;
  920. x = mpLE_SelX->text().toDouble();
  921. y = mpLE_SelY->text().toDouble();
  922. mfObjX = x;
  923. mfObjY = y;
  924. mbSetObj = true;
  925. mbRefresh = true;
  926. update();
  927. }
  928. void MainWindow::onClickSetDst()
  929. {
  930. }
  931. void MainWindow::onClickPlan()
  932. {
  933. }
  934. void MainWindow::onClickSetStart()
  935. {
  936. }
  937. /**
  938. * @brief ADCIntelligentVehicle::UpdateMap
  939. * @param strdata
  940. * @param nSize
  941. * @param index
  942. * @param dt
  943. * @param strmemname
  944. */
  945. void MainWindow::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  946. {
  947. // std::cout<<"update map "<<std::endl;
  948. int gpsunitsize = sizeof(iv::GPS_INS);
  949. int nMapSize = nSize/gpsunitsize;
  950. // std::cout<<"map size is "<<nMapSize<<std::endl;
  951. if(nMapSize < 1)return;
  952. int i;
  953. mMutexNavi.lock();
  954. m_navigation_data.clear();
  955. for(i=0;i<nMapSize;i++)
  956. {
  957. iv::GPS_INS x;
  958. memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
  959. iv::GPSData data(new iv::GPS_INS);
  960. *data = x;
  961. m_navigation_data.push_back(data);
  962. }
  963. mMutexNavi.unlock();
  964. mbRefresh = true;
  965. update();
  966. }
  967. /**
  968. * @brief MainWindow::UpdateGPSIMU
  969. * @param strdata
  970. * @param nSize
  971. * @param index
  972. * @param dt
  973. * @param strmemname
  974. */
  975. void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
  976. {
  977. iv::gps::gpsimu xgpsimu;
  978. if(!xgpsimu.ParseFromArray(strdata,nSize))
  979. {
  980. // gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
  981. return;
  982. }
  983. double flat,flon,fhead;
  984. flat = xgpsimu.lat();
  985. flon = xgpsimu.lon();
  986. fhead = xgpsimu.heading();
  987. double x,y;
  988. GaussProjCal(flon,flat,&x,&y);
  989. mfNowX = x - mx0;
  990. mfNowY = y- my0;
  991. mfNowHdg = (90- fhead)*M_PI/180.0;
  992. mbGPSUpdate = true;
  993. mbGPSViewUpdate = true;
  994. }
  995. void MainWindow::onClickLoadLane()
  996. {
  997. QString str = QFileDialog::getOpenFileName(this,tr("Open Lane Info file"),"",tr("Lane File(*.txt)"));
  998. if(str.isEmpty())return;
  999. QFile xFile;
  1000. xFile.setFileName(str);
  1001. QFileInfo fi(str);
  1002. QString filename = fi.fileName();
  1003. double fLastLat = 39;
  1004. double fLastLon = 117;
  1005. if(filename.contains(".txt"))
  1006. {
  1007. filename = filename.left(filename.length() -4);
  1008. }
  1009. if(xFile.open(QIODevice::ReadWrite))
  1010. {
  1011. iv::lpunit lpu;
  1012. strncpy(lpu.strlanename,filename.toLatin1().data(),filename.size());
  1013. QByteArray ba;
  1014. ba = xFile.read(xFile.size());
  1015. QString strdata = ba;
  1016. QStringList strlinedata= strdata.split("\n",QString::SkipEmptyParts);
  1017. int i;
  1018. int nsize = strlinedata.size();
  1019. // qDebug("line is %d",nsize);
  1020. for(i=0;i<nsize;i++)
  1021. {
  1022. QString strline = strlinedata.at(i);
  1023. QStringList strvaluedata = strline.split(" ",QString::SkipEmptyParts);
  1024. // qDebug("%d value size is %d ",i,strvaluedata.size());
  1025. iv::lanepoint lpleft,lpright;
  1026. if(strvaluedata.size() == 12)
  1027. {
  1028. int j;
  1029. QString strx[12];
  1030. for(j=0;j<12;j++)strx[j] = strvaluedata.at(j);
  1031. lpleft.mfLon = strx[0].toDouble();
  1032. lpleft.mfLat = strx[1].toDouble();
  1033. lpleft.mfHeight = strx[2].toDouble();
  1034. lpleft.nLaneType = strx[3].toInt();
  1035. lpleft.mfHeading = strx[4].toDouble();
  1036. lpleft.mfDis = strx[5].toDouble();
  1037. lpright.mfLon = strx[6].toDouble();
  1038. lpright.mfLat = strx[7].toDouble();
  1039. lpright.mfHeight = strx[8].toDouble();
  1040. lpright.nLaneType = strx[9].toInt();
  1041. lpright.mfHeading = strx[10].toDouble();
  1042. lpright.mfDis = strx[11].toDouble();
  1043. lpleft.mfHdg = (90-lpleft.mfHeading) * M_PI/180.0;
  1044. if(lpleft.mfHdg < 0)lpleft.mfHdg = lpleft.mfHdg + 2.0*M_PI;
  1045. lpright.mfHdg = (90-lpright.mfHeading) * M_PI/180.0;
  1046. if(lpright.mfHdg < 0)lpright.mfHdg = lpright.mfHdg + 2.0*M_PI;
  1047. double x,y;
  1048. double xlast,ylast;
  1049. GaussProjCal(lpleft.mfLon,lpleft.mfLat,&x,&y);
  1050. GaussProjCal(fLastLon,fLastLat,&xlast,&ylast);
  1051. if(sqrt(pow(xlast -x ,2)+pow(ylast -y,2))> 0.1)
  1052. {
  1053. lpu.mvectorlpleft.push_back(lpleft);
  1054. lpu.mvectorlpright.push_back(lpright);
  1055. fLastLat = lpleft.mfLat;
  1056. fLastLon = lpleft.mfLon;
  1057. }
  1058. else
  1059. {
  1060. // qDebug("no use this point");
  1061. }
  1062. }
  1063. }
  1064. if((mbSetOrigin == false)&&(lpu.mvectorlpleft.size() > 0))
  1065. {
  1066. glat0 = lpu.mvectorlpleft[0].mfLat;
  1067. glon0 = lpu.mvectorlpright[0].mfLon;
  1068. mpLELat0->setText(QString::number(glat0,'f',7));
  1069. mpLELon0->setText(QString::number(glon0,'f',7));
  1070. mbSetOrigin = true;
  1071. }
  1072. double x0,y0;
  1073. GaussProjCal(glon0,glat0,&x0,&y0);
  1074. int j;
  1075. for(j=0;j<lpu.mvectorlpleft.size();j++)
  1076. {
  1077. double x,y;
  1078. GaussProjCal(lpu.mvectorlpleft[j].mfLon,lpu.mvectorlpleft[j].mfLat,&x,&y);
  1079. lpu.mvectorlpleft[j].mfX = x - x0;
  1080. lpu.mvectorlpleft[j].mfY = y- y0;
  1081. GaussProjCal(lpu.mvectorlpright[j].mfLon,lpu.mvectorlpright[j].mfLat,&x,&y);
  1082. lpu.mvectorlpright[j].mfX = x - x0;
  1083. lpu.mvectorlpright[j].mfY = y- y0;
  1084. }
  1085. if(lpu.mvectorlpleft.size() > 0)
  1086. {
  1087. mvectorlp.push_back(lpu);
  1088. mpCBLane->addItem(filename);
  1089. int N = lpu.mvectorlpleft.size();
  1090. VectorXd x_veh(N);
  1091. VectorXd y_veh(N);
  1092. int j;
  1093. int ntype = 0;
  1094. int ntypecount = 1;
  1095. for(j=0;j<N;j++)
  1096. {
  1097. double x,y;
  1098. GaussProjCal(glon0,glat0,&x0,&y0);
  1099. GaussProjCal(lpu.mvectorlpleft[j].mfLon,lpu.mvectorlpleft[j].mfLat,&x,&y);
  1100. x_veh[j] = x - x0;
  1101. y_veh[j] = y - y0;
  1102. if(lpu.mvectorlpright[j].nLaneType != ntype)
  1103. {
  1104. ntype = lpu.mvectorlpright[j].nLaneType;
  1105. ntypecount++;
  1106. }
  1107. }
  1108. // CreateRoad(x_veh,y_veh);
  1109. // auto coeffs = polyfit(x_veh, y_veh, 1);
  1110. // double dismax = 0;
  1111. // for(j=0;j<N;j++)
  1112. // {
  1113. // double A = coeffs[1];
  1114. // double B = -1;
  1115. // double C = coeffs[0];
  1116. // double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
  1117. // if(dis>dismax)dismax = dis;
  1118. // }
  1119. // int nxxx = coeffs.size();
  1120. }
  1121. }
  1122. xFile.close();
  1123. mbRefresh = true;
  1124. update();
  1125. }
  1126. /**
  1127. * @brief MainWindow::CreateRoad
  1128. * Create Road Geometry
  1129. * @param xvals
  1130. * @param yvals
  1131. * @return
  1132. */
  1133. Road MainWindow::CreateRoad(VectorXd xvals, VectorXd yvals)
  1134. {
  1135. // Road xRoad;
  1136. // double LINE_ERROR = 0.15;
  1137. // int nsize = xvals.size();
  1138. // int nnotfit = nsize;
  1139. // int ncurpos = 0;
  1140. // int nrange = nsize;
  1141. // while(ncurpos < nsize)
  1142. // {
  1143. // // int N = nrange - ncurpos;
  1144. // VectorXd x_veh(nrange);
  1145. // VectorXd y_veh(nrange);
  1146. // int j;
  1147. // for(j=ncurpos;j<(ncurpos +nrange);j++)
  1148. // {
  1149. // x_veh[j-ncurpos] = xvals[j];
  1150. // y_veh[j - ncurpos] = yvals[j];
  1151. // }
  1152. // auto coeffs = polyfit(x_veh, y_veh, 1);
  1153. // double dismax = 0;
  1154. // for(j=ncurpos;j<(ncurpos +nrange);j++)
  1155. // {
  1156. // double A = coeffs[1];
  1157. // double B = -1;
  1158. // double C = coeffs[0];
  1159. // double dis = fabs(A*x_veh[j-ncurpos] + B*y_veh[j-ncurpos] +C )/sqrt(pow(A,2)+pow(B,2));
  1160. // if(dis>dismax)dismax = dis;
  1161. // }
  1162. // std::cout<<"dis is "<<dismax<<std::endl;
  1163. // if((dismax > LINE_ERROR)&&((nsize -(nrange+ncurpos))>50))
  1164. // {
  1165. // nrange = nrange/2;
  1166. // }
  1167. // else
  1168. // {
  1169. // std::cout<<"nrange is "<<nrange<<std::endl;
  1170. // xRoad.AddGeometryBlock();
  1171. // GeometryBlock * pgb = xRoad.GetGeometryBlock(xRoad.GetGeometryBlockCount()-1);
  1172. // // pgb->AddGeometryLine(1.0,x,y,len);
  1173. // ncurpos = ncurpos + nrange;
  1174. // nrange = nsize - ncurpos;
  1175. // std::cout<<"add a geo."<<std::endl;
  1176. // }
  1177. // }
  1178. // return xRoad;
  1179. }
  1180. void MainWindow::onClickMarkLane()
  1181. {
  1182. mnMarkLane = mpCBLane->currentIndex();
  1183. mbRefresh = true;
  1184. update();
  1185. }
  1186. void MainWindow::onClickRemoveLane()
  1187. {
  1188. int i = mpCBLane->currentIndex();
  1189. if((i>=0) && (i<mvectorlp.size()))
  1190. {
  1191. int j;
  1192. for(j=0;j<mvectorroadlane.size();j++)
  1193. {
  1194. if(mvectorroadlane[j] == i)
  1195. {
  1196. mvectorroadlane.erase(mvectorroadlane.begin() + j);
  1197. mpCBSelLane->removeItem(j);
  1198. break;
  1199. }
  1200. }
  1201. mvectorlp.erase(mvectorlp.begin() + i);
  1202. mpCBLane->removeItem(i);
  1203. mbRefresh = true;
  1204. update();
  1205. }
  1206. }
  1207. void MainWindow::onClickToRoad()
  1208. {
  1209. int i = mpCBLane->currentIndex();
  1210. if((i>=0) && (i<mvectorlp.size()))
  1211. {
  1212. int j;
  1213. bool bHave = false;
  1214. for(j=0;j<mvectorroadlane.size();j++)
  1215. {
  1216. if(mvectorroadlane[j] == i)
  1217. {
  1218. bHave = true;
  1219. break;
  1220. }
  1221. }
  1222. if(bHave == false)
  1223. {
  1224. mvectorroadlane.push_back(i);
  1225. mpCBSelLane->addItem(mvectorlp.at(i).strlanename);
  1226. }
  1227. }
  1228. }
  1229. void MainWindow::onClickToOpposite()
  1230. {
  1231. int i = mpCBLane->currentIndex();
  1232. if((i>=0) && (i<mvectorlp.size()))
  1233. {
  1234. int j;
  1235. bool bHave = false;
  1236. for(j=0;j<mvectorroadopposite.size();j++)
  1237. {
  1238. if(mvectorroadopposite[j] == i)
  1239. {
  1240. bHave = true;
  1241. break;
  1242. }
  1243. }
  1244. if(bHave == false)
  1245. {
  1246. mvectorroadopposite.push_back(i);
  1247. mpCBSelOpLane->addItem(mvectorlp.at(i).strlanename);
  1248. }
  1249. }
  1250. }
  1251. void MainWindow::onClickClearRoadLane()
  1252. {
  1253. mvectorroadlane.clear();
  1254. mvectorroadopposite.clear();
  1255. mpCBSelLane->clear();
  1256. mpCBSelOpLane->clear();
  1257. }
  1258. void MainWindow::onClickAddRoad()
  1259. {
  1260. if(mvectorroadlane.size() < 1)return;
  1261. int nlanesize = mvectorroadlane.size();
  1262. int noplanesize = mvectorroadopposite.size();
  1263. double notlinethresh = 1.0; //When heading change more than this value, is a arc.
  1264. int nsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
  1265. int * pntype = new int[nsize];
  1266. std::shared_ptr<int> ppntype; ppntype.reset(pntype);
  1267. std::vector<iv::lanepoint > * pvectorlp = &(mvectorlp[mvectorroadlane[0]].mvectorlpleft);
  1268. int i;
  1269. //Go throuh point for fit type.
  1270. for(i=0;i<5;i++)pntype[i] = 0;
  1271. for(i=5;i<(nsize-5);i++)
  1272. {
  1273. double s = pvectorlp->at(i).mfDis;
  1274. double head0 = pvectorlp->at(i).mfHeading;
  1275. int j;
  1276. double xcount = 0;
  1277. double headdifftotal = 0;
  1278. double headdiffavg = 0.0;
  1279. for(j=1;j<nsize;j++)
  1280. {
  1281. if((j>1)&&((pvectorlp->at(j).mfDis - s)>1.0))
  1282. {
  1283. break;
  1284. }
  1285. double headdiff = pvectorlp->at(j).mfHeading - head0;
  1286. if(headdiff > 300)headdiff = headdiff - 360;
  1287. if(headdiff < -300)headdiff = headdiff + 360;
  1288. headdifftotal = headdifftotal + headdiff;
  1289. xcount = xcount + 1.0;
  1290. }
  1291. if(xcount > 0)headdiffavg = headdifftotal/xcount;
  1292. if(fabs(headdiffavg) > (notlinethresh*2))
  1293. {
  1294. pntype[i] = 1;
  1295. }
  1296. else
  1297. {
  1298. pntype[i] = 0;
  1299. }
  1300. }
  1301. for(i=(nsize -5);i<nsize;i++)
  1302. {
  1303. pntype[i] = 0;
  1304. }
  1305. for(i=0;i<nsize;i++) //Remove Few Not Line Point.
  1306. {
  1307. if(pntype[i] != 0)
  1308. {
  1309. int j;
  1310. int ncount = 1;
  1311. for(j=(i+1);j<nsize;j++)
  1312. {
  1313. if(pntype[i] != 0)ncount++;
  1314. else break;
  1315. }
  1316. if(ncount < 10)pntype[i] = 0;
  1317. else
  1318. {
  1319. i = i+ ncount;
  1320. }
  1321. }
  1322. }
  1323. for(i=5;i<nsize;i++)
  1324. {
  1325. if(pntype[i] != pntype[i-1])
  1326. {
  1327. int j = i-1;
  1328. double disx = pvectorlp->at(i).mfDis;
  1329. for(j=(i-2);j>5;j--)
  1330. {
  1331. if(pntype[j] == pntype[i-1])
  1332. {
  1333. pntype[j] = 2; //Besel
  1334. }
  1335. if(fabs(pvectorlp->at(j).mfDis - disx)>1) //1 m besel
  1336. {
  1337. break;
  1338. }
  1339. }
  1340. pntype[i-1] = 2;
  1341. }
  1342. }
  1343. //Calc dis to ref line
  1344. double xor0,yor0;
  1345. GaussProjCal(glon0,glat0,&xor0,&yor0);
  1346. std::vector<std::vector<iv::distogeo>> xvectordiss;
  1347. std::vector<std::vector<iv::distogeo>> xvectoropdiss;
  1348. for(i=0;i<mvectorroadlane.size();i++)
  1349. {
  1350. std::vector<iv::distogeo> xvectordistogeo;
  1351. int j;
  1352. int nsizepoint = mvectorlp[mvectorroadlane[i]].mvectorlpright.size();
  1353. bool bHaveLast = false;
  1354. int nLast = 0;
  1355. for(j=0;j<nsizepoint;j++)
  1356. {
  1357. double fdismin = 1000;
  1358. double fS = 0;
  1359. int k;
  1360. double x,y;
  1361. double x0,y0;
  1362. iv::distogeo xdistogeo;
  1363. int nLastBig = 0;
  1364. bool bFindDisMin = false;
  1365. x = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfX;
  1366. y = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfY;
  1367. int kstart = 0;
  1368. if(bHaveLast)
  1369. {
  1370. kstart= nLast;
  1371. }
  1372. for(k=kstart;k<mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();k++)
  1373. {
  1374. x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
  1375. y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
  1376. double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
  1377. if(fdismin > fdis)
  1378. {
  1379. fdismin = fdis;
  1380. fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
  1381. bFindDisMin = true;
  1382. nLast = k;
  1383. }
  1384. if((bFindDisMin)&&(bHaveLast))
  1385. {
  1386. if(fdis>fdismin)
  1387. {
  1388. nLastBig++;
  1389. }
  1390. else
  1391. {
  1392. nLastBig = 0;
  1393. }
  1394. }
  1395. if(nLastBig > 10)break;
  1396. }
  1397. // std::cout<<" k is "<<k<<std::endl;
  1398. if(bHaveLast == false)
  1399. {
  1400. if(fdismin<10)bHaveLast = true;
  1401. }
  1402. else
  1403. {
  1404. if(fdismin > 10)bHaveLast = false;
  1405. }
  1406. double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
  1407. mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
  1408. double fHdgDiff = fHdg - mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfHdg;
  1409. if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
  1410. if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
  1411. if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
  1412. {
  1413. xdistogeo.mfdis = fdismin;
  1414. xdistogeo.mfs = fS;
  1415. mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS = fS;
  1416. }
  1417. else
  1418. {
  1419. xdistogeo.mfs = -1;
  1420. std::cout<<"point extend."<<std::endl;
  1421. mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS = -1;
  1422. }
  1423. xvectordistogeo.push_back(xdistogeo);
  1424. }
  1425. xvectordiss.push_back(xvectordistogeo);
  1426. }
  1427. for(i=1;i<mvectorroadlane.size();i++)
  1428. {
  1429. std::vector<iv::distogeo> xvectordistogeo;
  1430. int j;
  1431. int nsizepoint = mvectorlp[mvectorroadlane[i]].mvectorlpleft.size();
  1432. bool bHaveLast = false;
  1433. int nLast = 0;
  1434. for(j=0;j<nsizepoint;j++)
  1435. {
  1436. double fdismin = 1000;
  1437. double fS = 0;
  1438. int k;
  1439. double x,y;
  1440. double x0,y0;
  1441. iv::distogeo xdistogeo;
  1442. int nLastBig = 0;
  1443. bool bFindDisMin = false;
  1444. x = mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfX;
  1445. y = mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfY;
  1446. int kstart = 0;
  1447. if(bHaveLast)
  1448. {
  1449. kstart= nLast;
  1450. }
  1451. for(k=kstart;k<mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();k++)
  1452. {
  1453. x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
  1454. y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
  1455. double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
  1456. if(fdismin > fdis)
  1457. {
  1458. fdismin = fdis;
  1459. fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
  1460. bFindDisMin = true;
  1461. nLast = k;
  1462. }
  1463. if((bFindDisMin)&&(bHaveLast))
  1464. {
  1465. if(fdis>fdismin)
  1466. {
  1467. nLastBig++;
  1468. }
  1469. else
  1470. {
  1471. nLastBig = 0;
  1472. }
  1473. }
  1474. if(nLastBig > 10)break;
  1475. }
  1476. // std::cout<<" k is "<<k<<std::endl;
  1477. if(bHaveLast == false)
  1478. {
  1479. if(fdismin<10)bHaveLast = true;
  1480. }
  1481. else
  1482. {
  1483. if(fdismin > 10)bHaveLast = false;
  1484. }
  1485. double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
  1486. mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
  1487. double fHdgDiff = fHdg - mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfHdg;
  1488. if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
  1489. if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
  1490. if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
  1491. {
  1492. xdistogeo.mfdis = fdismin;
  1493. xdistogeo.mfs = fS;
  1494. mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfRelS = fS;
  1495. }
  1496. else
  1497. {
  1498. xdistogeo.mfs = -1;
  1499. std::cout<<"point extend."<<std::endl;
  1500. mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfRelS = -1;
  1501. }
  1502. xvectordistogeo.push_back(xdistogeo);
  1503. }
  1504. xvectordiss.push_back(xvectordistogeo);
  1505. }
  1506. for(i=0;i<mvectorroadopposite.size();i++)
  1507. {
  1508. std::vector<iv::distogeo> xvectordistogeo;
  1509. int j;
  1510. int nsizepoint = mvectorlp[mvectorroadopposite[i]].mvectorlpleft.size();
  1511. bool bHaveLast = false;
  1512. int nLast = 0;
  1513. for(j=0;j<nsizepoint;j++)
  1514. {
  1515. double fdismin = 1000;
  1516. double fS = 0;
  1517. int k;
  1518. double x,y;
  1519. double x0,y0;
  1520. iv::distogeo xdistogeo;
  1521. int nLastBig = 0;
  1522. bool bFindDisMin = false;
  1523. x = mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfX;
  1524. y = mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfY;
  1525. int nrefsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
  1526. int kstart = nrefsize-1;
  1527. if(bHaveLast)
  1528. {
  1529. kstart= nLast;
  1530. }
  1531. for(k=kstart;k>=0;k--)
  1532. {
  1533. x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
  1534. y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
  1535. double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
  1536. if(fdismin > fdis)
  1537. {
  1538. fdismin = fdis;
  1539. fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
  1540. bFindDisMin = true;
  1541. nLast = k;
  1542. }
  1543. if((bFindDisMin)&&(bHaveLast))
  1544. {
  1545. if(fdis>fdismin)
  1546. {
  1547. nLastBig++;
  1548. }
  1549. else
  1550. {
  1551. nLastBig = 0;
  1552. }
  1553. }
  1554. if(nLastBig > 10)break;
  1555. }
  1556. // std::cout<<" k is "<<k<<std::endl;
  1557. if(bHaveLast == false)
  1558. {
  1559. if(fdismin<10)bHaveLast = true;
  1560. }
  1561. else
  1562. {
  1563. if(fdismin > 10)bHaveLast = false;
  1564. }
  1565. double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
  1566. mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
  1567. double fHdgDiff = fHdg - mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfHdg;
  1568. if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
  1569. if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
  1570. if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
  1571. {
  1572. xdistogeo.mfdis = fdismin;
  1573. xdistogeo.mfs = fS;
  1574. mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfRelS = fS;
  1575. }
  1576. else
  1577. {
  1578. xdistogeo.mfs = -1;
  1579. std::cout<<"point extend."<<std::endl;
  1580. mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfRelS = -1;
  1581. }
  1582. xvectordistogeo.push_back(xdistogeo);
  1583. }
  1584. xvectoropdiss.push_back(xvectordistogeo);
  1585. }
  1586. for(i=0;i<mvectorroadopposite.size();i++)
  1587. {
  1588. std::vector<iv::distogeo> xvectordistogeo;
  1589. int j;
  1590. int nsizepoint = mvectorlp[mvectorroadopposite[i]].mvectorlpright.size();
  1591. bool bHaveLast = false;
  1592. int nLast = 0;
  1593. for(j=0;j<nsizepoint;j++)
  1594. {
  1595. double fdismin = 1000;
  1596. double fS = 0;
  1597. int k;
  1598. double x,y;
  1599. double x0,y0;
  1600. iv::distogeo xdistogeo;
  1601. int nLastBig = 0;
  1602. bool bFindDisMin = false;
  1603. x = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfX;
  1604. y = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfY;
  1605. int nrefsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
  1606. int kstart = nrefsize-1;
  1607. if(bHaveLast)
  1608. {
  1609. kstart= nLast;
  1610. }
  1611. for(k=kstart;k>=0;k--)
  1612. {
  1613. x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
  1614. y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
  1615. double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
  1616. if(fdismin > fdis)
  1617. {
  1618. fdismin = fdis;
  1619. fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
  1620. bFindDisMin = true;
  1621. nLast = k;
  1622. }
  1623. if((bFindDisMin)&&(bHaveLast))
  1624. {
  1625. if(fdis>fdismin)
  1626. {
  1627. nLastBig++;
  1628. }
  1629. else
  1630. {
  1631. nLastBig = 0;
  1632. }
  1633. }
  1634. if(nLastBig > 10)break;
  1635. }
  1636. // std::cout<<" k is "<<k<<std::endl;
  1637. if(bHaveLast == false)
  1638. {
  1639. if(fdismin<10)bHaveLast = true;
  1640. }
  1641. else
  1642. {
  1643. if(fdismin > 10)bHaveLast = false;
  1644. }
  1645. double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
  1646. mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
  1647. double fHdgDiff = fHdg - mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfHdg;
  1648. if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
  1649. if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
  1650. if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
  1651. {
  1652. xdistogeo.mfdis = fdismin;
  1653. xdistogeo.mfs = fS;
  1654. mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS = fS;
  1655. }
  1656. else
  1657. {
  1658. xdistogeo.mfs = -1;
  1659. std::cout<<"point extend."<<std::endl;
  1660. mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS = -1;
  1661. }
  1662. xvectordistogeo.push_back(xdistogeo);
  1663. }
  1664. xvectoropdiss.push_back(xvectordistogeo);
  1665. }
  1666. //Get Lane Type
  1667. std::vector<std::vector<iv::lanetype>> xvevelanetype;
  1668. std::vector<std::vector<iv::lanetype>> xvectoroplanetype;
  1669. for(i=0;i<nlanesize;i++)
  1670. {
  1671. std::vector<iv::lanetype> xvelanetype;
  1672. int j;
  1673. int nlasttype;
  1674. iv::lanetype xlt;
  1675. xlt.s = 0;
  1676. xlt.ntype = 0;
  1677. int nsizelp = mvectorlp[mvectorroadlane[i]].mvectorlpright.size();
  1678. if(nsizelp > 0)
  1679. {
  1680. xlt.ntype = mvectorlp[mvectorroadlane[i]].mvectorlpright[0].nLaneType;
  1681. nlasttype = xlt.ntype;
  1682. }
  1683. for(j=1;j<nsizelp;j++)
  1684. {
  1685. int nlt = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].nLaneType;
  1686. if((nlt != nlasttype)&&(mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS != -1))
  1687. {
  1688. xvelanetype.push_back(xlt);
  1689. xlt.ntype = nlt;
  1690. xlt.s = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS;
  1691. }
  1692. nlasttype = nlt;
  1693. }
  1694. xvelanetype.push_back(xlt);
  1695. xvevelanetype.push_back(xvelanetype);
  1696. }
  1697. for(i=0;i<noplanesize;i++)
  1698. {
  1699. std::vector<iv::lanetype> xvelanetype;
  1700. int j;
  1701. int nlasttype;
  1702. iv::lanetype xlt;
  1703. xlt.s = 0;
  1704. xlt.ntype = 0;
  1705. int nsizelp = mvectorlp[mvectorroadopposite[i]].mvectorlpright.size();
  1706. if(nsizelp > 0)
  1707. {
  1708. xlt.ntype = mvectorlp[mvectorroadopposite[i]].mvectorlpright[nsizelp-1].nLaneType;
  1709. nlasttype = xlt.ntype;
  1710. }
  1711. for(j=(nsizelp-2);j>=0;j--)
  1712. {
  1713. int nlt = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].nLaneType;
  1714. if((nlt != nlasttype)&&(mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS != -1))
  1715. {
  1716. xvelanetype.push_back(xlt);
  1717. xlt.ntype = nlt;
  1718. xlt.s = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS;
  1719. }
  1720. nlasttype = nlt;
  1721. }
  1722. xvelanetype.push_back(xlt);
  1723. xvectoroplanetype.push_back(xvelanetype);
  1724. }
  1725. //Get Lane Mark Width
  1726. std::vector<double> xvectormarkwidth;
  1727. std::vector<double> xvectoravgdis;
  1728. for(i=0;i<xvectordiss.size();i++)
  1729. {
  1730. double fdis = 0;
  1731. int j;
  1732. int ncount = 0;
  1733. for(j=0;j<xvectordiss[i].size();j++)
  1734. {
  1735. if(xvectordiss[i].at(j).mfs != -1)
  1736. {
  1737. fdis = fdis + xvectordiss[i].at(j).mfdis;
  1738. ncount++;
  1739. }
  1740. }
  1741. if(ncount>0)fdis = fdis/ncount;
  1742. xvectoravgdis.push_back(fdis);
  1743. }
  1744. for(i=1;i<mvectorroadlane.size();i++)
  1745. {
  1746. xvectormarkwidth.push_back(xvectoravgdis[mvectorroadlane.size()+i-1] - xvectoravgdis[i-1]);
  1747. }
  1748. std::vector<double> xvectoropmarkwidth;
  1749. std::vector<double> xvectoropavgdis;
  1750. for(i=0;i<xvectoropdiss.size();i++)
  1751. {
  1752. double fdis = 0;
  1753. int j;
  1754. int ncount = 0;
  1755. int nsize = xvectoropdiss[i].size();
  1756. for(j=0;j<nsize;j++)
  1757. {
  1758. if(xvectoropdiss[i].at(j).mfs != -1)
  1759. {
  1760. fdis = fdis + xvectoropdiss[i].at(j).mfdis;
  1761. ncount++;
  1762. }
  1763. }
  1764. if(ncount>0)fdis = fdis/ncount;
  1765. xvectoropavgdis.push_back(fdis);
  1766. }
  1767. if(xvectoropavgdis.size()>0)xvectoropmarkwidth.push_back(xvectoropavgdis[0]);
  1768. for(i=1;i<mvectorroadopposite.size();i++)
  1769. {
  1770. xvectoropmarkwidth.push_back(xvectoropavgdis[i] - xvectoropavgdis[i+mvectorroadopposite.size()-1]);
  1771. }
  1772. //Get Lane Width
  1773. std::vector<std::vector<iv::distogeo>> xvectorlanewidth;
  1774. std::vector<std::vector<iv::distogeo>> xvectoroplanewidth;
  1775. std::vector<iv::distogeo> xlanewidth;
  1776. xlanewidth.clear();
  1777. int j;
  1778. for(j=0;j<xvectordiss[0].size();j++)
  1779. {
  1780. if(xvectordiss[0].at(j).mfs != -1)xlanewidth.push_back(xvectordiss[0].at(j));
  1781. }
  1782. xvectorlanewidth.push_back(xlanewidth);
  1783. for(i=1;i<mvectorroadlane.size();i++)
  1784. {
  1785. xlanewidth.clear();
  1786. for(j=0;j<xvectordiss[i].size();j++)
  1787. {
  1788. if((xvectordiss[i].at(j).mfs!=-1)&&(xvectordiss[i-1+nlanesize].at(j).mfs!=-1))
  1789. {
  1790. iv::distogeo xdisg;
  1791. xdisg = xvectordiss[i].at(j);
  1792. xdisg.mfdis = xdisg.mfdis - xvectordiss[i-1+nlanesize].at(j).mfdis + xvectormarkwidth[i-1];
  1793. xlanewidth.push_back(xdisg);
  1794. }
  1795. }
  1796. xvectorlanewidth.push_back(xlanewidth);
  1797. }
  1798. xlanewidth.clear();
  1799. if(xvectoropmarkwidth.size()>0)
  1800. {
  1801. if(xvectoropmarkwidth[0] > 0.4)
  1802. {
  1803. int nsize = xvectoropdiss[0].size();
  1804. for(j=0;j<nsize;j++)
  1805. {
  1806. if(xvectoropdiss[0].at(j).mfs != -1)xlanewidth.push_back(xvectoropdiss[0].at(j));
  1807. }
  1808. xvectoroplanewidth.push_back(xlanewidth);
  1809. xlanewidth.clear();
  1810. nsize = xvectoropdiss[noplanesize].size();
  1811. for(j=0;j<nsize;j++)
  1812. {
  1813. if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
  1814. {
  1815. iv::distogeo xdisg;
  1816. xdisg = xvectoropdiss[noplanesize].at(j);
  1817. xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis ;
  1818. xlanewidth.push_back(xdisg);
  1819. }
  1820. }
  1821. xvectoroplanewidth.push_back(xlanewidth);
  1822. }
  1823. else
  1824. {
  1825. nsize = xvectoropdiss[noplanesize].size();
  1826. for(j=0;j<nsize;j++)
  1827. {
  1828. if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
  1829. {
  1830. iv::distogeo xdisg;
  1831. xdisg = xvectoropdiss[noplanesize].at(j);
  1832. xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis + xvectoropmarkwidth[0];
  1833. xlanewidth.push_back(xdisg);
  1834. }
  1835. }
  1836. xvectoroplanewidth.push_back(xlanewidth);
  1837. }
  1838. for(i=1;i<noplanesize;i++)
  1839. {
  1840. xlanewidth.clear();
  1841. nsize = xvectoropdiss[noplanesize+i].size();
  1842. for(j=0;j<nsize;j++)
  1843. {
  1844. if((xvectoropdiss[noplanesize+i].at(j).mfs != -1)&&(xvectoropdiss[i].at(j).mfs != -1))
  1845. {
  1846. iv::distogeo xdisg;
  1847. xdisg = xvectoropdiss[noplanesize+i].at(j);
  1848. xdisg.mfdis = xdisg.mfdis - xvectoropdiss[i].at(j).mfdis + xvectoropmarkwidth[i];
  1849. xlanewidth.push_back(xdisg);
  1850. }
  1851. }
  1852. xvectoroplanewidth.push_back(xlanewidth);
  1853. }
  1854. }
  1855. std::vector<iv::lanecoff> xvectorlanecoff;
  1856. std::vector<iv::lanecoff> xvectoroplanecoff;
  1857. for(i=0;i<nlanesize;i++)
  1858. {
  1859. int N = xvectorlanewidth[i].size();
  1860. VectorXd x_veh(N);
  1861. VectorXd y_veh(N);
  1862. for(j=0;j<N;j++)
  1863. {
  1864. x_veh[j] = xvectorlanewidth[i].at(j).mfs;
  1865. y_veh[j] =xvectorlanewidth[i].at(j).mfdis;
  1866. }
  1867. VectorXd coeffs = polyfit(x_veh, y_veh, 3);
  1868. iv::lanecoff xlanecoff;
  1869. xlanecoff.A = coeffs[0];
  1870. xlanecoff.B = coeffs[1];
  1871. xlanecoff.C = coeffs[2];
  1872. xlanecoff.D = coeffs[3];
  1873. xvectorlanecoff.push_back(xlanecoff);
  1874. }
  1875. for(i=0;i<xvectoroplanewidth.size();i++)
  1876. {
  1877. int N = xvectoroplanewidth[i].size();
  1878. VectorXd x_veh(N);
  1879. VectorXd y_veh(N);
  1880. for(j=0;j<N;j++)
  1881. {
  1882. x_veh[j] = xvectoroplanewidth[i].at(j).mfs;
  1883. y_veh[j] =xvectoroplanewidth[i].at(j).mfdis;
  1884. }
  1885. VectorXd coeffs = polyfit(x_veh, y_veh, 3);
  1886. iv::lanecoff xlanecoff;
  1887. xlanecoff.A = coeffs[0];
  1888. xlanecoff.B = coeffs[1];
  1889. xlanecoff.C = coeffs[2];
  1890. xlanecoff.D = coeffs[3];
  1891. xvectoroplanecoff.push_back(xlanecoff);
  1892. }
  1893. iv::lanecoff xlaneheightcoff;
  1894. int M = mvectorlp[0].mvectorlpleft.size();
  1895. VectorXd x_vehhg(M);
  1896. VectorXd y_vehhg(M);
  1897. for(j=0;j<M;j++)
  1898. {
  1899. x_vehhg[j] = mvectorlp[0].mvectorlpleft.at(j).mfDis;
  1900. y_vehhg[j] =mvectorlp[0].mvectorlpleft.at(j).mfHeight;
  1901. }
  1902. VectorXd coeffs = polyfit(x_vehhg, y_vehhg, 3);
  1903. xlaneheightcoff.A = coeffs[0];
  1904. xlaneheightcoff.B = coeffs[1];
  1905. xlaneheightcoff.C = coeffs[2];
  1906. xlaneheightcoff.D = coeffs[3];
  1907. geofit xgeofit;
  1908. int N = pvectorlp->size();
  1909. VectorXd x_veh(N);
  1910. VectorXd y_veh(N);
  1911. VectorXi t_veh(N);
  1912. double x0,y0;
  1913. GaussProjCal(glon0,glat0,&x0,&y0);
  1914. for(j=0;j<N;j++)
  1915. {
  1916. double x,y;
  1917. GaussProjCal(pvectorlp->at(j).mfLon,pvectorlp->at(j).mfLat,&x,&y);
  1918. x_veh[j] = x - x0;
  1919. y_veh[j] = y - y0;
  1920. t_veh[j] = pntype[j];
  1921. }
  1922. std::vector<geobase> xvectorgeo = xgeofit.getgeo(x_veh,y_veh,t_veh);
  1923. // OpenDrive od;
  1924. // std::string mapx = "map";
  1925. // od.SetHeader(1,1,mapx,1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
  1926. j= 0;
  1927. double xroadlen = 0;
  1928. for(j=0;j<xvectorgeo.size();j++)
  1929. {
  1930. xroadlen = xroadlen + xvectorgeo[j].mfLen;
  1931. }
  1932. mxodr.AddRoad(mpLE_RoadName->text().toLatin1().data(),xroadlen, QString::number(CreateRoadID()).toStdString(),"-1");
  1933. Road * p = mxodr.GetRoad(mxodr.GetRoadCount() - 1);
  1934. p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
  1935. double s = 0;
  1936. j= 0;
  1937. // for(j=0;j<4;j++)
  1938. for(j=0;j<xvectorgeo.size();j++)
  1939. {
  1940. p->AddGeometryBlock();
  1941. GeometryBlock * pgb = p->GetGeometryBlock(j);
  1942. geobase * pline;
  1943. geobase * pbez;
  1944. geobase * parc;
  1945. switch(xvectorgeo[j].mnType)
  1946. {
  1947. case 0:
  1948. pline = &xvectorgeo[j];
  1949. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  1950. break;
  1951. case 1:
  1952. parc = &xvectorgeo[j];
  1953. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  1954. break;
  1955. case 2:
  1956. pbez = &xvectorgeo[j];
  1957. std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  1958. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  1959. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  1960. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  1961. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
  1962. break;
  1963. }
  1964. s = s + xvectorgeo[j].mfLen;
  1965. }
  1966. p->AddLaneSection(0);
  1967. LaneSection * pLS = p->GetLaneSection(0);
  1968. Lane * pLL;
  1969. pLS->SetS(0);
  1970. pLS->AddLane(0,0,"none",false);
  1971. pLL = pLS->GetLane(0);
  1972. if(noplanesize != xvectoroplanewidth.size())
  1973. {
  1974. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  1975. }
  1976. for(i=0;i<nlanesize;i++)
  1977. {
  1978. pLS->AddLane(-1,(i+1)*(-1),"driving",false,false);
  1979. pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
  1980. pLL->AddWidthRecord(0,xvectorlanecoff[i].A,xvectorlanecoff[i].B,
  1981. xvectorlanecoff[i].C,xvectorlanecoff[i].D);
  1982. int j;
  1983. for(j=0;j<xvevelanetype[i].size();j++)
  1984. {
  1985. std::string strlanetype = "broken";
  1986. if(xvevelanetype[i].at(j).ntype != 0)
  1987. {
  1988. strlanetype = "solid";
  1989. }
  1990. pLL->AddRoadMarkRecord(xvevelanetype[i].at(j).s,strlanetype,"standard","standard",0.15,"false");
  1991. }
  1992. }
  1993. for(i=0;i<xvectoroplanewidth.size();i++)
  1994. {
  1995. pLS->AddLane(1,(i+1)*(1),"driving",false,false);
  1996. pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
  1997. if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
  1998. {
  1999. pLL->SetType("shoulder");
  2000. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2001. }
  2002. pLL->AddWidthRecord(0,xvectoroplanecoff[i].A,xvectoroplanecoff[i].B,
  2003. xvectoroplanecoff[i].C,xvectoroplanecoff[i].D);
  2004. int j;
  2005. int index = i;
  2006. if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
  2007. {
  2008. continue;
  2009. }
  2010. if(noplanesize != xvectoroplanewidth.size())
  2011. {
  2012. index = i-1;
  2013. }
  2014. else
  2015. index = i;
  2016. for(j=0;j<xvectoroplanetype[index].size();j++)
  2017. {
  2018. std::string strlanetype = "broken";
  2019. if(xvectoroplanetype[index].at(j).ntype != 0)
  2020. {
  2021. strlanetype = "solid";
  2022. }
  2023. pLL->AddRoadMarkRecord(xvectoroplanetype[index].at(j).s,strlanetype,"standard","standard",0.15,"false");
  2024. }
  2025. }
  2026. mpCBSelLane->clear();
  2027. mpCBSelOpLane->clear();
  2028. mvectorroadlane.clear();
  2029. mvectorroadopposite.clear();
  2030. updateCBRoad();
  2031. mbRefresh = true;
  2032. update();
  2033. // OpenDriveXmlWriter x(&mxodr);
  2034. // x.WriteFile("/home/nvidia/text.xodr");
  2035. }
  2036. int MainWindow::CreateRoadID(int ntype)
  2037. {
  2038. int i;
  2039. bool bUsed = false;
  2040. int nroadidstart = 10000;
  2041. if(ntype == 1) //not create by lane roaid
  2042. {
  2043. nroadidstart = 20000;
  2044. }
  2045. int nroadcount = mxodr.GetRoadCount();
  2046. if(nroadcount == 0)return nroadidstart;
  2047. int * proadid = new int[nroadcount];
  2048. for(i=0;i<nroadcount;i++)
  2049. {
  2050. proadid[i]=atoi(mxodr.GetRoad(i)->GetRoadId().data());
  2051. }
  2052. do
  2053. {
  2054. bUsed = false;
  2055. for(i=0;i<nroadcount;i++)
  2056. {
  2057. if(proadid[i] == nroadidstart)
  2058. {
  2059. nroadidstart++;
  2060. bUsed = true;
  2061. }
  2062. }
  2063. }while(bUsed);
  2064. delete proadid;
  2065. return nroadidstart;
  2066. }
  2067. int MainWindow::CreateJunctionID()
  2068. {
  2069. int i;
  2070. bool bUsed = false;
  2071. int njunctionidstart = 800000;
  2072. int njunctioncount = mxodr.GetJunctionCount();
  2073. if(njunctioncount == 0)return njunctionidstart;
  2074. int * pjunctionid = new int[njunctioncount];
  2075. for(i=0;i<njunctioncount;i++)
  2076. {
  2077. pjunctionid[i]=atoi(mxodr.GetJunction(i)->GetId().data());
  2078. }
  2079. do
  2080. {
  2081. bUsed = false;
  2082. for(i=0;i<njunctioncount;i++)
  2083. {
  2084. if(pjunctionid[i] == njunctionidstart)
  2085. {
  2086. njunctionidstart++;
  2087. bUsed = true;
  2088. }
  2089. }
  2090. }while(bUsed);
  2091. delete pjunctionid;
  2092. return njunctionidstart;
  2093. }
  2094. void MainWindow::onClickLoad()
  2095. {
  2096. QString str = QFileDialog::getOpenFileName(this,"Load XODR",".","*.xodr");
  2097. if(str.isEmpty())return;
  2098. OpenDrive * pxodr = new OpenDrive(); //because add to xodr,so don't delete
  2099. OpenDriveXmlParser x(pxodr);
  2100. if(!x.ReadFile(str.toStdString()))
  2101. {
  2102. QMessageBox::warning(this,"warn","Can't load xodr file.");
  2103. return;
  2104. }
  2105. bool bNeedMove = false;
  2106. bool bNeedAjustID = false;
  2107. unsigned short int revMajor,revMinor;
  2108. std::string name,date;
  2109. float version;
  2110. double north,south,east,west,lat0,lon0,hdg0;
  2111. if(pxodr->GetHeader() != 0)
  2112. {
  2113. pxodr->GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
  2114. }
  2115. double xMoveX = 0;
  2116. double xMoveY = 0;
  2117. if(mxodr.GetRoadCount() > 0)
  2118. {
  2119. bNeedAjustID = true;
  2120. }
  2121. if(mbSetOrigin == false)
  2122. {
  2123. bNeedMove = false;
  2124. glat0 = lat0;
  2125. glon0 = lon0;
  2126. mpLELon0->setText(QString::number(glon0,'f',7));
  2127. mpLELat0->setText(QString::number(glat0,'f',7));
  2128. mbSetOrigin = true;
  2129. }
  2130. else
  2131. {
  2132. if((glat0 == lat0)&&(glon0 == lon0))
  2133. {
  2134. bNeedMove = false;
  2135. }
  2136. else
  2137. {
  2138. bNeedMove = true;
  2139. double x0,y0,x1,y1;
  2140. GaussProjCal(glon0,glat0,&x0,&y0);
  2141. GaussProjCal(lon0,lat0,&x1,&y1);
  2142. xMoveX = x1 - x0;
  2143. xMoveY = y1 - y0;
  2144. }
  2145. }
  2146. if(bNeedMove)
  2147. {
  2148. MoveXODR(pxodr,xMoveX,xMoveY);
  2149. }
  2150. int noldroadcount = mxodr.GetRoadCount();
  2151. int noldjunctioncount = mxodr.GetJunctionCount();
  2152. int nnewcount = pxodr->GetRoadCount();
  2153. int nnewjunctioncount = pxodr->GetJunctionCount();
  2154. int i;
  2155. if(bNeedAjustID)
  2156. {
  2157. for(i=0;i<nnewcount;i++)
  2158. {
  2159. int nid = atoi(pxodr->GetRoad(i)->GetRoadId().data());
  2160. int j;
  2161. bool bNeedChange = false;
  2162. for(j=0;j<noldroadcount;j++)
  2163. {
  2164. if(nid == atoi(mxodr.GetRoad(j)->GetRoadId().data()))
  2165. {
  2166. bNeedChange = true;
  2167. break;
  2168. }
  2169. }
  2170. if(bNeedChange)
  2171. {
  2172. int nnewid = FindNewRoadID(&mxodr,pxodr);
  2173. ChangeXODRRoadID(pxodr,i,nnewid);
  2174. }
  2175. }
  2176. for(i=0;i<nnewjunctioncount;i++)
  2177. {
  2178. int nid = atoi(pxodr->GetJunction(i)->GetId().data());
  2179. int j;
  2180. bool bNeedChange = false;
  2181. for(j=0;j<noldjunctioncount;j++)
  2182. {
  2183. if(nid == atoi(mxodr.GetJunction(j)->GetId().data()))
  2184. {
  2185. bNeedChange = true;
  2186. break;
  2187. }
  2188. }
  2189. if(bNeedChange)
  2190. {
  2191. int nnewid = FindNewJunctionID(&mxodr,pxodr);
  2192. ChangeXODRJunctionID(pxodr,i,nnewid);
  2193. }
  2194. }
  2195. }
  2196. for(i=0;i<nnewcount;i++)
  2197. {
  2198. mxodr.GetRoadVector()->push_back(pxodr->GetRoadVector()->at(i));
  2199. // OpenDrive * px = &mxodr;
  2200. }
  2201. for(i=0;i<nnewjunctioncount;i++)
  2202. {
  2203. mxodr.GetJunctionVector()->push_back(pxodr->GetJunctionVector()->at(i));
  2204. }
  2205. updateCBRoad();
  2206. updateJunction();
  2207. mbRefresh = true;
  2208. update();
  2209. }
  2210. void MainWindow::onClickSave()
  2211. {
  2212. QString str = QFileDialog::getSaveFileName(this,"Save XODR",".","*.xodr");
  2213. if(str.isEmpty())return;
  2214. if(str.indexOf(".xodr")<0)str = str + ".xodr";
  2215. mxodr.SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
  2216. OpenDriveXmlWriter x(&mxodr);
  2217. x.WriteFile(str.toStdString());
  2218. }
  2219. void MainWindow::onClickRoadContact()
  2220. {
  2221. int ch1,ch2;
  2222. ch1 = mpCBRoad1->currentIndex();
  2223. ch2 = mpCBRoad2->currentIndex();
  2224. if((ch1 == -1)||(ch2 == -1))
  2225. {
  2226. return;
  2227. }
  2228. // if(ch1 == ch2)
  2229. // {
  2230. // QMessageBox::warning(this,"warn","road same");
  2231. // return;
  2232. // }
  2233. int ntype1,ntype2;
  2234. ntype1 = mpCBRC1->currentIndex();
  2235. ntype2 = mpCBRC2->currentIndex();
  2236. Road * p1 = mxodr.GetRoad(ch1);
  2237. Road * p2 = mxodr.GetRoad(ch2);
  2238. iv::roadcontact rc;
  2239. rc.mnroad1id = atoi(p1->GetRoadId().data());
  2240. rc.mnroad2id = atoi(p2->GetRoadId().data());
  2241. rc.mncon1 = ntype1;
  2242. rc.mncon2 = ntype2;
  2243. char strname[256];
  2244. snprintf(strname,256,"%s_%s_%s_%s",p1->GetRoadId().data(),mpCBRC1->currentText().toLatin1().data(),
  2245. p2->GetRoadId().data(),mpCBRC2->currentText().toLatin1().data());
  2246. mpCBRoadCon->clear();
  2247. mpCBRoadCon->addItem(strname);
  2248. mpCBLane1->clear();
  2249. mpCBLane2->clear();
  2250. mpCBLane1Lane2->clear();
  2251. mpCBLane1Lane2op->clear();
  2252. if(p1->GetLaneSectionCount()>0)
  2253. {
  2254. LaneSection * pLS;
  2255. if(ntype1 == 0)
  2256. pLS = p1->GetLaneSection(0);
  2257. else
  2258. pLS = p1->GetLaneSection(p1->GetLaneSectionCount()-1);
  2259. int i;
  2260. for(i=0;i<pLS->GetLaneCount();i++)
  2261. {
  2262. Lane * pL = pLS->GetLane(i);
  2263. // if((pL->GetId() != 0)&&(strncmp(pL->GetType().data(),"driving",255)==0))
  2264. if(pL->GetId() != 0)
  2265. mpCBLane1->addItem(QString::number(pL->GetId()));
  2266. }
  2267. }
  2268. if(p2->GetLaneSectionCount()>0)
  2269. {
  2270. LaneSection * pLS;
  2271. if(ntype1 == 0)
  2272. pLS = p2->GetLaneSection(0);
  2273. else
  2274. pLS = p2->GetLaneSection(p1->GetLaneSectionCount()-1);
  2275. int i;
  2276. for(i=0;i<pLS->GetLaneCount();i++)
  2277. {
  2278. Lane * pL = pLS->GetLane(i);
  2279. // if((pL->GetId() != 0)&&(strncmp(pL->GetType().data(),"driving",255)==0))
  2280. if(pL->GetId() != 0)
  2281. mpCBLane2->addItem(QString::number(pL->GetId()));
  2282. }
  2283. }
  2284. mvectorrc.clear();
  2285. mvectorrc.push_back(rc);
  2286. }
  2287. void MainWindow::onClickLaneContact()
  2288. {
  2289. int n1,n2;
  2290. n1 = mpCBLane1->currentText().toInt();
  2291. n2 = mpCBLane2->currentText().toInt();
  2292. iv::lanecontact xlc;
  2293. xlc.ml1 = n1;
  2294. xlc.ml2 = n2;
  2295. if(mvectorrc.size() < 1)return;
  2296. mvectorrc[0].mvectorlc.push_back(xlc);
  2297. char strname[256];
  2298. snprintf(strname,255,"%dto%d",n1,n2);
  2299. mpCBLane1Lane2->addItem(strname);
  2300. }
  2301. void MainWindow::onClickOpLaneContact()
  2302. {
  2303. int n1,n2;
  2304. n1 = mpCBLane1->currentText().toInt();
  2305. n2 = mpCBLane2->currentText().toInt();
  2306. iv::lanecontact xlc;
  2307. xlc.ml1 = n1;
  2308. xlc.ml2 = n2;
  2309. if(mvectorrc.size() < 1)return;
  2310. mvectorrc[0].mvectorlcop.push_back(xlc);
  2311. char strname[256];
  2312. snprintf(strname,255,"%dto%d",n1,n2);
  2313. mpCBLane1Lane2op->addItem(strname);
  2314. }
  2315. void MainWindow::onClickCreateRoad()
  2316. {
  2317. if(mvectorrc.size()<1)return;
  2318. Road * p1, *p2;
  2319. int nroad1index;
  2320. int nroad2index;
  2321. // p1 = mvectorrc[0].mp1;
  2322. // p2 = mvectorrc[0].mp2;
  2323. int i;
  2324. bool bhavep1 = false;
  2325. bool bhavep2 = false;
  2326. for(i=0;i<mxodr.GetRoadCount();i++)
  2327. {
  2328. if(mvectorrc[0].mnroad1id == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
  2329. {
  2330. bhavep1 = true;
  2331. p1 = mxodr.GetRoad(i);
  2332. nroad1index = i;
  2333. break;
  2334. }
  2335. }
  2336. if(bhavep1 == false)
  2337. {
  2338. QMessageBox::warning(this,"Warn","Road not found.");
  2339. return;
  2340. }
  2341. double off1,off2;
  2342. for(i=0;i<mxodr.GetRoadCount();i++)
  2343. {
  2344. if(mvectorrc[0].mnroad2id == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
  2345. {
  2346. bhavep2 = true;
  2347. p2 = mxodr.GetRoad(i);
  2348. nroad2index = i;
  2349. break;
  2350. }
  2351. }
  2352. if(bhavep2 == false)
  2353. {
  2354. QMessageBox::warning(this,"Warn","Road not found.");
  2355. return;
  2356. }
  2357. if(mvectorrc[0].mvectorlc.size()<1)
  2358. {
  2359. QMessageBox::warning(this,"warn","No Lane Contact.");
  2360. return;
  2361. }
  2362. double startx,starty,starthdg;
  2363. double endx,endy,endhdg;
  2364. double startheight,endheight;
  2365. bool bFromstart,bTostart;
  2366. if(mvectorrc[0].mncon1 == 0)
  2367. {
  2368. bFromstart = true;
  2369. starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
  2370. off1 = getoff(p1,mvectorrc[0].mvectorlc[0].ml1,true);
  2371. startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
  2372. starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
  2373. startx = startx + off1 * cos(starthdg -M_PI/2.0);
  2374. starty = starty + off1 * sin(starthdg -M_PI/2.0);
  2375. startheight = 0;
  2376. if(p1->GetElevationCount()>0)
  2377. {
  2378. startheight = p1->GetElevation(0)->GetA();
  2379. }
  2380. // if(mvectorrc[0].mvectorlc[0].ml1<0)
  2381. starthdg = starthdg +M_PI;if(starthdg >=2.0*M_PI)starthdg = starthdg -2.0*M_PI;
  2382. }
  2383. else
  2384. {
  2385. bFromstart = false;
  2386. if(GetEndPoint(p1,startx,starty,starthdg) != 0)
  2387. {
  2388. QMessageBox::warning(this,"warn","get start error.");
  2389. return;
  2390. }
  2391. off1 = getoff(p1,mvectorrc[0].mvectorlc[0].ml1,false);
  2392. startx = startx + off1 * cos(starthdg -M_PI/2.0);
  2393. starty = starty + off1 * sin(starthdg -M_PI/2.0);
  2394. startheight = 0;
  2395. if(p1->GetElevationCount()>0)
  2396. {
  2397. startheight = p1->GetElevation(0)->GetA()
  2398. +p1->GetElevation(0)->GetB() * pow(p1->GetRoadLength(),1)
  2399. +p1->GetElevation(0)->GetC() * pow(p1->GetRoadLength(),2)
  2400. +p1->GetElevation(0)->GetD() * pow(p1->GetRoadLength(),3);
  2401. }
  2402. }
  2403. if(mvectorrc[0].mncon2 == 0)
  2404. {
  2405. bTostart = true;
  2406. off2 = getoff(p2,mvectorrc[0].mvectorlc[0].ml2,true);
  2407. endx = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
  2408. endy = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
  2409. endhdg = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
  2410. endx = endx + off2 * cos(endhdg -M_PI/2.0);
  2411. endy = endy + off2 * sin(endhdg -M_PI/2.0);
  2412. endheight = 0;
  2413. if(p2->GetElevationCount()>0)
  2414. {
  2415. endheight = p2->GetElevation(0)->GetA();
  2416. }
  2417. }
  2418. else
  2419. {
  2420. bTostart = false;
  2421. off2 = getoff(p2,mvectorrc[0].mvectorlc[0].ml2,false);
  2422. if(GetEndPoint(p2,endx,endy,endhdg) != 0)
  2423. {
  2424. QMessageBox::warning(this,"warn","get end error.");
  2425. return;
  2426. }
  2427. endx = endx + off2 * cos(endhdg -M_PI/2.0);
  2428. endy = endy + off2 * sin(endhdg -M_PI/2.0);
  2429. endhdg = endhdg +M_PI;if(endhdg >=2.0*M_PI)endhdg = endhdg -2.0*M_PI;
  2430. endheight = 0;
  2431. if(p2->GetElevationCount()>0)
  2432. {
  2433. endheight = p2->GetElevation(0)->GetA()
  2434. +p2->GetElevation(0)->GetB() * pow(p2->GetRoadLength(),1)
  2435. +p2->GetElevation(0)->GetC() * pow(p2->GetRoadLength(),2)
  2436. +p2->GetElevation(0)->GetD() * pow(p2->GetRoadLength(),3);
  2437. }
  2438. }
  2439. //Create Geo
  2440. double R = mpLERoadType->text().toDouble();
  2441. std::vector<geobase> xvectorgeo;
  2442. std::vector<geobase> xvectorgeo1,xvectorgeo2;
  2443. switch(mpCBRoadType->currentIndex())
  2444. {
  2445. case 0:
  2446. xvectorgeo = CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
  2447. break;
  2448. case 1:
  2449. xvectorgeo = CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
  2450. break;
  2451. case 2:
  2452. xvectorgeo = CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
  2453. break;
  2454. default:
  2455. break;
  2456. }
  2457. if(mpCBRoadType->currentIndex() == 2)
  2458. {
  2459. for(i=0;i<xvectorgeo.size()/2;i++)
  2460. {
  2461. xvectorgeo1.push_back(xvectorgeo.at(i));
  2462. }
  2463. for(i=xvectorgeo.size()/2;i<xvectorgeo.size();i++)
  2464. {
  2465. xvectorgeo2.push_back(xvectorgeo.at(i));
  2466. }
  2467. }
  2468. if(xvectorgeo.size() == 0)
  2469. {
  2470. QMessageBox::warning(this,"warn","Create Road Fail.");
  2471. return;
  2472. }
  2473. double xroadlen = 0;
  2474. if(mpCBRoadType->currentIndex() != 2)
  2475. {
  2476. for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
  2477. mxodr.AddRoad("",xroadlen, QString::number(CreateRoadID()).toStdString(),"-1");
  2478. Road * p = mxodr.GetRoad(mxodr.GetRoadCount() - 1);
  2479. p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);
  2480. p1 = mxodr.GetRoad(nroad1index);
  2481. p2 = mxodr.GetRoad(nroad2index);
  2482. double s = 0;
  2483. int j;
  2484. j= 0;
  2485. for(j=0;j<xvectorgeo.size();j++)
  2486. {
  2487. p->AddGeometryBlock();
  2488. GeometryBlock * pgb = p->GetGeometryBlock(j);
  2489. geobase * pline;
  2490. geobase * pbez;
  2491. geobase * parc;
  2492. switch(xvectorgeo[j].mnType)
  2493. {
  2494. case 0:
  2495. pline = &xvectorgeo[j];
  2496. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  2497. break;
  2498. case 1:
  2499. parc = &xvectorgeo[j];
  2500. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  2501. break;
  2502. case 2:
  2503. pbez = &xvectorgeo[j];
  2504. std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  2505. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  2506. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  2507. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  2508. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
  2509. break;
  2510. }
  2511. s = s + xvectorgeo[j].mfLen;
  2512. }
  2513. p->AddLaneSection(0);
  2514. LaneSection * pLS = p->GetLaneSection(0);
  2515. pLS->SetS(0);
  2516. pLS->AddLane(0,0,"none",false);
  2517. double * pswidth,*pewidth;
  2518. std::vector<std::string> strvectorlanetype;
  2519. int nlanecount = mvectorrc[0].mvectorlc.size();
  2520. pswidth = new double[nlanecount];
  2521. pewidth = new double[nlanecount];
  2522. std::shared_ptr<double> ppswidth,ppewidth;
  2523. ppswidth.reset(pswidth);
  2524. ppewidth.reset(pewidth);
  2525. for(i=0;i<nlanecount;i++)
  2526. {
  2527. pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
  2528. strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
  2529. }
  2530. for(i=0;i<nlanecount;i++)
  2531. {
  2532. pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlc.at(i).ml2,bTostart);
  2533. }
  2534. double * pa,*pb;
  2535. pa = new double[nlanecount];
  2536. pb = new double[nlanecount];
  2537. std::shared_ptr<double> ppa,ppb;
  2538. ppa.reset(pa);
  2539. ppb.reset(pb);
  2540. for(i=0;i<nlanecount;i++)
  2541. {
  2542. pa[i] = pswidth[i];
  2543. pb[i] = (pewidth[i] - pa[i])/xroadlen;
  2544. }
  2545. for(i=0;i<nlanecount;i++)
  2546. {
  2547. pLS->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
  2548. Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
  2549. pLL->AddWidthRecord(0,pa[i],pb[i],
  2550. 0,0);
  2551. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2552. }
  2553. int noplanecount = mvectorrc[0].mvectorlcop.size();
  2554. if(noplanecount > 0)
  2555. {
  2556. pswidth = new double[noplanecount];
  2557. pewidth = new double[noplanecount];
  2558. ppswidth.reset(pswidth);
  2559. ppewidth.reset(pewidth);
  2560. strvectorlanetype.clear();
  2561. for(i=0;i<noplanecount;i++)
  2562. {
  2563. pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
  2564. strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
  2565. }
  2566. for(i=0;i<noplanecount;i++)
  2567. {
  2568. pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
  2569. }
  2570. pa = new double[noplanecount];
  2571. pb = new double[noplanecount];
  2572. ppa.reset(pa);
  2573. ppb.reset(pb);
  2574. for(i=0;i<noplanecount;i++)
  2575. {
  2576. pa[i] = pswidth[i];
  2577. pb[i] = (pewidth[i] - pa[i])/xroadlen;
  2578. }
  2579. for(i=0;i<noplanecount;i++)
  2580. {
  2581. pLS->AddLane(1,(i+1),strvectorlanetype[i],false,false);
  2582. Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
  2583. pLL->AddWidthRecord(0,pa[i],pb[i],
  2584. 0,0);
  2585. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2586. }
  2587. }
  2588. }
  2589. else
  2590. {
  2591. double xroadlen1 = 0;
  2592. double xroadlen2 = 0;
  2593. for(i=0;i<xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
  2594. for(i=0;i<xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
  2595. int index1 = mxodr.AddRoad("",xroadlen1, QString::number(CreateRoadID()).toStdString(),"-1");
  2596. int index2 = mxodr.AddRoad("",xroadlen2, QString::number(CreateRoadID()).toStdString(),"-1");
  2597. Road * proad2 = mxodr.GetRoad(index2);
  2598. Road * proad1 = mxodr.GetRoad(index1);
  2599. proad1->AddElevation(0,startheight,(endheight-startheight)/(xroadlen1+xroadlen2),0,0);
  2600. proad2->AddElevation(0,startheight+xroadlen1*(endheight-startheight)/(xroadlen1+xroadlen2),
  2601. (endheight-startheight)/(xroadlen1+xroadlen2),
  2602. 0,0);
  2603. p1 = mxodr.GetRoad(nroad1index);
  2604. p2 = mxodr.GetRoad(nroad2index);
  2605. // OpenDrive * px = &mxodr;
  2606. double s = 0;
  2607. int j;
  2608. j= 0;
  2609. for(j=0;j<xvectorgeo1.size();j++)
  2610. {
  2611. proad1->AddGeometryBlock();
  2612. GeometryBlock * pgb = proad1->GetGeometryBlock(j);
  2613. geobase * pline;
  2614. geobase * pbez;
  2615. geobase * parc;
  2616. switch(xvectorgeo1[j].mnType)
  2617. {
  2618. case 0:
  2619. pline = &xvectorgeo1[j];
  2620. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  2621. break;
  2622. case 1:
  2623. parc = &xvectorgeo1[j];
  2624. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  2625. break;
  2626. case 2:
  2627. pbez = &xvectorgeo1[j];
  2628. std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  2629. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  2630. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  2631. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  2632. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
  2633. break;
  2634. }
  2635. s = s + xvectorgeo1[j].mfLen;
  2636. }
  2637. for(j=0;j<xvectorgeo2.size();j++)
  2638. {
  2639. proad2->AddGeometryBlock();
  2640. GeometryBlock * pgb = proad2->GetGeometryBlock(j);
  2641. geobase * pline;
  2642. geobase * pbez;
  2643. geobase * parc;
  2644. switch(xvectorgeo2[j].mnType)
  2645. {
  2646. case 0:
  2647. pline = &xvectorgeo2[j];
  2648. pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
  2649. break;
  2650. case 1:
  2651. parc = &xvectorgeo2[j];
  2652. pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  2653. break;
  2654. case 2:
  2655. pbez = &xvectorgeo2[j];
  2656. std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
  2657. pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
  2658. pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
  2659. pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
  2660. pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
  2661. break;
  2662. }
  2663. s = s + xvectorgeo2[j].mfLen;
  2664. }
  2665. proad1->AddLaneSection(0);
  2666. LaneSection * pLS1 = proad1->GetLaneSection(0);
  2667. pLS1->SetS(0);
  2668. pLS1->AddLane(0,0,"none",false);
  2669. proad2->AddLaneSection(0);
  2670. LaneSection * pLS2 = proad2->GetLaneSection(0);
  2671. pLS2->SetS(0);
  2672. pLS2->AddLane(0,0,"none",false);
  2673. double * pswidth,*pewidth;
  2674. int nlanecount = mvectorrc[0].mvectorlc.size();
  2675. std::vector<std::string> strvectorlanetype;
  2676. pswidth = new double[nlanecount];
  2677. pewidth = new double[nlanecount];
  2678. std::shared_ptr<double> ppswidth,ppewidth;
  2679. ppswidth.reset(pswidth);
  2680. ppewidth.reset(pewidth);
  2681. for(i=0;i<nlanecount;i++)
  2682. {
  2683. pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
  2684. strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
  2685. }
  2686. for(i=0;i<nlanecount;i++)
  2687. {
  2688. pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlc.at(i).ml2,bTostart);
  2689. }
  2690. double * pa,*pb;
  2691. pa = new double[nlanecount];
  2692. pb = new double[nlanecount];
  2693. std::shared_ptr<double> ppa,ppb;
  2694. ppa.reset(pa);
  2695. ppb.reset(pb);
  2696. for(i=0;i<nlanecount;i++)
  2697. {
  2698. pa[i] = pswidth[i];
  2699. pb[i] = (pewidth[i] - pa[i])/(xroadlen1+xroadlen2);
  2700. }
  2701. for(i=0;i<nlanecount;i++)
  2702. {
  2703. pLS1->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
  2704. Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);
  2705. pLL->AddWidthRecord(0,pa[i],pb[i],
  2706. 0,0);
  2707. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2708. pLS2->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
  2709. pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);
  2710. pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
  2711. 0,0);
  2712. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2713. }
  2714. int noplanecount = mvectorrc[0].mvectorlcop.size();
  2715. if(noplanecount > 0)
  2716. {
  2717. pswidth = new double[noplanecount];
  2718. pewidth = new double[noplanecount];
  2719. ppswidth.reset(pswidth);
  2720. ppewidth.reset(pewidth);
  2721. strvectorlanetype.clear();
  2722. for(i=0;i<noplanecount;i++)
  2723. {
  2724. pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
  2725. strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
  2726. }
  2727. for(i=0;i<noplanecount;i++)
  2728. {
  2729. pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
  2730. }
  2731. pa = new double[noplanecount];
  2732. pb = new double[noplanecount];
  2733. ppa.reset(pa);
  2734. ppb.reset(pb);
  2735. for(i=0;i<noplanecount;i++)
  2736. {
  2737. pa[i] = pswidth[i];
  2738. pb[i] = (pewidth[i] - pa[i])/xroadlen;
  2739. }
  2740. for(i=0;i<noplanecount;i++)
  2741. {
  2742. pLS1->AddLane(1,(i+1),strvectorlanetype[i],false,false);
  2743. Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);
  2744. pLL->AddWidthRecord(0,pa[i],pb[i],
  2745. 0,0);
  2746. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2747. pLS2->AddLane(1,(i+1),strvectorlanetype[i],false,false);
  2748. pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);
  2749. pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
  2750. 0,0);
  2751. pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
  2752. }
  2753. }
  2754. }
  2755. updateCBRoad();
  2756. mbRefresh = true;
  2757. update();
  2758. }
  2759. int MainWindow::GetEndPoint(Road *proad, double &x, double &y, double &hdg)
  2760. {
  2761. GeometryBlock * pblock = proad->GetLastGeometryBlock();
  2762. RoadGeometry * pgeo = pblock->GetLastGeometry();
  2763. //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
  2764. switch (pgeo->GetGeomType()) {
  2765. case 0:
  2766. {
  2767. GeometryLine * pline = (GeometryLine *)pgeo;
  2768. x = pline->GetX() + pline->GetLength() * cos(pline->GetHdg());
  2769. y = pline->GetY() + pline->GetLength() * sin(pline->GetHdg());
  2770. hdg = pline->GetHdg();
  2771. }
  2772. return 0;
  2773. break;
  2774. case 1:
  2775. {
  2776. GeometryArc * parc = (GeometryArc *)pgeo;
  2777. double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
  2778. double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
  2779. x = x_center + fabs(1.0/parc->GetCurvature()) * cos(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
  2780. y = y_center + fabs(1.0/parc->GetCurvature()) * sin(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
  2781. hdg = parc->GetHdg() + parc->GetLength() * parc->GetCurvature();
  2782. return 0;
  2783. }
  2784. break;
  2785. case 2:
  2786. {
  2787. GeometrySpiral * pspiral = (GeometrySpiral *)pgeo;
  2788. pspiral->GetCoords(pspiral->GetS()+pspiral->GetLength(),x,y,hdg);
  2789. return 0;
  2790. }
  2791. break;
  2792. case 3:
  2793. QMessageBox::warning(this,"warn","type not supported.");
  2794. break;
  2795. case 4:
  2796. {
  2797. double xtem,ytem;
  2798. double xtem1,ytem1,x1,y1;
  2799. GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
  2800. double s = ppoly3->GetLength();
  2801. xtem = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
  2802. ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
  2803. x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
  2804. y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
  2805. s = ppoly3->GetLength()*0.99;
  2806. if(s>0)
  2807. {
  2808. xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
  2809. ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
  2810. x1 = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
  2811. y1 = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
  2812. hdg = geofit::CalcHdg(xtem1,ytem1,x1,y1);
  2813. }
  2814. else
  2815. {
  2816. hdg = 0;
  2817. }
  2818. return 0;
  2819. }
  2820. break;
  2821. default:
  2822. QMessageBox::warning(this,"warn","type not supported.");
  2823. break;
  2824. }
  2825. return -1;
  2826. }
  2827. void MainWindow::onChangeRoadType(int index)
  2828. {
  2829. if(index == 1)
  2830. {
  2831. mpLBRoadType->setVisible(false);
  2832. mpLERoadType->setVisible(false);
  2833. }
  2834. else
  2835. {
  2836. mpLBRoadType->setVisible(true);
  2837. mpLERoadType->setVisible(true);
  2838. }
  2839. if(index == 0)
  2840. {
  2841. mpLBRoadType->setText("Radius:");
  2842. mpLERoadType->setText("6.0");
  2843. }
  2844. if(index == 2)
  2845. {
  2846. mpLBRoadType->setText("Extend:");
  2847. mpLERoadType->setText("3.0");
  2848. }
  2849. }
  2850. std::vector<geobase> MainWindow::CreateLineGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg)
  2851. {
  2852. // std::vector<geobase> xvectorgeo;
  2853. // geobase xgeobezier;
  2854. // int nbtype;
  2855. // double fabc[3],fxy[2],fblen,fbhdg;
  2856. // geofit x;
  2857. // x.CreateBezier(startx,starty,starthdg,
  2858. // endx,endy,endhdg,
  2859. // 0.35,0.35,xgeobezier.mfu,xgeobezier.mfv,xgeobezier.mfLen,
  2860. // nbtype,fabc,&fbhdg,fxy,&fblen);
  2861. // if(nbtype == 2)
  2862. // {
  2863. // xgeobezier.mfHdg = starthdg;
  2864. // xgeobezier.mfX = startx;
  2865. // xgeobezier.mfY = starty;
  2866. // xgeobezier.mnType = 2;
  2867. // }
  2868. // else
  2869. // {
  2870. // xgeobezier.mnType = 0; //Line
  2871. // xgeobezier.mfHdgStart = fbhdg;
  2872. // xgeobezier.mfHdg = fbhdg;
  2873. // xgeobezier.mfX = fxy[0];
  2874. // xgeobezier.mfY = fxy[1];
  2875. // xgeobezier.mfLen = fblen;
  2876. // }
  2877. // xvectorgeo.push_back(xgeobezier);
  2878. // return xvectorgeo;
  2879. geobase linegeo;
  2880. linegeo.mnType = 0;
  2881. linegeo.mfX = startx;
  2882. linegeo.mfY = starty;
  2883. linegeo.mfHdg = geofit::CalcHdg(startx,starty,endx,endy);
  2884. linegeo.mfLen = sqrt(pow(endx - startx,2)+pow(endy - starty,2));
  2885. std::vector<geobase> xvectorgeo;
  2886. xvectorgeo.push_back(linegeo);
  2887. return xvectorgeo;
  2888. }
  2889. std::vector<geobase> MainWindow::CreateTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg,double R)
  2890. {
  2891. std::vector<geobase> xvectorgeo;
  2892. xvectorgeo.clear();
  2893. if(starthdg == endhdg)
  2894. {
  2895. std::cout<<"hdg same use line contact"<<std::endl;
  2896. return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
  2897. }
  2898. double a1,c1,a2,c2;
  2899. double inter_x,inter_y;
  2900. if((starthdg == M_PI/2.0)||(starthdg == 3.0*M_PI/2.0))
  2901. {
  2902. a2 = tan(endhdg) *(-1);
  2903. c2 = 0-a2*endx - endy;
  2904. a1 = 1;
  2905. c1 = startx *(-1);
  2906. inter_x = startx;
  2907. inter_y = 0 - a2*inter_x-c2;
  2908. }
  2909. else
  2910. {
  2911. if((endhdg == M_PI/2.0)||(endhdg == 3.0*M_PI/2.0))
  2912. {
  2913. a1 = tan(starthdg) *(-1);
  2914. c1 = 0-a1*startx - starty;
  2915. a2 = 1;
  2916. c2 = endx *(-1);
  2917. inter_x = endx;
  2918. inter_y = 0 - a1*inter_x-c1;
  2919. }
  2920. else
  2921. {
  2922. a1 = tan(starthdg) *(-1);
  2923. a2 = tan(endhdg) *(-1);
  2924. c1 = 0-a1*startx - starty;
  2925. c2 = 0-a2*endx - endy;
  2926. inter_x = (c1-c2)/(a2-a1);
  2927. inter_y = 0 - a1*inter_x - c1;
  2928. }
  2929. }
  2930. double dis1,dis2;
  2931. dis1 =sqrt(pow(inter_x - startx,2)+pow(inter_y - starty,2));
  2932. dis2 =sqrt(pow(inter_x - endx,2)+pow(inter_y - endy,2));
  2933. if((dis1<1.0)||(dis2<1.0))
  2934. {
  2935. std::cout<<"use line connect."<<std::endl;
  2936. return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
  2937. }
  2938. double hdgdiff = endhdg - starthdg;
  2939. if(hdgdiff >= M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
  2940. if(hdgdiff <= (-M_PI))hdgdiff = hdgdiff + 2.0*M_PI;
  2941. double slen = R*tan(fabs(hdgdiff/2.0));
  2942. if((dis1<slen)||(dis2<slen))
  2943. {
  2944. std::cout<<"radius is big. use line."<<std::endl;
  2945. return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
  2946. }
  2947. double p1_x,p1_y,p2_x,p2_y;
  2948. p1_x = inter_x- slen * cos(starthdg);
  2949. p1_y = inter_y- slen * sin(starthdg);
  2950. p2_x = inter_x+slen*cos(endhdg);
  2951. p2_y = inter_y+slen*sin(endhdg);
  2952. geobase linegeo;
  2953. linegeo.mnType = 0;
  2954. linegeo.mfX = startx;
  2955. linegeo.mfY = starty;
  2956. linegeo.mfHdg = starthdg;
  2957. linegeo.mfLen = sqrt(pow(p1_x - startx,2)+pow(p1_y - starty,2));
  2958. xvectorgeo.push_back(linegeo);
  2959. geobase arcgeo;
  2960. arcgeo.mnType = 1;
  2961. arcgeo.mfX = p1_x;
  2962. arcgeo.mfY = p1_y;
  2963. arcgeo.mfHdg = starthdg;
  2964. arcgeo.mfHdgStart = starthdg;
  2965. arcgeo.mfLen = R*fabs(hdgdiff);
  2966. arcgeo.mR = R*(fabs(hdgdiff)/hdgdiff);
  2967. arcgeo.mfEndX = p2_x;
  2968. arcgeo.mfEndY = p2_y;
  2969. xvectorgeo.push_back(arcgeo);
  2970. linegeo.mnType = 0;
  2971. linegeo.mfX = p2_x;
  2972. linegeo.mfY = p2_y;
  2973. linegeo.mfHdg = endhdg;
  2974. linegeo.mfLen = sqrt(pow(p2_x - endx,2)+pow(p2_y - endy,2));
  2975. xvectorgeo.push_back(linegeo);
  2976. return xvectorgeo;
  2977. }
  2978. std::string MainWindow::getlanetype(Road *p, int nlane, bool bstart)
  2979. {
  2980. Lane * plane = 0;
  2981. int i;
  2982. LaneSection * pLS;
  2983. if(bstart)
  2984. {
  2985. pLS = p->GetLaneSection(0);
  2986. }
  2987. else
  2988. {
  2989. pLS = p->GetLaneSection(p->GetLaneSectionCount() -1);
  2990. }
  2991. int nlanecount = pLS->GetLaneCount();
  2992. for(i=0;i<nlanecount;i++)
  2993. {
  2994. if(nlane == pLS->GetLane(i)->GetId())
  2995. {
  2996. plane = pLS->GetLane(i);
  2997. break;
  2998. }
  2999. }
  3000. if(plane == 0)
  3001. {
  3002. std::cout<<"MainWindow::getlanetype can't find lane : "<<nlane<<std::endl;
  3003. return "driving";
  3004. }
  3005. return plane->GetType();
  3006. }
  3007. double MainWindow::getlanewidth(Road * p, int nlane,bool bstart)
  3008. {
  3009. Lane * plane = 0;
  3010. double a,b,c,d;
  3011. double s;
  3012. if(bstart)
  3013. {
  3014. s = 0;
  3015. }
  3016. else
  3017. {
  3018. s = p->GetRoadLength();
  3019. }
  3020. int i;
  3021. LaneSection * pLS;
  3022. if(bstart)
  3023. {
  3024. pLS = p->GetLaneSection(0);
  3025. }
  3026. else
  3027. {
  3028. pLS = p->GetLaneSection(p->GetLaneSectionCount() -1);
  3029. }
  3030. int nlanecount = pLS->GetLaneCount();
  3031. for(i=0;i<nlanecount;i++)
  3032. {
  3033. if(nlane == pLS->GetLane(i)->GetId())
  3034. {
  3035. plane = pLS->GetLane(i);
  3036. break;
  3037. }
  3038. }
  3039. if(plane == 0)
  3040. {
  3041. std::cout<<"MainWindow::getlanewidth can't find lane : "<<nlane<<std::endl;
  3042. return 0;
  3043. }
  3044. if(bstart)
  3045. {
  3046. return plane->GetLaneWidth(0)->GetA();
  3047. }
  3048. else
  3049. {
  3050. s = p->GetRoadLength() - pLS->GetS();
  3051. LaneWidth * pLW = plane->GetLaneWidth(plane->GetLaneWidthCount()-1);
  3052. a = pLW->GetA();b = pLW->GetB();c = pLW->GetC();d = pLW->GetD();
  3053. return a+b*s+c*pow(s,2)+d*pow(s,3);
  3054. }
  3055. }
  3056. double MainWindow::getoff(Road *p, int nlane, bool bstart)
  3057. {
  3058. double off = 0;
  3059. int i;
  3060. if(bstart)
  3061. {
  3062. LaneSection * pLS = p->GetLaneSection(0);
  3063. if(nlane<0)
  3064. {
  3065. if(nlane == -1)return 0;
  3066. else
  3067. {
  3068. for(i=0;i<pLS->GetLaneCount();i++)
  3069. {
  3070. Lane * pL = pLS->GetLane(i);
  3071. if((pL->GetId()<0)&&(pL->GetId()>nlane))
  3072. {
  3073. off = off + pL->GetLaneWidth(0)->GetA();
  3074. }
  3075. }
  3076. }
  3077. }
  3078. else
  3079. {
  3080. if(nlane == 1)return 0;
  3081. else
  3082. {
  3083. for(i=0;i<pLS->GetLaneCount();i++)
  3084. {
  3085. Lane * pL = pLS->GetLane(i);
  3086. if((pL->GetId()>0)&&(pL->GetId()<nlane))
  3087. {
  3088. off = off - pL->GetLaneWidth(0)->GetA();
  3089. }
  3090. }
  3091. }
  3092. }
  3093. }
  3094. else
  3095. {
  3096. LaneSection * pLS = p->GetLaneSection(p->GetLaneSectionCount()-1);
  3097. if(nlane<0)
  3098. {
  3099. if(nlane == -1)return 0;
  3100. else
  3101. {
  3102. for(i=0;i<pLS->GetLaneCount();i++)
  3103. {
  3104. Lane * pL = pLS->GetLane(i);
  3105. if((pL->GetId()<0)&&(pL->GetId()>nlane))
  3106. {
  3107. double a,b,c,d;
  3108. a = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetA();
  3109. b = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetB();
  3110. c = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetC();
  3111. d = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetD();
  3112. double s = p->GetRoadLength();
  3113. off = off + a + b*s +c *s*s + d*s*s*s;
  3114. }
  3115. }
  3116. }
  3117. }
  3118. else
  3119. {
  3120. if(nlane == 1)return 0;
  3121. else
  3122. {
  3123. for(i=0;i<pLS->GetLaneCount();i++)
  3124. {
  3125. Lane * pL = pLS->GetLane(i);
  3126. if((pL->GetId()>0)&&(pL->GetId()<nlane))
  3127. {
  3128. double a,b,c,d;
  3129. a = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetA();
  3130. b = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetB();
  3131. c = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetC();
  3132. d = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetD();
  3133. double s = p->GetRoadLength();
  3134. off = off - a - b*s -c *s*s - d*s*s*s;
  3135. }
  3136. }
  3137. }
  3138. }
  3139. }
  3140. return off;
  3141. }
  3142. std::vector<geobase> MainWindow::CreateUTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg, double fextend)
  3143. {
  3144. std::vector<geobase> xvectorgeo;
  3145. double p1_x,p1_y, p2_x,p2_y,p1_hdg,p2_hdg;
  3146. p1_x = fextend*cos(starthdg) + startx;
  3147. p1_y = fextend*sin(starthdg) + starty;
  3148. p2_x = fextend*cos(endhdg + M_PI) + endx;
  3149. p2_y = fextend*sin(endhdg + M_PI) + endy;
  3150. p1_hdg = starthdg;
  3151. p2_hdg = endhdg;
  3152. if(starthdg == endhdg)
  3153. {
  3154. std::cout<<" hdg is same, can't create u turn."<<std::endl;
  3155. return xvectorgeo;
  3156. }
  3157. double hdgse = geofit::CalcHdg(p1_x,p1_y,p2_x,p2_y);
  3158. double hdgdiff = hdgse - p1_hdg;
  3159. if(hdgdiff < 0)hdgdiff = hdgdiff + 2.0*M_PI;
  3160. bool bPA = true; //Positive
  3161. if(hdgdiff >=M_PI)bPA = false;
  3162. double xdiff;
  3163. if(bPA)xdiff = hdgdiff - M_PI/2.0;
  3164. else xdiff = hdgdiff - 3.0*M_PI/2.0;
  3165. double xdis = sqrt(pow(p1_x-p2_x,2)+pow(p1_y-p2_y,2));
  3166. double R = xdis/(2.0*cos(xdiff));
  3167. double x_center,y_center;
  3168. double xhdgtocenter;
  3169. if(bPA)
  3170. {
  3171. xhdgtocenter = p1_hdg + M_PI/2.0;
  3172. if(xhdgtocenter >= 2.0*M_PI)xhdgtocenter = xhdgtocenter - M_PI*2.0;
  3173. }
  3174. else
  3175. {
  3176. xhdgtocenter = p1_hdg - M_PI/2.0;
  3177. if(xhdgtocenter < 0)xhdgtocenter = xhdgtocenter + 2.0*M_PI;
  3178. }
  3179. x_center = p1_x + R*cos(xhdgtocenter);
  3180. y_center = p1_y + R*sin(xhdgtocenter);
  3181. double xhdgcentertoarc;
  3182. if(bPA)
  3183. {
  3184. xhdgcentertoarc = hdgse - M_PI/2.0;
  3185. if(xhdgcentertoarc <0)xhdgcentertoarc = xhdgcentertoarc + 2.0*M_PI;
  3186. }
  3187. else
  3188. {
  3189. xhdgcentertoarc = hdgse + M_PI/2.0;
  3190. if(xhdgcentertoarc >= M_PI*2.0)xhdgcentertoarc = xhdgcentertoarc - M_PI*2.0;
  3191. }
  3192. double p3_x,p3_y,p3_hdg;
  3193. p3_hdg = hdgse;
  3194. p3_x = x_center + R* cos(xhdgcentertoarc);
  3195. p3_y = y_center + R* sin(xhdgcentertoarc);
  3196. // pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
  3197. geobase c2,c3;
  3198. c2.mfX = p1_x;
  3199. c2.mfY = p1_y;
  3200. c2.mfHdgStart = p1_hdg;
  3201. c3.mfX = p3_x;
  3202. c3.mfY = p3_y;
  3203. c3.mfHdgStart = hdgse;
  3204. c2.mnType = 1;
  3205. c3.mnType = 1;
  3206. if(bPA)
  3207. {
  3208. c2.mfLen = hdgdiff * R;
  3209. c2.mR = R;
  3210. c3.mfLen = c2.mfLen;
  3211. c3.mR = R;
  3212. }
  3213. else
  3214. {
  3215. c2.mfLen = (2.0*M_PI - hdgdiff) * R;
  3216. c2.mR = R*(-1);
  3217. c3.mfLen = c2.mfLen;
  3218. c3.mR = R;
  3219. }
  3220. geobase l1,l2;
  3221. if(fextend != 0)
  3222. {
  3223. l1.mfX = startx;
  3224. l1.mfY = starty;
  3225. l1.mfHdg = starthdg;
  3226. l1.mfLen = fextend;
  3227. l1.mnType = 0;
  3228. l2.mfX = p2_x;
  3229. l2.mfY = p2_y;
  3230. l2.mfHdg = p2_hdg;
  3231. l2.mfLen = fextend;
  3232. l2.mnType = 0;
  3233. xvectorgeo.push_back(l1);
  3234. }
  3235. xvectorgeo.push_back(c2);
  3236. xvectorgeo.push_back(c3);
  3237. if(fextend != 0)xvectorgeo.push_back(l2);
  3238. return xvectorgeo;
  3239. }
  3240. void MainWindow::ChangeXODRRoadID(OpenDrive *pxodr, int index, int newid)
  3241. {
  3242. Road * proad;
  3243. int nsize = pxodr->GetRoadCount();
  3244. int i;
  3245. if(index<0)return;
  3246. if(index>=nsize)return;
  3247. proad = pxodr->GetRoad(index);
  3248. int noldid = atoi(proad->GetRoadId().data());
  3249. char strid[255];
  3250. snprintf(strid,255,"%d",newid);
  3251. proad->SetRoadId(strid);
  3252. for(i=0;i<nsize;i++)
  3253. {
  3254. Road * proad2 = pxodr->GetRoad(i);
  3255. if(proad2->GetPredecessor()!= 0)
  3256. {
  3257. RoadLink * plink = proad2->GetPredecessor();
  3258. if(strncmp(plink->GetElementType().data(),"road",255)== 0)
  3259. {
  3260. if(atoi(plink->GetElementId().data()) == noldid)
  3261. {
  3262. plink->SetElementId(strid);
  3263. }
  3264. }
  3265. }
  3266. if(proad2->GetSuccessor()!= 0)
  3267. {
  3268. RoadLink * plink = proad2->GetSuccessor();
  3269. if(strncmp(plink->GetElementType().data(),"road",255)== 0)
  3270. {
  3271. if(atoi(plink->GetElementId().data()) == noldid)
  3272. {
  3273. plink->SetElementId(strid);
  3274. }
  3275. }
  3276. }
  3277. }
  3278. nsize = pxodr->GetJunctionCount();
  3279. for(i=0;i<nsize;i++)
  3280. {
  3281. Junction * pjunction = pxodr->GetJunction(i);
  3282. int j;
  3283. for(j=0;j<pjunction->GetJunctionConnectionCount();j++)
  3284. {
  3285. JunctionConnection * pjc = pjunction->GetJunctionConnection(j);
  3286. if(atoi(pjc->GetIncomingRoad().data()) == noldid)
  3287. {
  3288. pjc->SetIncomingRoad(strid);
  3289. }
  3290. if(atoi(pjc->GetConnectingRoad().data()) == noldid)
  3291. {
  3292. pjc->SetConnectingRoad(strid);
  3293. }
  3294. }
  3295. }
  3296. }
  3297. void MainWindow::ChangeXODRJunctionID(OpenDrive *pxodr, int index, int newid)
  3298. {
  3299. Junction * pjunction;
  3300. int nsize = pxodr->GetJunctionCount();
  3301. int i;
  3302. if(index<0)return;
  3303. if(index>=nsize)return;
  3304. pjunction = pxodr->GetJunction(index);
  3305. int noldid = atoi(pjunction->GetId().data());
  3306. char strid[255];
  3307. snprintf(strid,255,"%d",newid);
  3308. pjunction->SetId(strid);
  3309. for(i=0;i<nsize;i++)
  3310. {
  3311. Road * proad2 = pxodr->GetRoad(i);
  3312. if(proad2->GetPredecessor()!= 0)
  3313. {
  3314. RoadLink * plink = proad2->GetPredecessor();
  3315. if(strncmp(plink->GetElementType().data(),"junction",255)== 0)
  3316. {
  3317. if(atoi(plink->GetElementId().data()) == noldid)
  3318. {
  3319. plink->SetElementId(strid);
  3320. }
  3321. }
  3322. }
  3323. if(proad2->GetSuccessor()!= 0)
  3324. {
  3325. RoadLink * plink = proad2->GetSuccessor();
  3326. if(strncmp(plink->GetElementType().data(),"junction",255)== 0)
  3327. {
  3328. if(atoi(plink->GetElementId().data()) == noldid)
  3329. {
  3330. plink->SetElementId(strid);
  3331. }
  3332. }
  3333. }
  3334. }
  3335. }
  3336. void MainWindow::MoveXODR(OpenDrive *pxodr, double movex, double movey)
  3337. {
  3338. int nsize = pxodr->GetRoadCount();
  3339. int i;
  3340. for(i=0;i<nsize;i++)
  3341. {
  3342. Road * pRoad = pxodr->GetRoad(i);
  3343. int nblockcount = pRoad->GetGeometryBlockCount();
  3344. int j;
  3345. for(j=0;j<nblockcount;j++)
  3346. {
  3347. GeometryBlock * pgb = pRoad->GetGeometryBlock(j);
  3348. if(pgb != 0)
  3349. {
  3350. pgb->GetGeometryAt(0)->SetX(pgb->GetGeometryAt(0)->GetX() + movex);
  3351. pgb->GetGeometryAt(0)->SetY(pgb->GetGeometryAt(0)->GetY() + movey);
  3352. }
  3353. }
  3354. }
  3355. }
  3356. int MainWindow::FindNewRoadID(OpenDrive *pxodr1, OpenDrive *pxodr2)
  3357. {
  3358. int nroadsize1,nroadsize2;
  3359. nroadsize1 = pxodr1->GetRoadCount();
  3360. nroadsize2 = pxodr2->GetRoadCount();
  3361. int i;
  3362. int * pnid = new int[nroadsize1 + nroadsize2];
  3363. std::shared_ptr<int> ppnid;ppnid.reset(pnid);
  3364. for(i=0;i<nroadsize1;i++)
  3365. {
  3366. pnid[i] = atoi(pxodr1->GetRoad(i)->GetRoadId().data());
  3367. }
  3368. for(i=0;i<nroadsize2;i++)
  3369. {
  3370. pnid[i+nroadsize1] = atoi(pxodr2->GetRoad(i)->GetRoadId().data());
  3371. }
  3372. int nstartid = 40000;
  3373. bool bUsed = true;
  3374. while(bUsed == true)
  3375. {
  3376. bUsed = false;
  3377. for(i=0;i<(nroadsize1 + nroadsize2);i++)
  3378. {
  3379. if(pnid[i] == nstartid)
  3380. {
  3381. bUsed = true;
  3382. break;
  3383. }
  3384. }
  3385. if(bUsed == false)break;
  3386. nstartid++;
  3387. }
  3388. return nstartid;
  3389. }
  3390. int MainWindow::FindNewJunctionID(OpenDrive *pxodr1, OpenDrive *pxodr2)
  3391. {
  3392. int njunctionsize1,njunctionsize2;
  3393. njunctionsize1 = pxodr1->GetRoadCount();
  3394. njunctionsize2 = pxodr2->GetRoadCount();
  3395. int i;
  3396. int * pnid = new int[njunctionsize1 + njunctionsize2];
  3397. std::shared_ptr<int> ppnid;ppnid.reset(pnid);
  3398. for(i=0;i<njunctionsize1;i++)
  3399. {
  3400. pnid[i] = atoi(pxodr1->GetJunction(i)->GetId().data());
  3401. }
  3402. for(i=0;i<njunctionsize2;i++)
  3403. {
  3404. pnid[i+njunctionsize1] = atoi(pxodr2->GetJunction(i)->GetId().data());
  3405. }
  3406. int nstartid = 50000;
  3407. bool bUsed = true;
  3408. while(bUsed == true)
  3409. {
  3410. bUsed = false;
  3411. for(i=0;i<(njunctionsize1 + njunctionsize2);i++)
  3412. {
  3413. if(pnid[i] == nstartid)
  3414. {
  3415. bUsed = true;
  3416. break;
  3417. }
  3418. }
  3419. if(bUsed == false)break;
  3420. nstartid++;
  3421. }
  3422. return nstartid;
  3423. }
  3424. void MainWindow::onClickRoadMark()
  3425. {
  3426. mbRefresh = true;
  3427. update();
  3428. }
  3429. void MainWindow::onClickRoadDel()
  3430. {
  3431. if(mpCBRoad->count() == 0)
  3432. {
  3433. QMessageBox::warning(this,"warn","no road to be delete");
  3434. return;
  3435. }
  3436. int nroadid = mpCBRoad->currentText().toInt();
  3437. Road * pRoad = 0;
  3438. int nroadsize = mxodr.GetRoadCount();
  3439. int i;
  3440. int index;
  3441. for(i=0;i<nroadsize;i++)
  3442. {
  3443. if(nroadid == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
  3444. {
  3445. pRoad = mxodr.GetRoad(i);
  3446. index = i;
  3447. break;
  3448. }
  3449. }
  3450. if(pRoad == 0)
  3451. {
  3452. QMessageBox::warning(this,"warn","can't find road");
  3453. return;
  3454. }
  3455. mxodr.DeleteRoad(index);
  3456. nroadsize = mxodr.GetRoadCount();
  3457. for(i=0;i<nroadsize;i++)
  3458. {
  3459. pRoad = mxodr.GetRoad(i);
  3460. RoadLink * ppre,* pnext;
  3461. ppre = pRoad->GetPredecessor();
  3462. pnext = pRoad->GetSuccessor();
  3463. if(ppre != 0)
  3464. {
  3465. if(strncmp(ppre->GetElementType().data(),"road",255) == 0)
  3466. {
  3467. if(atoi(ppre->GetElementId().data()) == nroadid)
  3468. {
  3469. pRoad->RemovePredecessor();
  3470. LaneSection * pLS = pRoad->GetLaneSection(0);
  3471. int nlanecount = pLS->GetLaneCount();
  3472. int j;
  3473. for(j=0;j<nlanecount;j++)
  3474. {
  3475. Lane * pLane;
  3476. pLane = pLS->GetLane(j);
  3477. pLane->RemovePredecessor();
  3478. }
  3479. }
  3480. }
  3481. }
  3482. if(pnext != 0)
  3483. {
  3484. if(strncmp(pnext->GetElementType().data(),"road",255) == 0)
  3485. {
  3486. if(atoi(pnext->GetElementId().data()) == nroadid)
  3487. {
  3488. pRoad->RemoveSuccessor();
  3489. LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
  3490. int nlanecount = pLS->GetLaneCount();
  3491. int j;
  3492. for(j=0;j<nlanecount;j++)
  3493. {
  3494. Lane * pLane;
  3495. pLane = pLS->GetLane(j);
  3496. pLane->RemoveSuccessor();
  3497. }
  3498. }
  3499. }
  3500. }
  3501. }
  3502. //remove juction connection where id = roadid
  3503. int njunctioncount = mxodr.GetJunctionCount();
  3504. Junction * pjunction;
  3505. for(i=0;i<njunctioncount;i++)
  3506. {
  3507. pjunction = mxodr.GetJunction(i);
  3508. int j;
  3509. for(j=0;j<pjunction->GetJunctionConnectionCount();j++)
  3510. {
  3511. JunctionConnection * pjc = pjunction->GetJunctionConnection(j);
  3512. int nfromid = atoi(pjc->GetIncomingRoad().data());
  3513. int ntoid = atoi(pjc->GetConnectingRoad().data());
  3514. if((nfromid == nroadid)||(ntoid == nroadid))
  3515. {
  3516. pjunction->DeleteJunctionConnection(j);
  3517. j--;
  3518. }
  3519. }
  3520. }
  3521. updateCBRoad();
  3522. mbRefresh = true;
  3523. update();
  3524. }
  3525. void MainWindow::updateCBRoad()
  3526. {
  3527. mpCBRoad->clear();
  3528. mpCBRoad1->clear();
  3529. mpCBRoad2->clear();
  3530. mpCBJunctionIncommingRoad->clear();
  3531. mpCBJunctionConnectingRoad->clear();
  3532. mpCBPreNxtRoad->clear();
  3533. int i;
  3534. int nroadcount = mxodr.GetRoadCount();
  3535. for(i=0;i<nroadcount;i++)
  3536. {
  3537. const char * strname = mxodr.GetRoad(i)->GetRoadId().data();
  3538. mpCBRoad->addItem(strname);
  3539. mpCBRoad1->addItem(strname);
  3540. mpCBRoad2->addItem(strname);
  3541. mpCBJunctionIncommingRoad->addItem(strname);
  3542. mpCBJunctionConnectingRoad->addItem(strname);
  3543. mpCBPreNxtRoad->addItem(strname);
  3544. }
  3545. }
  3546. void MainWindow::onClickCBRoadChange(int index)
  3547. {
  3548. Road * pRoad = mxodr.GetRoad(index);
  3549. if(pRoad == 0)
  3550. {
  3551. // QMessageBox::warning(this,"WARN","MainWindow::onClickCBRoadChange road is NULL");
  3552. return;
  3553. }
  3554. int i;
  3555. mpLabelRoadShowPreID->setText("");
  3556. mpLabelRoadShowPreType2->setText("");
  3557. mpLabelRoadShowPreType1->setText("");
  3558. mpCBRoadShowPre->clear();
  3559. mpLabelRoadShowNxtID->setText("");
  3560. mpLabelRoadShowNxtType2->setText("");
  3561. mpLabelRoadShowNxtType1->setText("");
  3562. mpCBRoadShowNext->clear();
  3563. if(pRoad->GetPredecessor() != 0)
  3564. {
  3565. RoadLink * pRL = pRoad->GetPredecessor();
  3566. mpLabelRoadShowPreID->setText(pRL->GetElementId().data());
  3567. mpLabelRoadShowPreType2->setText(pRL->GetContactPoint().data());
  3568. mpLabelRoadShowPreType1->setText(pRL->GetElementType().data());
  3569. }
  3570. if(pRoad->GetSuccessor() != 0)
  3571. {
  3572. RoadLink * pRL = pRoad->GetSuccessor();
  3573. mpLabelRoadShowNxtID->setText(pRL->GetElementId().data());
  3574. mpLabelRoadShowNxtType2->setText(pRL->GetContactPoint().data());
  3575. mpLabelRoadShowNxtType1->setText(pRL->GetElementType().data());
  3576. }
  3577. LaneSection * pLS = pRoad->GetLaneSection(0);
  3578. int nlanecount = pLS->GetLaneCount();
  3579. for(i=0;i<nlanecount;i++)
  3580. {
  3581. char strout[255];
  3582. char strtem[255];
  3583. Lane * pLane = pLS->GetLane(i);
  3584. snprintf(strout,255,"%d type:%s",pLane->GetId(),pLane->GetType().data());
  3585. if(pLane->IsPredecessorSet())
  3586. {
  3587. snprintf(strtem,255," pre:%d",pLane->GetPredecessor());
  3588. strncat(strout,strtem,255);
  3589. }
  3590. mpCBRoadShowPre->addItem(strout);
  3591. }
  3592. pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
  3593. nlanecount = pLS->GetLaneCount();
  3594. for(i=0;i<nlanecount;i++)
  3595. {
  3596. char strout[255];
  3597. char strtem[255];
  3598. Lane * pLane = pLS->GetLane(i);
  3599. snprintf(strout,255,"%d type:%s",pLane->GetId(),pLane->GetType().data());
  3600. if(pLane->IsSuccessorSet())
  3601. {
  3602. snprintf(strtem,255," suc:%d",pLane->GetSuccessor());
  3603. strncat(strout,strtem,255);
  3604. }
  3605. mpCBRoadShowNext->addItem(strout);
  3606. }
  3607. }
  3608. void MainWindow::updateJunction()
  3609. {
  3610. int i;
  3611. int njunctioncount = mxodr.GetJunctionCount();
  3612. mpCBJunction->clear();
  3613. for(i=0;i<njunctioncount;i++)
  3614. {
  3615. Junction * pjunction = mxodr.GetJunction(i);
  3616. mpCBJunction->addItem(pjunction->GetId().data());
  3617. }
  3618. }
  3619. void MainWindow::onClickCBJunctionChange(int index)
  3620. {
  3621. mpCBJunctionConnection->clear();
  3622. Junction * pjunction = mxodr.GetJunction(index);
  3623. if(pjunction == NULL)
  3624. {
  3625. // QMessageBox::warning(this,"warn","MainWindow::onClickCBJunctionChange can't find junction");
  3626. return;
  3627. }
  3628. int nconnectioncount = pjunction->GetJunctionConnectionCount();
  3629. int i;
  3630. for(i=0;i<nconnectioncount;i++)
  3631. {
  3632. JunctionConnection * pJC = pjunction->GetJunctionConnection(i);
  3633. mpCBJunctionConnection->addItem(pJC->GetId().data());
  3634. }
  3635. mpCBJunctionFromTo->clear();
  3636. mpLabelJunctionConnectingRoad->setText("");
  3637. mpLabelJunctionContactPoint->setText("");
  3638. mpLabelJunctionIncommingRoad->setText("");
  3639. if(pjunction->GetJunctionConnectionCount()>0)
  3640. {
  3641. mpCBJunctionConnection->setCurrentIndex(0);
  3642. onClickCBJunctionConnectionChange(0);
  3643. }
  3644. }
  3645. void MainWindow::onClickCBJunctionConnectionChange(int index)
  3646. {
  3647. // mpCBJunctionIncommingRoad->clear();
  3648. // mpCBJunctionConnectingRoad->clear();
  3649. // mpCBJunctionFromLane->clear();
  3650. // mpCBJunctionToLane->clear();
  3651. Junction * pjunction = mxodr.GetJunction(mpCBJunction->currentIndex());
  3652. if(pjunction == NULL)
  3653. {
  3654. std::cout<<"MainWindow::onClickCBJunctionConnectionChange Junction NULL"<<std::endl;
  3655. return;
  3656. }
  3657. if((index<0)||(index>= pjunction->GetJunctionConnectionCount()))
  3658. {
  3659. // std::cout<<"MainWindow::onClickCBJunctionConnectionChange out range."<<std::endl;
  3660. return;
  3661. }
  3662. JunctionConnection * pJC = pjunction->GetJunctionConnection(index);
  3663. if(pJC == NULL)
  3664. {
  3665. std::cout<<"MainWindow::onClickCBJunctionConnectionChange Junction Connection NULL"<<std::endl;
  3666. return;
  3667. }
  3668. mpLabelJunctionIncommingRoad->setText(pJC->GetIncomingRoad().data());
  3669. mpLabelJunctionContactPoint->setText(pJC->GetContactPoint().data());
  3670. mpLabelJunctionConnectingRoad->setText(pJC->GetConnectingRoad().data());
  3671. unsigned int i;
  3672. unsigned int njunctionlanelinkcount = pJC->GetJunctionLaneLinkCount();
  3673. mpCBJunctionFromTo->clear();
  3674. for(i=0;i<njunctionlanelinkcount;i++)
  3675. {
  3676. char strout[255];
  3677. JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(i);
  3678. snprintf(strout,255,"%d to %d",pjll->GetFrom(),pjll->GetTo());
  3679. mpCBJunctionFromTo->addItem(strout);
  3680. }
  3681. }
  3682. void MainWindow::onClickCBJunctionIncommingChange(int index)
  3683. {
  3684. int i;
  3685. int ncount = mxodr.GetRoadCount();
  3686. if((index < 0)||(index>=ncount))
  3687. {
  3688. return;
  3689. }
  3690. Road * pRoad = mxodr.GetRoad(index);
  3691. if(pRoad == NULL)
  3692. {
  3693. return;
  3694. }
  3695. LaneSection * pLS;
  3696. if(mpCBJunctionContactPoint->currentIndex() == 0)
  3697. {
  3698. pLS = pRoad->GetLaneSection(0);
  3699. }
  3700. else
  3701. {
  3702. pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
  3703. }
  3704. int nlanecount = pLS->GetLaneCount();
  3705. mpCBJunctionFromLane->clear();
  3706. for(i=0;i<nlanecount;i++)
  3707. {
  3708. Lane * pLane = pLS->GetLane(i);
  3709. if(pLane->GetId() != 0)
  3710. {
  3711. mpCBJunctionFromLane->addItem(QString::number(pLane->GetId()));
  3712. }
  3713. }
  3714. }
  3715. void MainWindow::onClickCBJunctionConnectionroadChange(int index)
  3716. {
  3717. int i;
  3718. int ncount = mxodr.GetRoadCount();
  3719. if((index < 0)||(index>=ncount))
  3720. {
  3721. return;
  3722. }
  3723. Road * pRoad = mxodr.GetRoad(index);
  3724. if(pRoad == NULL)
  3725. {
  3726. return;
  3727. }
  3728. LaneSection * pLS;
  3729. pLS = pRoad->GetLaneSection(0);
  3730. int nlanecount = pLS->GetLaneCount();
  3731. mpCBJunctionToLane->clear();
  3732. for(i=0;i<nlanecount;i++)
  3733. {
  3734. Lane * pLane = pLS->GetLane(i);
  3735. if(pLane->GetId() != 0)
  3736. {
  3737. mpCBJunctionToLane->addItem(QString::number(pLane->GetId()));
  3738. }
  3739. }
  3740. }
  3741. void MainWindow::onClickCreateJunction()
  3742. {
  3743. mxodr.AddJunction("",QString::number(CreateJunctionID()).toStdString());
  3744. updateJunction();
  3745. }
  3746. void MainWindow::onClickCreateJunctionLaneLink()
  3747. {
  3748. Junction * pJunction= mxodr.GetJunction(mpCBJunction->currentIndex());
  3749. if(pJunction == NULL)
  3750. {
  3751. QMessageBox::warning(this,"warn","can't find junction");
  3752. return ;
  3753. }
  3754. Road * pFromRoad = mxodr.GetRoad(mpCBJunctionIncommingRoad->currentIndex());
  3755. if(pFromRoad == NULL)
  3756. {
  3757. QMessageBox::warning(this,"warn","can't find incomming road.");
  3758. return;
  3759. }
  3760. Road * pToRoad = mxodr.GetRoad(mpCBJunctionConnectingRoad->currentIndex());
  3761. if(pToRoad == NULL)
  3762. {
  3763. QMessageBox::warning(this,"warn","can't find connecting road.");
  3764. return;
  3765. }
  3766. string contacpoint = mpCBJunctionContactPoint->currentText().toStdString();
  3767. JunctionConnection * pJC = 0;
  3768. int njunctioncount = pJunction->GetJunctionConnectionCount();
  3769. int i;
  3770. for(i=0;i<njunctioncount;i++)
  3771. {
  3772. JunctionConnection * pJunCon = pJunction->GetJunctionConnection(i);
  3773. if((pJunCon->GetIncomingRoad() == pFromRoad->GetRoadId())&&(pJunCon->GetContactPoint() == contacpoint)&&(pJunCon->GetConnectingRoad() == pToRoad->GetRoadId()))
  3774. {
  3775. pJC = pJunCon;
  3776. break;
  3777. }
  3778. }
  3779. bool bNewJC = false;
  3780. if(pJC == 0)
  3781. {
  3782. int nnewJCid = pJunction->GetJunctionConnectionCount();
  3783. int j;
  3784. int njccount = pJunction->GetJunctionConnectionCount();
  3785. bool bNotUse = true;
  3786. do
  3787. {
  3788. for(j=0;j<njccount;j++)
  3789. {
  3790. if(nnewJCid == atoi(pJunction->GetJunctionConnection(j)->GetId().data()))
  3791. {
  3792. bNotUse = false;
  3793. nnewJCid++;
  3794. break;
  3795. }
  3796. }
  3797. }while(bNotUse == false);
  3798. unsigned int addindex = pJunction->AddJunctionConnection(QString::number(nnewJCid).toStdString(),
  3799. pFromRoad->GetRoadId(),
  3800. pToRoad->GetRoadId(),
  3801. contacpoint);
  3802. pJC = pJunction->GetJunctionConnection(addindex);
  3803. bNewJC = true;
  3804. }
  3805. int nfrom = mpCBJunctionFromLane->currentText().toInt();
  3806. int nto = mpCBJunctionToLane->currentText().toInt();
  3807. int njlcount = pJC->GetJunctionLaneLinkCount();
  3808. for(i=0;i<njlcount;i++)
  3809. {
  3810. JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(i);
  3811. if((pjll->GetFrom()== nfrom)&&(pjll->GetTo() == nto))
  3812. {
  3813. QMessageBox::warning(this,"warn","this lane link exist.");
  3814. return;
  3815. }
  3816. }
  3817. unsigned int nadd = pJC->AddJunctionLaneLink(mpCBJunctionFromLane->currentText().toInt(),
  3818. mpCBJunctionToLane->currentText().toInt());
  3819. char strout[255];
  3820. JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(nadd);
  3821. snprintf(strout,255,"%d to %d",pjll->GetFrom(),pjll->GetTo());
  3822. if(bNewJC)
  3823. {
  3824. mpCBJunctionConnection->addItem(pJC->GetId().data());
  3825. }
  3826. if(strncmp(contacpoint.data(),"start",255) == 0)
  3827. {
  3828. if(nfrom*nto<0)
  3829. {
  3830. pFromRoad->SetPredecessor("junction",pJunction->GetId(),contacpoint);
  3831. }
  3832. else
  3833. {
  3834. pFromRoad->SetSuccessor("junction",pJunction->GetId(),contacpoint);
  3835. }
  3836. }
  3837. else
  3838. {
  3839. if(nfrom*nto <0)
  3840. {
  3841. pFromRoad->SetSuccessor("junction",pJunction->GetId(),contacpoint);
  3842. }
  3843. else
  3844. {
  3845. pFromRoad->SetPredecessor("junction",pJunction->GetId(),contacpoint);
  3846. }
  3847. }
  3848. pToRoad->SetRoadJunction(pJunction->GetId());
  3849. }
  3850. void MainWindow::onClickDeleteJunctionLaneLink()
  3851. {
  3852. Junction * pJunction= mxodr.GetJunction(mpCBJunction->currentIndex());
  3853. if(pJunction == NULL)
  3854. {
  3855. QMessageBox::warning(this,"warn","can't find junction");
  3856. return ;
  3857. }
  3858. if(pJunction->GetJunctionConnectionCount() == 0)
  3859. {
  3860. mxodr.DeleteJunction(mpCBJunction->currentIndex());
  3861. updateJunction();
  3862. return;
  3863. }
  3864. JunctionConnection * pJC = pJunction->GetJunctionConnection(mpCBJunctionConnection->currentIndex());
  3865. if(pJC == NULL)
  3866. {
  3867. QMessageBox::warning(this,"warn","can't find junction connetion.");
  3868. return;
  3869. }
  3870. if(pJC->GetJunctionLaneLinkCount() == 0)
  3871. {
  3872. pJunction->DeleteJunctionConnection(mpCBJunctionConnection->currentIndex());
  3873. if(pJunction->GetJunctionConnectionCount() > 0)
  3874. onClickCBJunctionChange(mpCBJunction->currentIndex());
  3875. else
  3876. {
  3877. mxodr.DeleteJunction(mpCBJunction->currentIndex());
  3878. updateJunction();
  3879. }
  3880. return;
  3881. }
  3882. // std::string strincommingroad = mpLabelJunctionIncommingRoad->text().toStdString();
  3883. // std::string strconnectionroad = mpLabelJunctionConnectingRoad->text().toStdString();
  3884. // std::string strcontact = mpLabelJunctionContactPoint->text().toStdString();
  3885. int nllindex = mpCBJunctionFromTo->currentIndex();
  3886. pJC->DeleteJunctionLaneLink(nllindex);
  3887. onClickCBJunctionConnectionChange(mpCBJunctionConnection->currentIndex());
  3888. }
  3889. void MainWindow::onClickPreNxtRoadChange(int index)
  3890. {
  3891. mpCBPreNxtRelLane->clear();
  3892. Road * pRoad = mxodr.GetRoad(index);
  3893. if(pRoad == NULL)
  3894. {
  3895. return;
  3896. }
  3897. LaneSection * pLS;
  3898. if(strncmp(mpCBPreNxtConatact->currentText().toStdString().data(),"start",255)==0)
  3899. {
  3900. pLS = pRoad->GetLaneSection(0);
  3901. }
  3902. else
  3903. {
  3904. pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
  3905. }
  3906. if(pLS == NULL)
  3907. {
  3908. std::cout<<"MainWindow::onClickPreNxtRoadChange can't find lanesection"<<std::endl;
  3909. return;
  3910. }
  3911. int nlanecount = pLS->GetLaneCount();
  3912. int i;
  3913. for(i=0;i<nlanecount;i++)
  3914. {
  3915. Lane * pLane = pLS->GetLane(i);
  3916. char strout[255];
  3917. snprintf(strout,255,"%d %s",pLane->GetId(),pLane->GetType().data());
  3918. mpCBPreNxtRelLane->addItem(strout);
  3919. }
  3920. }
  3921. void MainWindow::onClickSetRoadPredecessor()
  3922. {
  3923. Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
  3924. if(pRoad == NULL)
  3925. {
  3926. return;
  3927. }
  3928. Road * pOtherRoad = mxodr.GetRoad(mpCBPreNxtRoad->currentIndex());
  3929. if(pOtherRoad == NULL)
  3930. {
  3931. return;
  3932. }
  3933. std::string strcontact = mpCBPreNxtConatact->currentText().toStdString();
  3934. int curlane = mpCBRoadShowPre->currentIndex();
  3935. int otherlane = mpCBPreNxtRelLane->currentIndex();
  3936. pRoad->SetPredecessor("road",pOtherRoad->GetRoadId(),strcontact);
  3937. LaneSection * pLS = pRoad->GetLaneSection(0);
  3938. Lane * pLane =pLS->GetLane(curlane);
  3939. LaneSection * pLSOther;
  3940. if(strncmp(strcontact.data(),"start",0) == 0)
  3941. {
  3942. pLSOther = pOtherRoad->GetLaneSection(0);
  3943. }
  3944. else
  3945. {
  3946. pLSOther = pOtherRoad->GetLaneSection(pOtherRoad->GetLaneSectionCount()-1);
  3947. }
  3948. Lane * pLaneOther = pLSOther->GetLane(otherlane);
  3949. if(pLaneOther == NULL)
  3950. {
  3951. QMessageBox::warning(this,"warn","no this lane.");
  3952. return;
  3953. }
  3954. pLane->SetPredecessor(pLaneOther->GetId());
  3955. onClickCBRoadChange(mpCBRoad->currentIndex());
  3956. }
  3957. void MainWindow::onClickSetRoadSuccessor()
  3958. {
  3959. Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
  3960. if(pRoad == NULL)
  3961. {
  3962. return;
  3963. }
  3964. Road * pOtherRoad = mxodr.GetRoad(mpCBPreNxtRoad->currentIndex());
  3965. if(pOtherRoad == NULL)
  3966. {
  3967. return;
  3968. }
  3969. std::string strcontact = mpCBPreNxtConatact->currentText().toStdString();
  3970. int curlane = mpCBRoadShowNext->currentIndex();
  3971. int otherlane = mpCBPreNxtRelLane->currentIndex();
  3972. pRoad->SetSuccessor("road",pOtherRoad->GetRoadId(),strcontact);
  3973. LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
  3974. Lane * pLane =pLS->GetLane(curlane);
  3975. LaneSection * pLSOther;
  3976. if(strncmp(strcontact.data(),"start",0) == 0)
  3977. {
  3978. pLSOther = pOtherRoad->GetLaneSection(0);
  3979. }
  3980. else
  3981. {
  3982. pLSOther = pOtherRoad->GetLaneSection(pOtherRoad->GetLaneSectionCount()-1);
  3983. }
  3984. Lane * pLaneOther = pLSOther->GetLane(otherlane);
  3985. if(pLaneOther == NULL)
  3986. {
  3987. QMessageBox::warning(this,"warn","no this lane.");
  3988. return;
  3989. }
  3990. pLane->SetSuccessor(pLaneOther->GetId());
  3991. onClickCBRoadChange(mpCBRoad->currentIndex());
  3992. }
  3993. void MainWindow::onClickDelRoadPredecessor()
  3994. {
  3995. Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
  3996. if(pRoad == NULL)
  3997. {
  3998. return;
  3999. }
  4000. int curlane = mpCBRoadShowPre->currentIndex();
  4001. LaneSection * pLS = pRoad->GetLaneSection(0);
  4002. Lane * pLane =pLS->GetLane(curlane);
  4003. if(pLane == 0)
  4004. {
  4005. return;
  4006. }
  4007. pLane->RemovePredecessor();
  4008. int i;
  4009. int nlanecount = pLS->GetLaneCount();
  4010. bool bAllNotHavePre = true;
  4011. for(i=0;i<nlanecount;i++)
  4012. {
  4013. Lane * pLa= pLS->GetLane(i);
  4014. if(pLa->IsPredecessorSet())
  4015. {
  4016. bAllNotHavePre = false;
  4017. break;
  4018. }
  4019. }
  4020. if(bAllNotHavePre)
  4021. {
  4022. pRoad->RemovePredecessor();
  4023. }
  4024. onClickCBRoadChange(mpCBRoad->currentIndex());
  4025. }
  4026. void MainWindow::onClickDelRoadSuccessor()
  4027. {
  4028. Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
  4029. if(pRoad == NULL)
  4030. {
  4031. return;
  4032. }
  4033. int curlane = mpCBRoadShowNext->currentIndex();
  4034. LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
  4035. Lane * pLane =pLS->GetLane(curlane);
  4036. if(pLane == 0)
  4037. {
  4038. return;
  4039. }
  4040. pLane->RemoveSuccessor();
  4041. int i;
  4042. int nlanecount = pLS->GetLaneCount();
  4043. bool bAllNotHaveNxt = true;
  4044. for(i=0;i<nlanecount;i++)
  4045. {
  4046. Lane * pLa= pLS->GetLane(i);
  4047. if(pLa->IsSuccessorSet())
  4048. {
  4049. bAllNotHaveNxt = false;
  4050. break;
  4051. }
  4052. }
  4053. if(bAllNotHaveNxt)
  4054. {
  4055. pRoad->RemoveSuccessor();
  4056. }
  4057. onClickCBRoadChange(mpCBRoad->currentIndex());
  4058. }
  4059. void MainWindow::onClickAutoConnect()
  4060. {
  4061. AutoConnect pAC(&mxodr);
  4062. pAC.Connect();
  4063. updateJunction();
  4064. }
  4065. void MainWindow::on_actionLoad_triggered()
  4066. {
  4067. onClickLoad();
  4068. }
  4069. void MainWindow::on_actionSave_triggered()
  4070. {
  4071. onClickSave();
  4072. }
  4073. void MainWindow::on_actionAutoConnect_triggered()
  4074. {
  4075. onClickAutoConnect();
  4076. }
  4077. void MainWindow::on_actionSet_Speed_triggered()
  4078. {
  4079. SpeedDialog sd(&mxodr,this);
  4080. int res = sd.exec();
  4081. }
  4082. void MainWindow::closeEvent(QCloseEvent *event)
  4083. {
  4084. if(mxodr.GetRoadCount() == 0)
  4085. {
  4086. event->accept(); // 接受退出信号,程序退出
  4087. return;
  4088. }
  4089. QMessageBox::StandardButton button;
  4090. button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
  4091. if(button==QMessageBox::No)
  4092. {
  4093. event->ignore(); // 忽略退出信号,程序继续进行
  4094. }
  4095. else if(button==QMessageBox::Yes)
  4096. {
  4097. event->accept(); // 接受退出信号,程序退出
  4098. }
  4099. }