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); }