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);
}
}
}
}
}
public bool Equals(GridLocation other)
{
return(X == other.X && Y == other.Y);
}
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]);
}
}
}
}
public GridLocation Jump(GridLocation current, GridLocation proposed)
{
int x = current.X;
int y = current.Y;
int dx = current.X - proposed.X;
int dy = current.Y - proposed.Y;
if (!_grid.IsNavigable(x, y))
{
return(GridLocation.Empty);
}
if (_goal == current)
{
return(current);
}
// Diagonal
if (dx != 0 && dy != 0)
{
if ((_grid.IsNavigable(x - dx, y + dy) && !_grid.IsNavigable(x - dx, y)) ||
(_grid.IsNavigable(x + dx, y - dy) && !_grid.IsNavigable(x, y - dy)))
{
return(current);
}
if (Jump(new GridLocation(x + dx, y), current) != GridLocation.Empty ||
Jump(new GridLocation(x, y + dy), current) != GridLocation.Empty)
{
return(current);
}
}
// Cardinal
else
{
if (dx != 0)
{
// Horizontal
if ((_grid.IsNavigable(x + dx, y + 1) && !_grid.IsNavigable(x, y + 1)) ||
(_grid.IsNavigable(x + dx, y - 1) && !_grid.IsNavigable(x, y - 1)))
{
return(current);
}
}
else
{
// Vertical
if ((_grid.IsNavigable(x + 1, y + dy) && !_grid.IsNavigable(x + 1, y)) ||
(_grid.IsNavigable(x - 1, y + dy) && !_grid.IsNavigable(x - 1, y)))
{
return(current);
}
}
}
if (_grid.IsNavigable(x + dx, y) || _grid.IsNavigable(x, y + dy))
{
return(Jump(new GridLocation(x + dx, y + dy), current));
}
return(GridLocation.Empty);
}
internal PathingNode this[GridLocation location] {
get { return(_grid[location.X, location.Y]); }
}