123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156315731583159316031613162316331643165316631673168316931703171317231733174317531763177317831793180318131823183318431853186318731883189319031913192319331943195319631973198319932003201320232033204320532063207320832093210321132123213321432153216321732183219322032213222322332243225322632273228322932303231323232333234323532363237323832393240324132423243324432453246324732483249325032513252325332543255325632573258325932603261326232633264326532663267326832693270327132723273327432753276327732783279328032813282328332843285328632873288328932903291329232933294329532963297329832993300330133023303330433053306330733083309331033113312331333143315331633173318331933203321332233233324332533263327332833293330333133323333333433353336333733383339334033413342334333443345334633473348334933503351335233533354335533563357335833593360336133623363336433653366336733683369337033713372337333743375337633773378337933803381338233833384338533863387338833893390339133923393339433953396339733983399340034013402340334043405340634073408340934103411341234133414341534163417341834193420342134223423342434253426342734283429343034313432343334343435343634373438343934403441344234433444344534463447344834493450345134523453345434553456345734583459346034613462346334643465346634673468346934703471347234733474347534763477347834793480348134823483348434853486348734883489349034913492349334943495349634973498349935003501350235033504350535063507350835093510351135123513351435153516351735183519352035213522352335243525352635273528352935303531353235333534353535363537353835393540354135423543354435453546354735483549355035513552355335543555355635573558355935603561356235633564356535663567356835693570357135723573357435753576357735783579358035813582358335843585358635873588358935903591359235933594359535963597359835993600360136023603360436053606360736083609361036113612361336143615361636173618361936203621362236233624362536263627362836293630363136323633363436353636363736383639364036413642364336443645364636473648364936503651365236533654365536563657365836593660366136623663366436653666366736683669367036713672367336743675367636773678367936803681368236833684368536863687368836893690369136923693369436953696369736983699370037013702370337043705370637073708370937103711371237133714371537163717371837193720372137223723372437253726372737283729373037313732373337343735373637373738373937403741374237433744374537463747374837493750375137523753375437553756375737583759376037613762376337643765376637673768376937703771377237733774377537763777377837793780378137823783378437853786378737883789379037913792379337943795379637973798379938003801380238033804380538063807380838093810381138123813381438153816381738183819382038213822382338243825382638273828382938303831383238333834383538363837383838393840384138423843384438453846384738483849385038513852385338543855385638573858385938603861386238633864386538663867386838693870387138723873387438753876387738783879388038813882388338843885388638873888388938903891389238933894389538963897389838993900390139023903390439053906390739083909391039113912391339143915391639173918391939203921392239233924392539263927392839293930393139323933393439353936393739383939394039413942394339443945394639473948394939503951395239533954395539563957395839593960396139623963396439653966396739683969397039713972397339743975397639773978397939803981398239833984398539863987398839893990399139923993399439953996399739983999400040014002400340044005400640074008400940104011401240134014401540164017401840194020402140224023402440254026402740284029403040314032403340344035403640374038403940404041404240434044404540464047404840494050405140524053405440554056405740584059406040614062406340644065406640674068406940704071407240734074407540764077407840794080408140824083408440854086408740884089409040914092409340944095409640974098409941004101410241034104410541064107410841094110411141124113411441154116411741184119412041214122412341244125412641274128412941304131413241334134413541364137413841394140414141424143414441454146414741484149415041514152415341544155415641574158415941604161416241634164416541664167416841694170417141724173417441754176417741784179418041814182418341844185418641874188418941904191419241934194419541964197419841994200420142024203420442054206420742084209421042114212421342144215421642174218421942204221422242234224422542264227422842294230423142324233423442354236423742384239424042414242424342444245424642474248424942504251425242534254425542564257425842594260426142624263426442654266426742684269427042714272427342744275427642774278427942804281428242834284428542864287428842894290429142924293429442954296429742984299430043014302430343044305430643074308430943104311431243134314431543164317431843194320432143224323432443254326432743284329433043314332433343344335433643374338433943404341434243434344434543464347434843494350435143524353435443554356435743584359436043614362436343644365436643674368436943704371437243734374437543764377437843794380438143824383438443854386438743884389439043914392439343944395439643974398439944004401440244034404440544064407440844094410441144124413441444154416441744184419442044214422442344244425442644274428442944304431443244334434443544364437443844394440444144424443444444454446444744484449445044514452445344544455445644574458445944604461446244634464446544664467446844694470447144724473447444754476447744784479448044814482448344844485448644874488448944904491449244934494449544964497449844994500450145024503450445054506450745084509451045114512451345144515451645174518451945204521452245234524452545264527452845294530453145324533453445354536453745384539454045414542454345444545454645474548454945504551455245534554455545564557455845594560456145624563456445654566456745684569457045714572457345744575457645774578457945804581458245834584458545864587458845894590459145924593459445954596459745984599460046014602460346044605460646074608460946104611461246134614461546164617461846194620462146224623462446254626462746284629463046314632463346344635463646374638463946404641464246434644464546464647464846494650465146524653465446554656465746584659466046614662466346644665466646674668466946704671467246734674467546764677467846794680468146824683468446854686468746884689469046914692469346944695469646974698469947004701470247034704470547064707470847094710471147124713471447154716471747184719472047214722472347244725472647274728472947304731473247334734473547364737473847394740474147424743474447454746474747484749475047514752475347544755475647574758475947604761476247634764476547664767 |
- #include "mainwindow.h"
- #include "ui_mainwindow.h"
- #include <iomanip>//不要忘记包含此头文件
- #include <QScrollBar>
- #include "gpsimu.pb.h"
- #include "gnss_coordinate_convert.h"
- #define VIEW_WIDTH 10000
- #define VIEW_HEIGHT 10000
- double glon0 = 117.0866293;
- double glat0 = 39.1364713;
- //double glon0 = 117;
- //double glat0 = 39;
- double ghdg0 = 360;
- MainWindow::MainWindow(QWidget *parent) :
- QMainWindow(parent),
- ui(new Ui::MainWindow)
- {
- ui->setupUi(this);
- mnfac = 1;
- // int aaa[10000000];
- mnMoveX = VIEW_WIDTH/2;
- mnMoveY = VIEW_HEIGHT/2;
- mnDefmnfac = mnfac;
- mnDefMoveX = mnMoveX;
- mnDefMoveY = mnMoveY;
- myview = new MyView(this);
- myview->setObjectName(QStringLiteral("graphicsView"));
- myview->setGeometry(QRect(30, 30, 600, 600));
- connect(myview,SIGNAL(dbclickxy(double,double)),this,SLOT(onClickXY(double,double)));
- image = new QImage(VIEW_WIDTH, VIEW_HEIGHT, QImage::Format_RGB32);//画布的初始化大小设为300*300,使用32位颜色
- myview->setCacheMode(myview->CacheBackground);
- painter = new QPainter(image);
- painter->end();
- scene = new QGraphicsScene;
- // painter->begin(image);
- QTabWidget * p = new QTabWidget(ui->centralWidget);
- p->setGeometry(30,30,300,300);
- mnFontHeight = ui->centralWidget->fontMetrics().height();
- CreateTab1View(p);
- mTabMain = p;
- QTimer * timer = new QTimer();
- connect(timer,SIGNAL(timeout()),this,SLOT(onTimer()));
- timer->start(1000);
- setWindowTitle("Create Map From Lane Info");
- }
- MainWindow::~MainWindow()
- {
- delete ui;
- }
- void MainWindow::resizeEvent(QResizeEvent *event)
- {
- qDebug("resize");
- QSize sizemain = ui->centralWidget->size();
- qDebug("size x = %d y=%d",sizemain.width(),sizemain.height());
- AdjustWPos(sizemain);
- }
- void MainWindow::AdjustWPos(QSize sizemain)
- {
- myview->setGeometry(0,30,sizemain.width()-mnFontHeight * 22 - 30,sizemain.height());
- mTabMain->setGeometry(sizemain.width()-mnFontHeight * 22,30,mnFontHeight * 22,sizemain.height()-50);
- // mgplidar->setGeometry(sizemain.width()-280,30,260,200);
- }
- void MainWindow::ExecPainter()
- {
- QTime x;
- x.start();
- // qDebug("painter.");
- painter->begin(image);
- qDebug("time is %d",x.elapsed());
- image->fill(QColor(255, 255, 255));//对画布进行填充
- // std::vector<iv::GPSData> navigation_data = brain->navigation_data;
- painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式,好看一点
- painter->translate(mnMoveX,mnMoveY);
- painter->setPen(Qt::black);
- painter->drawLine(VIEW_WIDTH/(-2),0,VIEW_WIDTH/2,0);
- painter->drawLine(0,VIEW_HEIGHT/(-2),0,VIEW_HEIGHT/2);
- int i;
- // int nfac = 5;;
- painter->setPen(Qt::blue);
- int nfac = mnfac;
- if(mbClick)
- {
- painter->setPen(Qt::red);
- painter->drawEllipse(QPoint(mClickX ,mClickY),mnMarkSize,mnMarkSize);
- painter->setPen(Qt::black);
- }
- if(mbSetObj)
- {
- painter->setPen(Qt::green);
- painter->drawRect(mfObjX*mnfac-mnMarkSize,mfObjY*mnfac*(-1)-mnMarkSize,mnMarkSize*2,mnMarkSize*2);
- painter->setPen(Qt::black);
- }
- painter->setPen(Qt::green);
- double x0,y0;
- GaussProjCal(glon0,glat0,&x0,&y0);
- painter->setPen(Qt::blue);
- // int nfac = mnfac;
- int selid = mpCBRoad->currentText().toInt();
- for(i=0;i<mxodr.GetRoadCount();i++)
- {
- // continue;
- int j;
- Road * pRoad = mxodr.GetRoad(i);
- painter->setPen(Qt::blue);
- if(selid == atoi(pRoad->GetRoadId().data()))
- {
- painter->setPen(Qt::red);
- }
- if(mxodr.GetRoad(i)->GetGeometryBlockCount()>0)
- {
- GeometryBlock * pgeob = pRoad->GetGeometryBlock(0);
- double x,y;
- RoadGeometry * pg;
- pg = pgeob->GetGeometryAt(0);
- x = pg->GetX();
- y = pg->GetY();
- double endx,endy,endhdg;
- GetEndPoint(pRoad,endx,endy,endhdg);
- x = (x+endx)/2;
- y = (y+endy)/2;
- painter->drawText(x*mnfac,y*mnfac*(-1),mxodr.GetRoad(i)->GetRoadId().data());
- }
- for(j=0;j<mxodr.GetRoad(i)->GetGeometryBlockCount();j++)
- {
- GeometryBlock * pgeob = pRoad->GetGeometryBlock(j);
- double x,y;
- double x_center,y_center;
- double R;
- RoadGeometry * pg;
- GeometryArc * parc;
- GeometryParamPoly3 * ppp3;
- GeometrySpiral *pSpiral;
- double rel_x,rel_y,rel_hdg;
- pg = pgeob->GetGeometryAt(0);
- x = pg->GetX();
- y = pg->GetY();
- if(j== 0)
- {
- if(selid == atoi(pRoad->GetRoadId().data()))
- {
- painter->setPen(Qt::green);
- painter->drawEllipse(x*mnfac-5,y*mnfac*(-1)-5,10,10);
- painter->setPen(Qt::red);
- }
- }
- switch (pg->GetGeomType()) {
- case 0:
- painter->drawLine(QPoint(x*mnfac,y*mnfac*(-1)),
- QPoint((x + pg->GetLength() * cos(pg->GetHdg()))*mnfac,(y + pg->GetLength() * sin(pg->GetHdg()))*mnfac*(-1)));
- break;
- case 1:
- pSpiral = (GeometrySpiral * )pg;
- {
- int ncount = pSpiral->GetLength() * mnfac;
- double sstep = pSpiral->GetLength()/((double)ncount);
- int k;
- double x0,y0,hdg0,s0;
- x0 = pSpiral->GetX();
- y0 = pSpiral->GetY();
- s0 = pSpiral->GetS();
- hdg0 = pSpiral->GetHdg() ;
- painter->setPen(Qt::red);
- for(k=0;k<ncount;k++)
- {
- pSpiral->GetCoords(s0+sstep*k,rel_x,rel_y,rel_hdg);
- x = rel_x;
- y = rel_y;
- painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- }
- painter->setPen(Qt::blue);
- }
- // qDebug("spi");
- break;
- case 2:
- {
- parc = (GeometryArc *)pg;
- R = abs(1.0/parc->GetCurvature());
- if(parc->GetCurvature() > 0)
- {
- x_center = pg->GetX() + R *cos(pg->GetHdg() + M_PI/2.0);
- y_center = pg->GetY() + R * sin(pg->GetHdg() + M_PI/2.0);
- }
- else
- {
- x_center = pg->GetX() + R *cos(pg->GetHdg() -M_PI/2.0);
- y_center = pg->GetY() + R * sin(pg->GetHdg() - M_PI/2.0);
- }
- int k;
- int ncount = parc->GetLength() * mnfac ;
- double curv = parc->GetCurvature();
- double hdgstep;
- double hdg0 = parc->GetHdg();
- double hdgnow = parc->GetHdg();
- if(ncount > 0) hdgstep= (parc->GetLength()/R)/ncount;
- for(k=0;k<ncount;k++)
- {
- double x_draw,y_draw;
- if(curv > 0)
- {
- hdgnow = hdg0 + k*hdgstep;
- x_draw = x_center + R *cos(hdgnow - M_PI/2.0);
- y_draw = y_center + R * sin(hdgnow - M_PI/2.0);
- }
- else
- {
- hdgnow = hdg0 - k * hdgstep;
- x_draw = x_center + R *cos(hdgnow + M_PI/2.0);
- y_draw = y_center + R * sin(hdgnow + M_PI/2.0);
- }
- painter->drawPoint(x_draw * mnfac ,y_draw * mnfac *(-1));
- }
- }
- break;
- case 4:
- {
- ppp3 = (GeometryParamPoly3 * )pg;
- int ncount = ppp3->GetLength()* mnfac;
- double sstep;
- if(ncount > 0)sstep = ppp3->GetLength()/ncount;
- else sstep = 10000.0;
- double s = 0;
- while(s < ppp3->GetLength())
- {
- double xtem,ytem;
- xtem = ppp3->GetuA() + ppp3->GetuB() * s + ppp3->GetuC() * s*s + ppp3->GetuD() * s*s*s;
- ytem = ppp3->GetvA() + ppp3->GetvB() * s + ppp3->GetvC() * s*s + ppp3->GetvD() * s*s*s;
- x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
- y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
- painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- s = s+ sstep;
- }
- }
- break;
- default:
- break;
- }
- // painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- }
- }
- for(i=0;i<mvectorlp.size();i++)
- {
- if(!mbShowLane)continue;
- if(i == mnMarkLane)
- {
- painter->setPen(Qt::red);
- }
- else
- {
- painter->setPen(Qt::blue);
- }
- int npsize = mvectorlp[i].mvectorlpleft.size();
- int j;
- for(j=0;j<npsize;j++)
- {
- double x,y;
- double lon,lat;
- lon = mvectorlp[i].mvectorlpleft[j].mfLon;
- lat = mvectorlp[i].mvectorlpleft[j].mfLat;
- GaussProjCal(lon,lat,&x,&y);
- x = x-x0;
- y= y-y0;
- painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- lon = mvectorlp[i].mvectorlpright[j].mfLon;
- lat = mvectorlp[i].mvectorlpright[j].mfLat;
- GaussProjCal(lon,lat,&x,&y);
- x = x-x0;
- y= y-y0;
- painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- }
- }
- painter->setPen(Qt::green);
- painter->end();
- }
- //刷新
- void MainWindow::paintEvent(QPaintEvent *)
- {
- if(mbRefresh)
- {
- ExecPainter();
- // qDebug(" time is %d ",x.elapsed());
- mbRefresh = false;
- }
- scene->clear();
- scene->addPixmap(QPixmap::fromImage(*image));
- myview->setScene(scene);
- myview->show();
- if(mbInit == false)
- {
- myview->horizontalScrollBar()->setValue((mnMoveX - 800));
- myview->verticalScrollBar()->setValue((mnMoveY - 500));
- mbInit = true;
- }
- // qDebug(" time 2 is %d ",x.elapsed());
- }
- void MainWindow::onTimer()
- {
- }
- void MainWindow::CreateTab1View(QTabWidget * p)
- {
- QGroupBox * pGroup = new QGroupBox();
- pGroup->setGeometry(0,0,mnFontHeight * 21,mnFontHeight * 110);
- QLabel * pLabel;
- QLineEdit * pLE;
- QPushButton * pPB;
- QSlider * pSlider;
- QComboBox * pCB;
- int nXPos = 10;
- int nYPos = 30;
- int i;
- int nSpace = mnFontHeight * 65/10;
- int nLEWidth = mnFontHeight * 6;
- int nLEHeight = mnFontHeight * 3/2;
- pLabel = new QLabel(pGroup);
- pLabel->setText("Lat0");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setText("Lon0");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setText("Head0");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLE = new QLineEdit(pGroup);
- pLE->setText(QString::number(glat0,'f',7));
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLELat0 = pLE;
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText(QString::number(glon0,'f',7));
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLELon0 = pLE;
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText(QString::number(ghdg0,'f',3));
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLEHead0 = pLE;
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pPB = new QPushButton(pGroup);
- pPB->setText("Restore Default View");
- pPB->setGeometry(nXPos,nYPos,mnFontHeight*10,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDefView()));
- nXPos = nXPos + mnFontHeight * 11;
- pPB = new QPushButton(pGroup);
- pPB->setText("Zoom One");
- pPB->setGeometry(nXPos,nYPos,mnFontHeight*6,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickZoomOne()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pLabel = new QLabel(pGroup);
- pLabel->setText("Scale");
- pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- nXPos = nXPos + mnFontHeight * 4;
- pSlider = new QSlider(pGroup);
- pSlider->setOrientation(Qt::Horizontal);
- pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
- pSlider->setRange(1,100);
- pSlider->setValue(mnfac);
- connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeScale(int)));
- mpSlider_Scale = pSlider;
- nXPos = nXPos + mnFontHeight*11;
- pLE = new QLineEdit(pGroup);
- pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- pLE->setText(QString::number(pSlider->value()));
- mpLE_Scale = pLE;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setText("MoveX");
- pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- nXPos = nXPos + mnFontHeight * 4;
- pSlider = new QSlider(pGroup);
- pSlider->setOrientation(Qt::Horizontal);
- pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
- pSlider->setRange(0,100);
- pSlider->setValue(mnMoveX*100/VIEW_WIDTH);
- connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMoveX(int)));
- mpSlider_MoveX = pSlider;
- nXPos = nXPos + mnFontHeight*11;
- pLE = new QLineEdit(pGroup);
- pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- pLE->setText(QString::number(pSlider->value()));
- mpLE_MoveX = pLE;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setText("MoveY");
- pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- nXPos = nXPos + mnFontHeight * 4;
- pSlider = new QSlider(pGroup);
- pSlider->setOrientation(Qt::Horizontal);
- pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
- pSlider->setRange(0,100);
- pSlider->setValue(mnMoveY*100/VIEW_HEIGHT);
- connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMoveY(int)));
- mpSlider_MoveY = pSlider;
- nXPos = nXPos + mnFontHeight*11;
- pLE = new QLineEdit(pGroup);
- pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- pLE->setText(QString::number(pSlider->value()));
- mpLE_MoveY = pLE;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pLabel = new QLabel(pGroup);
- pLabel->setText("Mark");
- pLabel->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- nXPos = nXPos + mnFontHeight * 4;
- pSlider = new QSlider(pGroup);
- pSlider->setOrientation(Qt::Horizontal);
- pSlider->setGeometry(nXPos,nYPos,mnFontHeight * 10,nLEHeight);
- pSlider->setRange(5,100);
- pSlider->setValue(mnMarkSize);
- connect(pSlider,SIGNAL(valueChanged(int)),this,SLOT(onChangeMark(int)));
- mpSlider_Mark = pSlider;
- nXPos = nXPos + mnFontHeight*11;
- pLE = new QLineEdit(pGroup);
- pLE->setGeometry(nXPos,nYPos,mnFontHeight*3,nLEHeight);
- pLE->setText(QString::number(pSlider->value()));
- mpLE_Mark = pLE;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pLabel = new QLabel(pGroup);
- pLabel->setText("SelLon");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setText("SelLat");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLE = new QLineEdit(pGroup);
- pLE->setText("");
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLE_SelX = pLE;
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText("");
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLE_SelY = pLE;
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLE = new QLineEdit(pGroup);
- pLE->setText("");
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLE_SelLon = pLE;
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText("");
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLE_SelLat = pLE;
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText("");
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLE->setText("360");
- mpLE_StartHeading = pLE;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pPB = new QPushButton(pGroup);
- pPB->setText("Load Lane");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLoadLane()));
- nXPos = nXPos + nSpace;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBLane = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("Remove");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRemoveLane()));
- nXPos = nXPos + nSpace;
- pPB = new QPushButton(pGroup);
- pPB->setText("Mark");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickMarkLane()));
- nXPos = nXPos + nSpace;
- pPB = new QPushButton(pGroup);
- pPB->setText("Clear Road Lane");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickClearRoadLane()));
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("To Road");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickToRoad()));
- nXPos = nXPos + nSpace;
- pPB = new QPushButton(pGroup);
- pPB->setText("To Opposite");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickToOpposite()));
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpLE_RoadName = pLE;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBSelLane = pCB;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBSelOpLane = pCB;
- pPB = new QPushButton(pGroup);
- pPB->setText("Add Road");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickAddRoad()));
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBRoad = pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBRoadChange(int)));
- pPB = new QPushButton(pGroup);
- pPB->setText("Mark");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadMark()));
- nXPos = nXPos + nSpace;
- pPB = new QPushButton(pGroup);
- pPB->setText("Del");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadDel()));
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("Predecessor:");
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("road");
- mpLabelRoadShowPreType1 = pLabel;
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("10010");
- mpLabelRoadShowPreID = pLabel;
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("start");
- mpLabelRoadShowPreType2 = pLabel;
- nXPos = nXPos + nSpace;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
- mpCBRoadShowPre = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("Successor:");
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("road");
- mpLabelRoadShowNxtType1 = pLabel;
- nXPos = nXPos + nSpace;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("10010");
- mpLabelRoadShowNxtID = pLabel;
- nXPos = nXPos + nSpace;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- pLabel->setText("start");
- mpLabelRoadShowNxtType2 = pLabel;
- nXPos = nXPos + nSpace;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
- mpCBRoadShowNext = pCB;
- // nXPos = 10;
- // nYPos = nYPos + mnFontHeight * 2;
- // pCB = new QComboBox(pGroup);
- // pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- // nXPos = nXPos + nSpace;
- // mpCBPreNxtCurLane = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBPreNxtRoad = pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickPreNxtRoadChange(int)));
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBPreNxtRelLane = pCB;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBPreNxtConatact = pCB;
- pCB->addItem("start");
- pCB->addItem("end");
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- pPB->setText("Set Predecessor");
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetRoadPredecessor()));
- pPB = new QPushButton(pGroup);
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- pPB->setText("Set Successor");
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetRoadSuccessor()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- pPB->setText("Del Predecessor");
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDelRoadPredecessor()));
- pPB = new QPushButton(pGroup);
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- pPB->setText("Del Successor");
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDelRoadSuccessor()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBRoad1 = pCB;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pCB->addItem("start");
- pCB->addItem("end");
- mpCBRC1 = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBRoad2 = pCB;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pCB->addItem("start");
- pCB->addItem("end");
- mpCBRC2 = pCB;
- pPB = new QPushButton(pGroup);
- pPB->setText("Road Contact");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickRoadContact()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth*2,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBRoadCon = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBLane1 = pCB;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBLane2 = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("Lane Contact");
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLaneContact()));
- nXPos = nXPos + nSpace*15/10;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBLane1Lane2 = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("OpLane Contact");
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickOpLaneContact()));
- nXPos = nXPos + nSpace*15/10;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBLane1Lane2op = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("Clear");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pPB = new QPushButton(pGroup);
- pPB->setText("Create Road");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateRoad()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBRoadType = pCB;
- pCB->addItem("Turn");
- pCB->addItem("Straight");
- pCB->addItem("U-Turn");
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onChangeRoadType(int)));
- pLabel = new QLabel(pGroup);
- pLabel->setText("Radius:");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpLBRoadType = pLabel;
- pLE = new QLineEdit(pGroup);
- pLE->setText("6.0");
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLERoadType = pLE;
- mpCBRoadType->setCurrentIndex(0);
- mpLBRoadType->setVisible(true);
- mpLERoadType->setVisible(true);
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pPB = new QPushButton(pGroup);
- pPB->setText("Create Junction");
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateJunction()));
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBJunction = pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionChange(int)));
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth*5/10,nLEHeight);
- mpCBJunctionConnection = pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionConnectionChange(int)));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpLabelJunctionIncommingRoad = pLabel;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpLabelJunctionContactPoint = pLabel;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpLabelJunctionConnectingRoad = pLabel;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpCBJunctionFromTo = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pLabel->setText("Incomming");
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBJunctionIncommingRoad = pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionIncommingChange(int)));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pLabel->setText("Connecting");
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBJunctionConnectingRoad= pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onClickCBJunctionConnectionroadChange(int)));
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pCB->addItem("start");
- pCB->addItem("end");
- mpCBJunctionContactPoint = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBJunctionFromLane = pCB;
- pCB = new QComboBox(pGroup);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- mpCBJunctionToLane = pCB;
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("Create Lane Link");
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickCreateJunctionLaneLink()));
- pPB = new QPushButton(pGroup);
- pPB->setText("Delete Lane Link");
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickDeleteJunctionLaneLink()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pPB = new QPushButton(pGroup);
- pPB->setText("Auto Connect Road");
- pPB->setGeometry(nXPos,nYPos,nLEWidth*15/10,nLEHeight);
- nXPos = nXPos + nSpace*15/10;
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickAutoConnect()));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 4;
- pPB = new QPushButton(pGroup);
- pPB->setText("Save");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSave()));
- nXPos = nXPos + nSpace;
- pPB = new QPushButton(pGroup);
- pPB->setText("Load");
- pPB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickLoad()));
- nXPos = nXPos + nSpace;
- QScrollArea * pScroll = new QScrollArea();
- pScroll->setWidget(pGroup);
- p->addTab(pScroll,"Calculate");
- }
- void MainWindow::onClickXY(double x, double y)
- {
- mClickX = x - mnMoveX;
- mClickY = y - mnMoveY;
- mbClick = true;
- mbRefresh = true;
- double selx,sely;
- double lon,lat;
- selx = mClickX;
- sely = mClickY * (-1);
- selx = selx/((double )mnfac);
- sely = sely/((double)mnfac);
- mpLE_SelX->setText(QString::number(selx,'f',3));
- mpLE_SelY->setText(QString::number(sely,'f',3));
- double x0,y0;
- GaussProjCal(glon0,glat0,&x0,&y0);
- GaussProjInvCal(x0+selx,y0+sely,&lon,&lat);
- mpLE_SelLon->setText(QString::number(lon,'f',7));
- mpLE_SelLat->setText(QString::number(lat,'f',7));
- update();
- }
- void MainWindow::onChangeScale(int scale)
- {
- mnfac = scale;
- mpLE_Scale->setText(QString::number(scale));
- mbRefresh = true;
- update();
- }
- void MainWindow::onChangeMoveX(int scale)
- {
- mnMoveX = VIEW_WIDTH * scale/100;
- mpLE_MoveX->setText(QString::number(scale));
- mbRefresh = true;
- update();
- }
- void MainWindow::onChangeMoveY(int scale)
- {
- mnMoveY = VIEW_HEIGHT * scale/100;
- mpLE_MoveY->setText(QString::number(scale));
- mbRefresh = true;
- update();
- }
- void MainWindow::onChangeMark(int scale)
- {
- mnMarkSize = scale;
- mpLE_Mark->setText(QString::number(scale));
- mbRefresh = true;
- update();
- }
- void MainWindow::onClickDefView()
- {
- mnfac = mnDefmnfac;
- mnMoveX = mnDefMoveX;
- mnMoveY = mnDefMoveY;
- mpSlider_Scale->setValue(mnfac);
- mpSlider_MoveX->setValue(mnMoveX*100/VIEW_WIDTH);
- mpSlider_MoveY->setValue(mnMoveY*100/VIEW_HEIGHT);
- // mbRefresh = true;
- update();
- }
- void MainWindow::onClickZoomOne()
- {
- myview->zoomone();
- }
- void MainWindow::onClickAsStart()
- {
- }
- void MainWindow::onClickAsDst()
- {
- double lon,lat;
- lon = mpLE_SelLon->text().toDouble();
- lat = mpLE_SelLat->text().toDouble();
- mpLE_DstLon->setText(QString::number(lon,'f',7));
- mpLE_DstLat->setText(QString::number(lat,'f',7));
- double x,y;
- x = mpLE_SelX->text().toDouble();
- y = mpLE_SelY->text().toDouble();
- mfObjX = x;
- mfObjY = y;
- mbSetObj = true;
- mbRefresh = true;
- update();
- }
- void MainWindow::onClickSetDst()
- {
- }
- void MainWindow::onClickPlan()
- {
- }
- void MainWindow::onClickSetStart()
- {
- }
- /**
- * @brief ADCIntelligentVehicle::UpdateMap
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void MainWindow::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- // std::cout<<"update map "<<std::endl;
- int gpsunitsize = sizeof(iv::GPS_INS);
- int nMapSize = nSize/gpsunitsize;
- // std::cout<<"map size is "<<nMapSize<<std::endl;
- if(nMapSize < 1)return;
- int i;
- mMutexNavi.lock();
- m_navigation_data.clear();
- for(i=0;i<nMapSize;i++)
- {
- iv::GPS_INS x;
- memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
- iv::GPSData data(new iv::GPS_INS);
- *data = x;
- m_navigation_data.push_back(data);
- }
- mMutexNavi.unlock();
- mbRefresh = true;
- update();
- }
- /**
- * @brief MainWindow::UpdateGPSIMU
- * @param strdata
- * @param nSize
- * @param index
- * @param dt
- * @param strmemname
- */
- void MainWindow::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
- {
- iv::gps::gpsimu xgpsimu;
- if(!xgpsimu.ParseFromArray(strdata,nSize))
- {
- // gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
- return;
- }
- double flat,flon,fhead;
- flat = xgpsimu.lat();
- flon = xgpsimu.lon();
- fhead = xgpsimu.heading();
- double x,y;
- GaussProjCal(flon,flat,&x,&y);
- mfNowX = x - mx0;
- mfNowY = y- my0;
- mfNowHdg = (90- fhead)*M_PI/180.0;
- mbGPSUpdate = true;
- mbGPSViewUpdate = true;
- }
- void MainWindow::onClickLoadLane()
- {
- QString str = QFileDialog::getOpenFileName(this,tr("Open Lane Info file"),"",tr("Lane File(*.txt)"));
- if(str.isEmpty())return;
- QFile xFile;
- xFile.setFileName(str);
- QFileInfo fi(str);
- QString filename = fi.fileName();
- double fLastLat = 39;
- double fLastLon = 117;
- if(filename.contains(".txt"))
- {
- filename = filename.left(filename.length() -4);
- }
- if(xFile.open(QIODevice::ReadWrite))
- {
- iv::lpunit lpu;
- strncpy(lpu.strlanename,filename.toLatin1().data(),filename.size());
- QByteArray ba;
- ba = xFile.read(xFile.size());
- QString strdata = ba;
- QStringList strlinedata= strdata.split("\n",QString::SkipEmptyParts);
- int i;
- int nsize = strlinedata.size();
- // qDebug("line is %d",nsize);
- for(i=0;i<nsize;i++)
- {
- QString strline = strlinedata.at(i);
- QStringList strvaluedata = strline.split(" ",QString::SkipEmptyParts);
- // qDebug("%d value size is %d ",i,strvaluedata.size());
- iv::lanepoint lpleft,lpright;
- if(strvaluedata.size() == 12)
- {
- int j;
- QString strx[12];
- for(j=0;j<12;j++)strx[j] = strvaluedata.at(j);
- lpleft.mfLon = strx[0].toDouble();
- lpleft.mfLat = strx[1].toDouble();
- lpleft.mfHeight = strx[2].toDouble();
- lpleft.nLaneType = strx[3].toInt();
- lpleft.mfHeading = strx[4].toDouble();
- lpleft.mfDis = strx[5].toDouble();
- lpright.mfLon = strx[6].toDouble();
- lpright.mfLat = strx[7].toDouble();
- lpright.mfHeight = strx[8].toDouble();
- lpright.nLaneType = strx[9].toInt();
- lpright.mfHeading = strx[10].toDouble();
- lpright.mfDis = strx[11].toDouble();
- lpleft.mfHdg = (90-lpleft.mfHeading) * M_PI/180.0;
- if(lpleft.mfHdg < 0)lpleft.mfHdg = lpleft.mfHdg + 2.0*M_PI;
- lpright.mfHdg = (90-lpright.mfHeading) * M_PI/180.0;
- if(lpright.mfHdg < 0)lpright.mfHdg = lpright.mfHdg + 2.0*M_PI;
- double x,y;
- double xlast,ylast;
- GaussProjCal(lpleft.mfLon,lpleft.mfLat,&x,&y);
- GaussProjCal(fLastLon,fLastLat,&xlast,&ylast);
- if(sqrt(pow(xlast -x ,2)+pow(ylast -y,2))> 0.1)
- {
- lpu.mvectorlpleft.push_back(lpleft);
- lpu.mvectorlpright.push_back(lpright);
- fLastLat = lpleft.mfLat;
- fLastLon = lpleft.mfLon;
- }
- else
- {
- // qDebug("no use this point");
- }
- }
- }
- if((mbSetOrigin == false)&&(lpu.mvectorlpleft.size() > 0))
- {
- glat0 = lpu.mvectorlpleft[0].mfLat;
- glon0 = lpu.mvectorlpright[0].mfLon;
- mpLELat0->setText(QString::number(glat0,'f',7));
- mpLELon0->setText(QString::number(glon0,'f',7));
- mbSetOrigin = true;
- }
- double x0,y0;
- GaussProjCal(glon0,glat0,&x0,&y0);
- int j;
- for(j=0;j<lpu.mvectorlpleft.size();j++)
- {
- double x,y;
- GaussProjCal(lpu.mvectorlpleft[j].mfLon,lpu.mvectorlpleft[j].mfLat,&x,&y);
- lpu.mvectorlpleft[j].mfX = x - x0;
- lpu.mvectorlpleft[j].mfY = y- y0;
- GaussProjCal(lpu.mvectorlpright[j].mfLon,lpu.mvectorlpright[j].mfLat,&x,&y);
- lpu.mvectorlpright[j].mfX = x - x0;
- lpu.mvectorlpright[j].mfY = y- y0;
- }
- if(lpu.mvectorlpleft.size() > 0)
- {
- mvectorlp.push_back(lpu);
- mpCBLane->addItem(filename);
- int N = lpu.mvectorlpleft.size();
- VectorXd x_veh(N);
- VectorXd y_veh(N);
- int j;
- int ntype = 0;
- int ntypecount = 1;
- for(j=0;j<N;j++)
- {
- double x,y;
- GaussProjCal(glon0,glat0,&x0,&y0);
- GaussProjCal(lpu.mvectorlpleft[j].mfLon,lpu.mvectorlpleft[j].mfLat,&x,&y);
- x_veh[j] = x - x0;
- y_veh[j] = y - y0;
- if(lpu.mvectorlpright[j].nLaneType != ntype)
- {
- ntype = lpu.mvectorlpright[j].nLaneType;
- ntypecount++;
- }
- }
- // CreateRoad(x_veh,y_veh);
- // auto coeffs = polyfit(x_veh, y_veh, 1);
- // double dismax = 0;
- // for(j=0;j<N;j++)
- // {
- // double A = coeffs[1];
- // double B = -1;
- // double C = coeffs[0];
- // double dis = fabs(A*x_veh[j] + B*y_veh[j] +C )/sqrt(pow(A,2)+pow(B,2));
- // if(dis>dismax)dismax = dis;
- // }
- // int nxxx = coeffs.size();
- }
- }
- xFile.close();
- mbRefresh = true;
- update();
- }
- /**
- * @brief MainWindow::CreateRoad
- * Create Road Geometry
- * @param xvals
- * @param yvals
- * @return
- */
- Road MainWindow::CreateRoad(VectorXd xvals, VectorXd yvals)
- {
- // Road xRoad;
- // double LINE_ERROR = 0.15;
- // int nsize = xvals.size();
- // int nnotfit = nsize;
- // int ncurpos = 0;
- // int nrange = nsize;
- // while(ncurpos < nsize)
- // {
- // // int N = nrange - ncurpos;
- // VectorXd x_veh(nrange);
- // VectorXd y_veh(nrange);
- // int j;
- // for(j=ncurpos;j<(ncurpos +nrange);j++)
- // {
- // x_veh[j-ncurpos] = xvals[j];
- // y_veh[j - ncurpos] = yvals[j];
- // }
- // auto coeffs = polyfit(x_veh, y_veh, 1);
- // double dismax = 0;
- // for(j=ncurpos;j<(ncurpos +nrange);j++)
- // {
- // double A = coeffs[1];
- // double B = -1;
- // double C = coeffs[0];
- // double dis = fabs(A*x_veh[j-ncurpos] + B*y_veh[j-ncurpos] +C )/sqrt(pow(A,2)+pow(B,2));
- // if(dis>dismax)dismax = dis;
- // }
- // std::cout<<"dis is "<<dismax<<std::endl;
- // if((dismax > LINE_ERROR)&&((nsize -(nrange+ncurpos))>50))
- // {
- // nrange = nrange/2;
- // }
- // else
- // {
- // std::cout<<"nrange is "<<nrange<<std::endl;
- // xRoad.AddGeometryBlock();
- // GeometryBlock * pgb = xRoad.GetGeometryBlock(xRoad.GetGeometryBlockCount()-1);
- // // pgb->AddGeometryLine(1.0,x,y,len);
- // ncurpos = ncurpos + nrange;
- // nrange = nsize - ncurpos;
- // std::cout<<"add a geo."<<std::endl;
- // }
- // }
- // return xRoad;
- }
- void MainWindow::onClickMarkLane()
- {
- mnMarkLane = mpCBLane->currentIndex();
- mbRefresh = true;
- update();
- }
- void MainWindow::onClickRemoveLane()
- {
- int i = mpCBLane->currentIndex();
- if((i>=0) && (i<mvectorlp.size()))
- {
- int j;
- for(j=0;j<mvectorroadlane.size();j++)
- {
- if(mvectorroadlane[j] == i)
- {
- mvectorroadlane.erase(mvectorroadlane.begin() + j);
- mpCBSelLane->removeItem(j);
- break;
- }
- }
- mvectorlp.erase(mvectorlp.begin() + i);
- mpCBLane->removeItem(i);
- mbRefresh = true;
- update();
- }
- }
- void MainWindow::onClickToRoad()
- {
- int i = mpCBLane->currentIndex();
- if((i>=0) && (i<mvectorlp.size()))
- {
- int j;
- bool bHave = false;
- for(j=0;j<mvectorroadlane.size();j++)
- {
- if(mvectorroadlane[j] == i)
- {
- bHave = true;
- break;
- }
- }
- if(bHave == false)
- {
- mvectorroadlane.push_back(i);
- mpCBSelLane->addItem(mvectorlp.at(i).strlanename);
- }
- }
- }
- void MainWindow::onClickToOpposite()
- {
- int i = mpCBLane->currentIndex();
- if((i>=0) && (i<mvectorlp.size()))
- {
- int j;
- bool bHave = false;
- for(j=0;j<mvectorroadopposite.size();j++)
- {
- if(mvectorroadopposite[j] == i)
- {
- bHave = true;
- break;
- }
- }
- if(bHave == false)
- {
- mvectorroadopposite.push_back(i);
- mpCBSelOpLane->addItem(mvectorlp.at(i).strlanename);
- }
- }
- }
- void MainWindow::onClickClearRoadLane()
- {
- mvectorroadlane.clear();
- mvectorroadopposite.clear();
- mpCBSelLane->clear();
- mpCBSelOpLane->clear();
- }
- void MainWindow::onClickAddRoad()
- {
- if(mvectorroadlane.size() < 1)return;
- int nlanesize = mvectorroadlane.size();
- int noplanesize = mvectorroadopposite.size();
- double notlinethresh = 1.0; //When heading change more than this value, is a arc.
- int nsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
- int * pntype = new int[nsize];
- std::shared_ptr<int> ppntype; ppntype.reset(pntype);
- std::vector<iv::lanepoint > * pvectorlp = &(mvectorlp[mvectorroadlane[0]].mvectorlpleft);
- int i;
- //Go throuh point for fit type.
- for(i=0;i<5;i++)pntype[i] = 0;
- for(i=5;i<(nsize-5);i++)
- {
- double s = pvectorlp->at(i).mfDis;
- double head0 = pvectorlp->at(i).mfHeading;
- int j;
- double xcount = 0;
- double headdifftotal = 0;
- double headdiffavg = 0.0;
- for(j=1;j<nsize;j++)
- {
- if((j>1)&&((pvectorlp->at(j).mfDis - s)>1.0))
- {
- break;
- }
- double headdiff = pvectorlp->at(j).mfHeading - head0;
- if(headdiff > 300)headdiff = headdiff - 360;
- if(headdiff < -300)headdiff = headdiff + 360;
- headdifftotal = headdifftotal + headdiff;
- xcount = xcount + 1.0;
- }
- if(xcount > 0)headdiffavg = headdifftotal/xcount;
- if(fabs(headdiffavg) > (notlinethresh*2))
- {
- pntype[i] = 1;
- }
- else
- {
- pntype[i] = 0;
- }
- }
- for(i=(nsize -5);i<nsize;i++)
- {
- pntype[i] = 0;
- }
- for(i=0;i<nsize;i++) //Remove Few Not Line Point.
- {
- if(pntype[i] != 0)
- {
- int j;
- int ncount = 1;
- for(j=(i+1);j<nsize;j++)
- {
- if(pntype[i] != 0)ncount++;
- else break;
- }
- if(ncount < 10)pntype[i] = 0;
- else
- {
- i = i+ ncount;
- }
- }
- }
- for(i=5;i<nsize;i++)
- {
- if(pntype[i] != pntype[i-1])
- {
- int j = i-1;
- double disx = pvectorlp->at(i).mfDis;
- for(j=(i-2);j>5;j--)
- {
- if(pntype[j] == pntype[i-1])
- {
- pntype[j] = 2; //Besel
- }
- if(fabs(pvectorlp->at(j).mfDis - disx)>1) //1 m besel
- {
- break;
- }
- }
- pntype[i-1] = 2;
- }
- }
- //Calc dis to ref line
- double xor0,yor0;
- GaussProjCal(glon0,glat0,&xor0,&yor0);
- std::vector<std::vector<iv::distogeo>> xvectordiss;
- std::vector<std::vector<iv::distogeo>> xvectoropdiss;
- for(i=0;i<mvectorroadlane.size();i++)
- {
- std::vector<iv::distogeo> xvectordistogeo;
- int j;
- int nsizepoint = mvectorlp[mvectorroadlane[i]].mvectorlpright.size();
- bool bHaveLast = false;
- int nLast = 0;
- for(j=0;j<nsizepoint;j++)
- {
- double fdismin = 1000;
- double fS = 0;
- int k;
- double x,y;
- double x0,y0;
- iv::distogeo xdistogeo;
- int nLastBig = 0;
- bool bFindDisMin = false;
- x = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfX;
- y = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfY;
- int kstart = 0;
- if(bHaveLast)
- {
- kstart= nLast;
- }
- for(k=kstart;k<mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();k++)
- {
- x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
- y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
- double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
- if(fdismin > fdis)
- {
- fdismin = fdis;
- fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
- bFindDisMin = true;
- nLast = k;
- }
- if((bFindDisMin)&&(bHaveLast))
- {
- if(fdis>fdismin)
- {
- nLastBig++;
- }
- else
- {
- nLastBig = 0;
- }
- }
- if(nLastBig > 10)break;
- }
- // std::cout<<" k is "<<k<<std::endl;
- if(bHaveLast == false)
- {
- if(fdismin<10)bHaveLast = true;
- }
- else
- {
- if(fdismin > 10)bHaveLast = false;
- }
- double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
- mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
- double fHdgDiff = fHdg - mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfHdg;
- if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
- if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
- if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
- {
- xdistogeo.mfdis = fdismin;
- xdistogeo.mfs = fS;
- mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS = fS;
- }
- else
- {
- xdistogeo.mfs = -1;
- std::cout<<"point extend."<<std::endl;
- mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS = -1;
- }
- xvectordistogeo.push_back(xdistogeo);
- }
- xvectordiss.push_back(xvectordistogeo);
- }
- for(i=1;i<mvectorroadlane.size();i++)
- {
- std::vector<iv::distogeo> xvectordistogeo;
- int j;
- int nsizepoint = mvectorlp[mvectorroadlane[i]].mvectorlpleft.size();
- bool bHaveLast = false;
- int nLast = 0;
- for(j=0;j<nsizepoint;j++)
- {
- double fdismin = 1000;
- double fS = 0;
- int k;
- double x,y;
- double x0,y0;
- iv::distogeo xdistogeo;
- int nLastBig = 0;
- bool bFindDisMin = false;
- x = mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfX;
- y = mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfY;
- int kstart = 0;
- if(bHaveLast)
- {
- kstart= nLast;
- }
- for(k=kstart;k<mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();k++)
- {
- x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
- y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
- double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
- if(fdismin > fdis)
- {
- fdismin = fdis;
- fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
- bFindDisMin = true;
- nLast = k;
- }
- if((bFindDisMin)&&(bHaveLast))
- {
- if(fdis>fdismin)
- {
- nLastBig++;
- }
- else
- {
- nLastBig = 0;
- }
- }
- if(nLastBig > 10)break;
- }
- // std::cout<<" k is "<<k<<std::endl;
- if(bHaveLast == false)
- {
- if(fdismin<10)bHaveLast = true;
- }
- else
- {
- if(fdismin > 10)bHaveLast = false;
- }
- double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
- mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
- double fHdgDiff = fHdg - mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfHdg;
- if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
- if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
- if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 3.2))
- {
- xdistogeo.mfdis = fdismin;
- xdistogeo.mfs = fS;
- mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfRelS = fS;
- }
- else
- {
- xdistogeo.mfs = -1;
- std::cout<<"point extend."<<std::endl;
- mvectorlp[mvectorroadlane[i]].mvectorlpleft[j].mfRelS = -1;
- }
- xvectordistogeo.push_back(xdistogeo);
- }
- xvectordiss.push_back(xvectordistogeo);
- }
- for(i=0;i<mvectorroadopposite.size();i++)
- {
- std::vector<iv::distogeo> xvectordistogeo;
- int j;
- int nsizepoint = mvectorlp[mvectorroadopposite[i]].mvectorlpleft.size();
- bool bHaveLast = false;
- int nLast = 0;
- for(j=0;j<nsizepoint;j++)
- {
- double fdismin = 1000;
- double fS = 0;
- int k;
- double x,y;
- double x0,y0;
- iv::distogeo xdistogeo;
- int nLastBig = 0;
- bool bFindDisMin = false;
- x = mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfX;
- y = mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfY;
- int nrefsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
- int kstart = nrefsize-1;
- if(bHaveLast)
- {
- kstart= nLast;
- }
- for(k=kstart;k>=0;k--)
- {
- x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
- y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
- double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
- if(fdismin > fdis)
- {
- fdismin = fdis;
- fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
- bFindDisMin = true;
- nLast = k;
- }
- if((bFindDisMin)&&(bHaveLast))
- {
- if(fdis>fdismin)
- {
- nLastBig++;
- }
- else
- {
- nLastBig = 0;
- }
- }
- if(nLastBig > 10)break;
- }
- // std::cout<<" k is "<<k<<std::endl;
- if(bHaveLast == false)
- {
- if(fdismin<10)bHaveLast = true;
- }
- else
- {
- if(fdismin > 10)bHaveLast = false;
- }
- double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
- mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
- double fHdgDiff = fHdg - mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfHdg;
- if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
- if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
- if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
- {
- xdistogeo.mfdis = fdismin;
- xdistogeo.mfs = fS;
- mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfRelS = fS;
- }
- else
- {
- xdistogeo.mfs = -1;
- std::cout<<"point extend."<<std::endl;
- mvectorlp[mvectorroadopposite[i]].mvectorlpleft[j].mfRelS = -1;
- }
- xvectordistogeo.push_back(xdistogeo);
- }
- xvectoropdiss.push_back(xvectordistogeo);
- }
- for(i=0;i<mvectorroadopposite.size();i++)
- {
- std::vector<iv::distogeo> xvectordistogeo;
- int j;
- int nsizepoint = mvectorlp[mvectorroadopposite[i]].mvectorlpright.size();
- bool bHaveLast = false;
- int nLast = 0;
- for(j=0;j<nsizepoint;j++)
- {
- double fdismin = 1000;
- double fS = 0;
- int k;
- double x,y;
- double x0,y0;
- iv::distogeo xdistogeo;
- int nLastBig = 0;
- bool bFindDisMin = false;
- x = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfX;
- y = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfY;
- int nrefsize = mvectorlp[mvectorroadlane[0]].mvectorlpleft.size();
- int kstart = nrefsize-1;
- if(bHaveLast)
- {
- kstart= nLast;
- }
- for(k=kstart;k>=0;k--)
- {
- x0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfX;
- y0 = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfY;
- double fdis = sqrt(pow(x-x0,2)+pow(y-y0,2));
- if(fdismin > fdis)
- {
- fdismin = fdis;
- fS = mvectorlp[mvectorroadlane[0]].mvectorlpleft[k].mfDis;
- bFindDisMin = true;
- nLast = k;
- }
- if((bFindDisMin)&&(bHaveLast))
- {
- if(fdis>fdismin)
- {
- nLastBig++;
- }
- else
- {
- nLastBig = 0;
- }
- }
- if(nLastBig > 10)break;
- }
- // std::cout<<" k is "<<k<<std::endl;
- if(bHaveLast == false)
- {
- if(fdismin<10)bHaveLast = true;
- }
- else
- {
- if(fdismin > 10)bHaveLast = false;
- }
- double fHdg = geofit::CalcHdg(x,y,mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfX,
- mvectorlp[mvectorroadlane[0]].mvectorlpleft[nLast].mfY);
- double fHdgDiff = fHdg - mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfHdg;
- if(fHdgDiff < 0)fHdgDiff = fHdgDiff + 2.0*M_PI;
- if(fHdgDiff >= 2.0*M_PI)fHdgDiff = fHdgDiff - 2.0*M_PI;
- if(((fHdgDiff>(M_PI/2.0-0.3))&&(fHdgDiff<(M_PI/2.0+0.3)))||(fdismin < 0.5))
- {
- xdistogeo.mfdis = fdismin;
- xdistogeo.mfs = fS;
- mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS = fS;
- }
- else
- {
- xdistogeo.mfs = -1;
- std::cout<<"point extend."<<std::endl;
- mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS = -1;
- }
- xvectordistogeo.push_back(xdistogeo);
- }
- xvectoropdiss.push_back(xvectordistogeo);
- }
- //Get Lane Type
- std::vector<std::vector<iv::lanetype>> xvevelanetype;
- std::vector<std::vector<iv::lanetype>> xvectoroplanetype;
- for(i=0;i<nlanesize;i++)
- {
- std::vector<iv::lanetype> xvelanetype;
- int j;
- int nlasttype;
- iv::lanetype xlt;
- xlt.s = 0;
- xlt.ntype = 0;
- int nsizelp = mvectorlp[mvectorroadlane[i]].mvectorlpright.size();
- if(nsizelp > 0)
- {
- xlt.ntype = mvectorlp[mvectorroadlane[i]].mvectorlpright[0].nLaneType;
- nlasttype = xlt.ntype;
- }
- for(j=1;j<nsizelp;j++)
- {
- int nlt = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].nLaneType;
- if((nlt != nlasttype)&&(mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS != -1))
- {
- xvelanetype.push_back(xlt);
- xlt.ntype = nlt;
- xlt.s = mvectorlp[mvectorroadlane[i]].mvectorlpright[j].mfRelS;
- }
- nlasttype = nlt;
- }
- xvelanetype.push_back(xlt);
- xvevelanetype.push_back(xvelanetype);
- }
- for(i=0;i<noplanesize;i++)
- {
- std::vector<iv::lanetype> xvelanetype;
- int j;
- int nlasttype;
- iv::lanetype xlt;
- xlt.s = 0;
- xlt.ntype = 0;
- int nsizelp = mvectorlp[mvectorroadopposite[i]].mvectorlpright.size();
- if(nsizelp > 0)
- {
- xlt.ntype = mvectorlp[mvectorroadopposite[i]].mvectorlpright[nsizelp-1].nLaneType;
- nlasttype = xlt.ntype;
- }
- for(j=(nsizelp-2);j>=0;j--)
- {
- int nlt = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].nLaneType;
- if((nlt != nlasttype)&&(mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS != -1))
- {
- xvelanetype.push_back(xlt);
- xlt.ntype = nlt;
- xlt.s = mvectorlp[mvectorroadopposite[i]].mvectorlpright[j].mfRelS;
- }
- nlasttype = nlt;
- }
- xvelanetype.push_back(xlt);
- xvectoroplanetype.push_back(xvelanetype);
- }
- //Get Lane Mark Width
- std::vector<double> xvectormarkwidth;
- std::vector<double> xvectoravgdis;
- for(i=0;i<xvectordiss.size();i++)
- {
- double fdis = 0;
- int j;
- int ncount = 0;
- for(j=0;j<xvectordiss[i].size();j++)
- {
- if(xvectordiss[i].at(j).mfs != -1)
- {
- fdis = fdis + xvectordiss[i].at(j).mfdis;
- ncount++;
- }
- }
- if(ncount>0)fdis = fdis/ncount;
- xvectoravgdis.push_back(fdis);
- }
- for(i=1;i<mvectorroadlane.size();i++)
- {
- xvectormarkwidth.push_back(xvectoravgdis[mvectorroadlane.size()+i-1] - xvectoravgdis[i-1]);
- }
- std::vector<double> xvectoropmarkwidth;
- std::vector<double> xvectoropavgdis;
- for(i=0;i<xvectoropdiss.size();i++)
- {
- double fdis = 0;
- int j;
- int ncount = 0;
- int nsize = xvectoropdiss[i].size();
- for(j=0;j<nsize;j++)
- {
- if(xvectoropdiss[i].at(j).mfs != -1)
- {
- fdis = fdis + xvectoropdiss[i].at(j).mfdis;
- ncount++;
- }
- }
- if(ncount>0)fdis = fdis/ncount;
- xvectoropavgdis.push_back(fdis);
- }
- if(xvectoropavgdis.size()>0)xvectoropmarkwidth.push_back(xvectoropavgdis[0]);
- for(i=1;i<mvectorroadopposite.size();i++)
- {
- xvectoropmarkwidth.push_back(xvectoropavgdis[i] - xvectoropavgdis[i+mvectorroadopposite.size()-1]);
- }
- //Get Lane Width
- std::vector<std::vector<iv::distogeo>> xvectorlanewidth;
- std::vector<std::vector<iv::distogeo>> xvectoroplanewidth;
- std::vector<iv::distogeo> xlanewidth;
- xlanewidth.clear();
- int j;
- for(j=0;j<xvectordiss[0].size();j++)
- {
- if(xvectordiss[0].at(j).mfs != -1)xlanewidth.push_back(xvectordiss[0].at(j));
- }
- xvectorlanewidth.push_back(xlanewidth);
- for(i=1;i<mvectorroadlane.size();i++)
- {
- xlanewidth.clear();
- for(j=0;j<xvectordiss[i].size();j++)
- {
- if((xvectordiss[i].at(j).mfs!=-1)&&(xvectordiss[i-1+nlanesize].at(j).mfs!=-1))
- {
- iv::distogeo xdisg;
- xdisg = xvectordiss[i].at(j);
- xdisg.mfdis = xdisg.mfdis - xvectordiss[i-1+nlanesize].at(j).mfdis + xvectormarkwidth[i-1];
- xlanewidth.push_back(xdisg);
- }
- }
- xvectorlanewidth.push_back(xlanewidth);
- }
- xlanewidth.clear();
- if(xvectoropmarkwidth.size()>0)
- {
- if(xvectoropmarkwidth[0] > 0.4)
- {
- int nsize = xvectoropdiss[0].size();
- for(j=0;j<nsize;j++)
- {
- if(xvectoropdiss[0].at(j).mfs != -1)xlanewidth.push_back(xvectoropdiss[0].at(j));
- }
- xvectoroplanewidth.push_back(xlanewidth);
- xlanewidth.clear();
- nsize = xvectoropdiss[noplanesize].size();
- for(j=0;j<nsize;j++)
- {
- if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
- {
- iv::distogeo xdisg;
- xdisg = xvectoropdiss[noplanesize].at(j);
- xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis ;
- xlanewidth.push_back(xdisg);
- }
- }
- xvectoroplanewidth.push_back(xlanewidth);
- }
- else
- {
- nsize = xvectoropdiss[noplanesize].size();
- for(j=0;j<nsize;j++)
- {
- if((xvectoropdiss[noplanesize].at(j).mfs!=-1)&&(xvectoropdiss[0].at(j).mfs!=-1))
- {
- iv::distogeo xdisg;
- xdisg = xvectoropdiss[noplanesize].at(j);
- xdisg.mfdis = xdisg.mfdis - xvectoropdiss[0].at(j).mfdis + xvectoropmarkwidth[0];
- xlanewidth.push_back(xdisg);
- }
- }
- xvectoroplanewidth.push_back(xlanewidth);
- }
- for(i=1;i<noplanesize;i++)
- {
- xlanewidth.clear();
- nsize = xvectoropdiss[noplanesize+i].size();
- for(j=0;j<nsize;j++)
- {
- if((xvectoropdiss[noplanesize+i].at(j).mfs != -1)&&(xvectoropdiss[i].at(j).mfs != -1))
- {
- iv::distogeo xdisg;
- xdisg = xvectoropdiss[noplanesize+i].at(j);
- xdisg.mfdis = xdisg.mfdis - xvectoropdiss[i].at(j).mfdis + xvectoropmarkwidth[i];
- xlanewidth.push_back(xdisg);
- }
- }
- xvectoroplanewidth.push_back(xlanewidth);
- }
- }
- std::vector<iv::lanecoff> xvectorlanecoff;
- std::vector<iv::lanecoff> xvectoroplanecoff;
- for(i=0;i<nlanesize;i++)
- {
- int N = xvectorlanewidth[i].size();
- VectorXd x_veh(N);
- VectorXd y_veh(N);
- for(j=0;j<N;j++)
- {
- x_veh[j] = xvectorlanewidth[i].at(j).mfs;
- y_veh[j] =xvectorlanewidth[i].at(j).mfdis;
- }
- VectorXd coeffs = polyfit(x_veh, y_veh, 3);
- iv::lanecoff xlanecoff;
- xlanecoff.A = coeffs[0];
- xlanecoff.B = coeffs[1];
- xlanecoff.C = coeffs[2];
- xlanecoff.D = coeffs[3];
- xvectorlanecoff.push_back(xlanecoff);
- }
- for(i=0;i<xvectoroplanewidth.size();i++)
- {
- int N = xvectoroplanewidth[i].size();
- VectorXd x_veh(N);
- VectorXd y_veh(N);
- for(j=0;j<N;j++)
- {
- x_veh[j] = xvectoroplanewidth[i].at(j).mfs;
- y_veh[j] =xvectoroplanewidth[i].at(j).mfdis;
- }
- VectorXd coeffs = polyfit(x_veh, y_veh, 3);
- iv::lanecoff xlanecoff;
- xlanecoff.A = coeffs[0];
- xlanecoff.B = coeffs[1];
- xlanecoff.C = coeffs[2];
- xlanecoff.D = coeffs[3];
- xvectoroplanecoff.push_back(xlanecoff);
- }
- iv::lanecoff xlaneheightcoff;
- int M = mvectorlp[0].mvectorlpleft.size();
- VectorXd x_vehhg(M);
- VectorXd y_vehhg(M);
- for(j=0;j<M;j++)
- {
- x_vehhg[j] = mvectorlp[0].mvectorlpleft.at(j).mfDis;
- y_vehhg[j] =mvectorlp[0].mvectorlpleft.at(j).mfHeight;
- }
- VectorXd coeffs = polyfit(x_vehhg, y_vehhg, 3);
- xlaneheightcoff.A = coeffs[0];
- xlaneheightcoff.B = coeffs[1];
- xlaneheightcoff.C = coeffs[2];
- xlaneheightcoff.D = coeffs[3];
- geofit xgeofit;
- int N = pvectorlp->size();
- VectorXd x_veh(N);
- VectorXd y_veh(N);
- VectorXi t_veh(N);
- double x0,y0;
- GaussProjCal(glon0,glat0,&x0,&y0);
- for(j=0;j<N;j++)
- {
- double x,y;
- GaussProjCal(pvectorlp->at(j).mfLon,pvectorlp->at(j).mfLat,&x,&y);
- x_veh[j] = x - x0;
- y_veh[j] = y - y0;
- t_veh[j] = pntype[j];
- }
- std::vector<geobase> xvectorgeo = xgeofit.getgeo(x_veh,y_veh,t_veh);
- // OpenDrive od;
- // std::string mapx = "map";
- // od.SetHeader(1,1,mapx,1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
- j= 0;
- double xroadlen = 0;
- for(j=0;j<xvectorgeo.size();j++)
- {
- xroadlen = xroadlen + xvectorgeo[j].mfLen;
- }
- mxodr.AddRoad(mpLE_RoadName->text().toLatin1().data(),xroadlen, QString::number(CreateRoadID()).toStdString(),"-1");
- Road * p = mxodr.GetRoad(mxodr.GetRoadCount() - 1);
- p->AddElevation(0,xlaneheightcoff.A,xlaneheightcoff.B,xlaneheightcoff.C,xlaneheightcoff.D);
- double s = 0;
- j= 0;
- // for(j=0;j<4;j++)
- for(j=0;j<xvectorgeo.size();j++)
- {
- p->AddGeometryBlock();
- GeometryBlock * pgb = p->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo[j].mnType)
- {
- case 0:
- pline = &xvectorgeo[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo[j];
- std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
- break;
- }
- s = s + xvectorgeo[j].mfLen;
- }
- p->AddLaneSection(0);
- LaneSection * pLS = p->GetLaneSection(0);
- Lane * pLL;
- pLS->SetS(0);
- pLS->AddLane(0,0,"none",false);
- pLL = pLS->GetLane(0);
- if(noplanesize != xvectoroplanewidth.size())
- {
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- }
- for(i=0;i<nlanesize;i++)
- {
- pLS->AddLane(-1,(i+1)*(-1),"driving",false,false);
- pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,xvectorlanecoff[i].A,xvectorlanecoff[i].B,
- xvectorlanecoff[i].C,xvectorlanecoff[i].D);
- int j;
- for(j=0;j<xvevelanetype[i].size();j++)
- {
- std::string strlanetype = "broken";
- if(xvevelanetype[i].at(j).ntype != 0)
- {
- strlanetype = "solid";
- }
- pLL->AddRoadMarkRecord(xvevelanetype[i].at(j).s,strlanetype,"standard","standard",0.15,"false");
- }
- }
- for(i=0;i<xvectoroplanewidth.size();i++)
- {
- pLS->AddLane(1,(i+1)*(1),"driving",false,false);
- pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
- if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
- {
- pLL->SetType("shoulder");
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- }
- pLL->AddWidthRecord(0,xvectoroplanecoff[i].A,xvectoroplanecoff[i].B,
- xvectoroplanecoff[i].C,xvectoroplanecoff[i].D);
- int j;
- int index = i;
- if((i==0)&&(noplanesize != xvectoroplanewidth.size()))
- {
- continue;
- }
- if(noplanesize != xvectoroplanewidth.size())
- {
- index = i-1;
- }
- else
- index = i;
- for(j=0;j<xvectoroplanetype[index].size();j++)
- {
- std::string strlanetype = "broken";
- if(xvectoroplanetype[index].at(j).ntype != 0)
- {
- strlanetype = "solid";
- }
- pLL->AddRoadMarkRecord(xvectoroplanetype[index].at(j).s,strlanetype,"standard","standard",0.15,"false");
- }
- }
- mpCBSelLane->clear();
- mpCBSelOpLane->clear();
- mvectorroadlane.clear();
- mvectorroadopposite.clear();
- updateCBRoad();
- mbRefresh = true;
- update();
- // OpenDriveXmlWriter x(&mxodr);
- // x.WriteFile("/home/nvidia/text.xodr");
- }
- int MainWindow::CreateRoadID(int ntype)
- {
- int i;
- bool bUsed = false;
- int nroadidstart = 10000;
- if(ntype == 1) //not create by lane roaid
- {
- nroadidstart = 20000;
- }
- int nroadcount = mxodr.GetRoadCount();
- if(nroadcount == 0)return nroadidstart;
- int * proadid = new int[nroadcount];
- for(i=0;i<nroadcount;i++)
- {
- proadid[i]=atoi(mxodr.GetRoad(i)->GetRoadId().data());
- }
- do
- {
- bUsed = false;
- for(i=0;i<nroadcount;i++)
- {
- if(proadid[i] == nroadidstart)
- {
- nroadidstart++;
- bUsed = true;
- }
- }
- }while(bUsed);
- delete proadid;
- return nroadidstart;
- }
- int MainWindow::CreateJunctionID()
- {
- int i;
- bool bUsed = false;
- int njunctionidstart = 800000;
- int njunctioncount = mxodr.GetJunctionCount();
- if(njunctioncount == 0)return njunctionidstart;
- int * pjunctionid = new int[njunctioncount];
- for(i=0;i<njunctioncount;i++)
- {
- pjunctionid[i]=atoi(mxodr.GetJunction(i)->GetId().data());
- }
- do
- {
- bUsed = false;
- for(i=0;i<njunctioncount;i++)
- {
- if(pjunctionid[i] == njunctionidstart)
- {
- njunctionidstart++;
- bUsed = true;
- }
- }
- }while(bUsed);
- delete pjunctionid;
- return njunctionidstart;
- }
- void MainWindow::onClickLoad()
- {
- QString str = QFileDialog::getOpenFileName(this,"Load XODR",".","*.xodr");
- if(str.isEmpty())return;
- OpenDrive * pxodr = new OpenDrive(); //because add to xodr,so don't delete
- OpenDriveXmlParser x(pxodr);
- if(!x.ReadFile(str.toStdString()))
- {
- QMessageBox::warning(this,"warn","Can't load xodr file.");
- return;
- }
- bool bNeedMove = false;
- bool bNeedAjustID = false;
- unsigned short int revMajor,revMinor;
- std::string name,date;
- float version;
- double north,south,east,west,lat0,lon0,hdg0;
- if(pxodr->GetHeader() != 0)
- {
- pxodr->GetHeader()->GetAllParams(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
- }
- double xMoveX = 0;
- double xMoveY = 0;
- if(mxodr.GetRoadCount() > 0)
- {
- bNeedAjustID = true;
- }
- if(mbSetOrigin == false)
- {
- bNeedMove = false;
- glat0 = lat0;
- glon0 = lon0;
- mpLELon0->setText(QString::number(glon0,'f',7));
- mpLELat0->setText(QString::number(glat0,'f',7));
- mbSetOrigin = true;
- }
- else
- {
- if((glat0 == lat0)&&(glon0 == lon0))
- {
- bNeedMove = false;
- }
- else
- {
- bNeedMove = true;
- double x0,y0,x1,y1;
- GaussProjCal(glon0,glat0,&x0,&y0);
- GaussProjCal(lon0,lat0,&x1,&y1);
- xMoveX = x1 - x0;
- xMoveY = y1 - y0;
- }
- }
- if(bNeedMove)
- {
- MoveXODR(pxodr,xMoveX,xMoveY);
- }
- int noldroadcount = mxodr.GetRoadCount();
- int noldjunctioncount = mxodr.GetJunctionCount();
- int nnewcount = pxodr->GetRoadCount();
- int nnewjunctioncount = pxodr->GetJunctionCount();
- int i;
- if(bNeedAjustID)
- {
- for(i=0;i<nnewcount;i++)
- {
- int nid = atoi(pxodr->GetRoad(i)->GetRoadId().data());
- int j;
- bool bNeedChange = false;
- for(j=0;j<noldroadcount;j++)
- {
- if(nid == atoi(mxodr.GetRoad(j)->GetRoadId().data()))
- {
- bNeedChange = true;
- break;
- }
- }
- if(bNeedChange)
- {
- int nnewid = FindNewRoadID(&mxodr,pxodr);
- ChangeXODRRoadID(pxodr,i,nnewid);
- }
- }
- for(i=0;i<nnewjunctioncount;i++)
- {
- int nid = atoi(pxodr->GetJunction(i)->GetId().data());
- int j;
- bool bNeedChange = false;
- for(j=0;j<noldjunctioncount;j++)
- {
- if(nid == atoi(mxodr.GetJunction(j)->GetId().data()))
- {
- bNeedChange = true;
- break;
- }
- }
- if(bNeedChange)
- {
- int nnewid = FindNewJunctionID(&mxodr,pxodr);
- ChangeXODRJunctionID(pxodr,i,nnewid);
- }
- }
- }
- for(i=0;i<nnewcount;i++)
- {
- mxodr.GetRoadVector()->push_back(pxodr->GetRoadVector()->at(i));
- // OpenDrive * px = &mxodr;
- }
- for(i=0;i<nnewjunctioncount;i++)
- {
- mxodr.GetJunctionVector()->push_back(pxodr->GetJunctionVector()->at(i));
- }
- updateCBRoad();
- updateJunction();
- mbRefresh = true;
- update();
- }
- void MainWindow::onClickSave()
- {
- QString str = QFileDialog::getSaveFileName(this,"Save XODR",".","*.xodr");
- if(str.isEmpty())return;
- if(str.indexOf(".xodr")<0)str = str + ".xodr";
- mxodr.SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
- OpenDriveXmlWriter x(&mxodr);
- x.WriteFile(str.toStdString());
- }
- void MainWindow::onClickRoadContact()
- {
- int ch1,ch2;
- ch1 = mpCBRoad1->currentIndex();
- ch2 = mpCBRoad2->currentIndex();
- if((ch1 == -1)||(ch2 == -1))
- {
- return;
- }
- // if(ch1 == ch2)
- // {
- // QMessageBox::warning(this,"warn","road same");
- // return;
- // }
- int ntype1,ntype2;
- ntype1 = mpCBRC1->currentIndex();
- ntype2 = mpCBRC2->currentIndex();
- Road * p1 = mxodr.GetRoad(ch1);
- Road * p2 = mxodr.GetRoad(ch2);
- iv::roadcontact rc;
- rc.mnroad1id = atoi(p1->GetRoadId().data());
- rc.mnroad2id = atoi(p2->GetRoadId().data());
- rc.mncon1 = ntype1;
- rc.mncon2 = ntype2;
- char strname[256];
- snprintf(strname,256,"%s_%s_%s_%s",p1->GetRoadId().data(),mpCBRC1->currentText().toLatin1().data(),
- p2->GetRoadId().data(),mpCBRC2->currentText().toLatin1().data());
- mpCBRoadCon->clear();
- mpCBRoadCon->addItem(strname);
- mpCBLane1->clear();
- mpCBLane2->clear();
- mpCBLane1Lane2->clear();
- mpCBLane1Lane2op->clear();
- if(p1->GetLaneSectionCount()>0)
- {
- LaneSection * pLS;
- if(ntype1 == 0)
- pLS = p1->GetLaneSection(0);
- else
- pLS = p1->GetLaneSection(p1->GetLaneSectionCount()-1);
- int i;
- for(i=0;i<pLS->GetLaneCount();i++)
- {
- Lane * pL = pLS->GetLane(i);
- // if((pL->GetId() != 0)&&(strncmp(pL->GetType().data(),"driving",255)==0))
- if(pL->GetId() != 0)
- mpCBLane1->addItem(QString::number(pL->GetId()));
- }
- }
- if(p2->GetLaneSectionCount()>0)
- {
- LaneSection * pLS;
- if(ntype1 == 0)
- pLS = p2->GetLaneSection(0);
- else
- pLS = p2->GetLaneSection(p1->GetLaneSectionCount()-1);
- int i;
- for(i=0;i<pLS->GetLaneCount();i++)
- {
- Lane * pL = pLS->GetLane(i);
- // if((pL->GetId() != 0)&&(strncmp(pL->GetType().data(),"driving",255)==0))
- if(pL->GetId() != 0)
- mpCBLane2->addItem(QString::number(pL->GetId()));
- }
- }
- mvectorrc.clear();
- mvectorrc.push_back(rc);
- }
- void MainWindow::onClickLaneContact()
- {
- int n1,n2;
- n1 = mpCBLane1->currentText().toInt();
- n2 = mpCBLane2->currentText().toInt();
- iv::lanecontact xlc;
- xlc.ml1 = n1;
- xlc.ml2 = n2;
- if(mvectorrc.size() < 1)return;
- mvectorrc[0].mvectorlc.push_back(xlc);
- char strname[256];
- snprintf(strname,255,"%dto%d",n1,n2);
- mpCBLane1Lane2->addItem(strname);
- }
- void MainWindow::onClickOpLaneContact()
- {
- int n1,n2;
- n1 = mpCBLane1->currentText().toInt();
- n2 = mpCBLane2->currentText().toInt();
- iv::lanecontact xlc;
- xlc.ml1 = n1;
- xlc.ml2 = n2;
- if(mvectorrc.size() < 1)return;
- mvectorrc[0].mvectorlcop.push_back(xlc);
- char strname[256];
- snprintf(strname,255,"%dto%d",n1,n2);
- mpCBLane1Lane2op->addItem(strname);
- }
- void MainWindow::onClickCreateRoad()
- {
- if(mvectorrc.size()<1)return;
- Road * p1, *p2;
- int nroad1index;
- int nroad2index;
- // p1 = mvectorrc[0].mp1;
- // p2 = mvectorrc[0].mp2;
- int i;
- bool bhavep1 = false;
- bool bhavep2 = false;
- for(i=0;i<mxodr.GetRoadCount();i++)
- {
- if(mvectorrc[0].mnroad1id == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
- {
- bhavep1 = true;
- p1 = mxodr.GetRoad(i);
- nroad1index = i;
- break;
- }
- }
- if(bhavep1 == false)
- {
- QMessageBox::warning(this,"Warn","Road not found.");
- return;
- }
- double off1,off2;
- for(i=0;i<mxodr.GetRoadCount();i++)
- {
- if(mvectorrc[0].mnroad2id == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
- {
- bhavep2 = true;
- p2 = mxodr.GetRoad(i);
- nroad2index = i;
- break;
- }
- }
- if(bhavep2 == false)
- {
- QMessageBox::warning(this,"Warn","Road not found.");
- return;
- }
- if(mvectorrc[0].mvectorlc.size()<1)
- {
- QMessageBox::warning(this,"warn","No Lane Contact.");
- return;
- }
- double startx,starty,starthdg;
- double endx,endy,endhdg;
- double startheight,endheight;
- bool bFromstart,bTostart;
- if(mvectorrc[0].mncon1 == 0)
- {
- bFromstart = true;
- starthdg = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
- off1 = getoff(p1,mvectorrc[0].mvectorlc[0].ml1,true);
- startx = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
- starty = p1->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
- startx = startx + off1 * cos(starthdg -M_PI/2.0);
- starty = starty + off1 * sin(starthdg -M_PI/2.0);
- startheight = 0;
- if(p1->GetElevationCount()>0)
- {
- startheight = p1->GetElevation(0)->GetA();
- }
- // if(mvectorrc[0].mvectorlc[0].ml1<0)
- starthdg = starthdg +M_PI;if(starthdg >=2.0*M_PI)starthdg = starthdg -2.0*M_PI;
- }
- else
- {
- bFromstart = false;
- if(GetEndPoint(p1,startx,starty,starthdg) != 0)
- {
- QMessageBox::warning(this,"warn","get start error.");
- return;
- }
- off1 = getoff(p1,mvectorrc[0].mvectorlc[0].ml1,false);
- startx = startx + off1 * cos(starthdg -M_PI/2.0);
- starty = starty + off1 * sin(starthdg -M_PI/2.0);
- startheight = 0;
- if(p1->GetElevationCount()>0)
- {
- startheight = p1->GetElevation(0)->GetA()
- +p1->GetElevation(0)->GetB() * pow(p1->GetRoadLength(),1)
- +p1->GetElevation(0)->GetC() * pow(p1->GetRoadLength(),2)
- +p1->GetElevation(0)->GetD() * pow(p1->GetRoadLength(),3);
- }
- }
- if(mvectorrc[0].mncon2 == 0)
- {
- bTostart = true;
- off2 = getoff(p2,mvectorrc[0].mvectorlc[0].ml2,true);
- endx = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetX();
- endy = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetY();
- endhdg = p2->GetGeometryBlock(0)->GetGeometryAt(0)->GetHdg();
- endx = endx + off2 * cos(endhdg -M_PI/2.0);
- endy = endy + off2 * sin(endhdg -M_PI/2.0);
- endheight = 0;
- if(p2->GetElevationCount()>0)
- {
- endheight = p2->GetElevation(0)->GetA();
- }
- }
- else
- {
- bTostart = false;
- off2 = getoff(p2,mvectorrc[0].mvectorlc[0].ml2,false);
- if(GetEndPoint(p2,endx,endy,endhdg) != 0)
- {
- QMessageBox::warning(this,"warn","get end error.");
- return;
- }
- endx = endx + off2 * cos(endhdg -M_PI/2.0);
- endy = endy + off2 * sin(endhdg -M_PI/2.0);
- endhdg = endhdg +M_PI;if(endhdg >=2.0*M_PI)endhdg = endhdg -2.0*M_PI;
- endheight = 0;
- if(p2->GetElevationCount()>0)
- {
- endheight = p2->GetElevation(0)->GetA()
- +p2->GetElevation(0)->GetB() * pow(p2->GetRoadLength(),1)
- +p2->GetElevation(0)->GetC() * pow(p2->GetRoadLength(),2)
- +p2->GetElevation(0)->GetD() * pow(p2->GetRoadLength(),3);
- }
- }
- //Create Geo
- double R = mpLERoadType->text().toDouble();
- std::vector<geobase> xvectorgeo;
- std::vector<geobase> xvectorgeo1,xvectorgeo2;
- switch(mpCBRoadType->currentIndex())
- {
- case 0:
- xvectorgeo = CreateTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
- break;
- case 1:
- xvectorgeo = CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
- break;
- case 2:
- xvectorgeo = CreateUTurnGeo(startx,starty,starthdg,endx,endy,endhdg,R);
- break;
- default:
- break;
- }
- if(mpCBRoadType->currentIndex() == 2)
- {
- for(i=0;i<xvectorgeo.size()/2;i++)
- {
- xvectorgeo1.push_back(xvectorgeo.at(i));
- }
- for(i=xvectorgeo.size()/2;i<xvectorgeo.size();i++)
- {
- xvectorgeo2.push_back(xvectorgeo.at(i));
- }
- }
- if(xvectorgeo.size() == 0)
- {
- QMessageBox::warning(this,"warn","Create Road Fail.");
- return;
- }
- double xroadlen = 0;
- if(mpCBRoadType->currentIndex() != 2)
- {
- for(i=0;i<xvectorgeo.size();i++)xroadlen = xroadlen + xvectorgeo[i].mfLen;
- mxodr.AddRoad("",xroadlen, QString::number(CreateRoadID()).toStdString(),"-1");
- Road * p = mxodr.GetRoad(mxodr.GetRoadCount() - 1);
- p->AddElevation(0,startheight,(endheight-startheight)/xroadlen,0,0);
- p1 = mxodr.GetRoad(nroad1index);
- p2 = mxodr.GetRoad(nroad2index);
- double s = 0;
- int j;
- j= 0;
- for(j=0;j<xvectorgeo.size();j++)
- {
- p->AddGeometryBlock();
- GeometryBlock * pgb = p->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo[j].mnType)
- {
- case 0:
- pline = &xvectorgeo[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo[j];
- std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
- break;
- }
- s = s + xvectorgeo[j].mfLen;
- }
- p->AddLaneSection(0);
- LaneSection * pLS = p->GetLaneSection(0);
- pLS->SetS(0);
- pLS->AddLane(0,0,"none",false);
- double * pswidth,*pewidth;
- std::vector<std::string> strvectorlanetype;
- int nlanecount = mvectorrc[0].mvectorlc.size();
- pswidth = new double[nlanecount];
- pewidth = new double[nlanecount];
- std::shared_ptr<double> ppswidth,ppewidth;
- ppswidth.reset(pswidth);
- ppewidth.reset(pewidth);
- for(i=0;i<nlanecount;i++)
- {
- pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
- strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
- }
- for(i=0;i<nlanecount;i++)
- {
- pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlc.at(i).ml2,bTostart);
- }
- double * pa,*pb;
- pa = new double[nlanecount];
- pb = new double[nlanecount];
- std::shared_ptr<double> ppa,ppb;
- ppa.reset(pa);
- ppb.reset(pb);
- for(i=0;i<nlanecount;i++)
- {
- pa[i] = pswidth[i];
- pb[i] = (pewidth[i] - pa[i])/xroadlen;
- }
- for(i=0;i<nlanecount;i++)
- {
- pLS->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
- Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,pa[i],pb[i],
- 0,0);
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- }
- int noplanecount = mvectorrc[0].mvectorlcop.size();
- if(noplanecount > 0)
- {
- pswidth = new double[noplanecount];
- pewidth = new double[noplanecount];
- ppswidth.reset(pswidth);
- ppewidth.reset(pewidth);
- strvectorlanetype.clear();
- for(i=0;i<noplanecount;i++)
- {
- pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
- strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
- }
- for(i=0;i<noplanecount;i++)
- {
- pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
- }
- pa = new double[noplanecount];
- pb = new double[noplanecount];
- ppa.reset(pa);
- ppb.reset(pb);
- for(i=0;i<noplanecount;i++)
- {
- pa[i] = pswidth[i];
- pb[i] = (pewidth[i] - pa[i])/xroadlen;
- }
- for(i=0;i<noplanecount;i++)
- {
- pLS->AddLane(1,(i+1),strvectorlanetype[i],false,false);
- Lane * pLL = pLS->GetLane(pLS->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,pa[i],pb[i],
- 0,0);
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- }
- }
- }
- else
- {
- double xroadlen1 = 0;
- double xroadlen2 = 0;
- for(i=0;i<xvectorgeo1.size();i++)xroadlen1 = xroadlen1 + xvectorgeo1[i].mfLen;
- for(i=0;i<xvectorgeo2.size();i++)xroadlen2 = xroadlen2 + xvectorgeo2[i].mfLen;
- int index1 = mxodr.AddRoad("",xroadlen1, QString::number(CreateRoadID()).toStdString(),"-1");
- int index2 = mxodr.AddRoad("",xroadlen2, QString::number(CreateRoadID()).toStdString(),"-1");
- Road * proad2 = mxodr.GetRoad(index2);
- Road * proad1 = mxodr.GetRoad(index1);
- proad1->AddElevation(0,startheight,(endheight-startheight)/(xroadlen1+xroadlen2),0,0);
- proad2->AddElevation(0,startheight+xroadlen1*(endheight-startheight)/(xroadlen1+xroadlen2),
- (endheight-startheight)/(xroadlen1+xroadlen2),
- 0,0);
- p1 = mxodr.GetRoad(nroad1index);
- p2 = mxodr.GetRoad(nroad2index);
- // OpenDrive * px = &mxodr;
- double s = 0;
- int j;
- j= 0;
- for(j=0;j<xvectorgeo1.size();j++)
- {
- proad1->AddGeometryBlock();
- GeometryBlock * pgb = proad1->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo1[j].mnType)
- {
- case 0:
- pline = &xvectorgeo1[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo1[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo1[j];
- std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
- break;
- }
- s = s + xvectorgeo1[j].mfLen;
- }
- for(j=0;j<xvectorgeo2.size();j++)
- {
- proad2->AddGeometryBlock();
- GeometryBlock * pgb = proad2->GetGeometryBlock(j);
- geobase * pline;
- geobase * pbez;
- geobase * parc;
- switch(xvectorgeo2[j].mnType)
- {
- case 0:
- pline = &xvectorgeo2[j];
- pgb->AddGeometryLine(s,pline->mfX,pline->mfY,pline->mfHdg,pline->mfLen);
- break;
- case 1:
- parc = &xvectorgeo2[j];
- pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- break;
- case 2:
- pbez = &xvectorgeo2[j];
- std::cout<<"u0:"<<pbez->mfu[0]<<std::endl;
- pgb->AddGeometryParamPoly3(s,pbez->mfX,pbez->mfY,
- pbez->mfHdg,pbez->mfLen,pbez->mfu[0],
- pbez->mfu[1],pbez->mfu[2],pbez->mfu[3],pbez->mfv[0],
- pbez->mfv[1],pbez->mfv[2],pbez->mfv[3]);
- break;
- }
- s = s + xvectorgeo2[j].mfLen;
- }
- proad1->AddLaneSection(0);
- LaneSection * pLS1 = proad1->GetLaneSection(0);
- pLS1->SetS(0);
- pLS1->AddLane(0,0,"none",false);
- proad2->AddLaneSection(0);
- LaneSection * pLS2 = proad2->GetLaneSection(0);
- pLS2->SetS(0);
- pLS2->AddLane(0,0,"none",false);
- double * pswidth,*pewidth;
- int nlanecount = mvectorrc[0].mvectorlc.size();
- std::vector<std::string> strvectorlanetype;
- pswidth = new double[nlanecount];
- pewidth = new double[nlanecount];
- std::shared_ptr<double> ppswidth,ppewidth;
- ppswidth.reset(pswidth);
- ppewidth.reset(pewidth);
- for(i=0;i<nlanecount;i++)
- {
- pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart);
- strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlc.at(i).ml1,bFromstart));
- }
- for(i=0;i<nlanecount;i++)
- {
- pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlc.at(i).ml2,bTostart);
- }
- double * pa,*pb;
- pa = new double[nlanecount];
- pb = new double[nlanecount];
- std::shared_ptr<double> ppa,ppb;
- ppa.reset(pa);
- ppb.reset(pb);
- for(i=0;i<nlanecount;i++)
- {
- pa[i] = pswidth[i];
- pb[i] = (pewidth[i] - pa[i])/(xroadlen1+xroadlen2);
- }
- for(i=0;i<nlanecount;i++)
- {
- pLS1->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
- Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,pa[i],pb[i],
- 0,0);
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- pLS2->AddLane(-1,(i+1)*(-1),strvectorlanetype[i],false,false);
- pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
- 0,0);
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- }
- int noplanecount = mvectorrc[0].mvectorlcop.size();
- if(noplanecount > 0)
- {
- pswidth = new double[noplanecount];
- pewidth = new double[noplanecount];
- ppswidth.reset(pswidth);
- ppewidth.reset(pewidth);
- strvectorlanetype.clear();
- for(i=0;i<noplanecount;i++)
- {
- pswidth[i] = getlanewidth(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart);
- strvectorlanetype.push_back(getlanetype(p1,mvectorrc[0].mvectorlcop.at(i).ml1,bFromstart));
- }
- for(i=0;i<noplanecount;i++)
- {
- pewidth[i] = getlanewidth(p2,mvectorrc[0].mvectorlcop.at(i).ml2,bTostart);
- }
- pa = new double[noplanecount];
- pb = new double[noplanecount];
- ppa.reset(pa);
- ppb.reset(pb);
- for(i=0;i<noplanecount;i++)
- {
- pa[i] = pswidth[i];
- pb[i] = (pewidth[i] - pa[i])/xroadlen;
- }
- for(i=0;i<noplanecount;i++)
- {
- pLS1->AddLane(1,(i+1),strvectorlanetype[i],false,false);
- Lane * pLL = pLS1->GetLane(pLS1->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,pa[i],pb[i],
- 0,0);
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- pLS2->AddLane(1,(i+1),strvectorlanetype[i],false,false);
- pLL = pLS2->GetLane(pLS2->GetLaneCount() - 1);
- pLL->AddWidthRecord(0,pa[i]+pb[i]*xroadlen1 ,pb[i],
- 0,0);
- pLL->AddRoadMarkRecord(0,"solid","standard","standard",0.15,"false");
- }
- }
- }
- updateCBRoad();
- mbRefresh = true;
- update();
- }
- int MainWindow::GetEndPoint(Road *proad, double &x, double &y, double &hdg)
- {
- GeometryBlock * pblock = proad->GetLastGeometryBlock();
- RoadGeometry * pgeo = pblock->GetLastGeometry();
- //0-line, 1-arc, 2-spiral 3-poly3 4-parampoly3
- switch (pgeo->GetGeomType()) {
- case 0:
- {
- GeometryLine * pline = (GeometryLine *)pgeo;
- x = pline->GetX() + pline->GetLength() * cos(pline->GetHdg());
- y = pline->GetY() + pline->GetLength() * sin(pline->GetHdg());
- hdg = pline->GetHdg();
- }
- return 0;
- break;
- case 1:
- {
- GeometryArc * parc = (GeometryArc *)pgeo;
- double x_center = parc->GetX() + (1.0/parc->GetCurvature()) * cos(parc->GetHdg() + M_PI/2.0);
- double y_center = parc->GetY() + (1.0/parc->GetCurvature()) * sin(parc->GetHdg()+ M_PI/2.0);
- x = x_center + fabs(1.0/parc->GetCurvature()) * cos(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
- y = y_center + fabs(1.0/parc->GetCurvature()) * sin(parc->GetHdg() + parc->GetLength() * parc->GetCurvature() - M_PI/2.0);
- hdg = parc->GetHdg() + parc->GetLength() * parc->GetCurvature();
- return 0;
- }
- break;
- case 2:
- {
- GeometrySpiral * pspiral = (GeometrySpiral *)pgeo;
- pspiral->GetCoords(pspiral->GetS()+pspiral->GetLength(),x,y,hdg);
- return 0;
- }
- break;
- case 3:
- QMessageBox::warning(this,"warn","type not supported.");
- break;
- case 4:
- {
- double xtem,ytem;
- double xtem1,ytem1,x1,y1;
- GeometryParamPoly3 * ppoly3 = (GeometryParamPoly3* )pgeo;
- double s = ppoly3->GetLength();
- xtem = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
- ytem = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
- x = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
- y = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
- s = ppoly3->GetLength()*0.99;
- if(s>0)
- {
- xtem1 = ppoly3->GetuA() + ppoly3->GetuB() * s + ppoly3->GetuC() * s*s + ppoly3->GetuD() * s*s*s ;
- ytem1 = ppoly3->GetvA() + ppoly3->GetvB() * s + ppoly3->GetvC() * s*s + ppoly3->GetvD() * s*s*s ;
- x1 = xtem*cos(ppoly3->GetHdg()) - ytem * sin(ppoly3->GetHdg()) + ppoly3->GetX();
- y1 = xtem*sin(ppoly3->GetHdg()) + ytem * cos(ppoly3->GetHdg()) + ppoly3->GetY();
- hdg = geofit::CalcHdg(xtem1,ytem1,x1,y1);
- }
- else
- {
- hdg = 0;
- }
- return 0;
- }
- break;
- default:
- QMessageBox::warning(this,"warn","type not supported.");
- break;
- }
- return -1;
- }
- void MainWindow::onChangeRoadType(int index)
- {
- if(index == 1)
- {
- mpLBRoadType->setVisible(false);
- mpLERoadType->setVisible(false);
- }
- else
- {
- mpLBRoadType->setVisible(true);
- mpLERoadType->setVisible(true);
- }
- if(index == 0)
- {
- mpLBRoadType->setText("Radius:");
- mpLERoadType->setText("6.0");
- }
- if(index == 2)
- {
- mpLBRoadType->setText("Extend:");
- mpLERoadType->setText("3.0");
- }
- }
- std::vector<geobase> MainWindow::CreateLineGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg)
- {
- // std::vector<geobase> xvectorgeo;
- // geobase xgeobezier;
- // int nbtype;
- // double fabc[3],fxy[2],fblen,fbhdg;
- // geofit x;
- // x.CreateBezier(startx,starty,starthdg,
- // endx,endy,endhdg,
- // 0.35,0.35,xgeobezier.mfu,xgeobezier.mfv,xgeobezier.mfLen,
- // nbtype,fabc,&fbhdg,fxy,&fblen);
- // if(nbtype == 2)
- // {
- // xgeobezier.mfHdg = starthdg;
- // xgeobezier.mfX = startx;
- // xgeobezier.mfY = starty;
- // xgeobezier.mnType = 2;
- // }
- // else
- // {
- // xgeobezier.mnType = 0; //Line
- // xgeobezier.mfHdgStart = fbhdg;
- // xgeobezier.mfHdg = fbhdg;
- // xgeobezier.mfX = fxy[0];
- // xgeobezier.mfY = fxy[1];
- // xgeobezier.mfLen = fblen;
- // }
- // xvectorgeo.push_back(xgeobezier);
- // return xvectorgeo;
- geobase linegeo;
- linegeo.mnType = 0;
- linegeo.mfX = startx;
- linegeo.mfY = starty;
- linegeo.mfHdg = geofit::CalcHdg(startx,starty,endx,endy);
- linegeo.mfLen = sqrt(pow(endx - startx,2)+pow(endy - starty,2));
- std::vector<geobase> xvectorgeo;
- xvectorgeo.push_back(linegeo);
- return xvectorgeo;
- }
- std::vector<geobase> MainWindow::CreateTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg,double R)
- {
- std::vector<geobase> xvectorgeo;
- xvectorgeo.clear();
- if(starthdg == endhdg)
- {
- std::cout<<"hdg same use line contact"<<std::endl;
- return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
- }
- double a1,c1,a2,c2;
- double inter_x,inter_y;
- if((starthdg == M_PI/2.0)||(starthdg == 3.0*M_PI/2.0))
- {
- a2 = tan(endhdg) *(-1);
- c2 = 0-a2*endx - endy;
- a1 = 1;
- c1 = startx *(-1);
- inter_x = startx;
- inter_y = 0 - a2*inter_x-c2;
- }
- else
- {
- if((endhdg == M_PI/2.0)||(endhdg == 3.0*M_PI/2.0))
- {
- a1 = tan(starthdg) *(-1);
- c1 = 0-a1*startx - starty;
- a2 = 1;
- c2 = endx *(-1);
- inter_x = endx;
- inter_y = 0 - a1*inter_x-c1;
- }
- else
- {
- a1 = tan(starthdg) *(-1);
- a2 = tan(endhdg) *(-1);
- c1 = 0-a1*startx - starty;
- c2 = 0-a2*endx - endy;
- inter_x = (c1-c2)/(a2-a1);
- inter_y = 0 - a1*inter_x - c1;
- }
- }
- double dis1,dis2;
- dis1 =sqrt(pow(inter_x - startx,2)+pow(inter_y - starty,2));
- dis2 =sqrt(pow(inter_x - endx,2)+pow(inter_y - endy,2));
- if((dis1<1.0)||(dis2<1.0))
- {
- std::cout<<"use line connect."<<std::endl;
- return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
- }
- double hdgdiff = endhdg - starthdg;
- if(hdgdiff >= M_PI)hdgdiff = hdgdiff - 2.0*M_PI;
- if(hdgdiff <= (-M_PI))hdgdiff = hdgdiff + 2.0*M_PI;
- double slen = R*tan(fabs(hdgdiff/2.0));
- if((dis1<slen)||(dis2<slen))
- {
- std::cout<<"radius is big. use line."<<std::endl;
- return CreateLineGeo(startx,starty,starthdg,endx,endy,endhdg);
- }
- double p1_x,p1_y,p2_x,p2_y;
- p1_x = inter_x- slen * cos(starthdg);
- p1_y = inter_y- slen * sin(starthdg);
- p2_x = inter_x+slen*cos(endhdg);
- p2_y = inter_y+slen*sin(endhdg);
- geobase linegeo;
- linegeo.mnType = 0;
- linegeo.mfX = startx;
- linegeo.mfY = starty;
- linegeo.mfHdg = starthdg;
- linegeo.mfLen = sqrt(pow(p1_x - startx,2)+pow(p1_y - starty,2));
- xvectorgeo.push_back(linegeo);
- geobase arcgeo;
- arcgeo.mnType = 1;
- arcgeo.mfX = p1_x;
- arcgeo.mfY = p1_y;
- arcgeo.mfHdg = starthdg;
- arcgeo.mfHdgStart = starthdg;
- arcgeo.mfLen = R*fabs(hdgdiff);
- arcgeo.mR = R*(fabs(hdgdiff)/hdgdiff);
- arcgeo.mfEndX = p2_x;
- arcgeo.mfEndY = p2_y;
- xvectorgeo.push_back(arcgeo);
- linegeo.mnType = 0;
- linegeo.mfX = p2_x;
- linegeo.mfY = p2_y;
- linegeo.mfHdg = endhdg;
- linegeo.mfLen = sqrt(pow(p2_x - endx,2)+pow(p2_y - endy,2));
- xvectorgeo.push_back(linegeo);
- return xvectorgeo;
- }
- std::string MainWindow::getlanetype(Road *p, int nlane, bool bstart)
- {
- Lane * plane = 0;
- int i;
- LaneSection * pLS;
- if(bstart)
- {
- pLS = p->GetLaneSection(0);
- }
- else
- {
- pLS = p->GetLaneSection(p->GetLaneSectionCount() -1);
- }
- int nlanecount = pLS->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- if(nlane == pLS->GetLane(i)->GetId())
- {
- plane = pLS->GetLane(i);
- break;
- }
- }
- if(plane == 0)
- {
- std::cout<<"MainWindow::getlanetype can't find lane : "<<nlane<<std::endl;
- return "driving";
- }
- return plane->GetType();
- }
- double MainWindow::getlanewidth(Road * p, int nlane,bool bstart)
- {
- Lane * plane = 0;
- double a,b,c,d;
- double s;
- if(bstart)
- {
- s = 0;
- }
- else
- {
- s = p->GetRoadLength();
- }
- int i;
- LaneSection * pLS;
- if(bstart)
- {
- pLS = p->GetLaneSection(0);
- }
- else
- {
- pLS = p->GetLaneSection(p->GetLaneSectionCount() -1);
- }
- int nlanecount = pLS->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- if(nlane == pLS->GetLane(i)->GetId())
- {
- plane = pLS->GetLane(i);
- break;
- }
- }
- if(plane == 0)
- {
- std::cout<<"MainWindow::getlanewidth can't find lane : "<<nlane<<std::endl;
- return 0;
- }
- if(bstart)
- {
- return plane->GetLaneWidth(0)->GetA();
- }
- else
- {
- s = p->GetRoadLength() - pLS->GetS();
- LaneWidth * pLW = plane->GetLaneWidth(plane->GetLaneWidthCount()-1);
- a = pLW->GetA();b = pLW->GetB();c = pLW->GetC();d = pLW->GetD();
- return a+b*s+c*pow(s,2)+d*pow(s,3);
- }
- }
- double MainWindow::getoff(Road *p, int nlane, bool bstart)
- {
- double off = 0;
- int i;
- if(bstart)
- {
- LaneSection * pLS = p->GetLaneSection(0);
- if(nlane<0)
- {
- if(nlane == -1)return 0;
- else
- {
- for(i=0;i<pLS->GetLaneCount();i++)
- {
- Lane * pL = pLS->GetLane(i);
- if((pL->GetId()<0)&&(pL->GetId()>nlane))
- {
- off = off + pL->GetLaneWidth(0)->GetA();
- }
- }
- }
- }
- else
- {
- if(nlane == 1)return 0;
- else
- {
- for(i=0;i<pLS->GetLaneCount();i++)
- {
- Lane * pL = pLS->GetLane(i);
- if((pL->GetId()>0)&&(pL->GetId()<nlane))
- {
- off = off - pL->GetLaneWidth(0)->GetA();
- }
- }
- }
- }
- }
- else
- {
- LaneSection * pLS = p->GetLaneSection(p->GetLaneSectionCount()-1);
- if(nlane<0)
- {
- if(nlane == -1)return 0;
- else
- {
- for(i=0;i<pLS->GetLaneCount();i++)
- {
- Lane * pL = pLS->GetLane(i);
- if((pL->GetId()<0)&&(pL->GetId()>nlane))
- {
- double a,b,c,d;
- a = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetA();
- b = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetB();
- c = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetC();
- d = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetD();
- double s = p->GetRoadLength();
- off = off + a + b*s +c *s*s + d*s*s*s;
- }
- }
- }
- }
- else
- {
- if(nlane == 1)return 0;
- else
- {
- for(i=0;i<pLS->GetLaneCount();i++)
- {
- Lane * pL = pLS->GetLane(i);
- if((pL->GetId()>0)&&(pL->GetId()<nlane))
- {
- double a,b,c,d;
- a = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetA();
- b = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetB();
- c = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetC();
- d = pL->GetLaneWidth(pL->GetLaneWidthCount()-1)->GetD();
- double s = p->GetRoadLength();
- off = off - a - b*s -c *s*s - d*s*s*s;
- }
- }
- }
- }
- }
- return off;
- }
- std::vector<geobase> MainWindow::CreateUTurnGeo(double startx, double starty, double starthdg, double endx, double endy, double endhdg, double fextend)
- {
- std::vector<geobase> xvectorgeo;
- double p1_x,p1_y, p2_x,p2_y,p1_hdg,p2_hdg;
- p1_x = fextend*cos(starthdg) + startx;
- p1_y = fextend*sin(starthdg) + starty;
- p2_x = fextend*cos(endhdg + M_PI) + endx;
- p2_y = fextend*sin(endhdg + M_PI) + endy;
- p1_hdg = starthdg;
- p2_hdg = endhdg;
- if(starthdg == endhdg)
- {
- std::cout<<" hdg is same, can't create u turn."<<std::endl;
- return xvectorgeo;
- }
- double hdgse = geofit::CalcHdg(p1_x,p1_y,p2_x,p2_y);
- double hdgdiff = hdgse - p1_hdg;
- if(hdgdiff < 0)hdgdiff = hdgdiff + 2.0*M_PI;
- bool bPA = true; //Positive
- if(hdgdiff >=M_PI)bPA = false;
- double xdiff;
- if(bPA)xdiff = hdgdiff - M_PI/2.0;
- else xdiff = hdgdiff - 3.0*M_PI/2.0;
- double xdis = sqrt(pow(p1_x-p2_x,2)+pow(p1_y-p2_y,2));
- double R = xdis/(2.0*cos(xdiff));
- double x_center,y_center;
- double xhdgtocenter;
- if(bPA)
- {
- xhdgtocenter = p1_hdg + M_PI/2.0;
- if(xhdgtocenter >= 2.0*M_PI)xhdgtocenter = xhdgtocenter - M_PI*2.0;
- }
- else
- {
- xhdgtocenter = p1_hdg - M_PI/2.0;
- if(xhdgtocenter < 0)xhdgtocenter = xhdgtocenter + 2.0*M_PI;
- }
- x_center = p1_x + R*cos(xhdgtocenter);
- y_center = p1_y + R*sin(xhdgtocenter);
- double xhdgcentertoarc;
- if(bPA)
- {
- xhdgcentertoarc = hdgse - M_PI/2.0;
- if(xhdgcentertoarc <0)xhdgcentertoarc = xhdgcentertoarc + 2.0*M_PI;
- }
- else
- {
- xhdgcentertoarc = hdgse + M_PI/2.0;
- if(xhdgcentertoarc >= M_PI*2.0)xhdgcentertoarc = xhdgcentertoarc - M_PI*2.0;
- }
- double p3_x,p3_y,p3_hdg;
- p3_hdg = hdgse;
- p3_x = x_center + R* cos(xhdgcentertoarc);
- p3_y = y_center + R* sin(xhdgcentertoarc);
- // pgb->AddGeometryArc(s,parc->mfX,parc->mfY,parc->mfHdgStart,parc->mfLen,1.0/parc->mR);
- geobase c2,c3;
- c2.mfX = p1_x;
- c2.mfY = p1_y;
- c2.mfHdgStart = p1_hdg;
- c3.mfX = p3_x;
- c3.mfY = p3_y;
- c3.mfHdgStart = hdgse;
- c2.mnType = 1;
- c3.mnType = 1;
- if(bPA)
- {
- c2.mfLen = hdgdiff * R;
- c2.mR = R;
- c3.mfLen = c2.mfLen;
- c3.mR = R;
- }
- else
- {
- c2.mfLen = (2.0*M_PI - hdgdiff) * R;
- c2.mR = R*(-1);
- c3.mfLen = c2.mfLen;
- c3.mR = R;
- }
- geobase l1,l2;
- if(fextend != 0)
- {
- l1.mfX = startx;
- l1.mfY = starty;
- l1.mfHdg = starthdg;
- l1.mfLen = fextend;
- l1.mnType = 0;
- l2.mfX = p2_x;
- l2.mfY = p2_y;
- l2.mfHdg = p2_hdg;
- l2.mfLen = fextend;
- l2.mnType = 0;
- xvectorgeo.push_back(l1);
- }
- xvectorgeo.push_back(c2);
- xvectorgeo.push_back(c3);
- if(fextend != 0)xvectorgeo.push_back(l2);
- return xvectorgeo;
- }
- void MainWindow::ChangeXODRRoadID(OpenDrive *pxodr, int index, int newid)
- {
- Road * proad;
- int nsize = pxodr->GetRoadCount();
- int i;
- if(index<0)return;
- if(index>=nsize)return;
- proad = pxodr->GetRoad(index);
- int noldid = atoi(proad->GetRoadId().data());
- char strid[255];
- snprintf(strid,255,"%d",newid);
- proad->SetRoadId(strid);
- for(i=0;i<nsize;i++)
- {
- Road * proad2 = pxodr->GetRoad(i);
- if(proad2->GetPredecessor()!= 0)
- {
- RoadLink * plink = proad2->GetPredecessor();
- if(strncmp(plink->GetElementType().data(),"road",255)== 0)
- {
- if(atoi(plink->GetElementId().data()) == noldid)
- {
- plink->SetElementId(strid);
- }
- }
- }
- if(proad2->GetSuccessor()!= 0)
- {
- RoadLink * plink = proad2->GetSuccessor();
- if(strncmp(plink->GetElementType().data(),"road",255)== 0)
- {
- if(atoi(plink->GetElementId().data()) == noldid)
- {
- plink->SetElementId(strid);
- }
- }
- }
- }
- nsize = pxodr->GetJunctionCount();
- for(i=0;i<nsize;i++)
- {
- Junction * pjunction = pxodr->GetJunction(i);
- int j;
- for(j=0;j<pjunction->GetJunctionConnectionCount();j++)
- {
- JunctionConnection * pjc = pjunction->GetJunctionConnection(j);
- if(atoi(pjc->GetIncomingRoad().data()) == noldid)
- {
- pjc->SetIncomingRoad(strid);
- }
- if(atoi(pjc->GetConnectingRoad().data()) == noldid)
- {
- pjc->SetConnectingRoad(strid);
- }
- }
- }
- }
- void MainWindow::ChangeXODRJunctionID(OpenDrive *pxodr, int index, int newid)
- {
- Junction * pjunction;
- int nsize = pxodr->GetJunctionCount();
- int i;
- if(index<0)return;
- if(index>=nsize)return;
- pjunction = pxodr->GetJunction(index);
- int noldid = atoi(pjunction->GetId().data());
- char strid[255];
- snprintf(strid,255,"%d",newid);
- pjunction->SetId(strid);
- for(i=0;i<nsize;i++)
- {
- Road * proad2 = pxodr->GetRoad(i);
- if(proad2->GetPredecessor()!= 0)
- {
- RoadLink * plink = proad2->GetPredecessor();
- if(strncmp(plink->GetElementType().data(),"junction",255)== 0)
- {
- if(atoi(plink->GetElementId().data()) == noldid)
- {
- plink->SetElementId(strid);
- }
- }
- }
- if(proad2->GetSuccessor()!= 0)
- {
- RoadLink * plink = proad2->GetSuccessor();
- if(strncmp(plink->GetElementType().data(),"junction",255)== 0)
- {
- if(atoi(plink->GetElementId().data()) == noldid)
- {
- plink->SetElementId(strid);
- }
- }
- }
- }
- }
- void MainWindow::MoveXODR(OpenDrive *pxodr, double movex, double movey)
- {
- int nsize = pxodr->GetRoadCount();
- int i;
- for(i=0;i<nsize;i++)
- {
- Road * pRoad = pxodr->GetRoad(i);
- int nblockcount = pRoad->GetGeometryBlockCount();
- int j;
- for(j=0;j<nblockcount;j++)
- {
- GeometryBlock * pgb = pRoad->GetGeometryBlock(j);
- if(pgb != 0)
- {
- pgb->GetGeometryAt(0)->SetX(pgb->GetGeometryAt(0)->GetX() + movex);
- pgb->GetGeometryAt(0)->SetY(pgb->GetGeometryAt(0)->GetY() + movey);
- }
- }
- }
- }
- int MainWindow::FindNewRoadID(OpenDrive *pxodr1, OpenDrive *pxodr2)
- {
- int nroadsize1,nroadsize2;
- nroadsize1 = pxodr1->GetRoadCount();
- nroadsize2 = pxodr2->GetRoadCount();
- int i;
- int * pnid = new int[nroadsize1 + nroadsize2];
- std::shared_ptr<int> ppnid;ppnid.reset(pnid);
- for(i=0;i<nroadsize1;i++)
- {
- pnid[i] = atoi(pxodr1->GetRoad(i)->GetRoadId().data());
- }
- for(i=0;i<nroadsize2;i++)
- {
- pnid[i+nroadsize1] = atoi(pxodr2->GetRoad(i)->GetRoadId().data());
- }
- int nstartid = 40000;
- bool bUsed = true;
- while(bUsed == true)
- {
- bUsed = false;
- for(i=0;i<(nroadsize1 + nroadsize2);i++)
- {
- if(pnid[i] == nstartid)
- {
- bUsed = true;
- break;
- }
- }
- if(bUsed == false)break;
- nstartid++;
- }
- return nstartid;
- }
- int MainWindow::FindNewJunctionID(OpenDrive *pxodr1, OpenDrive *pxodr2)
- {
- int njunctionsize1,njunctionsize2;
- njunctionsize1 = pxodr1->GetRoadCount();
- njunctionsize2 = pxodr2->GetRoadCount();
- int i;
- int * pnid = new int[njunctionsize1 + njunctionsize2];
- std::shared_ptr<int> ppnid;ppnid.reset(pnid);
- for(i=0;i<njunctionsize1;i++)
- {
- pnid[i] = atoi(pxodr1->GetJunction(i)->GetId().data());
- }
- for(i=0;i<njunctionsize2;i++)
- {
- pnid[i+njunctionsize1] = atoi(pxodr2->GetJunction(i)->GetId().data());
- }
- int nstartid = 50000;
- bool bUsed = true;
- while(bUsed == true)
- {
- bUsed = false;
- for(i=0;i<(njunctionsize1 + njunctionsize2);i++)
- {
- if(pnid[i] == nstartid)
- {
- bUsed = true;
- break;
- }
- }
- if(bUsed == false)break;
- nstartid++;
- }
- return nstartid;
- }
- void MainWindow::onClickRoadMark()
- {
- mbRefresh = true;
- update();
- }
- void MainWindow::onClickRoadDel()
- {
- if(mpCBRoad->count() == 0)
- {
- QMessageBox::warning(this,"warn","no road to be delete");
- return;
- }
- int nroadid = mpCBRoad->currentText().toInt();
- Road * pRoad = 0;
- int nroadsize = mxodr.GetRoadCount();
- int i;
- int index;
- for(i=0;i<nroadsize;i++)
- {
- if(nroadid == atoi(mxodr.GetRoad(i)->GetRoadId().data()))
- {
- pRoad = mxodr.GetRoad(i);
- index = i;
- break;
- }
- }
- if(pRoad == 0)
- {
- QMessageBox::warning(this,"warn","can't find road");
- return;
- }
- mxodr.DeleteRoad(index);
- nroadsize = mxodr.GetRoadCount();
- for(i=0;i<nroadsize;i++)
- {
- pRoad = mxodr.GetRoad(i);
- RoadLink * ppre,* pnext;
- ppre = pRoad->GetPredecessor();
- pnext = pRoad->GetSuccessor();
- if(ppre != 0)
- {
- if(strncmp(ppre->GetElementType().data(),"road",255) == 0)
- {
- if(atoi(ppre->GetElementId().data()) == nroadid)
- {
- pRoad->RemovePredecessor();
- LaneSection * pLS = pRoad->GetLaneSection(0);
- int nlanecount = pLS->GetLaneCount();
- int j;
- for(j=0;j<nlanecount;j++)
- {
- Lane * pLane;
- pLane = pLS->GetLane(j);
- pLane->RemovePredecessor();
- }
- }
- }
- }
- if(pnext != 0)
- {
- if(strncmp(pnext->GetElementType().data(),"road",255) == 0)
- {
- if(atoi(pnext->GetElementId().data()) == nroadid)
- {
- pRoad->RemoveSuccessor();
- LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
- int nlanecount = pLS->GetLaneCount();
- int j;
- for(j=0;j<nlanecount;j++)
- {
- Lane * pLane;
- pLane = pLS->GetLane(j);
- pLane->RemoveSuccessor();
- }
- }
- }
- }
- }
- //remove juction connection where id = roadid
- int njunctioncount = mxodr.GetJunctionCount();
- Junction * pjunction;
- for(i=0;i<njunctioncount;i++)
- {
- pjunction = mxodr.GetJunction(i);
- int j;
- for(j=0;j<pjunction->GetJunctionConnectionCount();j++)
- {
- JunctionConnection * pjc = pjunction->GetJunctionConnection(j);
- int nfromid = atoi(pjc->GetIncomingRoad().data());
- int ntoid = atoi(pjc->GetConnectingRoad().data());
- if((nfromid == nroadid)||(ntoid == nroadid))
- {
- pjunction->DeleteJunctionConnection(j);
- j--;
- }
- }
- }
- updateCBRoad();
- mbRefresh = true;
- update();
- }
- void MainWindow::updateCBRoad()
- {
- mpCBRoad->clear();
- mpCBRoad1->clear();
- mpCBRoad2->clear();
- mpCBJunctionIncommingRoad->clear();
- mpCBJunctionConnectingRoad->clear();
- mpCBPreNxtRoad->clear();
- int i;
- int nroadcount = mxodr.GetRoadCount();
- for(i=0;i<nroadcount;i++)
- {
- const char * strname = mxodr.GetRoad(i)->GetRoadId().data();
- mpCBRoad->addItem(strname);
- mpCBRoad1->addItem(strname);
- mpCBRoad2->addItem(strname);
- mpCBJunctionIncommingRoad->addItem(strname);
- mpCBJunctionConnectingRoad->addItem(strname);
- mpCBPreNxtRoad->addItem(strname);
- }
- }
- void MainWindow::onClickCBRoadChange(int index)
- {
- Road * pRoad = mxodr.GetRoad(index);
- if(pRoad == 0)
- {
- // QMessageBox::warning(this,"WARN","MainWindow::onClickCBRoadChange road is NULL");
- return;
- }
- int i;
- mpLabelRoadShowPreID->setText("");
- mpLabelRoadShowPreType2->setText("");
- mpLabelRoadShowPreType1->setText("");
- mpCBRoadShowPre->clear();
- mpLabelRoadShowNxtID->setText("");
- mpLabelRoadShowNxtType2->setText("");
- mpLabelRoadShowNxtType1->setText("");
- mpCBRoadShowNext->clear();
- if(pRoad->GetPredecessor() != 0)
- {
- RoadLink * pRL = pRoad->GetPredecessor();
- mpLabelRoadShowPreID->setText(pRL->GetElementId().data());
- mpLabelRoadShowPreType2->setText(pRL->GetContactPoint().data());
- mpLabelRoadShowPreType1->setText(pRL->GetElementType().data());
- }
- if(pRoad->GetSuccessor() != 0)
- {
- RoadLink * pRL = pRoad->GetSuccessor();
- mpLabelRoadShowNxtID->setText(pRL->GetElementId().data());
- mpLabelRoadShowNxtType2->setText(pRL->GetContactPoint().data());
- mpLabelRoadShowNxtType1->setText(pRL->GetElementType().data());
- }
- LaneSection * pLS = pRoad->GetLaneSection(0);
- int nlanecount = pLS->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- char strout[255];
- char strtem[255];
- Lane * pLane = pLS->GetLane(i);
- snprintf(strout,255,"%d type:%s",pLane->GetId(),pLane->GetType().data());
- if(pLane->IsPredecessorSet())
- {
- snprintf(strtem,255," pre:%d",pLane->GetPredecessor());
- strncat(strout,strtem,255);
- }
- mpCBRoadShowPre->addItem(strout);
- }
- pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
- nlanecount = pLS->GetLaneCount();
- for(i=0;i<nlanecount;i++)
- {
- char strout[255];
- char strtem[255];
- Lane * pLane = pLS->GetLane(i);
- snprintf(strout,255,"%d type:%s",pLane->GetId(),pLane->GetType().data());
- if(pLane->IsSuccessorSet())
- {
- snprintf(strtem,255," suc:%d",pLane->GetSuccessor());
- strncat(strout,strtem,255);
- }
- mpCBRoadShowNext->addItem(strout);
- }
- }
- void MainWindow::updateJunction()
- {
- int i;
- int njunctioncount = mxodr.GetJunctionCount();
- mpCBJunction->clear();
- for(i=0;i<njunctioncount;i++)
- {
- Junction * pjunction = mxodr.GetJunction(i);
- mpCBJunction->addItem(pjunction->GetId().data());
- }
- }
- void MainWindow::onClickCBJunctionChange(int index)
- {
- mpCBJunctionConnection->clear();
- Junction * pjunction = mxodr.GetJunction(index);
- if(pjunction == NULL)
- {
- // QMessageBox::warning(this,"warn","MainWindow::onClickCBJunctionChange can't find junction");
- return;
- }
- int nconnectioncount = pjunction->GetJunctionConnectionCount();
- int i;
- for(i=0;i<nconnectioncount;i++)
- {
- JunctionConnection * pJC = pjunction->GetJunctionConnection(i);
- mpCBJunctionConnection->addItem(pJC->GetId().data());
- }
- mpCBJunctionFromTo->clear();
- mpLabelJunctionConnectingRoad->setText("");
- mpLabelJunctionContactPoint->setText("");
- mpLabelJunctionIncommingRoad->setText("");
- if(pjunction->GetJunctionConnectionCount()>0)
- {
- mpCBJunctionConnection->setCurrentIndex(0);
- onClickCBJunctionConnectionChange(0);
- }
- }
- void MainWindow::onClickCBJunctionConnectionChange(int index)
- {
- // mpCBJunctionIncommingRoad->clear();
- // mpCBJunctionConnectingRoad->clear();
- // mpCBJunctionFromLane->clear();
- // mpCBJunctionToLane->clear();
- Junction * pjunction = mxodr.GetJunction(mpCBJunction->currentIndex());
- if(pjunction == NULL)
- {
- std::cout<<"MainWindow::onClickCBJunctionConnectionChange Junction NULL"<<std::endl;
- return;
- }
- if((index<0)||(index>= pjunction->GetJunctionConnectionCount()))
- {
- // std::cout<<"MainWindow::onClickCBJunctionConnectionChange out range."<<std::endl;
- return;
- }
- JunctionConnection * pJC = pjunction->GetJunctionConnection(index);
- if(pJC == NULL)
- {
- std::cout<<"MainWindow::onClickCBJunctionConnectionChange Junction Connection NULL"<<std::endl;
- return;
- }
- mpLabelJunctionIncommingRoad->setText(pJC->GetIncomingRoad().data());
- mpLabelJunctionContactPoint->setText(pJC->GetContactPoint().data());
- mpLabelJunctionConnectingRoad->setText(pJC->GetConnectingRoad().data());
- unsigned int i;
- unsigned int njunctionlanelinkcount = pJC->GetJunctionLaneLinkCount();
- mpCBJunctionFromTo->clear();
- for(i=0;i<njunctionlanelinkcount;i++)
- {
- char strout[255];
- JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(i);
- snprintf(strout,255,"%d to %d",pjll->GetFrom(),pjll->GetTo());
- mpCBJunctionFromTo->addItem(strout);
- }
- }
- void MainWindow::onClickCBJunctionIncommingChange(int index)
- {
- int i;
- int ncount = mxodr.GetRoadCount();
- if((index < 0)||(index>=ncount))
- {
- return;
- }
- Road * pRoad = mxodr.GetRoad(index);
- if(pRoad == NULL)
- {
- return;
- }
- LaneSection * pLS;
- if(mpCBJunctionContactPoint->currentIndex() == 0)
- {
- pLS = pRoad->GetLaneSection(0);
- }
- else
- {
- pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
- }
- int nlanecount = pLS->GetLaneCount();
- mpCBJunctionFromLane->clear();
- for(i=0;i<nlanecount;i++)
- {
- Lane * pLane = pLS->GetLane(i);
- if(pLane->GetId() != 0)
- {
- mpCBJunctionFromLane->addItem(QString::number(pLane->GetId()));
- }
- }
- }
- void MainWindow::onClickCBJunctionConnectionroadChange(int index)
- {
- int i;
- int ncount = mxodr.GetRoadCount();
- if((index < 0)||(index>=ncount))
- {
- return;
- }
- Road * pRoad = mxodr.GetRoad(index);
- if(pRoad == NULL)
- {
- return;
- }
- LaneSection * pLS;
- pLS = pRoad->GetLaneSection(0);
- int nlanecount = pLS->GetLaneCount();
- mpCBJunctionToLane->clear();
- for(i=0;i<nlanecount;i++)
- {
- Lane * pLane = pLS->GetLane(i);
- if(pLane->GetId() != 0)
- {
- mpCBJunctionToLane->addItem(QString::number(pLane->GetId()));
- }
- }
- }
- void MainWindow::onClickCreateJunction()
- {
- mxodr.AddJunction("",QString::number(CreateJunctionID()).toStdString());
- updateJunction();
- }
- void MainWindow::onClickCreateJunctionLaneLink()
- {
- Junction * pJunction= mxodr.GetJunction(mpCBJunction->currentIndex());
- if(pJunction == NULL)
- {
- QMessageBox::warning(this,"warn","can't find junction");
- return ;
- }
- Road * pFromRoad = mxodr.GetRoad(mpCBJunctionIncommingRoad->currentIndex());
- if(pFromRoad == NULL)
- {
- QMessageBox::warning(this,"warn","can't find incomming road.");
- return;
- }
- Road * pToRoad = mxodr.GetRoad(mpCBJunctionConnectingRoad->currentIndex());
- if(pToRoad == NULL)
- {
- QMessageBox::warning(this,"warn","can't find connecting road.");
- return;
- }
- string contacpoint = mpCBJunctionContactPoint->currentText().toStdString();
- JunctionConnection * pJC = 0;
- int njunctioncount = pJunction->GetJunctionConnectionCount();
- int i;
- for(i=0;i<njunctioncount;i++)
- {
- JunctionConnection * pJunCon = pJunction->GetJunctionConnection(i);
- if((pJunCon->GetIncomingRoad() == pFromRoad->GetRoadId())&&(pJunCon->GetContactPoint() == contacpoint)&&(pJunCon->GetConnectingRoad() == pToRoad->GetRoadId()))
- {
- pJC = pJunCon;
- break;
- }
- }
- bool bNewJC = false;
- if(pJC == 0)
- {
- int nnewJCid = pJunction->GetJunctionConnectionCount();
- int j;
- int njccount = pJunction->GetJunctionConnectionCount();
- bool bNotUse = true;
- do
- {
- for(j=0;j<njccount;j++)
- {
- if(nnewJCid == atoi(pJunction->GetJunctionConnection(j)->GetId().data()))
- {
- bNotUse = false;
- nnewJCid++;
- break;
- }
- }
- }while(bNotUse == false);
- unsigned int addindex = pJunction->AddJunctionConnection(QString::number(nnewJCid).toStdString(),
- pFromRoad->GetRoadId(),
- pToRoad->GetRoadId(),
- contacpoint);
- pJC = pJunction->GetJunctionConnection(addindex);
- bNewJC = true;
- }
- int nfrom = mpCBJunctionFromLane->currentText().toInt();
- int nto = mpCBJunctionToLane->currentText().toInt();
- int njlcount = pJC->GetJunctionLaneLinkCount();
- for(i=0;i<njlcount;i++)
- {
- JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(i);
- if((pjll->GetFrom()== nfrom)&&(pjll->GetTo() == nto))
- {
- QMessageBox::warning(this,"warn","this lane link exist.");
- return;
- }
- }
- unsigned int nadd = pJC->AddJunctionLaneLink(mpCBJunctionFromLane->currentText().toInt(),
- mpCBJunctionToLane->currentText().toInt());
- char strout[255];
- JunctionLaneLink * pjll = pJC->GetJunctionLaneLink(nadd);
- snprintf(strout,255,"%d to %d",pjll->GetFrom(),pjll->GetTo());
- if(bNewJC)
- {
- mpCBJunctionConnection->addItem(pJC->GetId().data());
- }
- if(strncmp(contacpoint.data(),"start",255) == 0)
- {
- if(nfrom*nto<0)
- {
- pFromRoad->SetPredecessor("junction",pJunction->GetId(),contacpoint);
- }
- else
- {
- pFromRoad->SetSuccessor("junction",pJunction->GetId(),contacpoint);
- }
- }
- else
- {
- if(nfrom*nto <0)
- {
- pFromRoad->SetSuccessor("junction",pJunction->GetId(),contacpoint);
- }
- else
- {
- pFromRoad->SetPredecessor("junction",pJunction->GetId(),contacpoint);
- }
- }
- pToRoad->SetRoadJunction(pJunction->GetId());
- }
- void MainWindow::onClickDeleteJunctionLaneLink()
- {
- Junction * pJunction= mxodr.GetJunction(mpCBJunction->currentIndex());
- if(pJunction == NULL)
- {
- QMessageBox::warning(this,"warn","can't find junction");
- return ;
- }
- if(pJunction->GetJunctionConnectionCount() == 0)
- {
- mxodr.DeleteJunction(mpCBJunction->currentIndex());
- updateJunction();
- return;
- }
- JunctionConnection * pJC = pJunction->GetJunctionConnection(mpCBJunctionConnection->currentIndex());
- if(pJC == NULL)
- {
- QMessageBox::warning(this,"warn","can't find junction connetion.");
- return;
- }
- if(pJC->GetJunctionLaneLinkCount() == 0)
- {
- pJunction->DeleteJunctionConnection(mpCBJunctionConnection->currentIndex());
- if(pJunction->GetJunctionConnectionCount() > 0)
- onClickCBJunctionChange(mpCBJunction->currentIndex());
- else
- {
- mxodr.DeleteJunction(mpCBJunction->currentIndex());
- updateJunction();
- }
- return;
- }
- // std::string strincommingroad = mpLabelJunctionIncommingRoad->text().toStdString();
- // std::string strconnectionroad = mpLabelJunctionConnectingRoad->text().toStdString();
- // std::string strcontact = mpLabelJunctionContactPoint->text().toStdString();
- int nllindex = mpCBJunctionFromTo->currentIndex();
- pJC->DeleteJunctionLaneLink(nllindex);
- onClickCBJunctionConnectionChange(mpCBJunctionConnection->currentIndex());
- }
- void MainWindow::onClickPreNxtRoadChange(int index)
- {
- mpCBPreNxtRelLane->clear();
- Road * pRoad = mxodr.GetRoad(index);
- if(pRoad == NULL)
- {
- return;
- }
- LaneSection * pLS;
- if(strncmp(mpCBPreNxtConatact->currentText().toStdString().data(),"start",255)==0)
- {
- pLS = pRoad->GetLaneSection(0);
- }
- else
- {
- pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
- }
- if(pLS == NULL)
- {
- std::cout<<"MainWindow::onClickPreNxtRoadChange can't find lanesection"<<std::endl;
- return;
- }
- int nlanecount = pLS->GetLaneCount();
- int i;
- for(i=0;i<nlanecount;i++)
- {
- Lane * pLane = pLS->GetLane(i);
- char strout[255];
- snprintf(strout,255,"%d %s",pLane->GetId(),pLane->GetType().data());
- mpCBPreNxtRelLane->addItem(strout);
- }
- }
- void MainWindow::onClickSetRoadPredecessor()
- {
- Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
- if(pRoad == NULL)
- {
- return;
- }
- Road * pOtherRoad = mxodr.GetRoad(mpCBPreNxtRoad->currentIndex());
- if(pOtherRoad == NULL)
- {
- return;
- }
- std::string strcontact = mpCBPreNxtConatact->currentText().toStdString();
- int curlane = mpCBRoadShowPre->currentIndex();
- int otherlane = mpCBPreNxtRelLane->currentIndex();
- pRoad->SetPredecessor("road",pOtherRoad->GetRoadId(),strcontact);
- LaneSection * pLS = pRoad->GetLaneSection(0);
- Lane * pLane =pLS->GetLane(curlane);
- LaneSection * pLSOther;
- if(strncmp(strcontact.data(),"start",0) == 0)
- {
- pLSOther = pOtherRoad->GetLaneSection(0);
- }
- else
- {
- pLSOther = pOtherRoad->GetLaneSection(pOtherRoad->GetLaneSectionCount()-1);
- }
- Lane * pLaneOther = pLSOther->GetLane(otherlane);
- if(pLaneOther == NULL)
- {
- QMessageBox::warning(this,"warn","no this lane.");
- return;
- }
- pLane->SetPredecessor(pLaneOther->GetId());
- onClickCBRoadChange(mpCBRoad->currentIndex());
- }
- void MainWindow::onClickSetRoadSuccessor()
- {
- Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
- if(pRoad == NULL)
- {
- return;
- }
- Road * pOtherRoad = mxodr.GetRoad(mpCBPreNxtRoad->currentIndex());
- if(pOtherRoad == NULL)
- {
- return;
- }
- std::string strcontact = mpCBPreNxtConatact->currentText().toStdString();
- int curlane = mpCBRoadShowNext->currentIndex();
- int otherlane = mpCBPreNxtRelLane->currentIndex();
- pRoad->SetSuccessor("road",pOtherRoad->GetRoadId(),strcontact);
- LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
- Lane * pLane =pLS->GetLane(curlane);
- LaneSection * pLSOther;
- if(strncmp(strcontact.data(),"start",0) == 0)
- {
- pLSOther = pOtherRoad->GetLaneSection(0);
- }
- else
- {
- pLSOther = pOtherRoad->GetLaneSection(pOtherRoad->GetLaneSectionCount()-1);
- }
- Lane * pLaneOther = pLSOther->GetLane(otherlane);
- if(pLaneOther == NULL)
- {
- QMessageBox::warning(this,"warn","no this lane.");
- return;
- }
- pLane->SetSuccessor(pLaneOther->GetId());
- onClickCBRoadChange(mpCBRoad->currentIndex());
- }
- void MainWindow::onClickDelRoadPredecessor()
- {
- Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
- if(pRoad == NULL)
- {
- return;
- }
- int curlane = mpCBRoadShowPre->currentIndex();
- LaneSection * pLS = pRoad->GetLaneSection(0);
- Lane * pLane =pLS->GetLane(curlane);
- if(pLane == 0)
- {
- return;
- }
- pLane->RemovePredecessor();
- int i;
- int nlanecount = pLS->GetLaneCount();
- bool bAllNotHavePre = true;
- for(i=0;i<nlanecount;i++)
- {
- Lane * pLa= pLS->GetLane(i);
- if(pLa->IsPredecessorSet())
- {
- bAllNotHavePre = false;
- break;
- }
- }
- if(bAllNotHavePre)
- {
- pRoad->RemovePredecessor();
- }
- onClickCBRoadChange(mpCBRoad->currentIndex());
- }
- void MainWindow::onClickDelRoadSuccessor()
- {
- Road * pRoad = mxodr.GetRoad(mpCBRoad->currentIndex());
- if(pRoad == NULL)
- {
- return;
- }
- int curlane = mpCBRoadShowNext->currentIndex();
- LaneSection * pLS = pRoad->GetLaneSection(pRoad->GetLaneSectionCount()-1);
- Lane * pLane =pLS->GetLane(curlane);
- if(pLane == 0)
- {
- return;
- }
- pLane->RemoveSuccessor();
- int i;
- int nlanecount = pLS->GetLaneCount();
- bool bAllNotHaveNxt = true;
- for(i=0;i<nlanecount;i++)
- {
- Lane * pLa= pLS->GetLane(i);
- if(pLa->IsSuccessorSet())
- {
- bAllNotHaveNxt = false;
- break;
- }
- }
- if(bAllNotHaveNxt)
- {
- pRoad->RemoveSuccessor();
- }
- onClickCBRoadChange(mpCBRoad->currentIndex());
- }
- void MainWindow::onClickAutoConnect()
- {
- AutoConnect pAC(&mxodr);
- pAC.Connect();
- updateJunction();
- }
- void MainWindow::on_actionLoad_triggered()
- {
- onClickLoad();
- }
- void MainWindow::on_actionSave_triggered()
- {
- onClickSave();
- }
- void MainWindow::on_actionAutoConnect_triggered()
- {
- onClickAutoConnect();
- }
- void MainWindow::on_actionSet_Speed_triggered()
- {
- SpeedDialog sd(&mxodr,this);
- int res = sd.exec();
- }
- void MainWindow::closeEvent(QCloseEvent *event)
- {
- if(mxodr.GetRoadCount() == 0)
- {
- event->accept(); // 接受退出信号,程序退出
- return;
- }
- QMessageBox::StandardButton button;
- button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
- if(button==QMessageBox::No)
- {
- event->ignore(); // 忽略退出信号,程序继续进行
- }
- else if(button==QMessageBox::Yes)
- {
- event->accept(); // 接受退出信号,程序退出
- }
- }
|