[31] | 1 | package vnsim.vehicular.routePlan.cityRouting; |
---|
| 2 | |
---|
| 3 | import java.util.ArrayList; |
---|
| 4 | import java.util.Iterator; |
---|
| 5 | |
---|
| 6 | import vnsim.map.object.Cross; |
---|
| 7 | import vnsim.map.object.Globals; |
---|
| 8 | import vnsim.map.object.Point; |
---|
| 9 | import vnsim.map.object.Road; |
---|
| 10 | import vnsim.vehicular.simulator.Location; |
---|
| 11 | import vnsim.vehicular.simulator.RouteSegment; |
---|
| 12 | import vnsim.vehicular.simulator.intersections.DirectedRoadSegment; |
---|
| 13 | import vnsim.vehicular.simulator.intersections.Intersection; |
---|
| 14 | |
---|
| 15 | public class CityRouteComputingUtlis { |
---|
| 16 | private static final double kCongestionRatio = 0.01; |
---|
| 17 | |
---|
| 18 | private ArrayList<RoadSegmentCongestion> updatedCongestion; |
---|
| 19 | |
---|
| 20 | public CityRouteComputingUtlis( |
---|
| 21 | ArrayList<RoadSegmentCongestion> updatedCongestion) { |
---|
| 22 | this.updatedCongestion = updatedCongestion; |
---|
| 23 | } |
---|
| 24 | |
---|
| 25 | private double getDistance(GraphNodeArrayList t, Point p) { |
---|
| 26 | // get the distance from the set for the corresponding point; |
---|
| 27 | Iterator<GraphNode> iter = t.iterator(); |
---|
| 28 | while (iter.hasNext()) { |
---|
| 29 | GraphNode n = (GraphNode) iter.next(); |
---|
| 30 | Point pct = (Point) (((Road) (Globals.map.roads.get(n.roadIndex))).points |
---|
| 31 | .get(n.pointIndex)); |
---|
| 32 | if (pct.equals(p)) |
---|
| 33 | return n.distance; |
---|
| 34 | } |
---|
| 35 | // System.out.println("ERROR! getDistance() NOT FOUND!\n"); |
---|
| 36 | return -1.0; |
---|
| 37 | } |
---|
| 38 | |
---|
| 39 | class GraphNode { |
---|
| 40 | // a point on the discrete map; |
---|
| 41 | // used for the dijkstra implementation; |
---|
| 42 | public int roadIndex; |
---|
| 43 | public int pointIndex; // these two uniquely identify the node; |
---|
| 44 | int parentRoadIndex; |
---|
| 45 | int parentPointIndex; // these two uniquely identify the parent |
---|
| 46 | public double distance; // used in the dijkstra implementation |
---|
| 47 | |
---|
| 48 | public GraphNode(int roadIndex, int pointIndex) { |
---|
| 49 | this.roadIndex = roadIndex; |
---|
| 50 | this.pointIndex = pointIndex; |
---|
| 51 | parentRoadIndex = -1; |
---|
| 52 | parentPointIndex = -1; |
---|
| 53 | distance = Double.MAX_VALUE; |
---|
| 54 | } |
---|
| 55 | |
---|
| 56 | public boolean differentFromJustIdx(GraphNode t) { |
---|
| 57 | |
---|
| 58 | if ((this.parentPointIndex != t.parentPointIndex) |
---|
| 59 | || (this.parentRoadIndex != t.parentRoadIndex) |
---|
| 60 | || (this.pointIndex != t.pointIndex) |
---|
| 61 | || (this.roadIndex != t.roadIndex)) { |
---|
| 62 | |
---|
| 63 | return true; |
---|
| 64 | } |
---|
| 65 | return false; |
---|
| 66 | } |
---|
| 67 | |
---|
| 68 | public boolean differentFrom(GraphNode t) { |
---|
| 69 | // System.out.println("Compar parent:"+this.parentRoadIndex+" |
---|
| 70 | // sourcePointIdx="+this.parentPointIndex+ "cu |
---|
| 71 | // parent"+t.parentRoadIndex+" cu |
---|
| 72 | // p"+t.parentPointIndex); |
---|
| 73 | // System.out.println("si copill:"+this.roadIndex+" |
---|
| 74 | // sourcePointIdx="+this.pointIndex+ "cu parent"+t.roadIndex+" cu |
---|
| 75 | // p"+t.pointIndex); |
---|
| 76 | boolean dif1 = false, dif2 = false, found = false; |
---|
| 77 | int i; |
---|
| 78 | Cross c; |
---|
| 79 | if ((t.parentRoadIndex == this.parentRoadIndex) |
---|
| 80 | && (t.parentPointIndex != this.parentPointIndex)) { |
---|
| 81 | dif1 = true; |
---|
| 82 | |
---|
| 83 | } else { |
---|
| 84 | if (t.parentRoadIndex != this.parentRoadIndex) { |
---|
| 85 | |
---|
| 86 | for (i = 0; i < Globals.map.roads.get(t.parentRoadIndex).crosses |
---|
| 87 | .size(); i++) { |
---|
| 88 | c = Globals.map.roads.get(t.parentRoadIndex).crosses |
---|
| 89 | .get(i); |
---|
| 90 | if (c.getCrossRoadIndex() == this.parentRoadIndex) { |
---|
| 91 | if ((c.getPointIndex() == t.parentPointIndex) |
---|
| 92 | && (c.getCrossPointIndex() == this.parentPointIndex)) { |
---|
| 93 | found = true; |
---|
| 94 | break; |
---|
| 95 | } |
---|
| 96 | } |
---|
| 97 | } |
---|
| 98 | if (!found) { |
---|
| 99 | dif1 = true; |
---|
| 100 | } |
---|
| 101 | |
---|
| 102 | } |
---|
| 103 | |
---|
| 104 | } |
---|
| 105 | |
---|
| 106 | // /////fara parent |
---|
| 107 | found = false; |
---|
| 108 | if ((t.roadIndex == this.roadIndex) |
---|
| 109 | && (t.pointIndex != this.pointIndex)) { |
---|
| 110 | dif2 = true; |
---|
| 111 | |
---|
| 112 | } else { |
---|
| 113 | if (t.roadIndex != this.roadIndex) { |
---|
| 114 | |
---|
| 115 | for (i = 0; i < Globals.map.roads.get(t.roadIndex).crosses |
---|
| 116 | .size(); i++) { |
---|
| 117 | c = Globals.map.roads.get(t.roadIndex).crosses.get(i); |
---|
| 118 | if (c.getCrossRoadIndex() == this.roadIndex) { |
---|
| 119 | if ((c.getPointIndex() == t.pointIndex) |
---|
| 120 | && (c.getCrossPointIndex() == this.pointIndex)) { |
---|
| 121 | found = true; |
---|
| 122 | break; |
---|
| 123 | } |
---|
| 124 | } |
---|
| 125 | } |
---|
| 126 | if (!found) { |
---|
| 127 | dif2 = true; |
---|
| 128 | } |
---|
| 129 | |
---|
| 130 | } |
---|
| 131 | |
---|
| 132 | } |
---|
| 133 | |
---|
| 134 | return (dif1 || dif2); |
---|
| 135 | } |
---|
| 136 | |
---|
| 137 | public GraphNode(int roadIndex, int pointIndex, int parentRoadIndex, |
---|
| 138 | int parentPointIndex, double distance) { |
---|
| 139 | this.roadIndex = roadIndex; |
---|
| 140 | this.pointIndex = pointIndex; |
---|
| 141 | this.parentRoadIndex = parentRoadIndex; |
---|
| 142 | this.parentPointIndex = parentPointIndex; |
---|
| 143 | this.distance = distance; |
---|
| 144 | } |
---|
| 145 | |
---|
| 146 | public GraphNode(GraphNode g) { |
---|
| 147 | this.roadIndex = g.roadIndex; |
---|
| 148 | this.pointIndex = g.pointIndex; |
---|
| 149 | this.parentRoadIndex = g.parentRoadIndex; |
---|
| 150 | this.parentPointIndex = g.parentPointIndex; |
---|
| 151 | this.distance = g.distance; |
---|
| 152 | } |
---|
| 153 | |
---|
| 154 | public boolean equals(GraphNode n) { |
---|
| 155 | Point p1 = (Point) ((Road) (Globals.map.roads.get(roadIndex))).points |
---|
| 156 | .get(pointIndex); |
---|
| 157 | Point p2 = (Point) ((Road) (Globals.map.roads.get(n.roadIndex))).points |
---|
| 158 | .get(n.pointIndex); |
---|
| 159 | return p1.equals(p2); |
---|
| 160 | } |
---|
| 161 | |
---|
| 162 | public String toString() { |
---|
| 163 | String ret = "[Road=" + roadIndex + ";Point=" + pointIndex + "]"; |
---|
| 164 | return ret; |
---|
| 165 | } |
---|
| 166 | |
---|
| 167 | public boolean containsLocation(Location dest) { |
---|
| 168 | int i, p1, p2; |
---|
| 169 | if (dest.roadIdx != this.roadIndex) { |
---|
| 170 | return false; |
---|
| 171 | } |
---|
| 172 | if (this.parentRoadIndex != this.roadIndex) { |
---|
| 173 | |
---|
| 174 | for (i = 0; i < Globals.map.roads.get(this.parentRoadIndex).crosses |
---|
| 175 | .size(); i++) { |
---|
| 176 | if (Globals.map.roads.get(this.parentRoadIndex).crosses |
---|
| 177 | .get(i).getCrossRoadIndex() == this.roadIndex) { |
---|
| 178 | p1 = Globals.map.roads.get(this.parentRoadIndex).crosses |
---|
| 179 | .get(i).getCrossPointIndex(); |
---|
| 180 | p2 = this.pointIndex; |
---|
| 181 | if (p1 < p2) { |
---|
| 182 | if (p1 < dest.ptIdx && dest.ptIdx < p2) { |
---|
| 183 | return true; |
---|
| 184 | } |
---|
| 185 | } else { |
---|
| 186 | if (p1 > dest.ptIdx && dest.ptIdx > p2) { |
---|
| 187 | return true; |
---|
| 188 | } |
---|
| 189 | } |
---|
| 190 | |
---|
| 191 | } |
---|
| 192 | } |
---|
| 193 | |
---|
| 194 | } else { |
---|
| 195 | |
---|
| 196 | p1 = this.parentPointIndex; |
---|
| 197 | p2 = this.pointIndex; |
---|
| 198 | if (p1 < p2) { |
---|
| 199 | if (p1 < dest.ptIdx && dest.ptIdx < p2) { |
---|
| 200 | return true; |
---|
| 201 | } |
---|
| 202 | } else { |
---|
| 203 | if (p1 > dest.ptIdx && dest.ptIdx > p2) { |
---|
| 204 | return true; |
---|
| 205 | } |
---|
| 206 | } |
---|
| 207 | |
---|
| 208 | } |
---|
| 209 | |
---|
| 210 | return false; |
---|
| 211 | } |
---|
| 212 | } |
---|
| 213 | |
---|
| 214 | class NeighborNode { |
---|
| 215 | GraphNode neigh; |
---|
| 216 | |
---|
| 217 | double dist; // distance to that neighbor |
---|
| 218 | |
---|
| 219 | public NeighborNode(int roadIndex, int pointIndex, double dist) { |
---|
| 220 | this.dist = dist; |
---|
| 221 | neigh = new GraphNode(roadIndex, pointIndex); |
---|
| 222 | } |
---|
| 223 | |
---|
| 224 | public String toString() { |
---|
| 225 | String ret = "[Neigh:" + neigh + " ; dist=" + dist + "]"; |
---|
| 226 | return ret; |
---|
| 227 | } |
---|
| 228 | } |
---|
| 229 | |
---|
| 230 | class GraphNodeArrayList { |
---|
| 231 | // a vector of "graph nodes"; it keeps the graph nodes ordered (performs |
---|
| 232 | // an insertion sorting, when |
---|
| 233 | // adding an element); it also prevent duplicates |
---|
| 234 | |
---|
| 235 | ArrayList<GraphNode> v; |
---|
| 236 | |
---|
| 237 | public GraphNodeArrayList() { |
---|
| 238 | v = new ArrayList<GraphNode>(); |
---|
| 239 | } |
---|
| 240 | |
---|
| 241 | public void addGraphNode(GraphNode n) { |
---|
| 242 | // insertion-sorting add, based on the 'distance' of the graph nodes |
---|
| 243 | for (int i = 0; i < v.size(); i++) { |
---|
| 244 | GraphNode node = (GraphNode) v.get(i); |
---|
| 245 | if (n.distance < node.distance) { |
---|
| 246 | v.add(i, n); |
---|
| 247 | return; |
---|
| 248 | } |
---|
| 249 | } |
---|
| 250 | v.add(n); |
---|
| 251 | } |
---|
| 252 | |
---|
| 253 | public GraphNode removeFirst() { |
---|
| 254 | // gets the first node and removes it from the collection |
---|
| 255 | if (v.size() > 0) { |
---|
| 256 | GraphNode n = (GraphNode) v.get(0); |
---|
| 257 | v.remove(0); |
---|
| 258 | return n; |
---|
| 259 | } |
---|
| 260 | return null; |
---|
| 261 | } |
---|
| 262 | |
---|
| 263 | public Iterator<GraphNode> iterator() { |
---|
| 264 | return v.iterator(); |
---|
| 265 | } |
---|
| 266 | |
---|
| 267 | public void setGraphNode(GraphNode n) { |
---|
| 268 | // looks through the vector, finds a graph node referring to the |
---|
| 269 | // same point (by |
---|
| 270 | // checking the roadSegment of the point), and replaces that graph |
---|
| 271 | // node; |
---|
| 272 | Point pct1 = (Point) (((Road) (Globals.map.roads.get(n.roadIndex))).points |
---|
| 273 | .get(n.pointIndex)); |
---|
| 274 | for (int i = 0; i < v.size(); i++) { |
---|
| 275 | GraphNode node = (GraphNode) v.get(i); |
---|
| 276 | Point pct2 = (Point) (((Road) (Globals.map.roads |
---|
| 277 | .get(node.roadIndex))).points.get(node.pointIndex)); |
---|
| 278 | if (pct1.equals(pct2)) { |
---|
| 279 | // that's the point; replace it |
---|
| 280 | v.set(i, n); |
---|
| 281 | } |
---|
| 282 | } |
---|
| 283 | } |
---|
| 284 | |
---|
| 285 | public int size() { |
---|
| 286 | return v.size(); |
---|
| 287 | } |
---|
| 288 | |
---|
| 289 | public boolean contains(GraphNode node) { |
---|
| 290 | for (int i = 0; i < v.size(); i++) { |
---|
| 291 | GraphNode node1 = (GraphNode) v.get(i); |
---|
| 292 | if (node1.equals(node)) |
---|
| 293 | return true; |
---|
| 294 | } |
---|
| 295 | return false; |
---|
| 296 | } |
---|
| 297 | } |
---|
| 298 | |
---|
| 299 | // ////////////////////////////////////////////////////////////// |
---|
| 300 | |
---|
| 301 | public ArrayList<Location> dijkstraPath(Location src, Location dest, |
---|
| 302 | Location parentAvoid, Location nodeAvoid) { |
---|
| 303 | // returns a vector of consecutive RouteSegments, the shortest route |
---|
| 304 | // between source and destination |
---|
| 305 | |
---|
| 306 | // System.out.println("Have to compute a route: from "+source+" -- to |
---|
| 307 | // "+destination); |
---|
| 308 | GraphNode avoid = new GraphNode(-1, -1); |
---|
| 309 | avoid.parentRoadIndex = parentAvoid.roadIdx; |
---|
| 310 | avoid.parentPointIndex = parentAvoid.ptIdx; |
---|
| 311 | avoid.roadIndex = nodeAvoid.roadIdx; |
---|
| 312 | avoid.pointIndex = nodeAvoid.ptIdx; |
---|
| 313 | |
---|
| 314 | // System.out.println("avoid is " + avoid.parentRoadIndex + " " |
---|
| 315 | // + avoid.parentPointIndex + " spre " + avoid.roadIndex + " " |
---|
| 316 | // + avoid.pointIndex); |
---|
| 317 | |
---|
| 318 | ArrayList<Location> routePoints = new ArrayList<Location>(); |
---|
| 319 | |
---|
| 320 | GraphNodeArrayList set = new GraphNodeArrayList(); |
---|
| 321 | |
---|
| 322 | GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx); |
---|
| 323 | ArrayList<NeighborNode> sourceNeighbors = getNeighborsWithCost(sourceGraphNode); |
---|
| 324 | GraphNode tempNode; |
---|
| 325 | // add the direct neighbors of the source to the set of nodes; |
---|
| 326 | for (int i = 0; i < sourceNeighbors.size(); i++) { |
---|
| 327 | NeighborNode srcNeig = sourceNeighbors.get(i); |
---|
| 328 | int rd = srcNeig.neigh.roadIndex; |
---|
| 329 | int pt = srcNeig.neigh.pointIndex; |
---|
| 330 | tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx, |
---|
| 331 | srcNeig.dist); |
---|
| 332 | if (avoid.roadIndex == -1) { |
---|
| 333 | set.addGraphNode(tempNode); |
---|
| 334 | } else { |
---|
| 335 | if ((avoid.roadIndex >= 0) && avoid.differentFrom(tempNode)) { |
---|
| 336 | set.addGraphNode(tempNode); |
---|
| 337 | } |
---|
| 338 | |
---|
| 339 | } |
---|
| 340 | |
---|
| 341 | } |
---|
| 342 | |
---|
| 343 | GraphNodeArrayList solved = new GraphNodeArrayList(); |
---|
| 344 | |
---|
| 345 | solved.addGraphNode(sourceGraphNode); |
---|
| 346 | |
---|
| 347 | while (true) { |
---|
| 348 | // get the first node |
---|
| 349 | GraphNode first = null; |
---|
| 350 | first = (GraphNode) set.removeFirst(); |
---|
| 351 | if (first == null) { |
---|
| 352 | return null; |
---|
| 353 | } |
---|
| 354 | solved.addGraphNode(first); |
---|
| 355 | // System.out.println("Solved:"+first+"; |
---|
| 356 | if ((first.roadIndex == dest.roadIdx) |
---|
| 357 | && (first.pointIndex == dest.ptIdx)) { |
---|
| 358 | // System.out.println("Solved is "+first); |
---|
| 359 | // first is solved; try to get the route built into 'ret' |
---|
| 360 | GraphNode aux = first; |
---|
| 361 | routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex)); |
---|
| 362 | while (true) { |
---|
| 363 | if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) { |
---|
| 364 | break; |
---|
| 365 | } |
---|
| 366 | |
---|
| 367 | routePoints.add(0, new Location(aux.parentRoadIndex, |
---|
| 368 | aux.parentPointIndex)); |
---|
| 369 | |
---|
| 370 | // System.out.println("Parent: road "+aux.parentRoadIndex+"; |
---|
| 371 | // find the parent in the 'solved' set |
---|
| 372 | Road rdAux = (Road) Globals.map.roads |
---|
| 373 | .get(aux.parentRoadIndex); |
---|
| 374 | Point ptAux = (Point) rdAux.points |
---|
| 375 | .get(aux.parentPointIndex); |
---|
| 376 | Iterator<GraphNode> iter = solved.iterator(); |
---|
| 377 | boolean found = false; |
---|
| 378 | while (iter.hasNext()) { |
---|
| 379 | GraphNode aux2 = (GraphNode) iter.next(); |
---|
| 380 | Road rdAux2 = (Road) Globals.map.roads |
---|
| 381 | .get(aux2.roadIndex); |
---|
| 382 | Point ptAux2 = (Point) rdAux2.points |
---|
| 383 | .get(aux2.pointIndex); |
---|
| 384 | if (ptAux2.equals(ptAux)) { |
---|
| 385 | aux = aux2; |
---|
| 386 | found = true; |
---|
| 387 | break; |
---|
| 388 | } |
---|
| 389 | } |
---|
| 390 | if (found == false) { |
---|
| 391 | System.out.println("ERROR! Parent NOT FOUND!:road " |
---|
| 392 | + aux.parentRoadIndex + "; point " |
---|
| 393 | + aux.parentPointIndex); |
---|
| 394 | break; |
---|
| 395 | } else { |
---|
| 396 | } |
---|
| 397 | } |
---|
| 398 | break; |
---|
| 399 | } |
---|
| 400 | |
---|
| 401 | if (first.containsLocation(dest)) { |
---|
| 402 | // System.out.println("Solved is "+first); |
---|
| 403 | // first is solved; try to get the route built into 'ret' |
---|
| 404 | GraphNode aux = first; |
---|
| 405 | first.roadIndex = dest.roadIdx; |
---|
| 406 | first.pointIndex = dest.ptIdx; |
---|
| 407 | |
---|
| 408 | routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex)); |
---|
| 409 | while (true) { |
---|
| 410 | if (aux.parentRoadIndex == -1 || aux.parentPointIndex == -1) { |
---|
| 411 | break; |
---|
| 412 | } |
---|
| 413 | |
---|
| 414 | routePoints.add(0, new Location(aux.parentRoadIndex, |
---|
| 415 | aux.parentPointIndex)); |
---|
| 416 | // System.out.println("Parent: road "+aux.parentRoadIndex+"; |
---|
| 417 | // find the parent in the 'solved' set |
---|
| 418 | Road rdAux = (Road) Globals.map.roads |
---|
| 419 | .get(aux.parentRoadIndex); |
---|
| 420 | Point ptAux = (Point) rdAux.points |
---|
| 421 | .get(aux.parentPointIndex); |
---|
| 422 | Iterator<GraphNode> iter = solved.iterator(); |
---|
| 423 | boolean found = false; |
---|
| 424 | while (iter.hasNext()) { |
---|
| 425 | GraphNode aux2 = (GraphNode) iter.next(); |
---|
| 426 | Road rdAux2 = (Road) Globals.map.roads |
---|
| 427 | .get(aux2.roadIndex); |
---|
| 428 | Point ptAux2 = (Point) rdAux2.points |
---|
| 429 | .get(aux2.pointIndex); |
---|
| 430 | if (ptAux2.equals(ptAux)) { |
---|
| 431 | aux = aux2; |
---|
| 432 | found = true; |
---|
| 433 | break; |
---|
| 434 | } |
---|
| 435 | } |
---|
| 436 | if (found == false) { |
---|
| 437 | System.out.println("ERROR! Parent NOT FOUND!:road " |
---|
| 438 | + aux.parentRoadIndex + "; point " |
---|
| 439 | + aux.parentPointIndex); |
---|
| 440 | break; |
---|
| 441 | } else { |
---|
| 442 | } |
---|
| 443 | } |
---|
| 444 | break; |
---|
| 445 | } |
---|
| 446 | |
---|
| 447 | // relaxation |
---|
| 448 | ArrayList<NeighborNode> vecini = getNeighborsWithCost(first); |
---|
| 449 | // System.out.println("Neighbors for "+first+":"+vecini); |
---|
| 450 | for (int j = 0; j < vecini.size(); j++) { |
---|
| 451 | NeighborNode n = (NeighborNode) vecini.get(j); |
---|
| 452 | if (solved.contains(n.neigh)) |
---|
| 453 | continue; |
---|
| 454 | // only if it's not solved |
---|
| 455 | Point ptAux = (Point) (((Road) (Globals.map.roads |
---|
| 456 | .get(n.neigh.roadIndex))).points |
---|
| 457 | .get(n.neigh.pointIndex)); |
---|
| 458 | double oldDist = getDistance(set, ptAux); |
---|
| 459 | double d1 = n.dist; |
---|
| 460 | if (oldDist == -1.0) { |
---|
| 461 | n.neigh.distance = d1 + first.distance; |
---|
| 462 | n.neigh.parentPointIndex = first.pointIndex; |
---|
| 463 | n.neigh.parentRoadIndex = first.roadIndex; |
---|
| 464 | if (avoid.roadIndex == -1) { |
---|
| 465 | set.addGraphNode(n.neigh); |
---|
| 466 | } else { |
---|
| 467 | if ((avoid.roadIndex >= 0) |
---|
| 468 | && avoid.differentFrom(n.neigh)) { |
---|
| 469 | set.addGraphNode(n.neigh); |
---|
| 470 | } |
---|
| 471 | } |
---|
| 472 | |
---|
| 473 | } else { |
---|
| 474 | if (d1 + first.distance < oldDist) { |
---|
| 475 | n.neigh.distance = d1 + first.distance; |
---|
| 476 | n.neigh.parentPointIndex = first.pointIndex; |
---|
| 477 | n.neigh.parentRoadIndex = first.roadIndex; |
---|
| 478 | if (avoid.roadIndex == -1) { |
---|
| 479 | set.addGraphNode(n.neigh); |
---|
| 480 | } else { |
---|
| 481 | if ((avoid.roadIndex >= 0) |
---|
| 482 | && avoid.differentFrom(n.neigh)) { |
---|
| 483 | set.addGraphNode(n.neigh); |
---|
| 484 | } |
---|
| 485 | |
---|
| 486 | } |
---|
| 487 | } |
---|
| 488 | } |
---|
| 489 | } |
---|
| 490 | } |
---|
| 491 | |
---|
| 492 | // now I have the 'routePoints' vector filled with Location objects; |
---|
| 493 | // do merging stuff; obtain the rez vector filled with RouteSegment |
---|
| 494 | // objects |
---|
| 495 | |
---|
| 496 | // System.out.println("Route before merge: points=" + routePoints); |
---|
| 497 | return routePoints; |
---|
| 498 | } |
---|
| 499 | |
---|
| 500 | private ArrayList<NeighborNode> getNeighborsWithCost(GraphNode node) { |
---|
| 501 | // get all the direct neighbors of 'node'; returns a vector of |
---|
| 502 | // 'NeighborNode' objects |
---|
| 503 | ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>(); |
---|
| 504 | |
---|
| 505 | // find the closest points on the same road which are cross-points |
---|
| 506 | // if there is none, take the end points of the road (except when the |
---|
| 507 | // node is already an end point); |
---|
| 508 | // also check if there are cross-roads in 'node'; |
---|
| 509 | Road road = (Road) Globals.map.roads.get(node.roadIndex); |
---|
| 510 | int nodeIndex = node.pointIndex; |
---|
| 511 | |
---|
| 512 | int min = Integer.MAX_VALUE; |
---|
| 513 | int max = -1; |
---|
| 514 | for (int i = 0; i < road.crosses.size(); i++) { |
---|
| 515 | Cross c = (Cross) road.crosses.get(i); |
---|
| 516 | int idx = c.getPointIndex(); |
---|
| 517 | if (idx > max && idx < nodeIndex) |
---|
| 518 | max = idx; |
---|
| 519 | if (idx < min && idx > nodeIndex) |
---|
| 520 | min = idx; |
---|
| 521 | Point pt = (Point) road.points.get(nodeIndex); |
---|
| 522 | if (idx == nodeIndex) { |
---|
| 523 | // have to go on the crossing road, to find the closest points; |
---|
| 524 | Road crossingRoad = (Road) Globals.map.roads.get(c |
---|
| 525 | .getCrossRoadIndex()); |
---|
| 526 | Point pt2 = null; |
---|
| 527 | int index2 = -1; |
---|
| 528 | // have to find the index of the point on the crossing road |
---|
| 529 | for (int j = 0; j < crossingRoad.points.size(); j++) { |
---|
| 530 | Point ptAux = (Point) crossingRoad.points.get(j); |
---|
| 531 | if (ptAux.equals(pt)) { |
---|
| 532 | // that's the one |
---|
| 533 | pt2 = ptAux; |
---|
| 534 | index2 = j; |
---|
| 535 | break; |
---|
| 536 | } |
---|
| 537 | } |
---|
| 538 | if (pt2 == null) { |
---|
| 539 | // System.out.println("RoutingModule.getNeighbors(): ERROR! |
---|
| 540 | // Could not find the point on the crossing |
---|
| 541 | // road:"+c.getCrossRoadIndex()); |
---|
| 542 | } else { |
---|
| 543 | // find the neighbors on the crossing road; |
---|
| 544 | int minCross = Integer.MAX_VALUE; |
---|
| 545 | int maxCross = -1; |
---|
| 546 | for (int k = 0; k < crossingRoad.crosses.size(); k++) { |
---|
| 547 | Cross otherCross = (Cross) crossingRoad.crosses.get(k); |
---|
| 548 | int otherIndex = otherCross.getPointIndex(); |
---|
| 549 | if (otherIndex > maxCross && otherIndex < index2) |
---|
| 550 | maxCross = otherIndex; |
---|
| 551 | if (otherIndex < minCross && otherIndex > index2) |
---|
| 552 | minCross = otherIndex; |
---|
| 553 | |
---|
| 554 | } |
---|
| 555 | if (minCross == Integer.MAX_VALUE) { |
---|
| 556 | // there was no min; have to consider last point of |
---|
| 557 | // crossing road |
---|
| 558 | // only if it is not the last |
---|
| 559 | if ((index2 != (crossingRoad.points.size() - 1))) { |
---|
| 560 | NeighborNode n = new NeighborNode(c |
---|
| 561 | .getCrossRoadIndex(), crossingRoad.points |
---|
| 562 | .size() - 1, getCostForDirectedRoadSegment( |
---|
| 563 | c.getCrossRoadIndex(), index2, |
---|
| 564 | crossingRoad.points.size() - 1)); |
---|
| 565 | rez.add(n); |
---|
| 566 | } |
---|
| 567 | } else { |
---|
| 568 | NeighborNode n = new NeighborNode( |
---|
| 569 | c.getCrossRoadIndex(), minCross, |
---|
| 570 | getCostForDirectedRoadSegment(c |
---|
| 571 | .getCrossRoadIndex(), index2, minCross)); |
---|
| 572 | rez.add(n); |
---|
| 573 | } |
---|
| 574 | if (maxCross == -1) { |
---|
| 575 | // there was no max; have to consider first point of |
---|
| 576 | // crossing road |
---|
| 577 | // only if it is not first |
---|
| 578 | if (index2 != 0) { |
---|
| 579 | NeighborNode n = new NeighborNode(c |
---|
| 580 | .getCrossRoadIndex(), 0, |
---|
| 581 | getCostForDirectedRoadSegment(c |
---|
| 582 | .getCrossRoadIndex(), index2, 0)); |
---|
| 583 | rez.add(n); |
---|
| 584 | } |
---|
| 585 | } else { |
---|
| 586 | NeighborNode n = new NeighborNode( |
---|
| 587 | c.getCrossRoadIndex(), maxCross, |
---|
| 588 | getCostForDirectedRoadSegment(c |
---|
| 589 | .getCrossRoadIndex(), index2, maxCross)); |
---|
| 590 | rez.add(n); |
---|
| 591 | } |
---|
| 592 | } |
---|
| 593 | } |
---|
| 594 | } |
---|
| 595 | if (min == Integer.MAX_VALUE) { |
---|
| 596 | // there was no min; have to consider last point of the road |
---|
| 597 | // ONLY if I am not the last! |
---|
| 598 | if (((road.points.size() - 1) != node.pointIndex)) { |
---|
| 599 | NeighborNode n = new NeighborNode(node.roadIndex, road.points |
---|
| 600 | .size() - 1, |
---|
| 601 | getCostForDirectedRoadSegment(node.roadIndex, |
---|
| 602 | node.pointIndex, road.points.size() - 1)); |
---|
| 603 | rez.add(n); |
---|
| 604 | } |
---|
| 605 | } else { |
---|
| 606 | NeighborNode n = new NeighborNode(node.roadIndex, min, |
---|
| 607 | getCostForDirectedRoadSegment(node.roadIndex, |
---|
| 608 | node.pointIndex, min)); |
---|
| 609 | rez.add(n); |
---|
| 610 | } |
---|
| 611 | if (max == -1) { |
---|
| 612 | // there was no max; have to consider first point of the road |
---|
| 613 | // only if I am not first |
---|
| 614 | if (node.pointIndex != 0) { |
---|
| 615 | NeighborNode n = new NeighborNode(node.roadIndex, 0, |
---|
| 616 | getCostForDirectedRoadSegment(node.roadIndex, |
---|
| 617 | node.pointIndex, 0)); |
---|
| 618 | rez.add(n); |
---|
| 619 | } |
---|
| 620 | } else { |
---|
| 621 | NeighborNode n = new NeighborNode(node.roadIndex, max, |
---|
| 622 | getCostForDirectedRoadSegment(node.roadIndex, |
---|
| 623 | node.pointIndex, max)); |
---|
| 624 | rez.add(n); |
---|
| 625 | } |
---|
| 626 | return rez; |
---|
| 627 | } |
---|
| 628 | |
---|
| 629 | private double getCostForDirectedRoadSegment(int r, int p1, int p2) { |
---|
| 630 | // all intersections must be in server db |
---|
| 631 | boolean dir; |
---|
| 632 | if (p1 < p2) { |
---|
| 633 | dir = true; |
---|
| 634 | } else { |
---|
| 635 | dir = false; |
---|
| 636 | } |
---|
| 637 | DirectedRoadSegment drs = new DirectedRoadSegment(r, p2, (!dir)); |
---|
| 638 | RoadSegmentCongestion rsc; |
---|
| 639 | for (int i = 0; i < updatedCongestion.size(); i++) { |
---|
| 640 | rsc = updatedCongestion.get(i); |
---|
| 641 | if (rsc.equals(drs)) { |
---|
| 642 | return getRoadSegmentDistance(r, p1, p2) |
---|
| 643 | * (1 + kCongestionRatio * rsc.getCongestion()); |
---|
| 644 | } |
---|
| 645 | } |
---|
| 646 | int time = Globals.engine.crtTime / Server.kTimeCompression; |
---|
| 647 | time = time / 3600; // hours |
---|
| 648 | int hour = time % 24; |
---|
| 649 | time = time / 24; |
---|
| 650 | int day = time % 7; |
---|
| 651 | |
---|
| 652 | Intersection it; |
---|
| 653 | DirectedRoadSegment tempDrs; |
---|
| 654 | for (int i = 0; i < Globals.map.allIntersections.size(); i++) { |
---|
| 655 | it = Globals.map.allIntersections.get(i); |
---|
| 656 | for (int j = 0; j < it.segments.size(); j++) { |
---|
| 657 | tempDrs = it.segments.get(j); |
---|
| 658 | if (tempDrs.equals(drs)) { |
---|
| 659 | Double congestion = Globals.lastWeekCongestions.get(i).get( |
---|
| 660 | j).getCongestion(day, hour); |
---|
| 661 | return getRoadSegmentDistance(r, p1, p2) |
---|
| 662 | * (1 + kCongestionRatio * congestion); |
---|
| 663 | } |
---|
| 664 | } |
---|
| 665 | } |
---|
| 666 | |
---|
| 667 | return getRoadSegmentDistance(r, p1, p2); |
---|
| 668 | |
---|
| 669 | } |
---|
| 670 | |
---|
| 671 | // road segment length in meters |
---|
| 672 | private static double getRoadSegmentDistance(int r, int p1, int p2) { |
---|
| 673 | Road road = Globals.map.roads.get(r); |
---|
| 674 | Point point1 = road.points.get(p1); |
---|
| 675 | Point point2 = road.points.get(p2); |
---|
| 676 | return Math.abs(point2.getDistance() - point1.getDistance()) * 1000; |
---|
| 677 | } |
---|
| 678 | |
---|
| 679 | // road segment length in meters |
---|
| 680 | public static Double getRoadSegmentDistance(DirectedRoadSegment drs) { |
---|
| 681 | Road road = Globals.map.roads.get(drs.roadIndex); |
---|
| 682 | Point p1 = road.points.get(drs.pointIndex); |
---|
| 683 | Point p2; |
---|
| 684 | Cross cross; |
---|
| 685 | if (drs.direction) { |
---|
| 686 | cross = getNextCross(drs.roadIndex, drs.pointIndex, true); |
---|
| 687 | if (cross == null) { // last point on the road |
---|
| 688 | p2 = road.points.get(road.points.size() - 1); |
---|
| 689 | } else { |
---|
| 690 | p2 = road.points.get(cross.getPointIndex()); |
---|
| 691 | } |
---|
| 692 | } else { |
---|
| 693 | cross = getNextCross(drs.roadIndex, drs.pointIndex, false); |
---|
| 694 | if (cross == null) { // first point on the road |
---|
| 695 | p2 = road.points.get(0); |
---|
| 696 | } else { |
---|
| 697 | p2 = road.points.get(cross.getPointIndex()); |
---|
| 698 | } |
---|
| 699 | } |
---|
| 700 | return Math.abs(p2.getDistance() - p1.getDistance()) * 1000; |
---|
| 701 | } |
---|
| 702 | |
---|
| 703 | // gets the next or previews cross from the Location(roadIndex, pointIndex) |
---|
| 704 | // located on the same road |
---|
| 705 | // if next is false return the previews cross |
---|
| 706 | private static Cross getNextCross(int roadIndex, int pointIndex, |
---|
| 707 | boolean next) { |
---|
| 708 | Road road = Globals.map.roads.get(roadIndex); |
---|
| 709 | Cross cross; |
---|
| 710 | int i; |
---|
| 711 | for (i = 0; i < road.crosses.size(); i++) { |
---|
| 712 | cross = road.crosses.get(i); |
---|
| 713 | if (cross.getPointIndex() == pointIndex) { |
---|
| 714 | break; |
---|
| 715 | } |
---|
| 716 | } |
---|
| 717 | if (next) { |
---|
| 718 | i++; |
---|
| 719 | } else { |
---|
| 720 | i--; |
---|
| 721 | } |
---|
| 722 | if (i < 0 || i >= road.crosses.size()) { |
---|
| 723 | return null; |
---|
| 724 | } |
---|
| 725 | return road.crosses.get(i); |
---|
| 726 | } |
---|
| 727 | |
---|
| 728 | public static ArrayList<RouteSegment> mergeCityRoute( |
---|
| 729 | ArrayList<Location> routePoints) { |
---|
| 730 | // takes a vector of Location objects (points) and returns a vector of |
---|
| 731 | // RouteSegment objects, |
---|
| 732 | // representing the route as a sequence of roads, rather than individual |
---|
| 733 | // points |
---|
| 734 | |
---|
| 735 | int p1, pLastCand, p2; // point indexes |
---|
| 736 | int r1, r2; // road indexes |
---|
| 737 | int index = 0; |
---|
| 738 | ArrayList<RouteSegment> ret = new ArrayList<RouteSegment>(); |
---|
| 739 | |
---|
| 740 | if (routePoints == null || routePoints.size() == 0) { |
---|
| 741 | System.out |
---|
| 742 | .println("ERROR! routingModule.dijkstraPath() - routePoints has no points!"); |
---|
| 743 | return ret; |
---|
| 744 | } |
---|
| 745 | |
---|
| 746 | p1 = ((Location) routePoints.get(0)).ptIdx; |
---|
| 747 | r1 = ((Location) routePoints.get(0)).roadIdx; |
---|
| 748 | pLastCand = ((Location) routePoints.get(0)).ptIdx; |
---|
| 749 | |
---|
| 750 | index++; |
---|
| 751 | |
---|
| 752 | while (index < routePoints.size()) { |
---|
| 753 | p2 = ((Location) routePoints.get(index)).ptIdx; |
---|
| 754 | r2 = ((Location) routePoints.get(index)).roadIdx; |
---|
| 755 | if (r2 == r1) { |
---|
| 756 | pLastCand = p2; |
---|
| 757 | } else { |
---|
| 758 | boolean found = false; |
---|
| 759 | // find (r1,pLastCand) on r2 |
---|
| 760 | Point ptLC2 = (Point) (((Road) (Globals.map.roads.get(r1))).points |
---|
| 761 | .get(pLastCand)); |
---|
| 762 | Road road2 = (Road) Globals.map.roads.get(r2); |
---|
| 763 | for (int i = 0; i < road2.points.size(); i++) { |
---|
| 764 | Point pt2 = (Point) road2.points.get(i); |
---|
| 765 | if (pt2.equals(ptLC2)) { |
---|
| 766 | found = true; |
---|
| 767 | ret.add(new RouteSegment((short) r1, (short) p1, |
---|
| 768 | (short) pLastCand)); |
---|
| 769 | p1 = i; |
---|
| 770 | r1 = r2; |
---|
| 771 | pLastCand = p2; |
---|
| 772 | break; |
---|
| 773 | } |
---|
| 774 | } |
---|
| 775 | if (!found) { |
---|
| 776 | System.out |
---|
| 777 | .println("RoutingModule.mergePoints() - situation not implemented!; returning null!"); |
---|
| 778 | return null; |
---|
| 779 | } |
---|
| 780 | } |
---|
| 781 | index++; |
---|
| 782 | if (index == routePoints.size()) { |
---|
| 783 | // it is the last iteration; add the last RouteSegment; |
---|
| 784 | ret.add(new RouteSegment((short) r1, (short) p1, |
---|
| 785 | (short) pLastCand)); |
---|
| 786 | } |
---|
| 787 | } |
---|
| 788 | // it happens that a route segment has the same source and destination |
---|
| 789 | // remove this route segment if it happens |
---|
| 790 | RouteSegment rs = ret.get(0); |
---|
| 791 | if (rs.pt1==rs.pt2) { |
---|
| 792 | ret.remove(0); |
---|
| 793 | } |
---|
| 794 | return ret; |
---|
| 795 | } |
---|
| 796 | } |
---|