private const int maxPathLength = 1000; // nodes
private static float getHcost(Nav2dNode node, Nav2dNode end)
{
var dx = Mathf.Abs(node.worldPos.x - end.worldPos.x);
var dy = Mathf.Abs(node.worldPos.y - end.worldPos.y);
return(NonDiagonalCost * (dx + dy) + (DiagonalCost - 2 * NonDiagonalCost) * Mathf.Min(dx, dy));
}
private void CreateNavCell(Vector3Int gridPos)
{
var cellsize = grid.cellSize.x;
var center = grid.GetCellCenterWorld(gridPos);
var northWest = center + new Vector3(-cellsize / 2, -cellsize / 2, 0);
// creating the nodes
var nodeCenter = new Nav2dNode(center);
// var nodeObjectCenter = Instantiate(node, nodeCenter.worldPos, Quaternion.identity, this.transform);
// nodeObjectCenter.node = nodeCenter;
var nodeNorthWest = new Nav2dNode(northWest);
//var nodeObjectWest = Instantiate(node, nodeNorthWest.worldPos, Quaternion.identity, this.transform);
//nodeObjectWest.node = nodeNorthWest;
//setting nodes in cells
nodes.Add(nodeCenter);
nodes.Add(nodeNorthWest);
var cell = cellsArray[new Vector2Int(gridPos.x, gridPos.y)] = new Nav2dCell(gridPos);
cell.setNode(nodeCenter, new Vector2Int(0, 0));
cell.setNode(nodeNorthWest, new Vector2Int(-1, -1));
var leftCell = FindNavCellAtPosition(gridPos + new Vector3Int(-1, 0, 0));
if (leftCell != null)
{
leftCell.setNode(nodeNorthWest, new Vector2Int(1, -1));
}
var downCell = FindNavCellAtPosition(gridPos + new Vector3Int(0, -1, 0));
if (downCell != null)
{
downCell.setNode(nodeNorthWest, new Vector2Int(-1, 1));
}
var leftDownCell = FindNavCellAtPosition(gridPos + new Vector3Int(-1, -1, 0));
if (leftDownCell != null)
{
leftDownCell.setNode(nodeNorthWest, new Vector2Int(1, 1));
}
//return cellsArray[gridPos.x, gridPos.y];
}
private void CreateAdjacentNodesConnections(Nav2dCell cell)
{
var cells = new Dictionary <string, Nav2dCell> {
{ "leftDown", FindNavCellAtPosition(cell.gridPos + new Vector3Int(-1, -1, 0)) },
{ "leftTop", FindNavCellAtPosition(cell.gridPos + new Vector3Int(-1, 1, 0)) }, // left down
{ "left", FindNavCellAtPosition(cell.gridPos + new Vector3Int(-1, 0, 0)) }, // left
{ "top", FindNavCellAtPosition(cell.gridPos + new Vector3Int(0, 1, 0)) }, // top
{ "down", FindNavCellAtPosition(cell.gridPos + new Vector3Int(0, -1, 0)) }, // down
{ "right", FindNavCellAtPosition(cell.gridPos + new Vector3Int(1, 0, 0)) },
{ "rightTop", FindNavCellAtPosition(cell.gridPos + new Vector3Int(1, 1, 0)) },
{ "rightDown", FindNavCellAtPosition(cell.gridPos + new Vector3Int(1, -1, 0)) }
};
// table of nodes to test connection
Nav2dNode[] centerNodeNeighbors = new Nav2dNode[9] {
cell.getNode(-1, -1),
cells["left"]?.getNode(0, 0),
cells["top"]?.getNode(-1, -1),
cells["down"]?.getNode(0, 0),
null,
cells["top"]?.getNode(0, 0),
cells["right"]?.getNode(-1, -1),
cells["right"]?.getNode(0, 0),
cells["rightTop"]?.getNode(-1, -1)
};
//sets connection two ways
cell.getNode(0, 0).setAccessibility(centerNodeNeighbors, LayerMask.GetMask(collisionLayers));
Nav2dNode[] northWestNodeNeighbors = new Nav2dNode[9] {
cells["leftDown"]?.getNode(0, 0),
cells["left"]?.getNode(-1, -1),
cells["left"]?.getNode(0, 0),
cells["down"]?.getNode(-1, -1),
null,
cells["top"]?.getNode(-1, -1),
cells["down"]?.getNode(0, 0),
cells["right"]?.getNode(-1, -1),
cell.getNode(0, 0)
};
//sets connection two ways
cell.getNode(-1, -1).setAccessibility(northWestNodeNeighbors, LayerMask.GetMask(collisionLayers));
}
public Nav2dNode findClosestAccessibleNodeInGrid(Vector3Int cellPos, Vector3 targetPos)
{
Nav2dNode node = null;
C5.HashSet <Vector3Int> explored = new C5.HashSet <Vector3Int>();
Queue <Vector3Int> opened = new Queue <Vector3Int>();
opened.Enqueue(cellPos);
// dont try to long
for (int i = 0; i < 20; i++)
{
Vector3Int current = opened.Dequeue();
Nav2dCell cell = FindNavCellAtPosition(current);
node = cell?.findClosestAccessibleNodeInCell(targetPos);
if (node != null)
{
break;
}
explored.Add(current);
Vector3Int[] neighbors =
{
current + new Vector3Int(0, 1, 0),
current + new Vector3Int(0, -1, 0),
current + new Vector3Int(-1, 0, 0),
current + new Vector3Int(1, 0, 0)
};
foreach (var n in neighbors)
{
if (!explored.Contains(n))
{
opened.Enqueue(n);
}
}
}
return(node);
}
public void setNode(Nav2dNode node, Vector2Int direction)
{
nodes[direction.x + 1, direction.y + 1] = node;
}
public static IEnumerable <Nav2dNode> findShortestPath(Nav2dNode start, Nav2dNode end, float determinationLevel)
{
System.Collections.Generic.IList <Nav2dNode> result = new List <Nav2dNode>();
Nav2dNode current = null;
if (start == null || end == null || !start.accessible || !end.accessible)
{
return(null);
}
var pCosts = new Dictionary <Nav2dNode, float>();
var gCosts = new Dictionary <Nav2dNode, float>();
var parent = new Dictionary <Nav2dNode, Nav2dNode>();
var opened = new IntervalHeap <Nav2dNode>(new NodeCompare(pCosts), MemoryType.Normal);
opened.Add(start);
gCosts[start] = 0;
pCosts[start] = getHcost(start, end);
while (!opened.IsEmpty)
{
current = opened.DeleteMin();
if (current == end)
{
break;
}
foreach (var neighbor in current.getNeighbors())
{
// add neighbor or not to the openedQueue.
if (neighbor.accessible)
{
float newCost;
float hCost;
lock (neighbor)
{
newCost = gCosts[current] + getGCost(current, neighbor);
hCost = getHcost(neighbor, end);
}
if (!gCosts.ContainsKey(neighbor) || newCost < gCosts[neighbor])
{
gCosts[neighbor] = newCost;
pCosts[neighbor] = gCosts[neighbor] + hCost;
opened.Add(neighbor);
parent[neighbor] = current;
}
}
}
}
do
{
result.Add(current);
} while (parent.TryGetValue(current, out current) && result.Count < maxPathLength);
// need to loop from start to stop path early depending on determination level
if (determinationLevel != -1)
{
return(result.Reverse().TakeWhile(x =>
// add start cost so that AI is not stuck inside costly node
x.travelCost < start.travelCost + determinationLevel
).Reverse());
}
return(result);
}
private static float getGCost(Nav2dNode current, Nav2dNode next)
{
var normalized = (current.worldPos - next.worldPos).normalized;
return(((normalized.x == 0 || normalized.y == 0) ? NonDiagonalCost : DiagonalCost) + next.travelCost);
}
