source: proiecte/ptvs/src/vnsim/vehicular/simulator/RoutingUtils.java @ 31

Last change on this file since 31 was 31, checked in by (none), 14 years ago
File size: 18.7 KB
Line 
1/************************************************************************************
2 * Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University
3 * All rights reserved.
4 * Refer to LICENSE for terms and conditions of use.
5 ***********************************************************************************/
6package vnsim.vehicular.simulator;
7
8
9
10import java.util.*;
11
12import vnsim.map.object.Cross;
13import vnsim.map.object.Globals;
14import vnsim.map.object.Point;
15import vnsim.map.object.Road;
16
17
18public class RoutingUtils {
19
20        public static ArrayList<RouteSegment> getMergedDijkstraPath(Location src, Location dest) {
21                if(src.roadIdx==dest.roadIdx) {
22                        ArrayList<RouteSegment> rs=new ArrayList<RouteSegment>();
23                        rs.add(new RouteSegment((short)src.roadIdx, (short)src.ptIdx, (short)dest.ptIdx));
24                        return rs;
25                }
26                ArrayList<Location> points=dijkstraPath(src, dest);
27
28                points.add(new Location(dest.roadIdx, dest.ptIdx));
29
30                ArrayList<RouteSegment> ret=mergeRoute(points);
31                //System.out.println("Route after merge: segments="+ret);
32                return ret;
33        }
34
35        private static ArrayList<RouteSegment> split(RouteSegment seg) {
36                ArrayList<RouteSegment> split=new ArrayList<RouteSegment> ();
37                split.add(seg);
38                Road r=Globals.map.roads.get(seg.roadIndex);
39                boolean done=false;
40                while(!done) {
41                        done=true;
42                        ArrayList<RouteSegment> newSplit=new ArrayList<RouteSegment> ();
43                        x:for(int i=0;i<split.size();i++) {
44                                RouteSegment rs=split.get(i);
45                                for(int j=0;j<r.crosses.size();j++) {
46                                        int ptCross=r.crosses.get(j).getPointIndex();
47                                        if(( rs.pt1<rs.pt2) && (ptCross < rs.pt2 && ptCross>rs.pt1 )) {
48                                                newSplit.add(new RouteSegment((short)seg.roadIndex, (short)rs.pt1, (short)ptCross));
49                                                newSplit.add(new RouteSegment((short)seg.roadIndex, (short)ptCross, (short)rs.pt2));
50                                                done=false;
51                                                continue x;
52                                        }
53                                        if(( rs.pt1>rs.pt2) && (ptCross < rs.pt1 && ptCross>rs.pt2 )) {
54                                                newSplit.add(new RouteSegment((short)seg.roadIndex, (short)rs.pt1, (short)ptCross));
55                                                newSplit.add(new RouteSegment((short)seg.roadIndex, (short)ptCross, (short)rs.pt2));
56                                                done=false;
57                                                continue x;
58                                        }
59                                }
60                                newSplit.add(rs);
61                        }
62                        split=newSplit;
63                }
64                return split;
65        }
66       
67        public static ArrayList<RouteSegment> splitRoute(ArrayList<RouteSegment> route) {
68                ArrayList<RouteSegment> ret=new ArrayList<RouteSegment> ();
69                for(int i=0;i<route.size();i++) {
70                        ret.addAll(split(route.get(i)));
71                }
72                return ret;
73        }
74
75        public static ArrayList<RouteSegment> getUnmergedDijkstraPath(Location src, Location dest) {
76                if(src.roadIdx==dest.roadIdx) {
77                        return split(new RouteSegment((short)src.roadIdx, (short)src.ptIdx, (short)dest.ptIdx));
78                }
79                ArrayList<Location> points=dijkstraPath(src, dest);
80                points.add(new Location(dest.roadIdx, dest.ptIdx));
81                ArrayList<RouteSegment> ret=new ArrayList<RouteSegment>();
82               
83                x:for(int i=0;i<points.size()-1;i++) {
84                        Location l1=points.get(i);
85                        Location l2=points.get(i+1);
86
87//                      System.out.println("L1="+l1+";L2="+l2);
88
89                        if(l1.roadIdx==l2.roadIdx) {
90                                RouteSegment seg=new RouteSegment((short)l1.roadIdx,(short)l1.ptIdx, (short)l2.ptIdx);
91                                ArrayList<RouteSegment> segs=split(seg);
92                                for(int j=0;j<segs.size();j++)
93                                        ret.add(segs.get(j));
94                                continue;
95                        }
96
97                        //is "l1" on "l2.roadIdx" ?
98                        Road r1=Globals.map.roads.get(l1.roadIdx);
99                        for(int j=0;j<r1.crosses.size();j++) {
100                                Cross c=r1.crosses.get(j);
101                                if(c.getCrossRoadIndex()==l2.roadIdx && c.getPointIndex()==l1.ptIdx) {
102                                        RouteSegment seg=new RouteSegment((short)l2.roadIdx, (short)c.getCrossPointIndex(), (short)l2.ptIdx);
103                                        ret.addAll(split(seg));
104                                        continue x;
105                                }
106                        }
107
108                        //is "l2" on "l1.roadIdx" ?
109                        Road r2=Globals.map.roads.get(l2.roadIdx);
110                        for(int j=0;j<r2.crosses.size();j++) {
111                                Cross c=r2.crosses.get(j);
112                                if(c.getCrossRoadIndex()==l1.roadIdx && c.getPointIndex()==l2.ptIdx) {
113                                        ret.addAll(split(new RouteSegment((short)l1.roadIdx, (short)l1.ptIdx, (short)c.getCrossPointIndex())));
114                                        continue x;
115                                }
116                        }
117                        System.out.println("getUnmergedPath - UNIMPLEMENTED!!!");
118                }
119                return ret;
120        }
121
122        private static ArrayList<Location> dijkstraPath(Location src, Location dest) {
123                //returns a vector of consecutive RouteSegments, the shortest route
124                //between src and dest
125
126        //      System.out.println("Have to compute a route: from "+src+" -- to "+dest);
127
128                ArrayList<Location> routePoints=new ArrayList<Location>();
129
130                GraphNodeArrayList set=new GraphNodeArrayList();
131
132//              Point destPoint=(Point) ((Road)(Globals.map.roads.get(dest.roadIndex))).points.get(dest.pt1);
133
134                GraphNode sourceGraphNode=new GraphNode(src.roadIdx,src.ptIdx);
135                ArrayList sourceNeighbors=getNeighbors(sourceGraphNode);
136
137                //add the direct neighbors of the source to the set of nodes;
138                for(int i=0;i<sourceNeighbors.size();i++) {
139                        NeighborNode srcNeig=(NeighborNode) sourceNeighbors.get(i);
140                        int rd=srcNeig.neigh.roadIndex;
141                        int pt=srcNeig.neigh.pointIndex;
142                        set.addGraphNode(new GraphNode(rd,pt,src.roadIdx,src.ptIdx,srcNeig.dist));
143                }
144
145                GraphNodeArrayList solved=new GraphNodeArrayList();
146
147                solved.addGraphNode(sourceGraphNode);
148
149                while(true) {
150                        //get the first node
151                        GraphNode first=(GraphNode) set.removeFirst();
152                        solved.addGraphNode(first);
153//                      System.out.println("Solved:"+first+"; solved="+solved.size()+";unsolved="+set.size());
154                        if(isPointOnRoad(first.roadIndex,first.pointIndex,dest.roadIdx)) {
155                                //first is solved; try to get the route built into 'ret'
156                                GraphNode aux=first;
157                                routePoints.add(0, new Location(aux.roadIndex,aux.pointIndex));
158                                while(true) {
159                                        if(aux.parentRoadIndex==-1 || aux.parentPointIndex==-1) {
160                                                break;
161                                        }
162
163                                        routePoints.add(0, new Location(aux.parentRoadIndex,aux.parentPointIndex));
164
165//                                      System.out.println("Parent: road "+aux.parentRoadIndex+"; point "+aux.parentPointIndex+"; distance="+aux.distance+"--");
166                                        //find the parent in the 'solved' set
167                                        Road rdAux=(Road)Globals.map.roads.get(aux.parentRoadIndex);
168                                        Point ptAux=(Point) rdAux.points.get(aux.parentPointIndex);
169                                        Iterator iter=solved.iterator();
170                                        boolean found=false;
171                                        while(iter.hasNext()) {
172                                                GraphNode aux2=(GraphNode) iter.next();
173                                                Road rdAux2=(Road)Globals.map.roads.get(aux2.roadIndex);
174                                                Point ptAux2=(Point) rdAux2.points.get(aux2.pointIndex);
175                                                if(ptAux2.equals(ptAux)) {
176                                                        aux=aux2;
177                                                        found=true;
178                                                        break;
179                                                }
180                                        }
181                                        if(found==false) {
182                                                System.out.println("ERROR! Parent NOT FOUND!:road "+aux.parentRoadIndex+"; point "+aux.parentPointIndex);
183                                                break;
184                                        } else {
185                                        }
186                                }
187                                break;
188                        }
189
190                        //relaxation
191                        ArrayList vecini=getNeighbors(first);
192
193//                      System.out.println("Neighbors for "+first+":"+vecini);
194
195                        for(int j=0;j<vecini.size();j++) {
196                                NeighborNode n=(NeighborNode) vecini.get(j);
197                                if(solved.contains(n.neigh)) continue;
198                                //only if it's not solved
199
200                                Point ptAux=(Point) (((Road)(Globals.map.roads.get(n.neigh.roadIndex))).points.get(n.neigh.pointIndex));
201                                double oldDist=getDistance(set, ptAux);
202                                double d1=n.dist;
203                                if(oldDist==-1.0) {
204//                                      System.out.println("ERROR!; roadIndex="+n.neigh.roadIndex+"; ptIndex="+n.neigh.pointIndex+"; lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
205                                        n.neigh.distance=d1+first.distance;
206                                        n.neigh.parentPointIndex=first.pointIndex;
207                                        n.neigh.parentRoadIndex=first.roadIndex;
208                                        set.addGraphNode(n.neigh);
209                                } else {
210                                        if(d1+first.distance < oldDist) {
211                                                //set.remove(n.neigh);
212                                                n.neigh.distance=d1+first.distance;
213                                                n.neigh.parentPointIndex=first.pointIndex;
214                                                n.neigh.parentRoadIndex=first.roadIndex;
215                                                //set.add(n.neigh);
216                                                set.setGraphNode(n.neigh);
217                                        }
218                                }
219                        }
220                }
221
222                //now I have the 'routePoints' vector filled with Location objects;
223                //do merging stuff; obtain the rez vector filled with RouteSegment objects
224               
225                //System.out.println("Route before merge: points="+routePoints);
226                return routePoints;
227        }
228
229        public static ArrayList<RouteSegment> mergeRoute(ArrayList<Location> routePoints) {
230                //takes a vector of Location objects (points) and returns a vector of RouteSegment objects,
231                //representing the route as a sequence of roads, rather than individual points
232
233                int p1, pLastCand, p2;    //point indexes
234                int r1, r2;    //road indexes
235                int index=0;
236                ArrayList<RouteSegment> ret=new ArrayList<RouteSegment>();
237
238                if(routePoints==null || routePoints.size()==0) {
239                        System.out.println("ERROR! routingModule.dijkstraPath() - routePoints has no points!");
240                        return ret;
241                }
242
243                p1=((Location) routePoints.get(0) ).ptIdx;
244                r1=((Location) routePoints.get(0) ).roadIdx;
245                pLastCand=((Location) routePoints.get(0) ).ptIdx;
246
247                index++;
248
249                while(index<routePoints.size()) {
250                        p2=( (Location) routePoints.get(index) ).ptIdx;
251                        r2=( (Location) routePoints.get(index) ).roadIdx;
252                        if(r2==r1) {
253                                pLastCand=p2;
254                        } else {
255                                //p2 could also be on r1; try to find it
256                                boolean found=false;
257                                Point pt1=(Point) (((Road)(Globals.map.roads.get(r2))).points.get(p2));
258                                Road road1=(Road)Globals.map.roads.get(r1);
259                                for(int i=0;i<road1.points.size();i++){
260                                        Point pt2=(Point) road1.points.get(i);
261                                        if(pt1.equals(pt2)) {
262                                                found=true;
263                                                ret.add(new RouteSegment((short)r1,(short)p1,(short)i));
264                                                p1=p2;
265                                                r1=r2;
266                                                pLastCand=p2;
267                                                break;
268                                        }
269                                }
270                                if(!found) {
271                                        //find (r1,pLastCand) on r2
272                                        Point ptLC2=(Point) (((Road)(Globals.map.roads.get(r1))).points.get(pLastCand));
273                                        Road road2=(Road)Globals.map.roads.get(r2);
274                                        for(int i=0;i<road2.points.size();i++) {
275                                                Point pt2=(Point)road2.points.get(i);
276                                                if(pt2.equals(ptLC2)) {
277                                                        found=true;
278                                                        ret.add(new RouteSegment((short)r1,(short)p1,(short)pLastCand));
279                                                        p1=i;
280                                                        r1=r2;
281                                                        pLastCand=p2;
282                                                        break;
283                                                }
284                                        }
285                                }
286                                if(!found) {
287                                        System.out.println("RoutingModule.mergePoints() - situation not implemented!; returning null!");
288                                        return null;
289                                }
290                        }
291                        index++;
292                        if(index==routePoints.size()) {
293                                //it is the last iteration; add the last RouteSegment;
294                                ret.add(new RouteSegment((short)r1,(short)p1,(short)pLastCand));
295                        }
296                }
297                return ret;
298        }
299
300        public static ArrayList getNeighbors(GraphNode node) {
301                //get all the direct neighbors of 'node'; returns a vector of 'NeighborNode' objects
302                ArrayList<NeighborNode> rez=new ArrayList<NeighborNode>();
303
304                //find the closest points on the same road which are cross-points
305                //if there is none, take the end points of the road (except when the
306                //node is already an end point);
307                //also check if there are cross-roads in 'node';
308                Road road=(Road) Globals.map.roads.get(node.roadIndex);
309                int nodeIndex=node.pointIndex;
310
311                int min=Integer.MAX_VALUE;
312                int max=-1;     
313                for(int i=0;i<road.crosses.size();i++) {
314                        Cross c=(Cross) road.crosses.get(i);
315                        int idx=c.getPointIndex();
316                        if(idx>max && idx<nodeIndex)
317                                max=idx;
318                        if(idx<min && idx>nodeIndex)
319                                min=idx;
320                        Point pt=(Point) road.points.get(nodeIndex);
321                        if(idx==nodeIndex) {
322                                //have to go on the crossing road, to find the closest points;
323                                Road crossingRoad=(Road) Globals.map.roads.get(c.getCrossRoadIndex());
324                                Point pt2=null;
325                                int index2=-1;
326                                //have to find the index of the point on the crossing road
327                                for(int j=0;j<crossingRoad.points.size();j++) {
328                                        Point ptAux = (Point) crossingRoad.points.get(j);
329                                        if(ptAux.equals(pt)) {
330                                                //that's the one
331                                                pt2=ptAux;
332                                                index2=j;
333                                                break;
334                                        }
335                                }
336                                if(pt2==null) {
337//                                      System.out.println("RoutingModule.getNeighbors(): ERROR! Could not find the point on the crossing road:"+c.getCrossRoadIndex());
338                                } else {
339                                        //find the neighbors on the crossing road;
340                                        int minCross=Integer.MAX_VALUE;
341                                        int maxCross=-1;
342                                        for(int k=0;k<crossingRoad.crosses.size();k++) {
343                                                Cross otherCross=(Cross) crossingRoad.crosses.get(k);
344                                                int otherIndex=otherCross.getPointIndex();
345                                                if(otherIndex>maxCross && otherIndex<index2)
346                                                        maxCross=otherIndex;
347                                                if(otherIndex<minCross && otherIndex>index2)
348                                                        minCross=otherIndex;
349                                               
350                                        }
351                                        if(minCross==Integer.MAX_VALUE) {
352                                                //there was no min; have to consider last point of crossing road
353                                                //only if it is not the last
354                                                if((index2!=(crossingRoad.points.size()-1))) {
355                                                        NeighborNode n=new NeighborNode(c.getCrossRoadIndex(),crossingRoad.points.size()-1,
356                                                                ((Point)(crossingRoad.points.get(crossingRoad.points.size()-1))).getDistance() - 
357                                                                ((Point)(crossingRoad.points.get(index2))).getDistance()
358                                                        );
359                                                        rez.add(n);
360                                                }
361                                        } else {
362                                                NeighborNode n=new NeighborNode(c.getCrossRoadIndex(),minCross,
363                                                        ((Point)(crossingRoad.points.get(minCross))).getDistance() - 
364                                                        ((Point)(crossingRoad.points.get(index2))).getDistance()
365                                                );
366                                                rez.add(n);
367                                        }
368                                        if(maxCross==-1) {
369                                                //there was no max; have to consider first point of crossing road
370                                                //only if it is not first
371                                                if(index2!=0) {
372                                                        NeighborNode n=new NeighborNode(c.getCrossRoadIndex(),0,
373                                                                ((Point)( crossingRoad.points.get(index2)  )).getDistance() - 
374                                                                ((Point)(crossingRoad.points.get(0))).getDistance()
375                                                        );
376                                                        rez.add(n);
377                                                }
378                                        } else {
379                                                NeighborNode n=new NeighborNode(c.getCrossRoadIndex(),maxCross,
380                                                        ((Point)( crossingRoad.points.get(index2)  )).getDistance() - 
381                                                        ((Point)(crossingRoad.points.get(maxCross))).getDistance()
382                                                );
383                                                rez.add(n);
384                                        }
385                                }
386                        }
387                }
388                if(min==Integer.MAX_VALUE) {
389                        //there was no min; have to consider last point of the road
390                        //ONLY if I am not the last!
391                        if(((road.points.size()-1)!=node.pointIndex)) {
392                                NeighborNode n=new NeighborNode(node.roadIndex, road.points.size()-1 , 
393                                        ((Point)(road.points.get(road.points.size()-1))).getDistance() - 
394                                        ((Point)(road.points.get(node.pointIndex))).getDistance() );
395                                rez.add(n);
396                        }
397                } else {
398                        NeighborNode n=new NeighborNode(node.roadIndex, min , 
399                                        ((Point)(road.points.get(min))).getDistance() - 
400                                        ((Point)(road.points.get(node.pointIndex))).getDistance() );
401                        rez.add(n);
402                }
403                if(max==-1){
404                        //there was no max; have to consider first point of the road
405                        //only if I am not first
406                        if(node.pointIndex!=0) {
407                                NeighborNode n=new NeighborNode(node.roadIndex, 0 , 
408                                        ((Point)(road.points.get(node.pointIndex))).getDistance() -
409                                        ((Point)(road.points.get(0))).getDistance() );
410                                rez.add(n);
411                        }
412                } else {
413                        NeighborNode n=new NeighborNode(node.roadIndex, max , 
414                                        ((Point)(road.points.get(node.pointIndex))).getDistance() -
415                                        ((Point)(road.points.get(max))).getDistance() );
416                        rez.add(n);     
417                }
418                return rez;
419        }
420
421        private static double getDistance(GraphNodeArrayList t, Point p) {
422                //get the distance from the set for the corresponding point;
423                Iterator iter=t.iterator();
424                while(iter.hasNext()) {
425                        GraphNode n=(GraphNode) iter.next();
426                        Point pct=(Point) (((Road)(Globals.map.roads.get(n.roadIndex))).points.get(n.pointIndex));
427                        if(pct.equals(p))
428                                return n.distance;
429                }
430//              System.out.println("ERROR! getDistance() NOT FOUND!\n");
431                return -1.0;
432        }
433
434        private static boolean isPointOnRoad(int roadIndex, int pointIndex, int otherRoadIndex) {
435                //[roadIndex,pointIndex] is also situated on "otherRoadIndex" ?
436                Location loc=new Location(roadIndex, pointIndex);
437                for(int i=0;i<Globals.map.roads.get(otherRoadIndex).points.size();i++) {
438                        Location otherLoc=new Location(otherRoadIndex, i);
439                        if(loc.equals(otherLoc))
440                                return true;
441                }
442                return false;
443        }
444       
445        static class GraphNode {
446                //a point on the discrete map;
447                //used for the dijkstra implementation;
448
449                public int roadIndex;
450                public int pointIndex; //these two uniquely identify the node;
451
452                int parentRoadIndex;
453                int parentPointIndex; //these two uniquely identify the parent
454
455                public double distance; //used in the dijkstra implementation
456
457                public GraphNode(int roadIndex, int pointIndex) {
458                        this.roadIndex=roadIndex;
459                        this.pointIndex=pointIndex;
460                        parentRoadIndex=-1;
461                        parentPointIndex=-1;
462                        distance=Double.MAX_VALUE;
463                }
464
465                public GraphNode(int roadIndex, int pointIndex, int parentRoadIndex, int parentPointIndex, double distance) {
466                        this.roadIndex=roadIndex;
467                        this.pointIndex=pointIndex;
468                        this.parentRoadIndex=parentRoadIndex;
469                        this.parentPointIndex=parentPointIndex;
470                        this.distance=distance;
471                }
472
473                public boolean equals(GraphNode n) {
474                        Point p1=(Point)((Road)(Globals.map.roads.get(roadIndex))).points.get(pointIndex);
475                        Point p2=(Point)((Road)(Globals.map.roads.get(n.roadIndex))).points.get(n.pointIndex);
476                        return p1.equals(p2);
477                }
478
479                public String toString() {
480                        String ret="[Road="+roadIndex+";Point="+pointIndex+"]";
481                        return ret;
482                }
483        }
484
485        static class NeighborNode {
486                GraphNode neigh;
487                double dist; //distance to that neighbor
488               
489                public NeighborNode(int roadIndex, int pointIndex, double dist) {
490                        this.dist=dist;
491                        neigh=new GraphNode(roadIndex, pointIndex);
492                }
493
494                public String toString() {
495                        String ret="[Neigh:"+neigh+" ; dist="+dist+"]";
496                        return ret;
497                }
498        }
499
500        static class GraphNodeArrayList {
501                //a vector of "graph nodes"; it keeps the graph nodes ordered (performs an insertion sorting, when
502                //adding an element); it also prevent duplicates
503
504                ArrayList<GraphNode> v;
505
506                public GraphNodeArrayList(){
507                        v=new ArrayList<GraphNode>();
508                }
509
510                public void addGraphNode(GraphNode n) {
511                        //insertion-sorting add, based on the 'distance' of the graph nodes
512                        for(int i=0;i<v.size();i++) {
513                                GraphNode node=(GraphNode) v.get(i);
514                                if(n.distance<node.distance) {
515                                        v.add(i,n);
516                                        return;
517                                }
518                        }
519                        v.add(n);
520                }
521
522                public GraphNode removeFirst() {
523                        //gets the first node and removes it from the collection
524                        if(v.size()>0) {
525                                GraphNode n=(GraphNode) v.get(0);
526                                v.remove(0);
527                                return n;
528                        }
529                        return null;
530                }
531
532                public Iterator iterator() {
533                        return v.iterator();
534                }
535
536                public void setGraphNode(GraphNode n) {
537                        //looks through the vector, finds a graph node referring to the same point (by
538                        //checking the location of the point), and replaces that graph node;
539                        Point pct1 = (Point) (((Road)(Globals.map.roads.get(n.roadIndex))).points.get(n.pointIndex));
540                        for(int i=0;i<v.size();i++) {
541                                GraphNode node=(GraphNode) v.get(i);
542                                Point pct2 = (Point) (((Road)(Globals.map.roads.get(node.roadIndex))).points.get(node.pointIndex));
543                                if(pct1.equals(pct2)) {
544                                        //that's the point; replace it
545                                        v.set(i,n);
546                                }
547                        }
548                }
549
550                public int size(){
551                        return v.size();
552                }
553
554                public boolean contains(GraphNode node) {
555                        for(int i=0;i<v.size();i++) {
556                                GraphNode node1=(GraphNode)v.get(i);
557                                if(node1.equals(node)) return true;
558                        }
559                        return false;
560                }
561        }
562}
Note: See TracBrowser for help on using the repository browser.