[31] | 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 | ***********************************************************************************/ |
---|
| 6 | package vnsim.vehicular.simulator; |
---|
| 7 | |
---|
| 8 | |
---|
| 9 | |
---|
| 10 | import java.util.*; |
---|
| 11 | |
---|
| 12 | import vnsim.map.object.Cross; |
---|
| 13 | import vnsim.map.object.Globals; |
---|
| 14 | import vnsim.map.object.Point; |
---|
| 15 | import vnsim.map.object.Road; |
---|
| 16 | |
---|
| 17 | |
---|
| 18 | public 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 | } |
---|