Context Navigation

← Previous Revision
Latest Revision
Next Revision →
Blame
Revision Log

RouteComputingUtils.java @ 31

Last change on this file since 31 was 31, checked in by (none), 14 years ago

File size: 77.8 KB

Line
1	/************************************************************************************
2	* Copyright (C) 2008 by Politehnica University of Bucharest and Rutgers University
3	* All rights reserved.
4	* Refer to LICENSE for terms and conditions of use.
5	***********************************************************************************/
6	package vnsim.vehicular.routePlan;
7
8
9	import java.util.ArrayList;
10	import java.util.Iterator;
11
12	import vnsim.map.object.Cross;
13	import vnsim.map.object.Globals;
14	import vnsim.map.object.Point;
15	import vnsim.map.object.Road;
16	import vnsim.vehicular.routePlan.infrastructureRouted.DelayRecord;
17	import vnsim.vehicular.scenarios.Route;
18	import vnsim.vehicular.simulator.Location;
19	import vnsim.vehicular.simulator.RouteSegment;
20	import vnsim.vehicular.simulator.RoutingUtils;
21	import vnsim.vehicular.simulator.intersections.DirectedRoadSegment;
22
23
24	public class RouteComputingUtils {
25	public final int SAME_AREA = 0;
26
27	public final int AREA_TO_MAIN = 1;
28
29	public final int MAIN_TO_MAIN = 2;
30
31	public RouteComputingUtils() {
32
33	}
34
35	public ArrayList<Location> shortestDetour(Location src, Location dest,
36	Location parentAvoid, Location nodeAvoid) {
37	GraphNode avoid = new GraphNode(nodeAvoid.roadIdx, nodeAvoid.ptIdx);
38	avoid.parentRoadIndex = parentAvoid.roadIdx;
39	avoid.parentPointIndex = parentAvoid.ptIdx;
40
41	return dijkstraPathByShortestDistance(src, dest, avoid);
42	}
43
44	public Route shortestPathByTimeAproximation(Location src, Location dest) {
45	GraphNode avoid = new GraphNode(-1, -1);
46	Route r;
47	ArrayList<Location> points = null;
48
49	points = dijkstraPathByTimeAproximation(src, dest, avoid);
50	points.add(dest);
51	if (points != null) {
52	r = new Route();
53	r.entry = src;
54	r.exit = dest;
55	r.route = RoutingUtils.mergeRoute(points);
56	return r;
57	}
58	return null;
59	}
60
61	public ArrayList<Route> bestEightRoutes(Location src, Location dest) {
62	ArrayList<Route> result = new ArrayList<Route>();
63	Point s, d;
64	int j;
65	s = Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx);
66	d = Globals.map.roads.get(dest.roadIdx).points.get(dest.ptIdx);
67	if (s.equals(d)) {
68	// System.out
69	// .println("The source and the destination are the same point");
70	return result;
71	}
72
73	GraphNode avoidable;
74
75	if (Globals.map.mapSpliter.inSameArea(
76	s.areaCode.belongingTo.areaCodeByte,
77	d.areaCode.belongingTo.areaCodeByte)) {
78	// System.out.println("Same Road Areas");
79	ArrayList<RouteSegment> ret;
80	Route r;
81	ArrayList<Location> points;
82	avoidable = new GraphNode(-1, -1);
83
84	points = dijkstraPathInSplittedMap(src, dest, avoidable, this.SAME_AREA);
85
86	if (points != null) {
87
88	points.add(new Location(dest.roadIdx, dest.ptIdx));
89
90	ret = mergeRoute(points);
91	r = new Route();
92	r.entry = src;
93	r.exit = dest;
94	r.route = ret;
95	result.add(r);
96	} else {
97	return null;
98	}
99	points = null;
100	points = dijkstraPathInSplittedMap(src, dest, avoidable, this.SAME_AREA);
101	if (points != null) {
102	points.add(new Location(dest.roadIdx, dest.ptIdx));
103
104	ret = mergeRoute(points);
105	r = new Route();
106	r.entry = src;
107	r.exit = dest;
108	r.route = ret;
109	result.add(r);
110	}
111
112	} else {
113	// System.out.println("Plec cu sursa=" + source.roadIdx + "-" +
114	// source.ptIdx
115	// + " destination=" + destination.roadIdx + "-" + destination.ptIdx);
116	Route r;
117	int k;
118	ArrayList<RouteSegment> tempRet;
119	int[] middleIdx = new int[8];
120	ArrayList<ArrayList<Location>> allPoints = new ArrayList<ArrayList<Location>>();
121	for (j = 0; j < 8; j++) {
122	allPoints.add(new ArrayList<Location>());
123	}
124
125	ArrayList<Location> pDest, pMain, pSource;
126	avoidable = new GraphNode(-1, -1);
127	pSource = dijkstraPathInSplittedMap(src, null, avoidable, this.AREA_TO_MAIN);
128	if (pSource == null) {
129	System.out
130	.println("No road was found between SOURCE and a main road");
131	return null;
132	}
133
134	for (k = 0; k < pSource.size(); k++) {
135
136	allPoints.get(0).add(
137	new Location(pSource.get(k).roadIdx,
138	pSource.get(k).ptIdx));
139	allPoints.get(1).add(
140	new Location(pSource.get(k).roadIdx,
141	pSource.get(k).ptIdx));
142	allPoints.get(2).add(
143	new Location(pSource.get(k).roadIdx,
144	pSource.get(k).ptIdx));
145	allPoints.get(3).add(
146	new Location(pSource.get(k).roadIdx,
147	pSource.get(k).ptIdx));
148	middleIdx[0] = middleIdx[1] = middleIdx[2] = middleIdx[3] = pSource
149	.size() - 1;
150
151	}
152	pSource = dijkstraPathInSplittedMap(src, null, avoidable, this.AREA_TO_MAIN);
153	if (pSource == null) {
154	// System.out
155	// .println("No second road was found between SOURCE and a main
156	// road");
157	middleIdx[4] = middleIdx[5] = middleIdx[6] = middleIdx[7] = -1;
158	} else {
159
160	for (k = 0; k < pSource.size(); k++) {
161
162	allPoints.get(4).add(
163	new Location(pSource.get(k).roadIdx,
164	pSource.get(k).ptIdx));
165	allPoints.get(5).add(
166	new Location(pSource.get(k).roadIdx,
167	pSource.get(k).ptIdx));
168	allPoints.get(6).add(
169	new Location(pSource.get(k).roadIdx,
170	pSource.get(k).ptIdx));
171	allPoints.get(7).add(
172	new Location(pSource.get(k).roadIdx,
173	pSource.get(k).ptIdx));
174	middleIdx[4] = middleIdx[5] = middleIdx[6] = middleIdx[7] = pSource
175	.size() - 1;
176
177	}
178	}
179	// Destination area
180	pSource = null;
181	avoidable = new GraphNode(-1, -1);
182	pSource = dijkstraPathInSplittedMap(dest, null, avoidable, this.AREA_TO_MAIN);
183	if (pSource == null) {
184	// System.out
185	// .println("No road was found between Destination and a main
186	// road");
187	return null;
188	}
189
190	for (k = 0; k < pSource.size(); k++) {
191
192	allPoints.get(0).add(
193	new Location(
194	pSource.get(pSource.size() - 1 - k).roadIdx,
195	pSource.get(pSource.size() - 1 - k).ptIdx));
196	allPoints.get(1).add(
197	new Location(
198	pSource.get(pSource.size() - 1 - k).roadIdx,
199	pSource.get(pSource.size() - 1 - k).ptIdx));
200	allPoints.get(6).add(
201	new Location(
202	pSource.get(pSource.size() - 1 - k).roadIdx,
203	pSource.get(pSource.size() - 1 - k).ptIdx));
204	allPoints.get(7).add(
205	new Location(
206	pSource.get(pSource.size() - 1 - k).roadIdx,
207	pSource.get(pSource.size() - 1 - k).ptIdx));
208
209	}
210
211	pSource = dijkstraPathInSplittedMap(dest, null, avoidable, this.AREA_TO_MAIN);
212	if (pSource == null) {
213	// System.out
214	// .println("No second road was found between Destination and a
215	// main road");
216
217	} else {
218
219	for (k = 0; k < pSource.size(); k++) {
220
221	allPoints.get(4)
222	.add(
223	new Location(pSource.get(pSource.size() - 1
224	- k).roadIdx, pSource.get(pSource
225	.size()
226	- 1 - k).ptIdx));
227	allPoints.get(5)
228	.add(
229	new Location(pSource.get(pSource.size() - 1
230	- k).roadIdx, pSource.get(pSource
231	.size()
232	- 1 - k).ptIdx));
233	allPoints.get(3)
234	.add(
235	new Location(pSource.get(pSource.size() - 1
236	- k).roadIdx, pSource.get(pSource
237	.size()
238	- 1 - k).ptIdx));
239	allPoints.get(2)
240	.add(
241	new Location(pSource.get(pSource.size() - 1
242	- k).roadIdx, pSource.get(pSource
243	.size()
244	- 1 - k).ptIdx));
245
246	}
247	}
248
249	for (j = 0; j < 8; j = j + 2) {
250	// System.out.println("--------------------------");
251	// System.out.println("-" + j + ": " + allPoints.get(j));
252	// System.out.println("-" + (j + 1) + ": " + allPoints.get(j +
253	// 1));
254	avoidable = new GraphNode(-1, -1);
255	if ((middleIdx[j] >= 0)
256	\|\| (middleIdx[j] < (allPoints.get(j).size() - 1))) {
257	// There have been found routes from the sorce to a main
258	// road and from destination to main road
259	pMain = null;
260	// System.out.println("La poz " + (j) + " CAUT ruta intre "
261	// + allPoints.get(j).get(middleIdx[j]) + " si "
262	// + allPoints.get(j).get(middleIdx[j] + 1));
263	pMain = dijkstraPathInSplittedMap(allPoints.get(j).get(middleIdx[j]),
264	allPoints.get(j).get(middleIdx[j] + 1), avoidable,
265	this.MAIN_TO_MAIN);
266	if (pMain == null) {
267	if (j < 4) {
268	k = 0;
269	} else {
270	k = 1;
271	}
272	// System.out
273	// .println("No initial route between two main roads for
274	// source route"
275	// + j / 2 + " and destination route " + k);
276	allPoints.get(j).clear();
277	allPoints.get(j + 1).clear();
278	continue;
279	} else {
280	// System.out.println("gasit la " + j + " ruta "
281	// + pMain.size() + " elemente: " + pMain);
282	for (k = 1; k < pMain.size(); k++) {
283
284	allPoints.get(j).add(
285	middleIdx[j] + k,
286	new Location(pMain.get(k).roadIdx, pMain
287	.get(k).ptIdx));
288	}
289	// System.out.println("final la " + j + "este:"
290	// + allPoints.get(j));
291	// System.out.println("La poz "
292	// + (j + 1)
293	// + " CAUT intre "
294	// + allPoints.get(j + 1).get(middleIdx[j + 1])
295	// + " si "
296	// + allPoints.get(j + 1)
297	// .get(middleIdx[j + 1] + 1));
298	pMain = dijkstraPathInSplittedMap(allPoints.get(j + 1).get(
299	middleIdx[j + 1]), allPoints.get(j + 1).get(
300	middleIdx[j + 1] + 1), avoidable,
301	this.MAIN_TO_MAIN);
302	if (pMain == null) {
303	if (j < 4) {
304	k = 0;
305	} else {
306	k = 1;
307	}
308	// System.out
309	// .println("No second route between two main roads
310	// for source route"
311	// + j
312	// / 2
313	// + " and destination route "
314	// + k);
315	allPoints.get(j + 1).clear();
316	continue;
317	} else {
318	// System.out.println("La poz " + (j + 1)
319	// + " in mijloc am " + pMain.size()
320	// + " elemente: " + pMain);
321	for (k = 1; k < pMain.size(); k++) {
322	allPoints.get(j + 1).add(
323	middleIdx[j + 1] + k,
324	new Location(pMain.get(k).roadIdx,
325	pMain.get(k).ptIdx));
326	}
327	// System.out.println("final la " + (j + 1) +
328	// "este:"
329	// + allPoints.get(j + 1));
330	}
331
332	}
333
334	} else {
335	allPoints.get(j).clear();
336	allPoints.get(j + 1).clear();
337	}
338	}
339
340	for (j = 0; j < 8; j++) {
341	// System.out.println("Route before merge: points="
342	// + allPoints.get(j));
343	tempRet = mergeRoute(allPoints.get(j));
344	r = new Route();
345	r.entry = src;
346	r.exit = dest;
347	r.route = tempRet;
348	result.add(r);
349
350	}
351	// System.out.println("Different Road Areas");
352	}
353
354	return result;
355	}
356
357	public Route bestRoute(Location src, Location dest) {
358	Route result = new Route();
359	Point s, d;
360	int j;
361	s = Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx);
362	d = Globals.map.roads.get(dest.roadIdx).points.get(dest.ptIdx);
363
364	System.out.println("area code");
365	//System.out.println("area code " + s.areaCode.areaCodeToString());
366
367	if (s.equals(d)) {
368	// System.out.println("The source and the destination are the same
369	// point");
370	return result;
371	}
372
373	GraphNode avoidable;
374	if (Globals.map.mapSpliter.inSameArea(
375	s.areaCode.belongingTo.areaCodeByte,
376	d.areaCode.belongingTo.areaCodeByte)) {
377	// System.out.println("Same Road Areas");
378	ArrayList<RouteSegment> ret;
379	Route r;
380	ArrayList<Location> points;
381	avoidable = new GraphNode(-1, -1);
382
383	points = dijkstraPathInSplittedMap(src, dest, avoidable, this.SAME_AREA);
384	if (points != null) {
385
386	points.add(new Location(dest.roadIdx, dest.ptIdx));
387
388	ret = mergeRoute(points);
389	r = new Route();
390	r.entry = src;
391	r.exit = dest;
392	r.route = ret;
393	result = r;
394	} else {
395	return null;
396	}
397
398	} else {
399	Route r;
400	int k;
401	ArrayList<Location> pointsSrc = null, pointsDest = null, pointsMiddle = null;
402
403	avoidable = new GraphNode(-1, -1);
404	pointsSrc = dijkstraPathInSplittedMap(src, null, avoidable, this.AREA_TO_MAIN);
405	avoidable = new GraphNode(-1, -1);
406	pointsDest = dijkstraPathInSplittedMap(dest, null, avoidable, this.AREA_TO_MAIN);
407	if (pointsDest == null \|\| pointsSrc == null) {
408	return null;
409	} else {
410	avoidable = new GraphNode(-1, -1);
411	pointsMiddle = dijkstraPathInSplittedMap(
412	pointsSrc.get(pointsSrc.size() - 1), pointsDest
413	.get(pointsDest.size() - 1), avoidable,
414	this.MAIN_TO_MAIN);
415	if (pointsMiddle == null) {
416	return null;
417	} else {
418	pointsMiddle.remove(0);
419	pointsSrc.addAll(pointsMiddle);
420	for (k = pointsDest.size() - 1; k >= 0; k--) {
421	pointsSrc.add(pointsDest.get(k));
422	}
423	result.entry = new Location(src.roadIdx, src.ptIdx);
424	result.exit = new Location(dest.roadIdx, dest.ptIdx);
425	result.route = mergeRoute(pointsSrc);
426
427	}
428	}
429	}
430	return result;
431	}
432
433	private ArrayList<Location> dijkstraPathInSplittedMap(Location src, Location dest,
434	GraphNode avoid, int type) {
435	// System.out.println("Avoid is: " + avoid.roadIndex + " "
436	// + avoid.pointIndex + " si parent " + avoid.parentRoadIndex
437	// + " " + avoid.parentPointIndex);
438	if (type == this.AREA_TO_MAIN) {
439	// System.out.println("Looking for a main street starting from road"
440	// + source.roadIdx + " and point " + source.ptIdx);
441
442	if(Globals.map.mapSpliter.isOnBorder(Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx).areaCode.belongingTo.areaCodeByte))
443	{
444	ArrayList<Location> routePoints = new ArrayList<Location>();
445	routePoints.add(src);
446	return routePoints;
447	}
448	}
449	if (type == this.MAIN_TO_MAIN) {
450	// System.out.println("Looking for a main routes btw r=" +
451	// source.roadIdx
452	// + " p= " + source.ptIdx + " and r=" + destination.roadIdx + " p="
453	// + destination.ptIdx);
454	}
455	if (type == this.SAME_AREA) {
456	// System.out.println("Looking for streets in same area rs="
457	// + source.roadIdx + " p= " + source.ptIdx + " and sourceRoadIdx="
458	// + destination.roadIdx + " p=" + destination.ptIdx);
459	}
460
461	Point s, d, tp;
462	s = Globals.map.roads.get(src.roadIdx).points.get(src.ptIdx);
463	if (type == this.AREA_TO_MAIN) {
464	d = null;
465	} else {
466	d = Globals.map.roads.get(dest.roadIdx).points.get(dest.ptIdx);
467	}
468
469	ArrayList<Location> routePoints = new ArrayList<Location>();
470	GraphNodeArrayList set = new GraphNodeArrayList();
471
472	GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
473	ArrayList sourceNeighbors = getNeighborsByTimeAproximation(sourceGraphNode);
474	GraphNode tempNode;
475
476	// add the direct neighbors of the source to the set of nodes;
477	for (int i = 0; i < sourceNeighbors.size(); i++) {
478	NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
479	int rd = srcNeig.neigh.roadIndex;
480	int pt = srcNeig.neigh.pointIndex;
481
482	if (type == this.AREA_TO_MAIN) {
483	if (Globals.map.mapSpliter
484	.inSameArea(
485	Globals.map.roads.get(rd).points.get(pt).areaCode.belongingTo.areaCodeByte,
486	s.areaCode.belongingTo.areaCodeByte)) {
487	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
488	srcNeig.dist);
489	if ((avoid.roadIndex == -1)
490	\|\| ((avoid.roadIndex >= 0) && avoid
491	.differentFromJustIdx(tempNode))) {
492	set.addGraphNode(tempNode);
493	}
494
495	}
496
497	}
498
499	if (type == this.MAIN_TO_MAIN) {
500
501	if (Globals.map.mapSpliter
502	.isOnBorder(Globals.map.roads.get(rd).points.get(pt).areaCode.belongingTo.areaCodeByte)) {
503
504	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
505	srcNeig.dist);
506	if ((avoid.roadIndex == -1)
507	\|\| ((avoid.roadIndex >= 0) && avoid
508	.differentFromJustIdx(tempNode))) {
509	set.addGraphNode(tempNode);
510	}
511	}
512	}
513
514	if (type == this.SAME_AREA) {
515	if (Globals.map.mapSpliter
516	.inSameArea(
517	Globals.map.roads.get(rd).points.get(pt).areaCode.belongingTo.areaCodeByte,
518	s.areaCode.belongingTo.areaCodeByte)) {
519	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
520	srcNeig.dist);
521
522	if ((avoid.roadIndex == -1)
523	\|\| ((avoid.roadIndex >= 0) && avoid
524	.differentFromJustIdx(tempNode))) {
525	set.addGraphNode(tempNode);
526	}
527	}
528
529	}
530
531	}
532
533	GraphNodeArrayList solved = new GraphNodeArrayList();
534	int maxIdx = -1;
535	double maxVal = 100000.0;
536
537	solved.addGraphNode(sourceGraphNode);
538
539	boolean finalizare = false;
540	while (true) {
541	// get the first node
542	GraphNode first;
543
544	first = (GraphNode) set.removeFirst();
545	if (first == null) {
546	if (avoid.roadIndex < 0) {
547	return null;
548	}
549	// System.out.println("AAAAAAAAAAAAAAAAAAADDDDDD avoid");
550	first = new GraphNode(avoid);
551	avoid.roadIndex = -1;
552	}
553	solved.addGraphNode(first);
554	// System.out.println("Solved:"+first+";
555	// solved="+solved.size()+";unsolved="+set.size());
556
557	finalizare = false;
558
559	if (type == this.MAIN_TO_MAIN) {
560	if (isPointOnRoad(first.roadIndex, first.pointIndex,
561	dest.roadIdx)) {
562	finalizare = true;
563	}
564
565	}
566
567	if (type == this.AREA_TO_MAIN) {
568	tp = Globals.map.roads.get(first.roadIndex).points
569	.get(first.pointIndex);
570	if (Globals.map.mapSpliter
571	.isOnBorder(tp.areaCode.belongingTo.areaCodeByte)) {
572	finalizare = true;
573	}
574
575	}
576	if (type == this.SAME_AREA) {
577	if (isPointOnRoad(first.roadIndex, first.pointIndex,
578	dest.roadIdx)) {
579	finalizare = true;
580	}
581	}
582
583	if (finalizare) {
584	// first is solved; try to get the route built into 'ret'
585	GraphNode aux = first;
586
587	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
588	while (true) {
589	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
590	break;
591	}
592
593	routePoints.add(0, new Location(aux.parentRoadIndex,
594	aux.parentPointIndex));
595
596	// System.out.println("Parent: road "+aux.parentRoadIndex+";
597	// point "+aux.parentPointIndex+";
598	// distance="+aux.distance+"--");
599	// find the parent in the 'solved' set
600	Road rdAux = (Road) Globals.map.roads
601	.get(aux.parentRoadIndex);
602	Point ptAux = (Point) rdAux.points
603	.get(aux.parentPointIndex);
604	Iterator iter = solved.iterator();
605	boolean found = false;
606	while (iter.hasNext()) {
607	GraphNode aux2 = (GraphNode) iter.next();
608	Road rdAux2 = (Road) Globals.map.roads
609	.get(aux2.roadIndex);
610	Point ptAux2 = (Point) rdAux2.points
611	.get(aux2.pointIndex);
612	if (ptAux2.equals(ptAux)) {
613	if ((aux.distance - aux2.distance < maxVal)) {
614	maxVal = aux.distance - aux2.distance;
615	maxIdx = routePoints.size();
616
617	}
618	aux = aux2;
619	found = true;
620
621	break;
622	}
623	}
624	if (found == false) {
625	System.out.println("ERROR! Parent NOT FOUND!:road "
626	+ aux.parentRoadIndex + "; point "
627	+ aux.parentPointIndex);
628	break;
629	} else {
630	}
631	}
632	break;
633	}
634
635	// relaxation
636	ArrayList vecini = getNeighborsByTimeAproximation(first);
637
638	// System.out.println("Neighbors for "+first+":"+vecini);
639
640	for (int j = 0; j < vecini.size(); j++) {
641	NeighborNode n = (NeighborNode) vecini.get(j);
642	if (solved.contains(n.neigh))
643	continue;
644	// only if it's not solved
645
646	Point ptAux = (Point) (((Road) (Globals.map.roads
647	.get(n.neigh.roadIndex))).points
648	.get(n.neigh.pointIndex));
649	double oldDist = getDistance(set, ptAux);
650	double d1 = n.dist;
651	if (oldDist == -1.0) {
652	finalizare = false;
653	if (type == this.AREA_TO_MAIN) {
654	if (Globals.map.mapSpliter
655	.inSameArea(
656	Globals.map.roads
657	.get(n.neigh.roadIndex).points
658	.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
659	s.areaCode.belongingTo.areaCodeByte)) {
660	finalizare = true;
661	}
662	}
663	if (type == this.MAIN_TO_MAIN) {
664	if (Globals.map.mapSpliter
665	.isOnBorder(Globals.map.roads
666	.get(n.neigh.roadIndex).points
667	.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte)) {
668	finalizare = true;
669	}
670	}
671	if (type == this.SAME_AREA) {
672	if (Globals.map.mapSpliter
673	.inSameArea(
674	Globals.map.roads
675	.get(n.neigh.roadIndex).points
676	.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
677	s.areaCode.belongingTo.areaCodeByte)) {
678	finalizare = true;
679	}
680	}
681	if (finalizare) {
682	n.neigh.distance = d1 + first.distance;
683	n.neigh.parentPointIndex = first.pointIndex;
684	n.neigh.parentRoadIndex = first.roadIndex;
685
686	if ((avoid.roadIndex == -1)
687	\|\| ((avoid.roadIndex >= 0) && avoid
688	.differentFromJustIdx(n.neigh))) {
689	set.addGraphNode(n.neigh);
690	}
691
692	}
693	} else {
694	if (d1 + first.distance < oldDist) {
695	finalizare = false;
696	if (type == this.AREA_TO_MAIN) {
697	if (Globals.map.mapSpliter
698	.inSameArea(
699	Globals.map.roads
700	.get(n.neigh.roadIndex).points
701	.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
702	s.areaCode.belongingTo.areaCodeByte)) {
703	finalizare = true;
704	}
705	}
706	if (type == this.MAIN_TO_MAIN) {
707	if (Globals.map.mapSpliter
708	.isOnBorder(Globals.map.roads
709	.get(n.neigh.roadIndex).points
710	.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte)) {
711	finalizare = true;
712	}
713	}
714	if (type == this.SAME_AREA) {
715	if (Globals.map.mapSpliter
716	.inSameArea(
717	Globals.map.roads
718	.get(n.neigh.roadIndex).points
719	.get(n.neigh.pointIndex).areaCode.belongingTo.areaCodeByte,
720	s.areaCode.belongingTo.areaCodeByte)) {
721	finalizare = true;
722	}
723	}
724	if (finalizare) {
725	n.neigh.distance = d1 + first.distance;
726	n.neigh.parentPointIndex = first.pointIndex;
727	n.neigh.parentRoadIndex = first.roadIndex;
728	if ((avoid.roadIndex == -1)
729	\|\| ((avoid.roadIndex >= 0) && avoid
730	.differentFromJustIdx(n.neigh))) {
731	set.addGraphNode(n.neigh);
732	}
733	}
734	}
735	}
736	}
737	}
738
739	// now I have the 'routePoints' vector filled with Location objects;
740	// do merging stuff; obtain the rez vector filled with RouteSegment
741	// objects
742
743	avoid.roadIndex = routePoints
744	.get((int) (routePoints.size() - maxIdx + 1)).roadIdx;
745	avoid.pointIndex = routePoints
746	.get((int) (routePoints.size() - maxIdx + 1)).ptIdx;
747	avoid.parentRoadIndex = routePoints
748	.get((int) (routePoints.size() - maxIdx)).roadIdx;
749	avoid.parentPointIndex = routePoints
750	.get((int) (routePoints.size() - maxIdx)).ptIdx;
751	avoid.distance = Integer.MAX_VALUE - 1;
752	;
753	// avoid.distance =0;
754	return routePoints;
755	}
756
757	public static ArrayList<RouteSegment> mergeRoute(
758	ArrayList<Location> routePoints) {
759	// takes a vector of Location objects (points) and returns a vector of
760	// RouteSegment objects,
761	// representing the route as a sequence of roads, rather than individual
762	// points
763
764	int p1, pLastCand, p2; // point indexes
765	int r1, r2; // road indexes
766	int index = 0;
767	ArrayList<RouteSegment> ret = new ArrayList<RouteSegment>();
768	// System.out.println("Primesc la intrare "+routePoints.toString());
769	if (routePoints == null \|\| routePoints.size() == 0) {
770	// System.out
771	// .println("ERROR! routingModule.dijkstraPath() - routePoints has
772	// no points!");
773	return ret;
774	}
775
776	p1 = ((Location) routePoints.get(0)).ptIdx;
777	r1 = ((Location) routePoints.get(0)).roadIdx;
778	pLastCand = ((Location) routePoints.get(0)).ptIdx;
779
780	index++;
781
782	while (index < routePoints.size()) {
783	p2 = ((Location) routePoints.get(index)).ptIdx;
784	r2 = ((Location) routePoints.get(index)).roadIdx;
785	if (r2 == r1) {
786	pLastCand = p2;
787	} else {
788	// p2 could also be on r1; try to find it
789	boolean found = false;
790	Point pt1 = (Point) (((Road) (Globals.map.roads.get(r2))).points
791	.get(p2));
792	Road road1 = (Road) Globals.map.roads.get(r1);
793	for (int i = 0; i < road1.points.size(); i++) {
794	Point pt2 = (Point) road1.points.get(i);
795	if (pt1.equals(pt2)) {
796	found = true;
797	ret.add(new RouteSegment((short) r1, (short) p1,
798	(short) i));
799	p1 = p2;
800	r1 = r2;
801	pLastCand = p2;
802	break;
803	}
804	}
805	if (!found) {
806	// find (r1,pLastCand) on r2
807	Point ptLC2 = (Point) (((Road) (Globals.map.roads.get(r1))).points
808	.get(pLastCand));
809	Road road2 = (Road) Globals.map.roads.get(r2);
810	for (int i = 0; i < road2.points.size(); i++) {
811	Point pt2 = (Point) road2.points.get(i);
812	if (pt2.equals(ptLC2)) {
813	found = true;
814	ret.add(new RouteSegment((short) r1, (short) p1,
815	(short) pLastCand));
816	p1 = i;
817	r1 = r2;
818	pLastCand = p2;
819	break;
820	}
821	}
822	}
823	if (!found) {
824	System.out
825	.println("RoutingModule.mergePoints() - situation not implemented!; returning null!");
826	System.out.println("for rute {" + r1 + ", " + p1
827	+ "] si [" + r2 + " ," + p2 + " ]");
828	return null;
829	// return new ArrayList<RouteSegment>();
830	}
831	}
832	index++;
833	if (index == routePoints.size()) {
834	// it is the last iteration; add the last RouteSegment;
835	ret.add(new RouteSegment((short) r1, (short) p1,
836	(short) pLastCand));
837	}
838	}
839	return ret;
840	}
841
842	public double getAproxTimeForSegment(int rd, int pt1, int pt2) {
843	double rez = 0.0, speed;
844	Road currentRoad = Globals.map.roads.get(rd);
845	int j, lanePercent;
846	if (pt1 > pt2) {
847	j = pt1;
848	pt1 = pt2;
849	pt2 = j;
850	}
851	if (currentRoad.getRoadinfo()[1] > 50) {
852	speed = 50.0;
853	} else {
854	speed = 130.0;
855	}
856	rez = (double) (currentRoad.points.get(pt2).getDistance());
857
858	rez = rez - (double) (currentRoad.points.get(pt1)).getDistance();
859	lanePercent = currentRoad.laneNo;
860	if (currentRoad.oneWay == 0) {
861	lanePercent = lanePercent / 2;
862	}
863
864	rez = (double) (rez * ((((5 - lanePercent) * Globals.routePlanConstants.increasePerLane) + 100)))
865	/ (speed * 100.0);
866
867	return rez;
868
869	}
870
871	public ArrayList getNeighborsByDistance(GraphNode node) {
872	// get all the direct neighbors of 'node'; returns a vector of
873	// 'NeighborNode' objects
874	ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>();
875
876	// find the closest points on the same road which are cross-points
877	// if there is none, take the end points of the road (except when the
878	// node is already an end point);
879	// also check if there are cross-roads in 'node';
880	Road road = (Road) Globals.map.roads.get(node.roadIndex);
881	int nodeIndex = node.pointIndex;
882
883	int min = Integer.MAX_VALUE;
884	int max = -1;
885	for (int i = 0; i < road.crosses.size(); i++) {
886	Cross c = (Cross) road.crosses.get(i);
887	int idx = c.getPointIndex();
888	if (idx > max && idx < nodeIndex)
889	max = idx;
890	if (idx < min && idx > nodeIndex)
891	min = idx;
892	Point pt = (Point) road.points.get(nodeIndex);
893	if (idx == nodeIndex) {
894	// have to go on the crossing road, to find the closest points;
895	Road crossingRoad = (Road) Globals.map.roads.get(c
896	.getCrossRoadIndex());
897	Point pt2 = null;
898	int index2 = -1;
899	// have to find the index of the point on the crossing road
900	for (int j = 0; j < crossingRoad.points.size(); j++) {
901	Point ptAux = (Point) crossingRoad.points.get(j);
902	if (ptAux.equals(pt)) {
903	// that's the one
904	pt2 = ptAux;
905	index2 = j;
906	break;
907	}
908	}
909	if (pt2 == null) {
910	// System.out.println("RoutingModule.getNeighbors(): ERROR!
911	// Could not find the point on the crossing
912	// road:"+c.getCrossRoadIndex());
913	} else {
914	// find the neighbors on the crossing road;
915	int minCross = Integer.MAX_VALUE;
916	int maxCross = -1;
917	for (int k = 0; k < crossingRoad.crosses.size(); k++) {
918	Cross otherCross = (Cross) crossingRoad.crosses.get(k);
919	int otherIndex = otherCross.getPointIndex();
920	if (otherIndex > maxCross && otherIndex < index2)
921	maxCross = otherIndex;
922	if (otherIndex < minCross && otherIndex > index2)
923	minCross = otherIndex;
924
925	}
926	if (minCross == Integer.MAX_VALUE) {
927	// there was no min; have to consider last point of
928	// crossing road
929	// only if it is not the last
930	if ((index2 != (crossingRoad.points.size() - 1))) {
931	NeighborNode n = new NeighborNode(
932	c.getCrossRoadIndex(),
933	crossingRoad.points.size() - 1,
934	((Point) (crossingRoad.points
935	.get(crossingRoad.points.size() - 1)))
936	.getDistance()
937	- ((Point) (crossingRoad.points
938	.get(index2)))
939	.getDistance());
940	rez.add(n);
941	}
942	} else {
943	NeighborNode n = new NeighborNode(
944	c.getCrossRoadIndex(), minCross,
945	((Point) (crossingRoad.points.get(minCross)))
946	.getDistance()
947	- ((Point) (crossingRoad.points
948	.get(index2))).getDistance());
949	rez.add(n);
950	}
951	if (maxCross == -1) {
952	// there was no max; have to consider first point of
953	// crossing road
954	// only if it is not first
955	if (index2 != 0) {
956	NeighborNode n = new NeighborNode(c
957	.getCrossRoadIndex(), 0,
958	((Point) (crossingRoad.points.get(index2)))
959	.getDistance()
960	- ((Point) (crossingRoad.points
961	.get(0))).getDistance());
962	rez.add(n);
963	}
964	} else {
965	NeighborNode n = new NeighborNode(
966	c.getCrossRoadIndex(), maxCross,
967	((Point) (crossingRoad.points.get(index2)))
968	.getDistance()
969	- ((Point) (crossingRoad.points
970	.get(maxCross))).getDistance());
971	rez.add(n);
972	}
973	}
974	}
975	}
976	if (min == Integer.MAX_VALUE) {
977	// there was no min; have to consider last point of the road
978	// ONLY if I am not the last!
979	if (((road.points.size() - 1) != node.pointIndex)) {
980	NeighborNode n = new NeighborNode(node.roadIndex, road.points
981	.size() - 1, ((Point) (road.points.get(road.points
982	.size() - 1))).getDistance()
983	- ((Point) (road.points.get(node.pointIndex)))
984	.getDistance());
985	rez.add(n);
986	}
987	} else {
988	NeighborNode n = new NeighborNode(node.roadIndex, min,
989	((Point) (road.points.get(min))).getDistance()
990	- ((Point) (road.points.get(node.pointIndex)))
991	.getDistance());
992	rez.add(n);
993	}
994	if (max == -1) {
995	// there was no max; have to consider first point of the road
996	// only if I am not first
997	if (node.pointIndex != 0) {
998	NeighborNode n = new NeighborNode(node.roadIndex, 0,
999	((Point) (road.points.get(node.pointIndex)))
1000	.getDistance()
1001	- ((Point) (road.points.get(0))).getDistance());
1002	rez.add(n);
1003	}
1004	} else {
1005	NeighborNode n = new NeighborNode(node.roadIndex, max,
1006	((Point) (road.points.get(node.pointIndex))).getDistance()
1007	- ((Point) (road.points.get(max))).getDistance());
1008	rez.add(n);
1009	}
1010	return rez;
1011	}
1012
1013	public ArrayList getNeighborsByTimeAproximation(GraphNode node) {
1014	// get all the direct neighbors of 'node'; returns a vector of
1015	// 'NeighborNode' objects
1016	ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>();
1017
1018	// find the closest points on the same road which are cross-points
1019	// if there is none, take the end points of the road (except when the
1020	// node is already an end point);
1021	// also check if there are cross-roads in 'node';
1022	Road road = (Road) Globals.map.roads.get(node.roadIndex);
1023	int nodeIndex = node.pointIndex;
1024
1025	int min = Integer.MAX_VALUE;
1026	int max = -1;
1027	for (int i = 0; i < road.crosses.size(); i++) {
1028	Cross c = (Cross) road.crosses.get(i);
1029	int idx = c.getPointIndex();
1030	if (idx > max && idx < nodeIndex)
1031	max = idx;
1032	if (idx < min && idx > nodeIndex)
1033	min = idx;
1034	Point pt = (Point) road.points.get(nodeIndex);
1035	if (idx == nodeIndex) {
1036	// have to go on the crossing road, to find the closest points;
1037	Road crossingRoad = (Road) Globals.map.roads.get(c
1038	.getCrossRoadIndex());
1039	Point pt2 = null;
1040	int index2 = -1;
1041	// have to find the index of the point on the crossing road
1042	for (int j = 0; j < crossingRoad.points.size(); j++) {
1043	Point ptAux = (Point) crossingRoad.points.get(j);
1044	if (ptAux.equals(pt)) {
1045	// that's the one
1046	pt2 = ptAux;
1047	index2 = j;
1048	break;
1049	}
1050	}
1051	if (pt2 == null) {
1052	// System.out.println("RoutingModule.getNeighbors(): ERROR!
1053	// Could not find the point on the crossing
1054	// road:"+c.getCrossRoadIndex());
1055	} else {
1056	// find the neighbors on the crossing road;
1057	int minCross = Integer.MAX_VALUE;
1058	int maxCross = -1;
1059	for (int k = 0; k < crossingRoad.crosses.size(); k++) {
1060	Cross otherCross = (Cross) crossingRoad.crosses.get(k);
1061	int otherIndex = otherCross.getPointIndex();
1062	if (otherIndex > maxCross && otherIndex < index2)
1063	maxCross = otherIndex;
1064	if (otherIndex < minCross && otherIndex > index2)
1065	minCross = otherIndex;
1066
1067	}
1068	if (minCross == Integer.MAX_VALUE) {
1069	// there was no min; have to consider last point of
1070	// crossing road
1071	// only if it is not the last
1072	if ((index2 != (crossingRoad.points.size() - 1))) {
1073	NeighborNode n = new NeighborNode(c
1074	.getCrossRoadIndex(), crossingRoad.points
1075	.size() - 1, getAproxTimeForSegment(c
1076	.getCrossRoadIndex(), index2,
1077	crossingRoad.points.size() - 1));
1078	// ((Point) (crossingRoad.points
1079	// .get(crossingRoad.points.size() - 1)))
1080	// .getDistance()
1081	// - ((Point) (crossingRoad.points
1082	// .get(index2)))
1083	// .getDistance());
1084	rez.add(n);
1085	}
1086	} else {
1087	NeighborNode n = new NeighborNode(
1088	c.getCrossRoadIndex(), minCross,
1089	getAproxTimeForSegment(c.getCrossRoadIndex(),
1090	minCross, index2));
1091	// ((Point) (crossingRoad.points.get(minCross)))
1092	// .getDistance()
1093	// - ((Point) (crossingRoad.points
1094	// .get(index2))).getDistance());
1095	rez.add(n);
1096	}
1097	if (maxCross == -1) {
1098	// there was no max; have to consider first point of
1099	// crossing road
1100	// only if it is not first
1101	if (index2 != 0) {
1102	NeighborNode n = new NeighborNode(c
1103	.getCrossRoadIndex(), 0,
1104	getAproxTimeForSegment(c
1105	.getCrossRoadIndex(), 0, index2));
1106	// ((Point) (crossingRoad.points.get(index2)))
1107	// .getDistance()
1108	// - ((Point) (crossingRoad.points
1109	// .get(0))).getDistance());
1110	rez.add(n);
1111	}
1112	} else {
1113	NeighborNode n = new NeighborNode(
1114	c.getCrossRoadIndex(), maxCross,
1115	getAproxTimeForSegment(c.getCrossRoadIndex(),
1116	index2, maxCross));
1117	// ((Point) (crossingRoad.points.get(index2)))
1118	// .getDistance()
1119	// - ((Point) (crossingRoad.points
1120	// .get(maxCross))).getDistance());
1121	rez.add(n);
1122	}
1123	}
1124	}
1125	}
1126	if (min == Integer.MAX_VALUE) {
1127	// there was no min; have to consider last point of the road
1128	// ONLY if I am not the last!
1129	if (((road.points.size() - 1) != node.pointIndex)) {
1130	NeighborNode n = new NeighborNode(node.roadIndex, road.points
1131	.size() - 1, getAproxTimeForSegment(node.roadIndex,
1132	node.pointIndex, road.points.size() - 1));
1133	// ((Point) (road.points.get(road.points
1134	// .size() - 1))).getDistance()
1135	// - ((Point) (road.points.get(node.pointIndex)))
1136	// .getDistance());
1137	rez.add(n);
1138	}
1139	} else {
1140	NeighborNode n = new NeighborNode(
1141	node.roadIndex,
1142	min,
1143	getAproxTimeForSegment(node.roadIndex, min, node.pointIndex));
1144	// ((Point) (road.points.get(min))).getDistance()
1145	// - ((Point) (road.points.get(node.pointIndex)))
1146	// .getDistance());
1147	rez.add(n);
1148	}
1149	if (max == -1) {
1150	// there was no max; have to consider first point of the road
1151	// only if I am not first
1152	if (node.pointIndex != 0) {
1153	NeighborNode n = new NeighborNode(node.roadIndex, 0,
1154	getAproxTimeForSegment(node.roadIndex, 0,
1155	node.pointIndex));
1156	// ((Point) (road.points.get(node.pointIndex)))
1157	// .getDistance()
1158	// - ((Point) (road.points.get(0))).getDistance());
1159	rez.add(n);
1160	}
1161	} else {
1162	NeighborNode n = new NeighborNode(
1163	node.roadIndex,
1164	max,
1165	getAproxTimeForSegment(node.roadIndex, node.pointIndex, max));
1166	// ((Point) (road.points.get(node.pointIndex))).getDistance()
1167	// - ((Point) (road.points.get(max))).getDistance());
1168	rez.add(n);
1169	}
1170	return rez;
1171	}
1172
1173	private double getDistance(GraphNodeArrayList t, Point p) {
1174	// get the distance from the set for the corresponding point;
1175	Iterator iter = t.iterator();
1176	while (iter.hasNext()) {
1177	GraphNode n = (GraphNode) iter.next();
1178	Point pct = (Point) (((Road) (Globals.map.roads.get(n.roadIndex))).points
1179	.get(n.pointIndex));
1180	if (pct.equals(p))
1181	return n.distance;
1182	}
1183	// System.out.println("ERROR! getDistance() NOT FOUND!\n");
1184	return -1.0;
1185	}
1186
1187	private boolean isPointOnRoad(int roadIndex, int pointIndex,
1188	int otherRoadIndex) {
1189	// [roadIndex,pointIndex] is also situated on "otherRoadIndex" ?
1190	Location loc = new Location(roadIndex, pointIndex);
1191	for (int i = 0; i < Globals.map.roads.get(otherRoadIndex).points.size(); i++) {
1192	Location otherLoc = new Location(otherRoadIndex, i);
1193	if (loc.equals(otherLoc))
1194	return true;
1195	}
1196	return false;
1197	}
1198
1199	private ArrayList<Location> dijkstraPathByShortestDistance(Location src, Location dest,
1200	GraphNode avoid) {
1201	// returns a vector of consecutive RouteSegments, the shortest route
1202	// between source and destination
1203
1204	// System.out.println("Have to compute a route: from "+source+" -- to
1205	// "+destination);
1206
1207	ArrayList<Location> routePoints = new ArrayList<Location>();
1208
1209	GraphNodeArrayList set = new GraphNodeArrayList();
1210
1211	// Point destPoint=(Point)
1212	// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);
1213
1214	GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
1215	ArrayList sourceNeighbors = getNeighborsByDistance(sourceGraphNode);
1216	GraphNode tempNode;
1217	// add the direct neighbors of the source to the set of nodes;
1218	for (int i = 0; i < sourceNeighbors.size(); i++) {
1219	NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
1220	int rd = srcNeig.neigh.roadIndex;
1221	int pt = srcNeig.neigh.pointIndex;
1222	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
1223	srcNeig.dist);
1224	if ((avoid.roadIndex == -1)
1225	\|\| ((avoid.roadIndex >= 0) && avoid.differentFrom(tempNode))) {
1226	set.addGraphNode(tempNode);
1227	}
1228
1229	}
1230
1231	GraphNodeArrayList solved = new GraphNodeArrayList();
1232
1233	solved.addGraphNode(sourceGraphNode);
1234
1235	while (true) {
1236	// get the first node
1237	GraphNode first = null;
1238	first = (GraphNode) set.removeFirst();
1239	if (first == null) {
1240	return null;
1241	}
1242	solved.addGraphNode(first);
1243	// System.out.println("Solved:"+first+";
1244	// solved="+solved.size()+";unsolved="+set.size());
1245	// if (isPointOnRoad(first.roadIndex, first.pointIndex,
1246	// destination.roadIdx)) {
1247	if ((first.roadIndex == dest.roadIdx)
1248	&& (first.pointIndex == dest.ptIdx)) {
1249	// System.out.println("Solved is "+first);
1250	// first is solved; try to get the route built into 'ret'
1251	GraphNode aux = first;
1252	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
1253	while (true) {
1254	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
1255	break;
1256	}
1257
1258	routePoints.add(0, new Location(aux.parentRoadIndex,
1259	aux.parentPointIndex));
1260
1261	// System.out.println("Parent: road "+aux.parentRoadIndex+";
1262	// point "+aux.parentPointIndex+";
1263	// distance="+aux.distance+"--");
1264	// find the parent in the 'solved' set
1265	Road rdAux = (Road) Globals.map.roads
1266	.get(aux.parentRoadIndex);
1267	Point ptAux = (Point) rdAux.points
1268	.get(aux.parentPointIndex);
1269	Iterator iter = solved.iterator();
1270	boolean found = false;
1271	while (iter.hasNext()) {
1272	GraphNode aux2 = (GraphNode) iter.next();
1273	Road rdAux2 = (Road) Globals.map.roads
1274	.get(aux2.roadIndex);
1275	Point ptAux2 = (Point) rdAux2.points
1276	.get(aux2.pointIndex);
1277	if (ptAux2.equals(ptAux)) {
1278	aux = aux2;
1279	found = true;
1280	break;
1281	}
1282	}
1283	if (found == false) {
1284	System.out.println("ERROR! Parent NOT FOUND!:road "
1285	+ aux.parentRoadIndex + "; point "
1286	+ aux.parentPointIndex);
1287	break;
1288	} else {
1289	}
1290	}
1291	break;
1292	}
1293	if (first.containsLocation(dest)) {
1294	// System.out.println("Solved is "+first);
1295	// first is solved; try to get the route built into 'ret'
1296	GraphNode aux = first;
1297	first.roadIndex = dest.roadIdx;
1298	first.pointIndex = dest.ptIdx;
1299
1300	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
1301	while (true) {
1302	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
1303	break;
1304	}
1305
1306	routePoints.add(0, new Location(aux.parentRoadIndex,
1307	aux.parentPointIndex));
1308
1309	// System.out.println("Parent: road "+aux.parentRoadIndex+";
1310	// point "+aux.parentPointIndex+";
1311	// distance="+aux.distance+"--");
1312	// find the parent in the 'solved' set
1313	Road rdAux = (Road) Globals.map.roads
1314	.get(aux.parentRoadIndex);
1315	Point ptAux = (Point) rdAux.points
1316	.get(aux.parentPointIndex);
1317	Iterator iter = solved.iterator();
1318	boolean found = false;
1319	while (iter.hasNext()) {
1320	GraphNode aux2 = (GraphNode) iter.next();
1321	Road rdAux2 = (Road) Globals.map.roads
1322	.get(aux2.roadIndex);
1323	Point ptAux2 = (Point) rdAux2.points
1324	.get(aux2.pointIndex);
1325	if (ptAux2.equals(ptAux)) {
1326	aux = aux2;
1327	found = true;
1328	break;
1329	}
1330	}
1331	if (found == false) {
1332	System.out.println("ERROR! Parent NOT FOUND!:road "
1333	+ aux.parentRoadIndex + "; point "
1334	+ aux.parentPointIndex);
1335	break;
1336	} else {
1337	}
1338	}
1339	break;
1340	}
1341
1342	// relaxation
1343	ArrayList vecini = getNeighborsByDistance(first);
1344
1345	// System.out.println("Neighbors for "+first+":"+vecini);
1346
1347	for (int j = 0; j < vecini.size(); j++) {
1348	NeighborNode n = (NeighborNode) vecini.get(j);
1349	if (solved.contains(n.neigh))
1350	continue;
1351	// only if it's not solved
1352
1353	Point ptAux = (Point) (((Road) (Globals.map.roads
1354	.get(n.neigh.roadIndex))).points
1355	.get(n.neigh.pointIndex));
1356	double oldDist = getDistance(set, ptAux);
1357	double d1 = n.dist;
1358	if (oldDist == -1.0) {
1359	// System.out.println("ERROR!;
1360	// roadIndex="+n.neigh.roadIndex+";
1361	// ptIndex="+n.neigh.pointIndex+";
1362	// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
1363	n.neigh.distance = d1 + first.distance;
1364	n.neigh.parentPointIndex = first.pointIndex;
1365	n.neigh.parentRoadIndex = first.roadIndex;
1366	if ((avoid.roadIndex == -1)
1367	\|\| ((avoid.roadIndex >= 0) && avoid
1368	.differentFrom(n.neigh))) {
1369	set.addGraphNode(n.neigh);
1370	}
1371
1372	} else {
1373	if (d1 + first.distance < oldDist) {
1374	// set.remove(n.neigh);
1375	n.neigh.distance = d1 + first.distance;
1376	n.neigh.parentPointIndex = first.pointIndex;
1377	n.neigh.parentRoadIndex = first.roadIndex;
1378	// set.add(n.neigh);
1379	if ((avoid.roadIndex == -1)
1380	\|\| ((avoid.roadIndex >= 0) && avoid
1381	.differentFrom(n.neigh))) {
1382	set.addGraphNode(n.neigh);
1383	}
1384	}
1385	}
1386	}
1387	}
1388
1389	// now I have the 'routePoints' vector filled with Location objects;
1390	// do merging stuff; obtain the rez vector filled with RouteSegment
1391	// objects
1392
1393	// System.out.println("Route before merge: points=" + routePoints);
1394	return routePoints;
1395	}
1396
1397	private ArrayList<Location> dijkstraPathTest(Location src, Location dest,
1398	GraphNode avoid) {
1399	// returns a vector of consecutive RouteSegments, the shortest route
1400	// between source and destination
1401
1402	// System.out.println("Have to compute a route: from "+source+" -- to
1403	// "+destination);
1404
1405	ArrayList<Location> routePoints = new ArrayList<Location>();
1406
1407	GraphNodeArrayList set = new GraphNodeArrayList();
1408
1409	// Point destPoint=(Point)
1410	// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);
1411
1412	GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
1413	ArrayList sourceNeighbors = getNeighborsByDistance(sourceGraphNode);
1414	GraphNode tempNode;
1415	// add the direct neighbors of the source to the set of nodes;
1416	for (int i = 0; i < sourceNeighbors.size(); i++) {
1417	NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
1418	int rd = srcNeig.neigh.roadIndex;
1419	int pt = srcNeig.neigh.pointIndex;
1420	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
1421	srcNeig.dist);
1422	if (avoid.roadIndex == -1) {
1423	set.addGraphNode(tempNode);
1424	} else {
1425	if ((avoid.roadIndex >= 0) && avoid.differentFromJustIdx(tempNode)) {
1426	set.addGraphNode(tempNode);
1427	}
1428
1429	}
1430	}
1431	GraphNodeArrayList solved = new GraphNodeArrayList();
1432
1433	solved.addGraphNode(sourceGraphNode);
1434
1435	int maxIdx = -1;
1436	double maxVal = Double.MAX_VALUE;
1437	while (true) {
1438	// get the first node
1439	GraphNode first = null;
1440	first = (GraphNode) set.removeFirst();
1441	if (first == null) {
1442	// System.out.println("null de aici1");
1443	return null;
1444	}
1445
1446	solved.addGraphNode(first);
1447	// System.out.println("Solved:"+first+";
1448	// solved="+solved.size()+";unsolved="+set.size());
1449	// if (isPointOnRoad(first.roadIndex, first.pointIndex,
1450	// destination.roadIdx)) {
1451	if (isPointOnRoad(first.roadIndex, first.pointIndex, dest.roadIdx)) {
1452	// System.out.println("Solved is "+first);
1453	// first is solved; try to get the route built into 'ret'
1454	GraphNode aux = first;
1455	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
1456	while (true) {
1457	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
1458	break;
1459	}
1460
1461	routePoints.add(0, new Location(aux.parentRoadIndex,
1462	aux.parentPointIndex));
1463
1464	// System.out.println("Parent: road "+aux.parentRoadIndex+";
1465	// point "+aux.parentPointIndex+";
1466	// distance="+aux.distance+"--");
1467	// find the parent in the 'solved' set
1468	Road rdAux = (Road) Globals.map.roads
1469	.get(aux.parentRoadIndex);
1470	Point ptAux = (Point) rdAux.points
1471	.get(aux.parentPointIndex);
1472	Iterator iter = solved.iterator();
1473	boolean found = false;
1474	while (iter.hasNext()) {
1475	GraphNode aux2 = (GraphNode) iter.next();
1476	Road rdAux2 = (Road) Globals.map.roads
1477	.get(aux2.roadIndex);
1478	Point ptAux2 = (Point) rdAux2.points
1479	.get(aux2.pointIndex);
1480	if (ptAux2.equals(ptAux)) {
1481	if ((aux.distance - aux2.distance < maxVal)) {
1482	maxVal = aux.distance - aux2.distance;
1483	maxIdx = routePoints.size();
1484
1485	}
1486
1487	aux = aux2;
1488	found = true;
1489	break;
1490	}
1491	}
1492	if (found == false) {
1493	System.out.println("ERROR! Parent NOT FOUND!:road "
1494	+ aux.parentRoadIndex + "; point "
1495	+ aux.parentPointIndex);
1496	break;
1497	} else {
1498	}
1499	}
1500	break;
1501	}
1502
1503	// relaxation
1504	ArrayList vecini = getNeighborsByDistance(first);
1505
1506	// System.out.println("Neighbors for "+first+":"+vecini);
1507
1508	for (int j = 0; j < vecini.size(); j++) {
1509	NeighborNode n = (NeighborNode) vecini.get(j);
1510	if (solved.contains(n.neigh))
1511	continue;
1512	// only if it's not solved
1513
1514	Point ptAux = (Point) (((Road) (Globals.map.roads
1515	.get(n.neigh.roadIndex))).points
1516	.get(n.neigh.pointIndex));
1517	double oldDist = getDistance(set, ptAux);
1518	double d1 = n.dist;
1519	if (oldDist == -1.0) {
1520	// System.out.println("ERROR!;
1521	// roadIndex="+n.neigh.roadIndex+";
1522	// ptIndex="+n.neigh.pointIndex+";
1523	// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
1524	n.neigh.distance = d1 + first.distance;
1525	n.neigh.parentPointIndex = first.pointIndex;
1526	n.neigh.parentRoadIndex = first.roadIndex;
1527	if (avoid.roadIndex == -1) {
1528	set.addGraphNode(n.neigh);
1529	} else {
1530
1531	if ((avoid.roadIndex >= 0)
1532	&& avoid.differentFromJustIdx(n.neigh)) {
1533	set.addGraphNode(n.neigh);
1534	}
1535	}
1536
1537	} else {
1538	if (d1 + first.distance < oldDist) {
1539	// set.remove(n.neigh);
1540	n.neigh.distance = d1 + first.distance;
1541	n.neigh.parentPointIndex = first.pointIndex;
1542	n.neigh.parentRoadIndex = first.roadIndex;
1543	// set.add(n.neigh);
1544	if ((avoid.roadIndex == -1)) {
1545	set.addGraphNode(n.neigh);
1546	} else {
1547	if ((avoid.roadIndex >= 0)
1548	&& avoid.differentFromJustIdx(n.neigh)) {
1549	set.addGraphNode(n.neigh);
1550	}
1551	}
1552	}
1553	}
1554	}
1555	}
1556
1557	// now I have the 'routePoints' vector filled with Location objects;
1558	// do merging stuff; obtain the rez vector filled with RouteSegment
1559	// objects
1560	if (maxIdx > -1) {
1561	avoid.roadIndex = routePoints.get((int) (routePoints.size()
1562	- maxIdx + 1)).roadIdx;
1563	avoid.pointIndex = routePoints.get((int) (routePoints.size()
1564	- maxIdx + 1)).ptIdx;
1565	avoid.parentRoadIndex = routePoints
1566	.get((int) (routePoints.size() - maxIdx)).roadIdx;
1567	avoid.parentPointIndex = routePoints
1568	.get((int) (routePoints.size() - maxIdx)).ptIdx;
1569	avoid.distance = Integer.MAX_VALUE - 1;
1570	}
1571	// System.out.println("Route before merge: points=" + routePoints);
1572	// System.out.println("null de aici2");
1573	return routePoints;
1574	}
1575
1576	private ArrayList<Location> dijkstraPathByTimeAproximation(Location src, Location dest,
1577	GraphNode avoid) {
1578	// returns a vector of consecutive RouteSegments, the shortest route
1579	// between source and destination
1580
1581	// System.out.println("Have to compute a route: from "+source+" -- to
1582	// "+destination);
1583
1584	ArrayList<Location> routePoints = new ArrayList<Location>();
1585
1586	GraphNodeArrayList set = new GraphNodeArrayList();
1587
1588	// Point destPoint=(Point)
1589	// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);
1590
1591	GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
1592	ArrayList sourceNeighbors = getNeighborsByTimeAproximation(sourceGraphNode);
1593	GraphNode tempNode;
1594	// add the direct neighbors of the source to the set of nodes;
1595	for (int i = 0; i < sourceNeighbors.size(); i++) {
1596	NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
1597	int rd = srcNeig.neigh.roadIndex;
1598	int pt = srcNeig.neigh.pointIndex;
1599	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
1600	srcNeig.dist);
1601	// if (avoid.roadIndex == -1) {
1602	// set.addGraphNode(tempNode);
1603	// } else {
1604	// if ((avoid.roadIndex >= 0) && avoid.differentFrom2(tempNode)) {
1605	set.addGraphNode(tempNode);
1606	// }
1607	//
1608	// }
1609	}
1610	GraphNodeArrayList solved = new GraphNodeArrayList();
1611
1612	solved.addGraphNode(sourceGraphNode);
1613
1614	int maxIdx = -1;
1615	double maxVal = Double.MAX_VALUE;
1616	while (true) {
1617	// get the first node
1618	GraphNode first = null;
1619	first = (GraphNode) set.removeFirst();
1620	if (first == null) {
1621	// System.out.println("null de aici1");
1622	return null;
1623	}
1624
1625	solved.addGraphNode(first);
1626	// System.out.println("Solved:"+first+";
1627	// solved="+solved.size()+";unsolved="+set.size());
1628	// if (isPointOnRoad(first.roadIndex, first.pointIndex,
1629	// destination.roadIdx)) {
1630	if (isPointOnRoad(first.roadIndex, first.pointIndex, dest.roadIdx)) {
1631	// System.out.println("Solved is "+first);
1632	// first is solved; try to get the route built into 'ret'
1633	GraphNode aux = first;
1634	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
1635	while (true) {
1636	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
1637	break;
1638	}
1639
1640	routePoints.add(0, new Location(aux.parentRoadIndex,
1641	aux.parentPointIndex));
1642
1643	// System.out.println("Parent: road "+aux.parentRoadIndex+";
1644	// point "+aux.parentPointIndex+";
1645	// distance="+aux.distance+"--");
1646	// find the parent in the 'solved' set
1647	Road rdAux = (Road) Globals.map.roads
1648	.get(aux.parentRoadIndex);
1649	Point ptAux = (Point) rdAux.points
1650	.get(aux.parentPointIndex);
1651	Iterator iter = solved.iterator();
1652	boolean found = false;
1653	while (iter.hasNext()) {
1654	GraphNode aux2 = (GraphNode) iter.next();
1655	Road rdAux2 = (Road) Globals.map.roads
1656	.get(aux2.roadIndex);
1657	Point ptAux2 = (Point) rdAux2.points
1658	.get(aux2.pointIndex);
1659	if (ptAux2.equals(ptAux)) {
1660	// if ((aux.distance - aux2.distance < maxVal)) {
1661	// maxVal = aux.distance - aux2.distance;
1662	// maxIdx = routePoints.size();
1663	//
1664	// }
1665
1666	aux = aux2;
1667	found = true;
1668	break;
1669	}
1670	}
1671	if (found == false) {
1672	System.out.println("ERROR! Parent NOT FOUND!:road "
1673	+ aux.parentRoadIndex + "; point "
1674	+ aux.parentPointIndex);
1675	break;
1676	} else {
1677	}
1678	}
1679	break;
1680	}
1681
1682	// relaxation
1683	ArrayList vecini = getNeighborsByTimeAproximation(first);
1684
1685	// System.out.println("Neighbors for "+first+":"+vecini);
1686
1687	for (int j = 0; j < vecini.size(); j++) {
1688	NeighborNode n = (NeighborNode) vecini.get(j);
1689	if (solved.contains(n.neigh))
1690	continue;
1691	// only if it's not solved
1692
1693	Point ptAux = (Point) (((Road) (Globals.map.roads
1694	.get(n.neigh.roadIndex))).points
1695	.get(n.neigh.pointIndex));
1696	double oldDist = getDistance(set, ptAux);
1697	double d1 = n.dist;
1698	if (oldDist == -1.0) {
1699	// System.out.println("ERROR!;
1700	// roadIndex="+n.neigh.roadIndex+";
1701	// ptIndex="+n.neigh.pointIndex+";
1702	// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
1703	n.neigh.distance = d1 + first.distance;
1704	n.neigh.parentPointIndex = first.pointIndex;
1705	n.neigh.parentRoadIndex = first.roadIndex;
1706	// if (avoid.roadIndex == -1) {
1707	// set.addGraphNode(n.neigh);
1708	// } else {
1709	//
1710	// if ((avoid.roadIndex >= 0)
1711	// && avoid.differentFrom2(n.neigh)) {
1712	set.addGraphNode(n.neigh);
1713	// }
1714	// }
1715
1716	} else {
1717	if (d1 + first.distance < oldDist) {
1718	// set.remove(n.neigh);
1719	n.neigh.distance = d1 + first.distance;
1720	n.neigh.parentPointIndex = first.pointIndex;
1721	n.neigh.parentRoadIndex = first.roadIndex;
1722	// // set.add(n.neigh);
1723	// if ((avoid.roadIndex == -1)) {
1724	// set.addGraphNode(n.neigh);
1725	// } else {
1726	// if ((avoid.roadIndex >= 0)
1727	// && avoid.differentFrom2(n.neigh)) {
1728	set.addGraphNode(n.neigh);
1729	// }
1730	// }
1731	}
1732	}
1733	}
1734	}
1735
1736	// now I have the 'routePoints' vector filled with Location objects;
1737	// do merging stuff; obtain the rez vector filled with RouteSegment
1738	// objects
1739	if (maxIdx > -1) {
1740	avoid.roadIndex = routePoints.get((int) (routePoints.size()
1741	- maxIdx + 1)).roadIdx;
1742	avoid.pointIndex = routePoints.get((int) (routePoints.size()
1743	- maxIdx + 1)).ptIdx;
1744	avoid.parentRoadIndex = routePoints
1745	.get((int) (routePoints.size() - maxIdx)).roadIdx;
1746	avoid.parentPointIndex = routePoints
1747	.get((int) (routePoints.size() - maxIdx)).ptIdx;
1748	avoid.distance = Integer.MAX_VALUE - 1;
1749	}
1750	// System.out.println("Route before merge: points=" + routePoints);
1751	// System.out.println("null de aici2");
1752	return routePoints;
1753	}
1754
1755	// public double getValueForRute(ArrayList<RouteSegment> rute) {
1756	// double rez = 0.0;
1757	// int i, j, k;
1758	// double speed, lanePercent, distance;
1759	// Road currentRoad;
1760	// Point currentPoint;
1761	// RouteSegment currentSegment;
1762	// int pt1, pt2;
1763	// for (i = 0; i < rute.size(); i++) {
1764	// currentSegment = rute.get(i);
1765	// currentRoad = Globals.map.roads.get(currentSegment.roadIndex);
1766	//
1767	// pt1 = currentSegment.pt1;
1768	// pt2 = currentSegment.pt2;
1769	// if (pt1 > pt2) {
1770	// j = pt1;
1771	// pt1 = pt2;
1772	// pt2 = j;
1773	// }
1774	// if (currentRoad.getRoadinfo()[1] > 50) {
1775	// speed = 50.0;
1776	// } else {
1777	// speed = 130.0;
1778	// }
1779	// distance = (double) (currentRoad.points.get(pt2).getDistance());
1780	//
1781	// distance = distance
1782	// - (double) (currentRoad.points.get(pt1)).getDistance();
1783	// lanePercent = currentRoad.laneNo;
1784	// if (currentRoad.oneWay == 0) {
1785	// lanePercent = lanePercent / 2;
1786	// }
1787	//
1788	// rez = rez
1789	// + (double) (distance * ((((5 - lanePercent) * Globals.routePlanConstants.increasePerLane) + 100)))
1790	// / (speed * 100.0);
1791	//
1792	// }
1793	//
1794	// return rez;
1795	// }
1796
1797	public static double getDistanceForRoute(RouteSegment[] route) {
1798	double dist = 0.0, d;
1799	int i, p1, p2, k;
1800	if (route == null) {
1801	return -1.0;
1802	}
1803	RouteSegment s;
1804	for (i = 0; i < route.length; i++) {
1805	s = route[i];
1806	p1 = s.pt1;
1807	p2 = s.pt2;
1808	if (p2 < p1) {
1809	k = p2;
1810	p2 = p1;
1811	p1 = k;
1812	}
1813	d = (double) Globals.map.roads.get(s.roadIndex).points.get(p2)
1814	.getDistance()
1815	- Globals.map.roads.get(s.roadIndex).points.get(p1)
1816	.getDistance();
1817	d = Math.abs(d);
1818	dist = dist + d;
1819	}
1820
1821	return dist;
1822	}
1823
1824	class GraphNode {
1825	// a point on the discrete map;
1826	// used for the dijkstra implementation;
1827
1828	public int roadIndex;
1829
1830	public int pointIndex; // these two uniquely identify the node;
1831
1832	int parentRoadIndex;
1833
1834	int parentPointIndex; // these two uniquely identify the parent
1835
1836	public double distance; // used in the dijkstra implementation
1837
1838	public GraphNode(int roadIndex, int pointIndex) {
1839	this.roadIndex = roadIndex;
1840	this.pointIndex = pointIndex;
1841	parentRoadIndex = -1;
1842	parentPointIndex = -1;
1843	distance = Double.MAX_VALUE;
1844	}
1845
1846	public boolean differentFromJustIdx(GraphNode t) {
1847
1848	if ((this.parentPointIndex != t.parentPointIndex)
1849	\|\| (this.parentRoadIndex != t.parentRoadIndex)
1850	\|\| (this.pointIndex != t.pointIndex)
1851	\|\| (this.roadIndex != t.roadIndex)) {
1852
1853	return true;
1854	}
1855	return false;
1856	}
1857
1858	public boolean differentFrom(GraphNode t) {
1859	// System.out.println("Compar parent:"+this.parentRoadIndex+"
1860	// sourcePointIdx="+this.parentPointIndex+ "cu parent"+t.parentRoadIndex+" cu
1861	// p"+t.parentPointIndex);
1862	// System.out.println("si copill:"+this.roadIndex+"
1863	// sourcePointIdx="+this.pointIndex+ "cu parent"+t.roadIndex+" cu
1864	// p"+t.pointIndex);
1865	boolean dif1 = false, dif2 = false, found = false;
1866	int i, j;
1867	Cross c;
1868	if ((t.parentRoadIndex == this.parentRoadIndex)
1869	&& (t.parentPointIndex != this.parentPointIndex)) {
1870	dif1 = true;
1871
1872	} else {
1873	if (t.parentRoadIndex != this.parentRoadIndex) {
1874
1875	for (i = 0; i < Globals.map.roads.get(t.parentRoadIndex).crosses
1876	.size(); i++) {
1877	c = Globals.map.roads.get(t.parentRoadIndex).crosses
1878	.get(i);
1879	if (c.getCrossRoadIndex() == this.parentRoadIndex) {
1880	if ((c.getPointIndex() == t.parentPointIndex)
1881	&& (c.getCrossPointIndex() == this.parentPointIndex)) {
1882	found = true;
1883	break;
1884	}
1885	}
1886	}
1887	if (!found) {
1888	dif1 = true;
1889	}
1890
1891	}
1892
1893	}
1894
1895	///////fara parent
1896	found = false;
1897	if ((t.roadIndex == this.roadIndex)
1898	&& (t.pointIndex != this.pointIndex)) {
1899	dif2 = true;
1900
1901	} else {
1902	if (t.roadIndex != this.roadIndex) {
1903
1904	for (i = 0; i < Globals.map.roads.get(t.roadIndex).crosses
1905	.size(); i++) {
1906	c = Globals.map.roads.get(t.roadIndex).crosses.get(i);
1907	if (c.getCrossRoadIndex() == this.roadIndex) {
1908	if ((c.getPointIndex() == t.pointIndex)
1909	&& (c.getCrossPointIndex() == this.pointIndex)) {
1910	found = true;
1911	break;
1912	}
1913	}
1914	}
1915	if (!found) {
1916	dif2 = true;
1917	}
1918
1919	}
1920
1921	}
1922
1923	return (dif1 \|\| dif2);
1924	}
1925
1926	public GraphNode(int roadIndex, int pointIndex, int parentRoadIndex,
1927	int parentPointIndex, double distance) {
1928	this.roadIndex = roadIndex;
1929	this.pointIndex = pointIndex;
1930	this.parentRoadIndex = parentRoadIndex;
1931	this.parentPointIndex = parentPointIndex;
1932	this.distance = distance;
1933	}
1934
1935	public GraphNode(GraphNode g) {
1936	this.roadIndex = g.roadIndex;
1937	this.pointIndex = g.pointIndex;
1938	this.parentRoadIndex = g.parentRoadIndex;
1939	this.parentPointIndex = g.parentPointIndex;
1940	this.distance = g.distance;
1941	}
1942
1943	public boolean equals(GraphNode n) {
1944	Point p1 = (Point) ((Road) (Globals.map.roads.get(roadIndex))).points
1945	.get(pointIndex);
1946	Point p2 = (Point) ((Road) (Globals.map.roads.get(n.roadIndex))).points
1947	.get(n.pointIndex);
1948	return p1.equals(p2);
1949	}
1950
1951	public String toString() {
1952	String ret = "[Road=" + roadIndex + ";Point=" + pointIndex + "]";
1953	return ret;
1954	}
1955
1956	public boolean containsLocation(Location dest) {
1957	int i, j, p1, p2;
1958	Cross c;
1959	Road r;
1960	if (dest.roadIdx != this.roadIndex) {
1961	return false;
1962	}
1963	if (this.parentRoadIndex != this.roadIndex) {
1964
1965	for (i = 0; i < Globals.map.roads.get(this.parentRoadIndex).crosses
1966	.size(); i++) {
1967	if (Globals.map.roads.get(this.parentRoadIndex).crosses
1968	.get(i).getCrossRoadIndex() == this.roadIndex) {
1969	p1 = Globals.map.roads.get(this.parentRoadIndex).crosses
1970	.get(i).getCrossPointIndex();
1971	p2 = this.pointIndex;
1972	if (p1 < p2) {
1973	if (p1 < dest.ptIdx && dest.ptIdx < p2) {
1974	return true;
1975	}
1976	} else {
1977	if (p1 > dest.ptIdx && dest.ptIdx > p2) {
1978	return true;
1979	}
1980	}
1981
1982	}
1983	}
1984
1985	} else {
1986
1987	p1 = this.parentPointIndex;
1988	p2 = this.pointIndex;
1989	if (p1 < p2) {
1990	if (p1 < dest.ptIdx && dest.ptIdx < p2) {
1991	return true;
1992	}
1993	} else {
1994	if (p1 > dest.ptIdx && dest.ptIdx > p2) {
1995	return true;
1996	}
1997	}
1998
1999	}
2000
2001	return false;
2002	}
2003	}
2004
2005	class NeighborNode {
2006	GraphNode neigh;
2007
2008	double dist; // distance to that neighbor
2009
2010	public NeighborNode(int roadIndex, int pointIndex, double dist) {
2011	this.dist = dist;
2012	neigh = new GraphNode(roadIndex, pointIndex);
2013	}
2014
2015	public String toString() {
2016	String ret = "[Neigh:" + neigh + " ; dist=" + dist + "]";
2017	return ret;
2018	}
2019	}
2020
2021	class GraphNodeArrayList {
2022	// a vector of "graph nodes"; it keeps the graph nodes ordered (performs
2023	// an insertion sorting, when
2024	// adding an element); it also prevent duplicates
2025
2026	ArrayList<GraphNode> v;
2027
2028	public GraphNodeArrayList() {
2029	v = new ArrayList<GraphNode>();
2030	}
2031
2032	public void addGraphNode(GraphNode n) {
2033	// insertion-sorting add, based on the 'distance' of the graph nodes
2034	for (int i = 0; i < v.size(); i++) {
2035	GraphNode node = (GraphNode) v.get(i);
2036	if (n.distance < node.distance) {
2037	v.add(i, n);
2038	return;
2039	}
2040	}
2041	v.add(n);
2042	}
2043
2044	public GraphNode removeFirst() {
2045	// gets the first node and removes it from the collection
2046	if (v.size() > 0) {
2047	GraphNode n = (GraphNode) v.get(0);
2048	v.remove(0);
2049	return n;
2050	}
2051	return null;
2052	}
2053
2054	public Iterator iterator() {
2055	return v.iterator();
2056	}
2057
2058	public void setGraphNode(GraphNode n) {
2059	// looks through the vector, finds a graph node referring to the
2060	// same point (by
2061	// checking the roadSegment of the point), and replaces that graph
2062	// node;
2063	Point pct1 = (Point) (((Road) (Globals.map.roads.get(n.roadIndex))).points
2064	.get(n.pointIndex));
2065	for (int i = 0; i < v.size(); i++) {
2066	GraphNode node = (GraphNode) v.get(i);
2067	Point pct2 = (Point) (((Road) (Globals.map.roads
2068	.get(node.roadIndex))).points.get(node.pointIndex));
2069	if (pct1.equals(pct2)) {
2070	// that's the point; replace it
2071	v.set(i, n);
2072	}
2073	}
2074	}
2075
2076	public int size() {
2077	return v.size();
2078	}
2079
2080	public boolean contains(GraphNode node) {
2081	for (int i = 0; i < v.size(); i++) {
2082	GraphNode node1 = (GraphNode) v.get(i);
2083	if (node1.equals(node))
2084	return true;
2085	}
2086	return false;
2087	}
2088	}
2089
2090	// public ArrayList<Route> bestSPRoutes(Location src, Location dest) {
2091	// ArrayList<Route> rez = new ArrayList<Route>();
2092	// int i;
2093	// Route r = null;
2094	// ArrayList<Location> points = null;
2095	// GraphNode avoid = new GraphNode(-1, -1);
2096	// // System.out.println("dij cu source=" + source + "si destination=" + destination
2097	// // + " si avoid" + avoid);
2098	// i = 0;
2099	// while (i < 3) {
2100	//
2101	// points = null;
2102	// points = dijkstraPathTest(src, dest, avoid);
2103	// // System.out.println("cu avoid dijjkstra da " + points);
2104	// r = new Route();
2105	// r.route = mergeRoute(points);
2106	//
2107	// if (r.route == null) {
2108	// break;
2109	// }
2110	// // System.out.println("dupa merge " + r.route);
2111	// r.entry = src;
2112	// r.exit = dest;
2113	// rez.add(r);
2114	// i++;
2115	//
2116	// }
2117	//
2118	// return null;
2119	// }
2120
2121	// //////////////////////////////////////////////////////////////
2122
2123	public ArrayList<Location> dijkstraPathWithDelay(Location src,
2124	Location dest, Location parentAvoid, Location nodeAvoid) {
2125	// returns a vector of consecutive RouteSegments, the shortest route
2126	// between source and destination
2127
2128	// System.out.println("Have to compute a route: from "+source+" -- to
2129	// "+destination);
2130	GraphNode avoid = new GraphNode(-1, -1);
2131	avoid.parentRoadIndex = parentAvoid.roadIdx;
2132	avoid.parentPointIndex = parentAvoid.ptIdx;
2133	avoid.roadIndex = nodeAvoid.roadIdx;
2134	avoid.pointIndex = nodeAvoid.ptIdx;
2135
2136	// System.out.println("avoid is " + avoid.parentRoadIndex + " "
2137	// + avoid.parentPointIndex + " spre " + avoid.roadIndex + " "
2138	// + avoid.pointIndex);
2139
2140	ArrayList<Location> routePoints = new ArrayList<Location>();
2141
2142	GraphNodeArrayList set = new GraphNodeArrayList();
2143
2144	// Point destPoint=(Point)
2145	// ((Road)(Globals.map.roads.get(destination.roadIndex))).points.get(destination.pt1);
2146
2147	GraphNode sourceGraphNode = new GraphNode(src.roadIdx, src.ptIdx);
2148	ArrayList sourceNeighbors = getNeighborsWithDelay(sourceGraphNode);
2149	GraphNode tempNode;
2150	// add the direct neighbors of the source to the set of nodes;
2151	for (int i = 0; i < sourceNeighbors.size(); i++) {
2152	NeighborNode srcNeig = (NeighborNode) sourceNeighbors.get(i);
2153	int rd = srcNeig.neigh.roadIndex;
2154	int pt = srcNeig.neigh.pointIndex;
2155	tempNode = new GraphNode(rd, pt, src.roadIdx, src.ptIdx,
2156	srcNeig.dist);
2157	if (avoid.roadIndex == -1) {
2158	set.addGraphNode(tempNode);
2159	} else {
2160	if ((avoid.roadIndex >= 0)
2161	&& avoid.differentFrom(tempNode)) {
2162	set.addGraphNode(tempNode);
2163	}
2164
2165	}
2166
2167	}
2168
2169	GraphNodeArrayList solved = new GraphNodeArrayList();
2170
2171	solved.addGraphNode(sourceGraphNode);
2172
2173	while (true) {
2174	// get the first node
2175	GraphNode first = null;
2176	first = (GraphNode) set.removeFirst();
2177	if (first == null) {
2178	return null;
2179	}
2180	solved.addGraphNode(first);
2181	// System.out.println("Solved:"+first+";
2182	// solved="+solved.size()+";unsolved="+set.size());
2183	// if (isPointOnRoad(first.roadIndex, first.pointIndex,
2184	// destination.roadIdx)) {
2185	if ((first.roadIndex == dest.roadIdx)
2186	&& (first.pointIndex == dest.ptIdx)) {
2187	// System.out.println("Solved is "+first);
2188	// first is solved; try to get the route built into 'ret'
2189	GraphNode aux = first;
2190	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
2191	while (true) {
2192	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
2193	break;
2194	}
2195
2196	routePoints.add(0, new Location(aux.parentRoadIndex,
2197	aux.parentPointIndex));
2198
2199	// System.out.println("Parent: road "+aux.parentRoadIndex+";
2200	// point "+aux.parentPointIndex+";
2201	// distance="+aux.distance+"--");
2202	// find the parent in the 'solved' set
2203	Road rdAux = (Road) Globals.map.roads
2204	.get(aux.parentRoadIndex);
2205	Point ptAux = (Point) rdAux.points
2206	.get(aux.parentPointIndex);
2207	Iterator iter = solved.iterator();
2208	boolean found = false;
2209	while (iter.hasNext()) {
2210	GraphNode aux2 = (GraphNode) iter.next();
2211	Road rdAux2 = (Road) Globals.map.roads
2212	.get(aux2.roadIndex);
2213	Point ptAux2 = (Point) rdAux2.points
2214	.get(aux2.pointIndex);
2215	if (ptAux2.equals(ptAux)) {
2216	aux = aux2;
2217	found = true;
2218	break;
2219	}
2220	}
2221	if (found == false) {
2222	System.out.println("ERROR! Parent NOT FOUND!:road "
2223	+ aux.parentRoadIndex + "; point "
2224	+ aux.parentPointIndex);
2225	break;
2226	} else {
2227	}
2228	}
2229	break;
2230	}
2231
2232	if (first.containsLocation(dest)) {
2233	// System.out.println("Solved is "+first);
2234	// first is solved; try to get the route built into 'ret'
2235	GraphNode aux = first;
2236	first.roadIndex = dest.roadIdx;
2237	first.pointIndex = dest.ptIdx;
2238
2239	routePoints.add(0, new Location(aux.roadIndex, aux.pointIndex));
2240	while (true) {
2241	if (aux.parentRoadIndex == -1 \|\| aux.parentPointIndex == -1) {
2242	break;
2243	}
2244
2245	routePoints.add(0, new Location(aux.parentRoadIndex,
2246	aux.parentPointIndex));
2247
2248	// System.out.println("Parent: road "+aux.parentRoadIndex+";
2249	// point "+aux.parentPointIndex+";
2250	// distance="+aux.distance+"--");
2251	// find the parent in the 'solved' set
2252	Road rdAux = (Road) Globals.map.roads
2253	.get(aux.parentRoadIndex);
2254	Point ptAux = (Point) rdAux.points
2255	.get(aux.parentPointIndex);
2256	Iterator iter = solved.iterator();
2257	boolean found = false;
2258	while (iter.hasNext()) {
2259	GraphNode aux2 = (GraphNode) iter.next();
2260	Road rdAux2 = (Road) Globals.map.roads
2261	.get(aux2.roadIndex);
2262	Point ptAux2 = (Point) rdAux2.points
2263	.get(aux2.pointIndex);
2264	if (ptAux2.equals(ptAux)) {
2265	aux = aux2;
2266	found = true;
2267	break;
2268	}
2269	}
2270	if (found == false) {
2271	System.out.println("ERROR! Parent NOT FOUND!:road "
2272	+ aux.parentRoadIndex + "; point "
2273	+ aux.parentPointIndex);
2274	break;
2275	} else {
2276	}
2277	}
2278	break;
2279	}
2280
2281	// relaxation
2282	ArrayList vecini = getNeighborsWithDelay(first);
2283
2284	// System.out.println("Neighbors for "+first+":"+vecini);
2285
2286	for (int j = 0; j < vecini.size(); j++) {
2287	NeighborNode n = (NeighborNode) vecini.get(j);
2288	if (solved.contains(n.neigh))
2289	continue;
2290	// only if it's not solved
2291
2292	Point ptAux = (Point) (((Road) (Globals.map.roads
2293	.get(n.neigh.roadIndex))).points
2294	.get(n.neigh.pointIndex));
2295	double oldDist = getDistance(set, ptAux);
2296	double d1 = n.dist;
2297	if (oldDist == -1.0) {
2298	// System.out.println("ERROR!;
2299	// roadIndex="+n.neigh.roadIndex+";
2300	// ptIndex="+n.neigh.pointIndex+";
2301	// lat="+ptAux.getLatitude()+";long="+ptAux.getLongitude());
2302	n.neigh.distance = d1 + first.distance;
2303	n.neigh.parentPointIndex = first.pointIndex;
2304	n.neigh.parentRoadIndex = first.roadIndex;
2305
2306	if (avoid.roadIndex == -1) {
2307	set.addGraphNode(n.neigh);
2308	} else {
2309	if ((avoid.roadIndex >= 0)
2310	&& avoid.differentFrom(n.neigh)) {
2311	set.addGraphNode(n.neigh);
2312	}
2313
2314	}
2315
2316	} else {
2317	if (d1 + first.distance < oldDist) {
2318	// set.remove(n.neigh);
2319	n.neigh.distance = d1 + first.distance;
2320	n.neigh.parentPointIndex = first.pointIndex;
2321	n.neigh.parentRoadIndex = first.roadIndex;
2322	if (avoid.roadIndex == -1) {
2323	set.addGraphNode(n.neigh);
2324	} else {
2325	if ((avoid.roadIndex >= 0)
2326	&& avoid.differentFrom(n.neigh)) {
2327	set.addGraphNode(n.neigh);
2328	}
2329
2330	}
2331
2332	}
2333	}
2334	}
2335	}
2336
2337	// now I have the 'routePoints' vector filled with Location objects;
2338	// do merging stuff; obtain the rez vector filled with RouteSegment
2339	// objects
2340
2341	// System.out.println("Route before merge: points=" + routePoints);
2342	return routePoints;
2343	}
2344
2345	public ArrayList getNeighborsWithDelay(GraphNode node) {
2346	// get all the direct neighbors of 'node'; returns a vector of
2347	// 'NeighborNode' objects
2348	ArrayList<NeighborNode> rez = new ArrayList<NeighborNode>();
2349
2350	// find the closest points on the same road which are cross-points
2351	// if there is none, take the end points of the road (except when the
2352	// node is already an end point);
2353	// also check if there are cross-roads in 'node';
2354	Road road = (Road) Globals.map.roads.get(node.roadIndex);
2355	int nodeIndex = node.pointIndex;
2356
2357	int min = Integer.MAX_VALUE;
2358	int max = -1;
2359	for (int i = 0; i < road.crosses.size(); i++) {
2360	Cross c = (Cross) road.crosses.get(i);
2361	int idx = c.getPointIndex();
2362	if (idx > max && idx < nodeIndex)
2363	max = idx;
2364	if (idx < min && idx > nodeIndex)
2365	min = idx;
2366	Point pt = (Point) road.points.get(nodeIndex);
2367	if (idx == nodeIndex) {
2368	// have to go on the crossing road, to find the closest points;
2369	Road crossingRoad = (Road) Globals.map.roads.get(c
2370	.getCrossRoadIndex());
2371	Point pt2 = null;
2372	int index2 = -1;
2373	// have to find the index of the point on the crossing road
2374	for (int j = 0; j < crossingRoad.points.size(); j++) {
2375	Point ptAux = (Point) crossingRoad.points.get(j);
2376	if (ptAux.equals(pt)) {
2377	// that's the one
2378	pt2 = ptAux;
2379	index2 = j;
2380	break;
2381	}
2382	}
2383	if (pt2 == null) {
2384	// System.out.println("RoutingModule.getNeighbors(): ERROR!
2385	// Could not find the point on the crossing
2386	// road:"+c.getCrossRoadIndex());
2387	} else {
2388	// find the neighbors on the crossing road;
2389	int minCross = Integer.MAX_VALUE;
2390	int maxCross = -1;
2391	for (int k = 0; k < crossingRoad.crosses.size(); k++) {
2392	Cross otherCross = (Cross) crossingRoad.crosses.get(k);
2393	int otherIndex = otherCross.getPointIndex();
2394	if (otherIndex > maxCross && otherIndex < index2)
2395	maxCross = otherIndex;
2396	if (otherIndex < minCross && otherIndex > index2)
2397	minCross = otherIndex;
2398
2399	}
2400	if (minCross == Integer.MAX_VALUE) {
2401	// there was no min; have to consider last point of
2402	// crossing road
2403	// only if it is not the last
2404	if ((index2 != (crossingRoad.points.size() - 1))) {
2405	NeighborNode n = new NeighborNode(c
2406	.getCrossRoadIndex(), crossingRoad.points
2407	.size() - 1,
2408	getDelayForDirectedRoadSegment(c
2409	.getCrossRoadIndex(), index2,
2410	crossingRoad.points.size() - 1));
2411	// ((Point) (crossingRoad.points
2412	// .get(crossingRoad.points.size() - 1)))
2413	// .getDistance()
2414	// - ((Point) (crossingRoad.points
2415	// .get(index2)))
2416	// .getDistance());
2417	rez.add(n);
2418	}
2419	} else {
2420	NeighborNode n = new NeighborNode(
2421	c.getCrossRoadIndex(), minCross,
2422	getDelayForDirectedRoadSegment(c
2423	.getCrossRoadIndex(), index2, minCross));
2424	// ((Point) (crossingRoad.points.get(minCross)))
2425	// .getDistance()
2426	// - ((Point) (crossingRoad.points
2427	// .get(index2))).getDistance());
2428	rez.add(n);
2429	}
2430	if (maxCross == -1) {
2431	// there was no max; have to consider first point of
2432	// crossing road
2433	// only if it is not first
2434	if (index2 != 0) {
2435	NeighborNode n = new NeighborNode(c
2436	.getCrossRoadIndex(), 0,
2437	getDelayForDirectedRoadSegment(c
2438	.getCrossRoadIndex(), index2, 0));
2439	// ((Point) (crossingRoad.points.get(index2)))
2440	// .getDistance()
2441	// - ((Point) (crossingRoad.points
2442	// .get(0))).getDistance());
2443	rez.add(n);
2444	}
2445	} else {
2446	NeighborNode n = new NeighborNode(
2447	c.getCrossRoadIndex(), maxCross,
2448	getDelayForDirectedRoadSegment(c
2449	.getCrossRoadIndex(), index2, maxCross));
2450	// ((Point) (crossingRoad.points.get(index2)))
2451	// .getDistance()
2452	// - ((Point) (crossingRoad.points
2453	// .get(maxCross))).getDistance());
2454	rez.add(n);
2455	}
2456	}
2457	}
2458	}
2459	if (min == Integer.MAX_VALUE) {
2460	// there was no min; have to consider last point of the road
2461	// ONLY if I am not the last!
2462	if (((road.points.size() - 1) != node.pointIndex)) {
2463	NeighborNode n = new NeighborNode(node.roadIndex, road.points
2464	.size() - 1,
2465	getDelayForDirectedRoadSegment(node.roadIndex,
2466	node.pointIndex, road.points.size() - 1));
2467	// ((Point) (road.points.get(road.points
2468	// .size() - 1))).getDistance()
2469	// - ((Point) (road.points.get(node.pointIndex)))
2470	// .getDistance());
2471	rez.add(n);
2472	}
2473	} else {
2474	NeighborNode n = new NeighborNode(node.roadIndex, min,
2475	getDelayForDirectedRoadSegment(node.roadIndex,
2476	node.pointIndex, min));
2477	// ((Point) (road.points.get(min))).getDistance()
2478	// - ((Point) (road.points.get(node.pointIndex)))
2479	// .getDistance());
2480	rez.add(n);
2481	}
2482	if (max == -1) {
2483	// there was no max; have to consider first point of the road
2484	// only if I am not first
2485	if (node.pointIndex != 0) {
2486	NeighborNode n = new NeighborNode(node.roadIndex, 0,
2487	getDelayForDirectedRoadSegment(node.roadIndex,
2488	node.pointIndex, 0));
2489	// ((Point) (road.points.get(node.pointIndex)))
2490	// .getDistance()
2491	// - ((Point) (road.points.get(0))).getDistance());
2492	rez.add(n);
2493	}
2494	} else {
2495	NeighborNode n = new NeighborNode(node.roadIndex, max,
2496	getDelayForDirectedRoadSegment(node.roadIndex,
2497	node.pointIndex, max));
2498	// ((Point) (road.points.get(node.pointIndex))).getDistance()
2499	// - ((Point) (road.points.get(max))).getDistance());
2500	rez.add(n);
2501	}
2502	return rez;
2503	}
2504
2505	public long getDelayForDirectedRoadSegment(int r, int p1, int p2) {
2506	boolean dir;
2507	DirectedRoadSegment dr = null;
2508	if (p1 < p2) {
2509	dir = true;
2510	} else {
2511	dir = false;
2512	}
2513	// try{
2514	dr = new DirectedRoadSegment(r, p2, (!dir));
2515	// }catch(Exception e){
2516	// e.printStackTrace();
2517	// }
2518	int i;
2519	for (i = 0; i < Globals.routePlanConstants.delays.size(); i++) {
2520	// System.out.println("Compar: " + Globals.delays.get(i).location
2521	// + " cu " + dr);
2522	if (Globals.routePlanConstants.delays.get(i).roadSegment.equals(dr)) {
2523	ArrayList<DelayRecord> tmp = Globals.routePlanConstants.delays;
2524	// System.out.println("^^^^^^^^^^^^^^^^^^^^^^got average
2525	// "+Globals.delays.get(i).getDelay());
2526	return Globals.routePlanConstants.delays.get(i).getDelay();
2527	}
2528
2529	}
2530	return 0L;
2531
2532	}
2533	}

Note: See TracBrowser for help on using the repository browser.

Context Navigation

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

Download in other formats: