public Point[] FindPath(Point startPosition, Point endPosition, out int depth, FindPathData userData = null)
{
var closedNodes = new List <PathTreeNode>();
var openNodes = new List <PathTreeNode>();
openNodes.Add(new PathTreeNode(startPosition, null, 0));
while (true)
{
if (openNodes.Count == 0)
{
depth = 0;
return(null);
}
var currentNode = openNodes[0];
var currentPosition = currentNode.Position;
if (!closedNodes.Contains(currentNode))
{
if (currentPosition == endPosition)
{
return(CreatePath(currentNode, out depth, userData));
}
PathTreeNode left = null, up = null, right = null, down = null;
left = ProcessNode(currentNode, -1, 0, openNodes, closedNodes, userData);
up = ProcessNode(currentNode, 0, -1, openNodes, closedNodes, userData);
right = ProcessNode(currentNode, 1, 0, openNodes, closedNodes, userData);
down = ProcessNode(currentNode, 0, 1, openNodes, closedNodes, userData);
if (left != null && up != null)
{
ProcessNode(currentNode, -1, -1, openNodes, closedNodes, userData);
}
if (right != null && up != null)
{
ProcessNode(currentNode, 1, -1, openNodes, closedNodes, userData);
}
if (right != null && down != null)
{
ProcessNode(currentNode, 1, 1, openNodes, closedNodes, userData);
}
if (left != null && down != null)
{
ProcessNode(currentNode, -1, 1, openNodes, closedNodes, userData);
}
closedNodes.Add(currentNode);
}
openNodes.RemoveAt(0);
}
}
private PathTreeNode ProcessNode(PathTreeNode currentNode, int columnOffset, int rowOffset, List <PathTreeNode> openNodes, List <PathTreeNode> closedNodes, FindPathData userData)
{
var newNode = new PathTreeNode(currentNode.Position.X + columnOffset, currentNode.Position.Y + rowOffset, currentNode, 0);
if ((CheckNode == null || CheckNode(newNode.Position.X, newNode.Position.Y, userData)) &&
!CheckNodeList(closedNodes, newNode.Position) &&
!CheckNodeList(openNodes, newNode.Position))
{
openNodes.Add(newNode);
return(newNode);
}
return(null);
}
/// <summary>
/// Processes the node.
/// </summary>
/// <param name="currentNode">The current node.</param>
/// <param name="columnOffset">The column offset.</param>
/// <param name="rowOffset">The row offset.</param>
/// <param name="openNodes">The list of open nodes that will be added to as nodes are processed.</param>
/// <param name="closedNodes">The closed nodes.</param>
/// <param name="endPosition">The end position.</param>
/// <param name="userData">The user data.</param>
/// <returns>A new node positioned next to the current node based on columnOffset and rowOffset.</returns>
private PathTreeNode ProcessNode(PathTreeNode currentNode, int columnOffset, int rowOffset, ICollection <PathTreeNode> openNodes, IEnumerable <PathTreeNode> closedNodes, Point endPosition, FindPathData userData)
{
var position = new Point(currentNode.Position.X + columnOffset, currentNode.Position.Y + rowOffset);
var weight = currentNode.Weight + userData?.GetWeight(position, endPosition) ?? 0;
var newNode = new PathTreeNode(position, currentNode, weight);
if (CheckNode != null && !CheckNode(newNode.Position.X, newNode.Position.Y, userData) || AnyNodeIsAtPoint(closedNodes, newNode.Position) || AnyNodeIsAtPoint(openNodes, newNode.Position))
{
return(null);
}
openNodes.Add(newNode);
return(newNode);
}
/// <summary>
/// Creates the path.
/// </summary>
/// <param name="node">The node.</param>
/// <param name="depth">An output variable; the depth of the path.</param>
/// <param name="userData">The user data.</param>
/// <returns>A list of points defining the found path.</returns>
private Point[] CreatePath(PathTreeNode node, out int depth, FindPathData userData)
{
var output = new List <Point>();
var parent = node;
while (parent != null)
{
output.Insert(0, parent.Position);
parent = parent.Parent;
}
if (userData != null)
{
for (var index = output.Count - 1; index >= 0; --index)
{
var poppingWaypoints = userData.PopWaypointTest(output[index], index);
if (!poppingWaypoints)
{
break;
}
output.RemoveAt(index);
}
if (output.Count == 0)
{
depth = 0;
return(output.ToArray());
}
if (userData.PopFirstWaypoint)
{
output.RemoveAt(0);
}
if (userData.PopLastNWaypoints > 0)
{
if (userData.PopLastNWaypoints > output.Count)
{
depth = 0;
output.Clear();
return(output.ToArray());
}
output.RemoveRange(output.Count - userData.PopLastNWaypoints, userData.PopLastNWaypoints);
}
}
depth = output.Count;
if (!TrimPaths)
{
return(output.ToArray());
}
var indicesToRemove = new List <int>();
for (var index = 1; index < output.Count - 1; ++index)
{
var previousPoint = output[index - 1];
var currentPoint = output[index];
var nextPoint = output[index + 1];
if (PointsContinueHorizontally(previousPoint, currentPoint, nextPoint) || PointsContinuesVertically(previousPoint, currentPoint, nextPoint) || PointsContinueDiagonally(previousPoint, currentPoint, nextPoint))
{
indicesToRemove.Add(index);
}
}
for (var index = indicesToRemove.Count - 1; index >= 0; --index)
{
output.RemoveAt(indicesToRemove[index]);
}
return(output.ToArray());
}
public PathTreeNode(int column, int row, PathTreeNode parent, int weight)
: this(new Point(column, row), parent, weight)
{
}
public PathTreeNode(Point position, PathTreeNode parent, int weight)
{
this.Position = position;
this.Parent = parent;
this.Weight = weight;
}