Created
May 15, 2023 13:31
-
-
Save smourier/40495257014a6b68ada7b814a066f4a8 to your computer and use it in GitHub Desktop.
A* implementation in C# (.NET core 6+)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| public static class AStar | |
| { | |
| public interface IHasNeighbours<T> | |
| { | |
| IEnumerable<T> Neighbours { get; } | |
| } | |
| public static IEnumerable<T> FindPath<T>(T start, T destination, Func<T, T, double> distance, Func<T, double>? estimate = null) where T : IHasNeighbours<T> | |
| { | |
| estimate ??= (n) => distance(n, destination); | |
| var closed = new HashSet<T>(); | |
| var queue = new PriorityQueue<Path<T>, double>(); | |
| queue.Enqueue(new Path<T>(start), 0); | |
| while (queue.Count > 0) | |
| { | |
| var path = queue.Dequeue(); | |
| if (closed.Contains(path.LastStep)) | |
| continue; | |
| if (path.LastStep.Equals(destination)) | |
| return path.Reverse(); | |
| closed.Add(path.LastStep); | |
| foreach (var node in path.LastStep.Neighbours) | |
| { | |
| var newPath = path.AddStep(node, distance(path.LastStep, node)); | |
| queue.Enqueue(newPath, newPath.TotalCost + estimate(node)); | |
| } | |
| } | |
| return Enumerable.Empty<T>(); | |
| } | |
| private class Path<TNode> : IEnumerable<TNode> | |
| { | |
| public Path(TNode start) | |
| : this(start, null, 0) | |
| { | |
| } | |
| private Path(TNode lastStep, Path<TNode>? previousSteps, double totalCost) | |
| { | |
| LastStep = lastStep; | |
| PreviousSteps = previousSteps; | |
| TotalCost = totalCost; | |
| } | |
| public TNode LastStep { get; } | |
| public Path<TNode>? PreviousSteps { get; } | |
| public double TotalCost { get; } | |
| public Path<TNode> AddStep(TNode step, double stepCost) => new(step, this, TotalCost + stepCost); | |
| IEnumerator IEnumerable.GetEnumerator() => GetEnumerator(); | |
| public IEnumerator<TNode> GetEnumerator() | |
| { | |
| for (var p = this; p != null; p = p.PreviousSteps) | |
| { | |
| yield return p.LastStep; | |
| } | |
| } | |
| } | |
| } |
Another sample usage:
var edges = new List<Edge>();
for (var i = 0; i < 14; i++)
{
edges.Add(new Edge(i));
}
void addedge(int from, int to, double cost)
{
edges[from].AddEdge(edges[to], cost);
edges[to].AddEdge(edges[from], cost);
}
// http://graphonline.ru/en/?graph=xiloEvjFkRNQbQKv
addedge(0, 1, 3);
addedge(0, 2, 6);
addedge(0, 3, 5);
addedge(1, 4, 9);
addedge(1, 5, 8);
addedge(2, 6, 12);
addedge(2, 7, 14);
addedge(3, 8, 7);
addedge(8, 9, 5);
addedge(8, 10, 6);
addedge(9, 11, 1);
addedge(9, 12, 10);
addedge(9, 13, 2);
foreach (var n in AStar.FindPath(edges[0], edges[9], Edge.GetDistance))
{
Console.WriteLine(n);
}
public class Edge : AStar.IHasNeighbours<Edge>
{
public Edge(int index)
{
Index = index;
EdgesCosts = new List<Tuple<Edge, double>>();
}
public int Index { get; }
public List<Tuple<Edge, double>> EdgesCosts { get; }
public override string ToString() => Index.ToString();
public void AddEdge(Edge to, double cost) => EdgesCosts.Add(new Tuple<Edge, double>(to, cost));
public IEnumerable<Edge> Neighbours => EdgesCosts.Select(c => c.Item1);
public static double GetDistance(Edge from, Edge to) => from.EdgesCosts.Find(e => e.Item1 == to)?.Item2 ?? int.MinValue;
}
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Sample usage: