I decided to write a pathfinding algorithm in order to expose myself to the world of pathfinding and for me to further understand more advanced algorithms such as A*. I chose C# due to its flexibility compared to other languages such as Java.
I will start with the basic objects and work up to the more complex ones.
vec2.cs:
struct vec2
{
public int x, y;
public vec2(int x, int y)
{
this.x = x;
this.y = y;
}
public static bool operator== (vec2 a, vec2 b) {
return a.x == b.x && a.y == b.y;
}
public static bool operator !=(vec2 a, vec2 b)
{
return !(a == b);
}
public static vec2 operator+ (vec2 a, vec2 b)
{
return new vec2 { x = a.x + b.x, y = a.y + b.y };
}
//Distance formula
public static double operator* (vec2 a, vec2 b) {
return Math.Sqrt( Math.Pow(b.x-a.x,2) + Math.Pow(b.y-a.y,2) );
}
}
Tile.cs:
struct Tile
{
public enum TileType
{
FREE, OBSTACLE
}
public TileType ttype;
public vec2 xy;
public int x {get {return xy.x;}}
public int y {get {return xy.y;}}
public static bool operator== (Tile a, Tile b) {
return a.x == b.x && a.y == b.y && a.ttype == b.ttype;
}
public static bool operator !=(Tile a, Tile b)
{
return !(a == b);
}
}
Map.cs:
class Map
{
Tile[,] map;
public Map(Tile[,] t) {
this.map = t;
}
public int X { get { return map.GetLength(0); } }
public int Y { get { return map.GetLength(1); } }
public Tile this[int x, int y] {
get { return map[x, y]; }
set { map[x, y] = value; }
}
public static Map makeMap(string[] map)
{
Tile[,] t = new Tile[map.Length, map[0].Length];
for(int i = 0; i < map.Length; i++)
for (int j = 0; j < map[0].Length; j++)
{
char c = map[i].ToCharArray()[j];
t[i, j] = new Tile { xy = new vec2(i, j), ttype = c == '#' ? Tile.TileType.OBSTACLE : Tile.TileType.FREE };
}
return new Map(t);
}
}
DupeKeyComparer.cs:
class DupeKeyComparer<TKey> : IComparer<TKey> where TKey : IComparable
{
public int Compare(TKey x, TKey y)
{
int c = x.CompareTo(y);
return (c == 0) ? 1 : c;
}
}
SimplePathfinder.cs:
class SimplePathfinder
{
Stack<Tile> stack;
Map map;
vec2 initialStart, initialEnd;
bool allowDiagonalMovement;
int iterations;
public int Iterations {get {return iterations;}}
public SimplePathfinder(Map map, vec2 start, vec2 end, bool allowDiagonalMovement)
{
this.map = map;
this.initialStart = start;
this.initialEnd = end;
this.allowDiagonalMovement = allowDiagonalMovement;
stack = new Stack<Tile>();
}
public bool findPath()
{
iterations = 0;
bool result = findPath(initialStart, initialEnd);
Console.Clear();
printMap();
return result;
}
bool findPath(vec2 start, vec2 end)
{
if ((iterations++ & 4) >> 1 == 1)
{
Console.Clear();
printMap();
}
if (start == end) return true;
//Sorted by shortest direct path first
var q = new SortedList<double, Tile>(allowDiagonalMovement ? 6 : 4, new DupeKeyComparer<double>());
if (start.x + 1 < map.X) q.Add((start + new vec2 { x = 1, y = 0 }) * end, map[start.x + 1, start.y]);
if (start.x - 1 >= 0) q.Add((start + new vec2 { x = -1, y = 0 }) * end, map[start.x - 1, start.y]);
if (start.y + 1 < map.Y) q.Add((start + new vec2 { x = 0, y = 1 }) * end, map[start.x, start.y + 1]);
if (start.y - 1 >= 0) q.Add((start + new vec2 { x = 0, y = -1 }) * end, map[start.x, start.y - 1]);
if (allowDiagonalMovement)
{
if (start.x + 1 < map.X && start.y + 1 < map.Y)
q.Add((start + new vec2 { x = 1, y = 1 }) * end, map[start.x + 1, start.y + 1]);
if (start.x + 1 < map.X && start.y - 1 >= 0)
q.Add((start + new vec2 { x = 1, y = -1 }) * end, map[start.x + 1, start.y - 1]);
if (start.x - 1 >= 0 && start.y + 1 < map.Y)
q.Add((start + new vec2 { x = -1, y = 1 }) * end, map[start.x - 1, start.y + 1]);
if (start.x - 1 >= 0 && start.y - 1 >= 0)
q.Add((start + new vec2 { x = -1, y = -1 }) * end, map[start.x - 1, start.y - 1]);
}
foreach(var p in q) {
if (p.Value.ttype == Tile.TileType.OBSTACLE ||
stack.Count != 0 && stack.Contains(p.Value)) continue;
stack.Push(map[start.x, start.y]);
if (findPath(p.Value.xy, end)) return true;
}
stack.Pop();
return false;
}
//For debugging purposes only.
void printMap()
{
for (int i = 0; i < map.X; i++)
for (int j = 0; j < map.Y; j++)
{
Console.SetCursorPosition(j, i);
Console.Write(map[i, j].ttype == Tile.TileType.FREE ? "-" : "#");
}
var bufStack = new Stack<Tile>(stack);
Tile t;
while (bufStack.Count != 0)
{
t = bufStack.Pop();
Console.SetCursorPosition(t.y, t.x);
Console.Write("o");
}
Console.SetCursorPosition(initialStart.y, initialStart.x);
Console.Write("S");
Console.SetCursorPosition(initialEnd.y, initialEnd.x);
Console.Write("E");
}
}
Sample client code:
var map = Map.makeMap(map2);
var sp = new SimplePathfinder(map, new vec2(1, 0), new vec2(19, 30), false);
Console.WriteLine("Started pathfinding...");
var success = sp.findPath();
Console.SetCursorPosition(0, map.X);
Console.WriteLine(String.Format("Done pathfinding! Result: {0}, took {1} iterations for {2} tiles.", success, sp.Iterations, map.X*map.Y));
Sample output:
############################### Soo#-ooooooooooooooo-#--------# #-o#-o#############o-####--#--# #-oooo#--------#-ooo-------#--# #######--#--####-o##########--# #--#oo---#--#-oooo#--------#--# #--#oo#######-o####--####--#--# #-ooooooooooooo#-----#--#--#--# #-o###################--#--#### #-ooooooo#-ooooooo#-----#-ooo-# #######-o#-o####-o#--####oo#o-# #-----#-o#-o#-oooo#-----#oo#o-# #--####-o#-o#-o####--#--#oo#o-# #-------oooo#-oooo#--#--#oo#o-# ################-o####--#oo#o-# #-ooooooooooooo#-o#-oooo#oo#o-# #-o##########-o#-o#-o#-o#-o#o-# #-oooooooooo#-oooo#-o#-oooo#oo# #--#######-o#######-o#######-o# #--------#-oooooooooo#-------oE ############################### Done pathfinding! Result: True, took 1846 iterations for 651 tiles.
Self-criticism
I did not really optimize my algorithm, mostly because I wrote it on a sheet of paper and then copied it. I did not look at any sample implementations when I was working on this; only after finishing this did I realize that this somewhat resembles Dijkstra's algorithm. In this maze, I think my algorithm had a complexity of about O(3n) where n is the number of tiles.
My pathfinder takes forever to find a path if there are no possible paths; I resolved it would take about
(n-1)!
iterations for the pathfinder to figure this out. I need to determine whether or not points A and B are in the same closed room, but I did not include this behavior in my algorithm. Perhaps I will include it in the future, but for now I find this situation like trying to solve the halting problem.The pathfinder takes an unexpectedly long amount of time to run if diagonals are enabled. I thought it would take less time as diagonals almost always take shorter paths, but I was wrong. Perhaps it was a simple mistake I made in the code.
I made liberal use of SortedList. Sorting is expensive, and I "hacked" the comparer to allow duplicate keys for possibilities with the same distances to point B.
The pathfinder does not catch itself when it sometimes takes the long way to an object instead of the straight path. It's not a huge problem since it is a valid path, but annoying nonetheless.
*
operator for distance, as it is frequently used with vectors for cross product. \$\endgroup\$ – 1201ProgramAlarm Dec 20 '15 at 5:07A*
algorithm in C# you can read my series on it: blogs.msdn.microsoft.com/ericlippert/tag/astar \$\endgroup\$ – Eric Lippert Dec 20 '15 at 18:49