////////////////////////////////////////////////////////////////////////// public void Init(FindPathOptions options, iExplorer <T> exlorer, T start, T[] goal) { if (Result == FindPathResult.None) { // save data Options = options; Explorer = exlorer; Start = start; Goal = goal; // set running state Result = FindPathResult.Running; // allocate collections Path = new LinkedList <T>(); OpenSet = new FastPriorityQueue <PathNode <T> >(Pathfinder.c_AdaptiveBufferExperiance.Average); ClosedSet = new HashSet <T>(); ClosedNodes = new LinkedList <PathNode <T> >(); // create start node, add to open set var startNode = new PathNode <T>(start) { СameFrom = null, PathCost = 0.0f, PathCostEstimated = 0.0f, Cost = 0.0f }; OpenSet.Enqueue(startNode, startNode.Cost); } }
public Cell[] GreedyFind(Vector2Int start, Vector2Int goal) { Reset(); Cell startCell = _grid[start]; //first point Cell goalCell = _grid[goal]; //last point //Console.SetCursorPosition(1, 4); //Console.WriteLine($"{goalCell.Location.X}-----{goalCell.Location.Y} ---- Greedy"); _open.Enqueue(startCell, 0); // not checked var bounds = _grid.Size; Cell node = null; while (_open.Count > 0) { node = _open.Dequeue(); node.Closed = true; var g = node.G + 1; if (goalCell.Location == node.Location) { break; } Vector2Int proposed = new Vector2Int(0, 0); for (var i = 0; i < PathingConstants.Directions.Length; i++) { var direction = PathingConstants.Directions[i]; proposed.X = node.Location.X + direction.X; proposed.Y = node.Location.Y + direction.Y; if (proposed.X < 0 || proposed.X >= bounds.X || proposed.Y < 0 || proposed.Y >= bounds.Y) { continue; } Cell neighbour = _grid[proposed]; if (neighbour.Blocked) { continue; } if (_grid[neighbour.Location].Closed) { continue; } if (!_open.Contains(neighbour)) { neighbour.G = g; neighbour.H = Heuristic(neighbour, node); neighbour.Parent = node; // F will be set by the queue _open.Enqueue(neighbour, neighbour.G + neighbour.H + neighbour.Value); } else if (g + neighbour.H + neighbour.Value < neighbour.F) { neighbour.G = g; neighbour.F = neighbour.G + neighbour.H + neighbour.Value; neighbour.Parent = node; } } } var path = new Stack <Cell>(); while (node != null) { path.Push(node); node = node.Parent; } return(path.ToArray()); }
public Cell[] Find(Vector2Int start, Vector2Int goal) { Reset(); Cell startCell = _grid[start]; Cell goalCell = _grid[goal]; _open.Enqueue(startCell, 0); var bounds = _grid.Size; Cell node = null; while (_open.Count > 0) { node = _open.Dequeue(); node.Closed = true; var cBlock = false; var g = node.G + 1; if (goalCell.Location == node.Location) { break; } Vector2Int proposed = new Vector2Int(0, 0); for (var i = 0; i < PathingConstants.Directions.Length; i++) { var direction = PathingConstants.Directions[i]; proposed.X = node.Location.X + direction.X; proposed.Y = node.Location.Y + direction.Y; // Bounds checking if (proposed.X < 0 || proposed.X >= bounds.X || proposed.Y < 0 || proposed.Y >= bounds.Y) { continue; } Cell neighbour = _grid[proposed]; if (neighbour.Blocked) { if (i < 4) { cBlock = true; } continue; } // Prevent slipping between blocked cardinals by an open diagonal if (i >= 4 && cBlock) { continue; } if (_grid[neighbour.Location].Closed) { continue; } if (!_open.Contains(neighbour)) { neighbour.G = g; neighbour.H = Heuristic(neighbour, node); neighbour.Parent = node; // F will be set by the queue _open.Enqueue(neighbour, neighbour.G + neighbour.H); } else if (g + neighbour.H < neighbour.F) { neighbour.G = g; neighbour.F = neighbour.G + neighbour.H; neighbour.Parent = node; } } } var path = new Stack <Cell>(); while (node != null) { path.Push(node); node = node.Parent; } return(path.ToArray()); }