public List <GridLocation> FindPath()
{
var startNode = new PathingNode(_start)
{
F = 0, G = 0, Opened = true
};
_open.Enqueue(startNode, startNode.F);
while (_open.Count != 0)
{
PathingNode node = _open.Dequeue();
node.Closed = true;
if (node.Location == _goal)
{
return(Trace(node));
}
IdentitySuccessors(node);
}
return(null);
}
private List <GridLocation> Trace(PathingNode node)
{
var path = new List <GridLocation> {
node.Location
};
while (node.Parent != null)
{
node = node.Parent;
path.Add(node.Location);
}
path.Reverse();
return(path);
}
public Grid(bool[,] navigable)
{
_boundsMinX = 0;
_boundsMaxX = navigable.GetUpperBound(0);
_boundsMinY = 0;
_boundsMaxY = navigable.GetUpperBound(1);
_navigable = navigable;
// Initialise the Grid
_grid = new PathingNode[_boundsMaxX + 1, _boundsMaxY + 1];
for (var x = _boundsMinX; x <= _boundsMaxX; x++)
{
for (var y = _boundsMinY; y <= _boundsMaxY; y++)
{
_grid[x, y] = new PathingNode(x, y);
}
}
}
private void IdentitySuccessors(PathingNode node)
{
foreach (PathingNode neighbour in _grid.Neighbours(node))
{
GridLocation jumpPoint = Jump(neighbour.Location, node.Location);
if (jumpPoint != GridLocation.Empty)
{
PathingNode jumpNode = _grid[jumpPoint];
if (jumpNode.Closed)
{
continue;
}
double d = Heuristic(Math.Abs(jumpPoint.X - node.Location.X), Math.Abs(jumpPoint.Y - node.Location.Y));
double ng = node.G + d;
if (!jumpNode.Opened || ng < jumpNode.G)
{
jumpNode.G = ng;
if (!jumpNode.H.HasValue)
{
jumpNode.H = Heuristic(Math.Abs(jumpPoint.X - _goal.X), Math.Abs(jumpPoint.Y - _goal.Y));
}
jumpNode.F = jumpNode.G + jumpNode.H.Value;
jumpNode.Parent = node;
if (!jumpNode.Opened)
{
_open.Enqueue(jumpNode, jumpNode.F);
jumpNode.Opened = true;
}
else
{
_open.UpdatePriority(jumpNode, jumpNode.F);
}
}
}
}
}
internal IEnumerable <PathingNode> Neighbours(PathingNode node)
{
if (node.Parent != null)
{
GridLocation n = node.Location;
GridLocation p = node.Parent.Location;
GridLocation dNorm = new GridLocation(
(n.X - p.X) / Math.Max(Math.Abs(n.X - p.X), 1),
(n.Y - p.Y) / Math.Max(Math.Abs(n.Y - p.Y), 1));
// Diagonal
if (dNorm.X != 0 && dNorm.Y != 0)
{
if (IsNavigable(n.X, n.Y + dNorm.Y))
{
yield return(_grid[n.X, n.Y + dNorm.Y]);
}
if (IsNavigable(n.X + dNorm.X, n.Y))
{
yield return(_grid[n.X + dNorm.X, n.Y]);
}
if ((IsNavigable(n.X, n.Y + dNorm.Y) || IsNavigable(n.X + dNorm.X, n.Y)) && IsNavigable(n.X + dNorm.X, n.Y + dNorm.Y))
{
yield return(_grid[n.X + dNorm.X, n.Y + dNorm.Y]);
}
if (!IsNavigable(n.X - dNorm.X, n.Y) && IsNavigable(n.X, n.Y + dNorm.Y) && IsNavigable(n.X - dNorm.X, n.Y + dNorm.Y))
{
yield return(_grid[n.X - dNorm.X, n.Y + dNorm.Y]);
}
if (!IsNavigable(n.X, n.Y - dNorm.Y) && IsNavigable(n.X + dNorm.X, n.Y) && IsNavigable(n.X + dNorm.X, n.Y - dNorm.Y))
{
yield return(_grid[n.X + dNorm.X, n.Y - dNorm.Y]);
}
}
// Cardinal
else
{
if (dNorm.X == 0)
{
if (IsNavigable(n.X, n.Y + dNorm.Y))
{
yield return(_grid[n.X, n.Y + dNorm.Y]);
if (!IsNavigable(n.X + 1, n.Y) && IsNavigable(n.X + 1, n.Y + dNorm.Y))
{
yield return(_grid[n.X + 1, n.Y + dNorm.Y]);
}
if (!IsNavigable(n.X - 1, n.Y) && IsNavigable(n.X - 1, n.Y + dNorm.Y))
{
yield return(_grid[n.X - 1, n.Y + dNorm.Y]);
}
}
}
else if (IsNavigable(n.X + dNorm.X, n.Y))
{
yield return(_grid[n.X + dNorm.X, n.Y]);
if (!IsNavigable(n.X, n.Y + 1) && IsNavigable(n.X + dNorm.X, n.Y + 1))
{
yield return(_grid[n.X + dNorm.X, n.Y + 1]);
}
if (!IsNavigable(n.X, n.Y - 1) && IsNavigable(n.X + dNorm.X, n.Y - 1))
{
yield return(_grid[n.X + dNorm.X, n.Y - 1]);
}
}
}
}
else
{
for (var i = 0; i < Directions.Length; i++)
{
int propX = node.Location.X + Directions[i].X;
int propY = node.Location.Y + Directions[i].Y;
if (IsNavigable(propX, propY))
{
yield return(this[propX, propY]);
}
}
}
}
