|
@@ -171,6 +171,10 @@ xodrdijkstra::xodrdijkstra(OpenDrive * pxodr)
|
|
{
|
|
{
|
|
fseclen = px->GetRoadLength() - px->GetLaneSection(j)->GetS();
|
|
fseclen = px->GetRoadLength() - px->GetLaneSection(j)->GetS();
|
|
}
|
|
}
|
|
|
|
+// if((px->GetRoadId() == "10019")||(px->GetRoadId() == "10020"))
|
|
|
|
+// {
|
|
|
|
+// int axy = 1;
|
|
|
|
+// }
|
|
roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,2,j,px);
|
|
roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),fseclen,2,j,px);
|
|
|
|
|
|
// roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),2,j,px);
|
|
// roadedge xroad(atoi(px->GetRoadId().data()),atoi(px->GetRoadJunction().data()),px->GetRoadLength(),2,j,px);
|
|
@@ -1254,6 +1258,8 @@ std::vector<int> xodrdijkstra::getpath(int srcroadid, int nsrclr, int dstroadid,
|
|
{
|
|
{
|
|
givlog->debug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
|
|
givlog->debug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
|
|
mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
|
|
mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
|
|
|
|
+// qDebug("%d %d %d %d",i,mroadedge[rtnpath[rtnpath.size()-1-i]].mroadid,mroadedge[rtnpath[rtnpath.size()-1-i]].mnleftright,
|
|
|
|
+// mroadedge[rtnpath[rtnpath.size()-1-i]].mnsectionid);
|
|
rtnpath2.push_back(rtnpath[rtnpath.size()-1-i]);
|
|
rtnpath2.push_back(rtnpath[rtnpath.size()-1-i]);
|
|
}
|
|
}
|
|
|
|
|
|
@@ -1273,12 +1279,26 @@ int xodrdijkstra::getroadedgefromvertex(int nstart, int nend)
|
|
{
|
|
{
|
|
int i;
|
|
int i;
|
|
int nrtn = -1;
|
|
int nrtn = -1;
|
|
|
|
+ double flen = -1;
|
|
|
|
+// int nlast = -1;
|
|
for(i=0;i<mroadedge.size();i++)
|
|
for(i=0;i<mroadedge.size();i++)
|
|
{
|
|
{
|
|
if((nstart == mroadedge[i].mvertexstart)&&(nend == mroadedge[i].mvertexend))
|
|
if((nstart == mroadedge[i].mvertexstart)&&(nend == mroadedge[i].mvertexend))
|
|
{
|
|
{
|
|
- nrtn = i;
|
|
|
|
- break;
|
|
|
|
|
|
+ if(flen >0)
|
|
|
|
+ {
|
|
|
|
+ if(mroadedge[i].mlen < flen)
|
|
|
|
+ {
|
|
|
|
+ flen = mroadedge[i].mlen;
|
|
|
|
+ nrtn = i;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ flen = mroadedge[i].mlen;
|
|
|
|
+ nrtn = i;
|
|
|
|
+ }
|
|
|
|
+ // break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return nrtn;
|
|
return nrtn;
|