public static void Recalculate(INavigationTarget target) {
cellSize = target.CellSize;
width = target.XCells;
height = target.ZCells;
maxHeight = target.MaxHeight;
maxSlope = target.MaxSlope;
worldOffset = new Vector3(-width * cellSize / 2f, 0, -height * cellSize / 2f);
linearCost = cellSize;
diagonalCost = (float)Math.Sqrt(2) * cellSize;
var halfCell = new Vector3(cellSize / 2, 0, cellSize / 2);
cells = new Cell[width, height];
// Scan the area of the mesh from above to find the walkable parts
int xLength = cells.GetLength(0);
int zLength = cells.GetLength(1);
for (int x = 0; x < xLength; x++) {
for (int z = 0; z < zLength; z++) {
cells[x, z] = new Cell {
Center = worldOffset + new Vector3(x * cellSize, maxHeight, z * cellSize) + halfCell,
X = x,
Z = z
};
target.SetCellIntersections(cells[x, z]);
}
}
}
public static void FindPath(Vector3 start, Vector3 end, Action<List<Vector3>> onComplete, bool ignoreWalkable) {
Cell startCell = GetCell(start);
Cell endCell = GetCell(end);
foreach (Cell cell in cells) {
cell.Open = false;
cell.Closed = false;
}
// Initialize search sets
startCell.Node = new Node();
startCell.FScore = GetEndDistance(startCell, endCell);
startCell.GScore = 0;
startCell.Open = true;
var open = new CellMinPriorityQueue();
open.Insert(startCell);
// Placeholder for neighbors, use as ref to avoid garbage collection
Cell[] neighbors = new Cell[8];
while (open.Length > 0) {
Cell current = open.Peek();
// Are we at the end?
if (current == endCell) {
ConstructPath(start, end, onComplete, current);
return;
}
current.Open = false;
current.Closed = true;
open.Remove();
// Get all of the neighbours of the current cell that are walkable
GetWalkableNeighbors(current, ignoreWalkable, ref neighbors);
foreach (Cell neighbor in neighbors) {
if (null == neighbor) {
continue;
}
// Work out the cost to the neighbour via the current node
float tentativeGScore = current.GScore + neighbor.NeighborDistance;
// Have we processed this already?
if (neighbor.Closed && tentativeGScore >= neighbor.GScore) {
continue;
}
if (neighbor.Open && !(tentativeGScore < neighbor.GScore)) {
continue;
}
neighbor.Node = new Node {
Previous = current
};
neighbor.GScore = tentativeGScore;
neighbor.FScore = tentativeGScore + GetEndDistance(neighbor, endCell);
if (neighbor.Open) {
continue;
}
neighbor.Open = true;
open.Insert(neighbor);
}
}
}
public void SetCellIntersections(Cell cell) {
cell.Center.y = 0;
cell.Walkable = true;
}
private static void GetWalkableNeighbors(Cell cell, bool ignoreWalkable, ref Cell[] neighbors) {
int i = 0;
foreach (int[] offset in NEIGHBOR_OFFSETS) {
int x = offset[0];
int z = offset[1];
if (cell.X + x < 0 || cell.X + x >= width || cell.Z + z < 0 || cell.Z + z >= height) {
continue;
}
Cell neighbor = cells[cell.X + x, cell.Z + z];
neighbor.NeighborDistance = (x + z) % 2 == 0 ? diagonalCost : linearCost;
float heightOffset = Mathf.Abs(cell.Center.y - neighbor.Center.y);
if ((neighbor.Walkable && heightOffset < maxSlope) || ignoreWalkable) {
neighbors[i++] = neighbor;
}
}
// Clear out any left over neighbors we may have had
for (; i < neighbors.Length; i++) {
neighbors[i] = null;
}
}
private static float GetEndDistance(Cell current, Cell end) {
float xDist = (end.X - current.X) * cellSize;
float zDist = (end.Z - current.Z) * cellSize;
return (float)Math.Sqrt(xDist * xDist + zDist * zDist);
}
private static void ConstructPath(Vector3 start, Vector3 end, Action<List<Vector3>> onComplete, Cell current) {
Node scan = current.Node;
List<Vector3> path = new List<Vector3> {
end // Use the actual end spot and not the cell center once we found the end cell
};
// Scan backwards from the end along the path until Previous = 0
while (null != scan?.Previous) {
path.Insert(0, scan.Previous.Center);
scan = scan.Previous.Node;
}
// Replace the start cell (center) with the actual start location
path.RemoveAt(0);
path.Insert(0, start);
// return the path we found
onComplete(path);
}
