-
-
Notifications
You must be signed in to change notification settings - Fork 102
/
PathFinder.java
112 lines (103 loc) · 4.11 KB
/
PathFinder.java
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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
package net.aufdemrand.denizen.utilities;
import net.aufdemrand.denizen.Settings;
import net.aufdemrand.denizen.objects.dLocation;
import java.util.HashSet;
public class PathFinder {
public static class Node {
public dLocation position;
public double cost;
public double pathCost;
public Node next;
public Node nextListElement;
}
static class MinHeap {
public Node head;
public boolean hasNext() {
return head != null;
}
public void add(Node node) {
if (head == null) {
head = node;
}
else if (head.next == null && node.cost < head.cost) {
node.nextListElement = head;
head = node;
}
else {
Node cur = head;
while (cur.nextListElement != null && cur.nextListElement.cost < node.cost)
{
cur = cur.nextListElement;
}
node.nextListElement = cur.nextListElement;
cur.nextListElement = node;
}
}
public Node extractFirst()
{
Node res = head;
head = head.nextListElement;
return res;
}
}
public static dLocation[] surroundings = new dLocation[] {
new dLocation(null, 1, 0, 0),
new dLocation(null, -1, 0, 0),
new dLocation(null, 0, 0, 1),
new dLocation(null, 0, 0, -1),
new dLocation(null, 0, 1, 1),
new dLocation(null, 0, 1, -1),
new dLocation(null, 1, 1, 0),
new dLocation(null, -1, 1, 0),
new dLocation(null, 0, -1, 1),
new dLocation(null, 0, -1, -1),
new dLocation(null, 1, -1, 0),
new dLocation(null, -1, -1, 0)
};
public static Node findPath(dLocation start, dLocation end, int radius) {
int maxRadius = Settings.pathfindingMaxDistance();
Node snode = new Node();
snode.position = start;
MinHeap heaplist = new MinHeap();
heaplist.add(snode);
HashSet<dLocation> tried = new HashSet<dLocation>(100);
tried.add(start);
while (heaplist.hasNext()) {
Node curr = heaplist.extractFirst();
if (curr.position.distanceSquared(end) <= radius * radius) {
Node n = new Node();
n.position = start;
n.cost = curr.pathCost + 1;
n.pathCost = curr.cost + 1;
n.next = curr;
return n;
}
for (int i = 0; i < surroundings.length; i++) {
dLocation surr = surroundings[i];
dLocation point = new dLocation(start.getWorld(), curr.position.getX() + surr.getX(), curr.position.getY() + surr.getY(),
curr.position.getZ() + surr.getZ());
if (pointIsFree(point, maxRadius, start) && !tried.contains(point)) {
tried.add(point);
Node node = new Node();
node.position = point;
double surrCost = lengthSquared(surr);
node.cost = curr.pathCost + surrCost + point.distanceSquared(end);
node.pathCost = curr.pathCost + surrCost;
node.next = curr;
heaplist.add(node);
}
}
}
return null; // Give up
}
public static double lengthSquared(dLocation loc) {
return loc.getX() * loc.getX() + loc.getY() * loc.getY() + loc.getZ() * loc.getZ();
}
public static boolean pointIsFree(dLocation point, int maxRadius, dLocation start) {
return point.getWorld() != null
&& !point.getBlock().getType().isSolid()
&& !point.clone().add(0, 1, 0).getBlock().getType().isSolid()
&& point.clone().add(0, -1, 0).getBlock().getType().isSolid()
&& point.distanceSquared(start) < maxRadius * maxRadius;
}
}