public static List<Point> FindPath(int[,] field, Point start, Point goal)
{
//step 1
var closedSet = new Collection<PathNode>();
var openSet = new Collection<PathNode>();
//step 2
PathNode startNode = new PathNode()
{
Position = start,
CameFrom = null,
PathLengthFromStart = 0,
HeuristicEstimatePathLength = GetHeuristicPathLength(start, goal)
};
openSet.Add(startNode);
while (openSet.Count > 0)
{
//step 3
var currentNode = openSet.OrderBy(node =>
node.EstimateFullPathLength).First();
//step 4.
if (currentNode.Position == goal)
return GetPathForNode(currentNode);
//step 5.
openSet.Remove(currentNode);
closedSet.Add(currentNode);
//step 6.
foreach (var neighbourNode in GetNeighbours(currentNode, goal, field))
{
//step 7.
if (closedSet.Count(node => node.Position == neighbourNode.Position) > 0)
continue;
var openNode = openSet.FirstOrDefault(node =>
node.Position == neighbourNode.Position);
//step 8.
if (openNode == null)
openSet.Add(neighbourNode);
else
if (openNode.PathLengthFromStart > neighbourNode.PathLengthFromStart)
{
//step 9.
openNode.CameFrom = currentNode;
openNode.PathLengthFromStart = neighbourNode.PathLengthFromStart;
}
}
}
//step 10.
return null;
}
private static List<Point> GetPathForNode(PathNode pathNode)
{
var result = new List<Point>();
var currentNode = pathNode;
while (currentNode != null)
{
result.Add(currentNode.Position);
currentNode = currentNode.CameFrom;
}
result.Reverse();
return result;
}
private static Collection<PathNode> GetNeighbours(PathNode pathNode, Point goal, int[,] field)
{
var result = new Collection<PathNode>();
Point[] neighbourPoints = new Point[4];
neighbourPoints[0] = new Point(pathNode.Position.X + 1, pathNode.Position.Y);
neighbourPoints[1] = new Point(pathNode.Position.X - 1, pathNode.Position.Y);
neighbourPoints[2] = new Point(pathNode.Position.X, pathNode.Position.Y + 1);
neighbourPoints[3] = new Point(pathNode.Position.X, pathNode.Position.Y - 1);
foreach (var point in neighbourPoints)
{
if (point.X < 0 || point.X >= field.GetLength(0))
continue;
if (point.Y < 0 || point.Y >= field.GetLength(1))
continue;
//mark1
if ((field[point.X, point.Y] != 0) && (field[point.X, point.Y] != 1) && (field[point.X, point.Y] != 2))
continue;
var neighbourNode = new PathNode()
{
Position = point,
CameFrom = pathNode,
PathLengthFromStart = pathNode.PathLengthFromStart +
GetDistanceBetweenNeighbours(),
HeuristicEstimatePathLength = GetHeuristicPathLength(point, goal)
};
result.Add(neighbourNode);
}
return result;
}