precise_gps/SearchNode.java at master · dbousque/precise_gps · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92


public class SearchNode implements Comparable<SearchNode>
{

	public Way[] ways;
	public boolean[] takeReverses;
	public double length;
	public double time;
	public double priority;

	public SearchNode(SearchNode currentNode, Way newWay, boolean takeReverse, Node endPoint)
	{
		if (currentNode == null)
		{
			if (newWay == null)
			{
				ways = new Way[0];
				takeReverses = new boolean[0];
				length = 0.0;
				time = 0.0;
				priority = 0.0;
			}
			else
			{
				ways = new Way[1];
				ways[0] = newWay;
				takeReverses = new boolean[1];
				takeReverses[0] = takeReverse;
				length = newWay.length;
				time = newWay.time;
				priority = Priority.getPriority(this, endPoint, takeReverse);
			}
		}
		else
		{
			ways = copyWay(currentNode.ways, newWay);
			takeReverses = copyReverses(currentNode.takeReverses, takeReverse);
			length = currentNode.length + newWay.length;
			if (Double.isNaN(length))
			{
				System.out.println("LOL");
				System.out.println(currentNode.length);
				System.out.println(newWay.length);
			}
			time = currentNode.time + newWay.time + Operations.getTimeCostOfEndNode(newWay, takeReverse);
			priority = Priority.getPriority(this, endPoint, takeReverse);
		}
	}

	public static Way[] copyWay(Way[] oldWays, Way newWay)
	{
		int		i;
		Way[]	res;

		res = new Way[oldWays.length + 1];
		i = 0;
		while (i < oldWays.length)
		{
			res[i] = oldWays[i];
			i++;
		}
		res[i] = newWay;
		return (res);
	}

	public static boolean[] copyReverses(boolean[] oldReverses, boolean takeReverse)
	{
		int			i;
		boolean[]	res;

		res = new boolean[oldReverses.length + 1];
		i = 0;
		while (i < oldReverses.length)
		{
			res[i] = oldReverses[i];
			i++;
		}
		res[i] = takeReverse;
		return (res);
	}

	public int compareTo(SearchNode otherNode)
	{
		if (priority > otherNode.priority)
			return (1);
		if (priority == otherNode.priority)
			return (0);
		return (-1);
	}

}