Permalink
Switch branches/tags
Nothing to show
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
100 lines (88 sloc) 3.5 KB
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;
}
}
}