-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathPathFinding.cs
99 lines (88 loc) · 3.5 KB
/
PathFinding.cs
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
using System.Collections.Generic;
using StrategicGame.Common;
namespace StrategicGame.Server {
struct PathSegment {
public readonly Int2 Start;
public readonly Int2 End;
public PathSegment(Int2 start, Int2 end) {
Start = start;
End = end;
}
}
static class Pathfinding {
public delegate bool OccpiedPred(Int2 cell);
public static List<PathSegment> Find(OccpiedPred occupied, Int2 from, Int2 to) {
var pathInfo = new Dictionary<Int2, PathNode> { { from, new PathNode(from) } };
var toProcess = new List<NodeToProcess> { new NodeToProcess(from, 0) };
bool found = false;
Int2 cell = from;
while (toProcess.Count > 0) {
cell = PopBestNode(toProcess);
var currentLength = pathInfo[cell].PathLength;
if (cell == to) {
found = true;
break;
}
var lengthFromCurrent = currentLength + 1;
foreach (var neighbour in Neighbours(cell))
if (!occupied(neighbour)) {
PathNode pathNode;
if (pathInfo.TryGetValue(neighbour, out pathNode)) {
if (lengthFromCurrent < pathNode.PathLength)
pathInfo[neighbour] = new PathNode(cell, lengthFromCurrent);
} else {
pathInfo.Add(neighbour, new PathNode(cell, lengthFromCurrent));
toProcess.Add(new NodeToProcess(neighbour, neighbour.DistanceSquared(to)));
}
}
}
if (!found)
cell = pathInfo.Keys.MinBy(c => c.DistanceSquared(to));
return RestorePath(pathInfo, cell);
}
struct NodeToProcess {
public readonly Int2 Node;
public readonly int EstimatedDistance;
public NodeToProcess(Int2 node, int distance) {
Node = node;
EstimatedDistance = distance;
}
}
struct PathNode {
public readonly Int2 ComeFrom;
public readonly int PathLength;
public PathNode(Int2 comeFrom, int pathLength = 0) {
ComeFrom = comeFrom;
PathLength = pathLength;
}
}
static Int2 PopBestNode(List<NodeToProcess> toProcess) {
Int2 cell;
int minIdx = 0;
for (int i = 1; i < toProcess.Count; ++i)
if (toProcess[i].EstimatedDistance < toProcess[minIdx].EstimatedDistance)
minIdx = i;
cell = toProcess[minIdx].Node;
toProcess.RemoveAt(minIdx);
return cell;
}
static List<PathSegment> RestorePath(Dictionary<Int2, PathNode> pathInfo, Int2 end) {
var path = new List<PathSegment>();
var curr = end;
PathNode node;
while ((node = pathInfo[curr]).PathLength > 0) {
var prev = node.ComeFrom;
path.Add(new PathSegment(prev, curr));
curr = prev;
};
path.Reverse();
return path;
}
static IEnumerable<Int2> Neighbours(Int2 cell) {
yield return cell + Int2.Left;
yield return cell + Int2.Right;
yield return cell + Int2.Up;
yield return cell + Int2.Down;
}
}
}