source: proiecte/ptvs/src/vnsim/vehicular/routePlan/cityRouting/CityRouteComputingUtlis.java @ 31

Last change on this file since 31 was 31, checked in by (none), 14 years ago
File size: 23.6 KB
Line 
1package vnsim.vehicular.routePlan.cityRouting;
2
3import java.util.ArrayList;
4import java.util.Iterator;
5
6import vnsim.map.object.Cross;
7import vnsim.map.object.Globals;
8import vnsim.map.object.Point;
9import vnsim.map.object.Road;
10import vnsim.vehicular.simulator.Location;
11import vnsim.vehicular.simulator.RouteSegment;
12import vnsim.vehicular.simulator.intersections.DirectedRoadSegment;
13import vnsim.vehicular.simulator.intersections.Intersection;
14
15public 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}
Note: See TracBrowser for help on using the repository browser.