internal virtual IEnumerable <PathingNode> Neighbours(PathingNode node)
{
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]);
}
}
}
protected List <Vector2Int> Trace(PathingNode node)
{
var path = new List <Vector2Int> {
node.Location
};
while (node.Parent != null)
{
node = node.Parent;
path.Add(node.Location);
}
path.Reverse();
return(path);
}
private void IdentitySuccessors(PathingNode node)
{
foreach (PathingNode neighbour in _graph.Neighbours(node))
{
System.DateTime s = System.DateTime.Now;
Vector2Int jumpPoint = Jump(neighbour.Location, node.Location);
JumpCostTime += ((System.DateTime.Now - s).TotalMilliseconds);
if (jumpPoint != Utils.InvalidGrid)
{
PathingNode jumpNode = _graph[jumpPoint];
if (jumpNode.Closed)
{
continue;
}
double d = Utils.Heuristic_Diagonal(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 = Utils.Heuristic_Diagonal(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;
OnNodeAddOpenSet?.Invoke(jumpNode);
}
else
{
_open.UpdatePriority(jumpNode, jumpNode.F);
}
}
}
}
}
public GridGraph(byte[,] 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);
}
}
}
public override List <Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
{
if (_graph == null)
{
return(null);
}
_start = start;
_goal = goal;
JumpCostTime = 0;
var startNode = new PathingNode(_start)
{
F = 0, G = 0, Opened = true
};
_open.Enqueue(startNode, startNode.F);
this.OnNodeAddOpenSet?.Invoke(startNode);
while (_open.Count != 0)
{
PathingNode node = _open.Dequeue();
node.Closed = true;
this.OnNodeVisited?.Invoke(node);
if (node.Location == _goal)
{
return(Trace(node));
}
IdentitySuccessors(node);
}
return(null);
}
internal override IEnumerable <PathingNode> Neighbours(PathingNode node)
{
if (node.Parent != null)
{
Vector2Int n = node.Location;
Vector2Int p = node.Parent.Location;
Vector2Int dNorm = new Vector2Int(
(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 override List <Vector2Int> FindPath(Vector2Int start, Vector2Int goal)
{
if (_graph == null)
{
return(null);
}
_start = start;
_goal = goal;
var startNode = new PathingNode(_start)
{
F = 0, G = 0, Opened = true
};
_open.Enqueue(startNode, startNode.F);
this.OnNodeAddOpenSet?.Invoke(startNode);
while (_open.Count != 0)
{
PathingNode curNode = _open.Dequeue();
curNode.Closed = true;
this.OnNodeVisited?.Invoke(curNode);
if (curNode.Location == _goal)
{
return(Trace(curNode));
}
foreach (var neighbour in _graph.Neighbours(curNode))
{
if (neighbour.Closed)
{
continue;
}
double d = Utils.Heuristic_Diagonal(Math.Abs(neighbour.Location.x - curNode.Location.x),
Math.Abs(neighbour.Location.y - curNode.Location.y));
double ng = curNode.G + d;
if (!neighbour.Opened || ng < neighbour.G)
{
neighbour.G = ng;
if (!neighbour.H.HasValue)
{
neighbour.H = Utils.Heuristic_Diagonal(Math.Abs(neighbour.Location.x - _goal.x),
Math.Abs(neighbour.Location.y - _goal.y));
}
neighbour.F = (neighbour.G + neighbour.H.Value);
neighbour.Parent = curNode;
if (!neighbour.Opened)
{
_open.Enqueue(neighbour, neighbour.F);
neighbour.Opened = true;
OnNodeAddOpenSet?.Invoke(neighbour);
}
else
{
_open.UpdatePriority(neighbour, neighbour.F);
}
}
}
}
return(null);
}
