void IMapNeighbourMethod.Invoke(IMap map, ref MapCell source, ref MapCell neighbour, Directions direction) { if ((source.Walkability & (byte)direction) != (byte)direction) { return; } // If neighbor is diagonal to current, graph.Cost(current,neighbor) // will return Sqrt(2). Otherwise it will return only the cost of // the neighbor, which depends on its type, as set in the TileType enum. // So if this is a normal floor tile (1) and it's neighbor is an // adjacent (not diagonal) floor tile (1), newCost will be 2, // or if the neighbor is diagonal, 1+Sqrt(2). And that will be the // value assigned to costSoFar[neighbor] below. int newCost = _costSoFar[_currentIndex] + map.Cost(_locomotion, _currentIndex, neighbour.Index); // If there's no cost assigned to the neighbor yet, or if the new // cost is lower than the assigned one, add newCost for this neighbor if (!_costSoFar.ContainsKey(neighbour.Index) || newCost < _costSoFar[neighbour.Index]) { // If we're replacing the previous cost, remove it if (_costSoFar.ContainsKey(neighbour.Index)) { _costSoFar.Remove(neighbour.Index); _cameFrom.Remove(neighbour.Index); } _costSoFar.Add(neighbour.Index, newCost); _cameFrom.Add(neighbour.Index, _currentIndex); ref MapCell goal = ref map.GetCell(_goalIndex); int priority = newCost + Heuristic(map, ref neighbour, ref goal); if (!_pool.TryDequeue(out AStarPriorityQueueNode? neighbourNode)) { neighbourNode = new AStarPriorityQueueNode { CellIndex = neighbour.Index }; } else { neighbourNode !.CellIndex = neighbour.Index; } _frontier.Enqueue(neighbourNode, priority); }
// Conduct the A* search public Route GetPath(IMap map, ref MapCell start, ref MapCell goal, Locomotion locomotion) { _locomotion = locomotion; if (!_pool.TryDequeue(out AStarPriorityQueueNode? startNode)) { startNode = new AStarPriorityQueueNode { CellIndex = start.Index }; } else { startNode !.CellIndex = start.Index; } // Add the starting location to the frontier with a priority of 0 _frontier.Enqueue(startNode, 0); _cameFrom.Add(start.Index, start.Index); // is set to start, None in example _costSoFar.Add(start.Index, 0); while (_frontier.Count > 0) { // Get the Location from the frontier that has the lowest // priority, then remove that Location from the frontier AStarPriorityQueueNode current = _frontier.Dequeue(); // If we're at the goal Location, stop looking. if (current.CellIndex == goal.Index) { break; } // Neighbors will return a List of valid tile Locations // that are next to, diagonal to, above or below current _currentIndex = current.CellIndex; _goalIndex = goal.Index; map.ForEachNeighbour(current.CellIndex, this); _pool.Enqueue(current); } var path = new Route(); int step = goal.Index; while (step != start.Index) { if (!_cameFrom.ContainsKey(step)) { return(Empty); } path.Add(step); step = _cameFrom[step]; } // path.Add(start); path.Reverse(); _frontier.Clear(); _costSoFar.Clear(); _cameFrom.Clear(); return(path); }