Skip to content
{{ message }}

Instantly share code, notes, and snippets.

# mminer/Pathfinding.cs

Created Mar 19, 2021
A* pathfinding implementation.
 using System; using System.Linq; using System.Collections.Generic; public static class Pathfinding { /// /// A* implementation. Finds a path from start to goal. /// /// Where to begin. /// Where we want to end up. /// Heuristic function that estimates the cost to reach the goal from node n. /// Gets the weight of the edge from current to neighbor. /// Gets nodes adjacent to the given one. /// Shortest path between the two nodes, or null if no path exists. public static List FindShortestPath( T start, T goal, Func getCostEstimate, Func getEdgeWeight, Func> getNeighbors) where T : IEquatable { // Adapted from https://en.wikipedia.org/wiki/A*_search_algorithm#Pseudocode. // I translated the pseudocode as directly as possible and kept variable names the same. // Except for h and d, which are terrible names. I renamed them to getCostEstimate and getEdgeWeight respectively. // The set of discovered nodes that may need to be (re-)expanded. // Initially, only the start node is known. // This is usually implemented as a min-heap or priority queue rather than a hash-set. var openSet = new HashSet {start}; // For node n, cameFrom[n] is the node immediately preceding it on the cheapest path from start to n currently known. var cameFrom = new Dictionary(); // For node n, gScore[n] is the cost of the cheapest path from start to n currently known. var gScore = new Dictionary(); gScore[start] = 0; // For node n, fScore[n] = gScore[n] + getCostEstimate(n). // fScore[n] represents our current best guess as to how short a path from start to finish can be if it goes through n. var fScore = new Dictionary(); fScore[start] = getCostEstimate(start); while (openSet.Count > 0) { // Find the node in openSet with the lowest fScore value. // This operation can occur in O(1) time if openSet is a min-heap or a priority queue. var current = openSet .OrderBy(position => fScore.TryGetValue(position, out var fValue) ? fValue : int.MaxValue) .First(); if (current.Equals(goal)) { return ReconstructPath(cameFrom, current); } openSet.Remove(current); var neighbors = getNeighbors(current); foreach (var neighbor in neighbors) { // tentativeGScore is the distance from start to the neighbor through current. var tentativeGScore = gScore[current]+ getEdgeWeight(current, neighbor); var neighborGScore = gScore.TryGetValue(neighbor, out var gValue) ? gValue : int.MaxValue; if (tentativeGScore < neighborGScore) { // This path to neighbor is better than any previous one. Record it! cameFrom[neighbor] = current; gScore[neighbor] = tentativeGScore; fScore[neighbor] = gScore[neighbor] + getCostEstimate(neighbor); if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } } } } // Open set is empty but goal was never reached. return null; } static List ReconstructPath(Dictionary cameFrom, T current) { var totalPath = new List {current}; while (cameFrom.ContainsKey(current)) { current = cameFrom[current]; totalPath.Insert(0, current); } return totalPath; } }
to join this conversation on GitHub. Already have an account? Sign in to comment