A* pathfinding implementation.
using System; | |
using System.Linq; | |
using System.Collections.Generic; | |
public static class Pathfinding | |
{ | |
/// <summary> | |
/// A* implementation. Finds a path from start to goal. | |
/// </summary> | |
/// <param name="start">Where to begin.</param> | |
/// <param name="goal">Where we want to end up.</param> | |
/// <param name="getCostEstimate">Heuristic function that estimates the cost to reach the goal from node n.</param> | |
/// <param name="getEdgeWeight">Gets the weight of the edge from current to neighbor.</param> | |
/// <param name="getNeighbors">Gets nodes adjacent to the given one.</param> | |
/// <returns>Shortest path between the two nodes, or null if no path exists.</returns> | |
public static List<T> FindShortestPath<T>( | |
T start, | |
T goal, | |
Func<T, int> getCostEstimate, | |
Func<T, T, int> getEdgeWeight, | |
Func<T, IEnumerable<T>> getNeighbors) where T : IEquatable<T> | |
{ | |
// 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<T> {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<T, T>(); | |
// For node n, gScore[n] is the cost of the cheapest path from start to n currently known. | |
var gScore = new Dictionary<T, int>(); | |
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<T, int>(); | |
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<T> ReconstructPath<T>(Dictionary<T, T> cameFrom, T current) | |
{ | |
var totalPath = new List<T> {current}; | |
while (cameFrom.ContainsKey(current)) | |
{ | |
current = cameFrom[current]; | |
totalPath.Insert(0, current); | |
} | |
return totalPath; | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment