12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708270927102711271227132714271527162717271827192720272127222723272427252726272727282729273027312732273327342735273627372738273927402741274227432744274527462747274827492750275127522753275427552756275727582759276027612762276327642765276627672768276927702771277227732774277527762777277827792780278127822783278427852786278727882789279027912792279327942795279627972798279928002801280228032804280528062807280828092810281128122813281428152816281728182819282028212822282328242825282628272828282928302831283228332834283528362837283828392840284128422843284428452846284728482849285028512852285328542855285628572858285928602861286228632864286528662867286828692870287128722873287428752876287728782879288028812882288328842885288628872888288928902891289228932894289528962897289828992900290129022903290429052906290729082909291029112912291329142915291629172918291929202921292229232924292529262927292829292930293129322933293429352936293729382939294029412942294329442945294629472948294929502951295229532954295529562957295829592960296129622963296429652966296729682969297029712972297329742975297629772978297929802981298229832984298529862987298829892990299129922993299429952996299729982999300030013002300330043005300630073008300930103011301230133014301530163017301830193020302130223023302430253026302730283029303030313032303330343035303630373038303930403041304230433044304530463047304830493050305130523053305430553056305730583059306030613062306330643065306630673068306930703071307230733074307530763077307830793080308130823083308430853086308730883089309030913092309330943095309630973098309931003101310231033104310531063107310831093110311131123113311431153116311731183119312031213122312331243125312631273128312931303131313231333134313531363137313831393140314131423143314431453146314731483149315031513152315331543155315631573158315931603161316231633164316531663167316831693170317131723173317431753176317731783179318031813182318331843185318631873188318931903191319231933194319531963197319831993200320132023203320432053206320732083209321032113212321332143215321632173218321932203221322232233224322532263227322832293230323132323233323432353236323732383239324032413242324332443245324632473248324932503251325232533254325532563257325832593260326132623263326432653266326732683269327032713272327332743275327632773278327932803281328232833284328532863287328832893290329132923293329432953296329732983299330033013302330333043305330633073308330933103311331233133314331533163317331833193320332133223323332433253326332733283329333033313332333333343335333633373338333933403341334233433344334533463347334833493350335133523353335433553356335733583359336033613362336333643365336633673368336933703371337233733374337533763377337833793380338133823383338433853386338733883389339033913392339333943395339633973398339934003401340234033404340534063407340834093410341134123413341434153416341734183419342034213422342334243425342634273428342934303431343234333434343534363437343834393440344134423443344434453446344734483449345034513452345334543455345634573458345934603461346234633464346534663467346834693470347134723473347434753476347734783479348034813482348334843485348634873488348934903491349234933494349534963497349834993500350135023503350435053506350735083509351035113512351335143515351635173518351935203521352235233524352535263527352835293530353135323533353435353536353735383539354035413542354335443545354635473548354935503551355235533554355535563557355835593560356135623563356435653566356735683569357035713572357335743575357635773578357935803581358235833584358535863587358835893590359135923593359435953596359735983599360036013602360336043605360636073608360936103611361236133614361536163617361836193620362136223623362436253626362736283629363036313632363336343635363636373638363936403641364236433644364536463647364836493650365136523653365436553656365736583659366036613662366336643665366636673668366936703671367236733674367536763677367836793680368136823683368436853686368736883689369036913692369336943695369636973698369937003701370237033704370537063707370837093710371137123713371437153716371737183719372037213722372337243725372637273728372937303731373237333734373537363737373837393740374137423743374437453746374737483749375037513752375337543755375637573758375937603761376237633764376537663767376837693770377137723773377437753776377737783779378037813782378337843785378637873788378937903791379237933794379537963797379837993800380138023803380438053806380738083809381038113812381338143815381638173818381938203821382238233824382538263827382838293830383138323833383438353836383738383839384038413842384338443845384638473848384938503851385238533854385538563857385838593860386138623863386438653866386738683869387038713872387338743875387638773878387938803881388238833884388538863887388838893890389138923893389438953896389738983899390039013902390339043905390639073908390939103911391239133914391539163917391839193920392139223923392439253926392739283929393039313932393339343935393639373938393939403941394239433944394539463947394839493950395139523953395439553956395739583959396039613962396339643965396639673968396939703971397239733974397539763977397839793980398139823983398439853986398739883989399039913992399339943995399639973998399940004001400240034004400540064007400840094010401140124013401440154016401740184019402040214022402340244025402640274028402940304031403240334034403540364037403840394040404140424043404440454046404740484049405040514052405340544055405640574058405940604061406240634064406540664067406840694070407140724073407440754076407740784079408040814082408340844085408640874088408940904091409240934094409540964097409840994100410141024103410441054106410741084109411041114112411341144115411641174118411941204121412241234124412541264127412841294130413141324133413441354136413741384139414041414142414341444145414641474148414941504151415241534154415541564157415841594160416141624163416441654166416741684169417041714172417341744175417641774178417941804181418241834184418541864187418841894190419141924193419441954196419741984199420042014202420342044205420642074208420942104211421242134214421542164217421842194220422142224223422442254226422742284229423042314232423342344235423642374238423942404241424242434244424542464247424842494250425142524253425442554256425742584259426042614262426342644265426642674268426942704271427242734274427542764277427842794280428142824283428442854286428742884289429042914292429342944295429642974298429943004301430243034304430543064307430843094310431143124313431443154316431743184319432043214322432343244325432643274328432943304331433243334334433543364337433843394340434143424343434443454346434743484349435043514352435343544355435643574358435943604361436243634364436543664367436843694370437143724373437443754376437743784379438043814382438343844385438643874388438943904391439243934394439543964397439843994400440144024403440444054406440744084409441044114412441344144415441644174418441944204421442244234424442544264427442844294430443144324433443444354436443744384439444044414442444344444445444644474448444944504451445244534454445544564457445844594460446144624463446444654466446744684469447044714472447344744475447644774478447944804481448244834484448544864487448844894490449144924493449444954496449744984499450045014502450345044505450645074508450945104511451245134514451545164517451845194520452145224523452445254526452745284529453045314532453345344535453645374538453945404541454245434544454545464547454845494550455145524553455445554556455745584559456045614562456345644565456645674568456945704571457245734574457545764577457845794580458145824583458445854586458745884589459045914592459345944595459645974598459946004601460246034604460546064607460846094610461146124613461446154616461746184619462046214622462346244625462646274628462946304631463246334634463546364637463846394640464146424643464446454646464746484649465046514652465346544655465646574658465946604661466246634664466546664667466846694670467146724673467446754676467746784679468046814682468346844685468646874688468946904691469246934694469546964697469846994700470147024703470447054706470747084709471047114712471347144715471647174718471947204721472247234724472547264727472847294730473147324733473447354736473747384739474047414742474347444745474647474748474947504751475247534754475547564757475847594760476147624763476447654766476747684769477047714772477347744775477647774778477947804781478247834784478547864787478847894790479147924793479447954796479747984799480048014802480348044805480648074808480948104811481248134814481548164817481848194820482148224823482448254826482748284829483048314832483348344835483648374838483948404841484248434844484548464847484848494850485148524853485448554856485748584859486048614862486348644865486648674868486948704871487248734874487548764877487848794880488148824883488448854886488748884889489048914892489348944895489648974898489949004901490249034904490549064907490849094910491149124913491449154916491749184919492049214922492349244925492649274928492949304931493249334934493549364937493849394940494149424943494449454946494749484949495049514952495349544955495649574958495949604961496249634964496549664967496849694970497149724973497449754976497749784979498049814982498349844985498649874988498949904991499249934994499549964997499849995000500150025003500450055006500750085009501050115012501350145015501650175018501950205021502250235024502550265027502850295030503150325033503450355036503750385039504050415042504350445045504650475048504950505051505250535054505550565057505850595060506150625063506450655066506750685069507050715072507350745075507650775078507950805081508250835084508550865087508850895090509150925093509450955096509750985099510051015102510351045105510651075108510951105111511251135114511551165117511851195120512151225123512451255126512751285129513051315132513351345135513651375138513951405141514251435144514551465147514851495150515151525153515451555156515751585159516051615162516351645165516651675168516951705171517251735174517551765177517851795180518151825183518451855186518751885189519051915192519351945195519651975198519952005201520252035204520552065207520852095210521152125213521452155216521752185219522052215222522352245225522652275228522952305231523252335234523552365237523852395240524152425243524452455246524752485249525052515252525352545255525652575258525952605261526252635264526552665267526852695270527152725273 |
- #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 * gw;
- MainWindow::MainWindow(QWidget *parent) :
- QMainWindow(parent),
- ui(new Ui::MainWindow)
- {
- gw = this;
- 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;
- // mpscene = new QGraphicsScene;//(-300, -300, 600, 600);
- mpscene = new QGraphicsScene(0,0,VIEW_WIDTH,VIEW_HEIGHT);
- mpscene->setBackgroundBrush(Qt::darkGreen);
- // painter->begin(image);
- QTabWidget * p = new QTabWidget(ui->centralWidget);
- p->setGeometry(30,30,300,300);
- mnFontHeight = ui->centralWidget->fontMetrics().height();
- CreateTab1View(p);
- mpLabel_Status = new QLabel(this);
- ui->statusBar->addPermanentWidget(mpLabel_Status);
- 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;
- x = x + mfViewMoveX;
- y = y + mfViewMoveY;
- 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;
- GeometryPoly3 *ppoly;
- double rel_x,rel_y,rel_hdg;
- pg = pgeob->GetGeometryAt(0);
- x = pg->GetX();
- y = pg->GetY();
- x = x + mfViewMoveX;
- y = y + mfViewMoveY;
- 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;
- x = x + mfViewMoveX;
- y = y + mfViewMoveY;
- 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);
- }
- x_draw = x_draw + mfViewMoveX;
- y_draw = y_draw + mfViewMoveY;
- painter->drawPoint(x_draw * mnfac ,y_draw * mnfac *(-1));
- }
- }
- break;
- case 3:
- {
- ppoly = (GeometryPoly3 *)pg;
- x = pg->GetX();
- y = pg->GetY();
- double A,B,C,D;
- A = ppoly->GetA();
- B = ppoly->GetB();
- C = ppoly->GetC();
- D = ppoly->GetD();
- const double steplim = 0.1;
- double du = steplim;
- double u = 0;
- double v = 0;
- double oldx,oldy;
- oldx = x;
- oldy = y;
- double xstart,ystart;
- xstart = x;
- ystart = y;
- double hdgstart = ppoly->GetHdg();
- double flen = 0;
- while(flen < ppoly->GetLength())
- {
- double fdis = 0;
- v = A + B*u + C*u*u + D*u*u*u;
- x = xstart + u*cos(hdgstart) - v*sin(hdgstart);
- y = ystart + u*sin(hdgstart) + v*cos(hdgstart);
- fdis = sqrt(pow(x- oldx,2)+pow(y-oldy,2));
- oldx = x;
- oldy = y;
- if(fdis>(steplim*2.0))du = du/2.0;
- flen = flen + fdis;
- u = u + du;
- std::cout<<" x: "<<x<<" y:"<<y<<std::endl;
- x = x + mfViewMoveX;
- y = y + mfViewMoveY;
- painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- }
- }
- break;
- case 4:
- {
- ppp3 = (GeometryParamPoly3 * )pg;
- int ncount = ppp3->GetLength()* mnfac;
- double sstep;
- double arclength = ppp3->GetLength();
- if(ncount > 0)sstep = ppp3->GetLength()/ncount;
- else sstep = 10000.0;
- double s = 0;
- while(s < ppp3->GetLength())
- {
- double xtem,ytem;
- double pRange = s/arclength;
- xtem = ppp3->GetuA() + ppp3->GetuB() * pRange + ppp3->GetuC() * pRange*pRange + ppp3->GetuD() * pRange*pRange*pRange;
- ytem = ppp3->GetvA() + ppp3->GetvB() * pRange + ppp3->GetvC() * pRange*pRange + ppp3->GetvD() * pRange*pRange*pRange;
- x = xtem*cos(ppp3->GetHdg()) - ytem * sin(ppp3->GetHdg()) + ppp3->GetX();
- y = xtem*sin(ppp3->GetHdg()) + ytem * cos(ppp3->GetHdg()) + ppp3->GetY();
- x = x + mfViewMoveX;
- y = y + mfViewMoveY;
- 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;
- x = x + mfViewMoveX;
- y = y + mfViewMoveY;
- painter->drawPoint((int)(x*mnfac),(int)(y*(-1.0*mnfac)));
- }
- }
- painter->setPen(Qt::green);
- painter->end();
- }
- //刷新
- void MainWindow::paintEvent(QPaintEvent *)
- {
- if(mnViewMode == 1)
- {
- if(mbRefresh)
- {
- // UpdateScene();
- }
- myview->setScene(mpscene);
- myview->show();
- return;
- }
- 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("View Mode");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pCB = new QComboBox(pGroup);
- pCB->addItem("Line");
- pCB->addItem("Scene");
- pCB->setCurrentIndex(0);
- pCB->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpCBViewMode = pCB;
- connect(pCB,SIGNAL(currentIndexChanged(int)),this,SLOT(onViewModeChange(int)));
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 3;
- 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 * 2;
- pLabel = new QLabel(pGroup);
- pLabel->setText("ViewMove");
- pLabel->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText(QString::number(mfViewMoveX,'f',2));
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLEViewMoveX = pLE;
- nXPos = nXPos + nSpace;
- pLE = new QLineEdit(pGroup);
- pLE->setText(QString::number(mfViewMoveY,'f',2));
- pLE->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpLEViewMoveY = 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 * 2;
- pPB = new QPushButton(pGroup);
- pPB->setText("Set Move");
- pPB->setGeometry(nXPos,nYPos,mnFontHeight*10,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickSetMove()));
- nXPos = 30 + mnFontHeight*10;
- pPB = new QPushButton(pGroup);
- pPB->setText("Reset Move");
- pPB->setGeometry(nXPos,nYPos,mnFontHeight*10,nLEHeight);
- connect(pPB,SIGNAL(clicked(bool)),this,SLOT(onClickReSetMove()));
- 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;
- QCheckBox * pCheck = new QCheckBox(pGroup);
- pCheck->setText("act");
- pCheck->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpCheckFromSel = pCheck;
- mpCheckFromSel->setChecked(false);
- 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;
- pCheck = new QCheckBox(pGroup);
- pCheck->setText("act");
- pCheck->setGeometry(nXPos,nYPos,nLEWidth,nLEHeight);
- mpCheckToSel = pCheck;
- mpCheckToSel->setChecked(false);
- nXPos = 10;
- nYPos = nYPos + mnFontHeight * 2;
- 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);
- if(mnViewMode == 0)
- {
- 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);
- double rel_x,rel_y;
- rel_x = selx - mfViewMoveX;
- rel_y = sely - mfViewMoveY;
- Road * pRoad = 0;
- GeometryBlock * pgeob;
- double fdis,nearx,neary,hdg;
- double fs;
- int nlane;
- if(xodrfunc::GetNearPoint(rel_x,rel_y,&mxodr,&pRoad,&pgeob,fdis,nearx,neary,hdg,50,&fs,&nlane) == 0)
- {
- qDebug("s:%f dis is %f nlane is %d",fs,fdis,nlane);
- char strout[1000];
- snprintf(strout,1000,"Road:%s s:%f dis:%f nlane:%d",pRoad->GetRoadId().data(),fs,fdis,nlane);
- mpLabel_Status->setText(strout);
- ui->statusBar->showMessage(strout,3000);
- int i;
- int nsize = mpCBRoad->count();
- for(i=0;i<nsize;i++)
- {
- if(mpCBRoad->itemText(i).toStdString() == pRoad->GetRoadId())
- {
- break;
- }
- }
- if(i<nsize)
- {
- if(i != mpCBRoad->currentIndex())
- mpCBRoad->setCurrentIndex(i);
- }
- }
- if(mpCheckFromSel->isChecked())
- {
- if(pRoad != 0)ComboToString(pRoad->GetRoadId(),mpCBRoad1);
- mpCheckFromSel->setChecked(false);
- }
- if(mpCheckToSel->isChecked())
- {
- if(pRoad != 0)ComboToString(pRoad->GetRoadId(),mpCBRoad2);
- mpCheckToSel->setChecked(false);
- }
- mpLE_SelLon->setText(QString::number(lon,'f',7));
- mpLE_SelLat->setText(QString::number(lat,'f',7));
- update();
- }
- void MainWindow::ComboToString(std::string strroadid,QComboBox * pCB)
- {
- int i;
- int nsize = pCB->count();
- for(i=0;i<nsize;i++)
- {
- if(pCB->itemText(i).toStdString() == strroadid)
- {
- break;
- }
- }
- if(i<nsize)
- {
- if(i != pCB->currentIndex())
- pCB->setCurrentIndex(i);
- }
- }
- 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()
- {
- int nrtn;
- std::string strrtn = " ";
- std::string strroadname = mpLE_RoadName->text().toStdString();
- nrtn = ServiceXODRMake.AddRoadFromeLanePoint(mvectorlp,mvectorroadlane,mvectorroadopposite,glon0,
- glat0,mxodr,strroadname,strrtn);
- mpCBSelLane->clear();
- mpCBSelOpLane->clear();
- mvectorroadlane.clear();
- mvectorroadopposite.clear();
- updateCBRoad();
- mbRefresh = true;
- update();
- return;
- 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;
- }
- static bool IsNaN(double dat)
- {
- qint64 & ref=*(qint64 *)&dat;
- return (ref&0x7FF0000000000000) == 0x7FF0000000000000 && (ref&0xfffffffffffff)!=0;
- }
- 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;
- }
- int nroadnum = pxodr->GetRoadCount();
- int i;
- for(i=0;i<nroadnum;i++)
- {
- Road * pRoad = pxodr->GetRoad(i);
- if(IsNaN(pRoad->GetRoadLength()))
- {
- pxodr->DeleteRoad(i);
- i--;
- nroadnum--;
- qDebug("delete road %s because length is NaN",pRoad->GetRoadId().data());
- }
- }
- 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);
- // mxodr.SetHeader(revMajor,revMinor,name,version,date,north,south,east,west,lat0,lon0,hdg0);
- // std::string strgeoref;
- // std::string struserData;
- // pxodr->GetHeader()->GetgeoReference(strgeoref);
- // pxodr->GetHeader()->GetuserData(struserData);
- // mxodr.GetHeader()->SetgeoReference(strgeoref);
- // mxodr.GetHeader()->SetuserData(struserData);
- }
- 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));
- }
- if((mxodr.GetRoadCount()>0)&&(mxodr.GetHeader() == 0))
- mxodr.SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
- 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";
- if(mxodr.GetHeader() == NULL)
- {
- mxodr.SetHeader(1,1,"adcmap",1.1,QDateTime::currentDateTime().toString("yyyy-MM-dd").toLatin1().data(),0,0,0,0,glat0,glon0,ghdg0);
- mxodr.GetHeader()->SetVendor("adc");
- }
- else
- {
- mxodr.GetHeader()->SetVendor("adc");
- }
- 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;
- SaveBack();
- 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();
- if(p1->GetLaneOffsetCount()>0)
- {
- off1 = off1 - p1->GetLaneOffset(0)->Geta();
- }
- 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);
- if(p1->GetLaneOffsetCount()>0)
- {
- LaneOffset * pLO = p1->GetLaneOffset(p1->GetLaneOffsetCount()-1);
- double froadlen = p1->GetRoadLength();
- double sdis = froadlen - pLO->GetS();
- double foffset = pLO->Geta() + pLO->Getb()*(sdis) + pLO->Getc() * sdis * sdis
- +pLO->Getd() * sdis * sdis * sdis;
- off1 = off1 - foffset;
- }
- 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();
- if(p2->GetLaneOffsetCount()>0)
- {
- off2 = off2 - p2->GetLaneOffset(0)->Geta();
- }
- 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;
- }
- if(p2->GetLaneOffsetCount()>0)
- {
- LaneOffset * pLO = p2->GetLaneOffset(p2->GetLaneOffsetCount()-1);
- double froadlen = p2->GetRoadLength();
- double sdis = froadlen - pLO->GetS();
- double foffset = pLO->Geta() + pLO->Getb()*(sdis) + pLO->Getc() * sdis * sdis
- +pLO->Getd() * sdis * sdis * sdis;
- off2 = off2 - foffset;
- }
- 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*(-1);
- }
- 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->GetJunctionCount();
- njunctionsize2 = pxodr2->GetJunctionCount();
- 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;
- }
- SaveBack();
- 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();
- if(mpCBViewMode->currentIndex() == 1)
- {
- UpdateScene();
- }
- }
- 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()
- {
- if(mxodr.GetRoadCount() <= 0)
- {
- QMessageBox::warning(this,"Warning","OpenDrive Road Count is 0.",QMessageBox::YesAll);
- return;
- }
- 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()
- {
- std::string strroadid = mpCBRoad->currentText().toStdString();
- SpeedDialog sd(&mxodr,strroadid,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(); // 接受退出信号,程序退出
- }
- }
- void MainWindow::on_actionSet_Traffic_Light_triggered()
- {
- std::string strroadid = mpCBRoad->currentText().toStdString();
- TrafficLightDialog td(&mxodr,strroadid,this);
- int res = td.exec();
- }
- void MainWindow::on_actionEdit_Road_Lane_triggered()
- {
- }
- void MainWindow::on_actionEdit_Road_triggered()
- {
- SaveBack();
- std::string strroadid = mpCBRoad->currentText().toStdString();
- RoadEditDialog rd(&mxodr,strroadid,this);
- int res = rd.exec();
- updateCBRoad();
- mbRefresh = true;
- update();
- if(mpCBViewMode->currentIndex() == 1)
- {
- UpdateScene();
- }
- }
- void MainWindow::on_actionAdd_Road_From_RTK_triggered()
- {
- SaveBack();
- DialogAddRoadFromRTK arfrd(&mxodr,glon0,glat0,this);
- arfrd.exec();
- mpLELat0->setText(QString::number(glat0,'f',7));
- mpLELon0->setText(QString::number(glon0,'f',7));
- updateCBRoad();
- mbRefresh = true;
- update();
- }
- void MainWindow::onClickSetMove()
- {
- double fMoveX = mpLE_SelX->text().toDouble();
- double fMoveY = mpLE_SelY->text().toDouble();
- mfViewMoveX = mfViewMoveX - fMoveX;
- mfViewMoveY = mfViewMoveY - fMoveY;
- mbRefresh = true;
- update();
- mpLEViewMoveX->setText(QString::number(mfViewMoveX,'f',2));
- mpLEViewMoveY->setText(QString::number(mfViewMoveY,'f',2));
- // updateView();
- }
- void MainWindow::onClickReSetMove()
- {
- mfViewMoveX = 0;
- mfViewMoveY = 0;
- mbRefresh = true;
- update();
- mpLEViewMoveX->setText(QString::number(mfViewMoveX,'f',2));
- mpLEViewMoveY->setText(QString::number(mfViewMoveY,'f',2));
- }
- void MainWindow::on_actionSummary_Road_triggered()
- {
- int nroadnum = mxodr.GetRoadCount();
- int i;
- double flen = 0;
- for(i=0;i<nroadnum;i++)
- {
- flen = flen + mxodr.GetRoad(i)->GetRoadLength();
- }
- char strout[256];
- snprintf(strout,256,"Road Count:%d Length Total:%f",nroadnum,flen);
- QMessageBox::information(this,"Summary",QString(strout),QMessageBox::YesAll);
- }
- void MainWindow::on_actionSplit_Road_triggered()
- {
- }
- void MainWindow::UpdateScene()
- {
- int i;
- int nsize = mvectorviewitem.size();
- for(i=0;i<nsize;i++)
- {
- mpscene->removeItem(mvectorviewitem.at(i));
- delete mvectorviewitem.at(i);
- }
- mvectorviewitem.clear();
- nsize = mxodr.GetRoadCount();
- std::vector<RoadDigit> xvectorrd;
- for(i=0;i<nsize;i++)
- {
- RoadDigit xrd(mxodr.GetRoad(i),5.0);
- xvectorrd.push_back(xrd);
- // UpdateSceneRoad(mxodr.GetRoad(i));
- // qDebug("update road %d",i);
- }
- for(i=0;i<nsize;i++)
- {
- std::vector<QGraphicsPathItem *> xvectorlanepath = xodrscenfunc::GetRoadLaneItem(&(xvectorrd[i]));
- int j;
- int ncount = xvectorlanepath.size();
- for(j=0;j<ncount;j++)
- {
- QGraphicsPathItem * pitem = xvectorlanepath[j];
- pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
- mpscene->addItem(pitem);
- mvectorviewitem.push_back(pitem);
- }
- }
- for(i=0;i<nsize;i++)
- {
- std::vector<QGraphicsPathItem *> xvectormarkpath = xodrscenfunc::GetRoadMarkItem(&(xvectorrd[i]));
- int j;
- int ncount = xvectormarkpath.size();
- for(j=0;j<ncount;j++)
- {
- QGraphicsPathItem * pitem = xvectormarkpath[j];
- pitem->setPos(mfViewMoveX +VIEW_WIDTH/2,-mfViewMoveY+VIEW_HEIGHT/2);
- mpscene->addItem(pitem);
- mvectorviewitem.push_back(pitem);
- }
- }
- mbRefresh = false;
- }
- void MainWindow::onViewModeChange(int index)
- {
- if(index == 1)
- {
- UpdateScene();
- }
- mnViewMode = index;
- mbRefresh = true;
- update();
- }
- void MainWindow::on_actionCalc_Road_S_triggered()
- {
- DialogCalcS calcs(&mxodr,glon0,glat0,this);
- calcs.exec();
- }
- void MainWindow::on_actionBack_triggered()
- {
- if(mvectorxodrback.size()>0)
- {
- mxodr = mvectorxodrback.at(mvectorxodrback.size() -1);
- updateCBRoad();
- mbRefresh = true;
- update();
- if(mpCBViewMode->currentIndex() == 1)
- {
- UpdateScene();
- }
- mvectorxodrback.erase(mvectorxodrback.begin() + mvectorxodrback.size()-1);
- }
- else
- {
- QMessageBox::warning(this,"Warning","No Back Option",QMessageBox::YesAll);
- }
- }
- void MainWindow::SaveBack()
- {
- if(mxodr.GetRoadCount() > 0)
- {
- mvectorxodrback.push_back(mxodr);
- if(mvectorxodrback.size() > 10)
- {
- mvectorxodrback.erase(mvectorxodrback.begin());
- }
- }
- }
|