private static void Step( AStarGrid grid, MinHeap open, Position[] cameFrom, float[] costSoFar, Offset[] movementPattern, Position current, Position end) { // Get the cost associated with getting to the current position var initialCost = costSoFar[grid.GetIndexUnchecked(current.x, current.y)]; // Get all directions we can move to according to the movement pattern and the dimensions of the grid foreach (var option in GetMovementOptions(current, grid.DimX, grid.DimY, movementPattern)) { var position = current + option; var cellCost = grid.GetCellCostUnchecked(position); // Ignore this option if the cell is blocked if (float.IsInfinity(cellCost)) { continue; } var index = grid.GetIndexUnchecked(position.x, position.y); // Compute how much it would cost to get to the new position via this path var newCost = initialCost + cellCost * option.Cost; // Compare it with the best cost we have so far, 0 means we don't have any path that gets here yet var oldCost = costSoFar[index]; if (!(oldCost <= 0) && !(newCost < oldCost)) { continue; } // Update the best path and the cost if this path is cheaper costSoFar[index] = newCost; cameFrom[index] = current; // Use the heuristic to compute how much it will probably cost // to get from here to the end, and store the node in the open list var expectedCost = newCost + ManhattanDistance(position, end); open.Push(new MinHeapNode(position, expectedCost)); MessageOpen(position); } }
private static List <Position> PartiallyReconstructPath(AStarGrid grid, Position start, Position end, Position[] cameFrom) { var path = new List <Position> { end }; #if DEBUG var current = end; do { var previous = cameFrom[grid.GetIndexUnchecked(current.x, current.y)]; // If the path is invalid, probably becase we've not closed // a node yet, return an empty list if (current == previous) { return(new List <Position>()); } current = previous; path.Add(current); } while (current != start); #endif return(path); }
private static List <Position> ReconstructPath(AStarGrid grid, Position start, Position end, Position[] cameFrom) { var path = new List <Position> { end }; var current = end; do { var previous = cameFrom[grid.GetIndexUnchecked(current.x, current.y)]; current = previous; path.Add(current); } while (current != start); return(path); }