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 | } |
---|