"Simple" pathfinding algorithm

Question

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

1. 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.

2. 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.

3. 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.

4. 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.

5. 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.

Answer 1

Let's just review this vector; there's plenty to talk about here.

This may seem like nit picking but hey, first, you asked, and second, build strong foundations in your helper types. If they are weak, you will build a weak, buggy program on top of them. Make them do exactly what they say on the tin. This is a 2-vector of integers, so make it follow the conventions of a 2-vector of integers.

struct vec2

Follow C# naming conventions. No abbrvs! Use PascalCase for names.

public int x, y;

Mutable fields in a struct are badness; so many bugs come from this. Public fields in general in C# are considered to be a bad practice. Use properties backed by private fields. Set them once. Never change them. If you need to change a vector then you make a new vector, same way that if you need to change an integer, you don't turn 12 into 17 when you add 5. 12 stays the same.

public static bool operator== (vec2 a, vec2 b) {

The implementation is fine, but I would always override Equals when overriding ==. Even in this case where it is not strictly necessary I think it is wise.

    public static vec2 operator+ (vec2 a, vec2 b)
    {
        return new vec2 { x = a.x + b.x, y = a.y + b.y };
    }

This is "make an empty vector and then mutate it". You already have a constructor for that:

new vec2(x : a.x + b.x, y : a.y + b.y );

is better.

//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) );
}

NO NO NO. Do not make cute overloads of operators! Ever! The product of two vectors is not the distance between the points they point at when they both begin at the origin! The product of two vectors is either the dot product or the cross product, and this is neither of them.

You're representing a mathematical object here. Use the conventions of mathematics that we're all familiar with. You need the distance between two vectors? Subtract them and take the magnitude.

So let's start over.

struct Vector2
{
    public int X { get; private set; }
    public int Y { get; private set; }

The C# compiler generates the field, getter and setter for you. In C# 6 you can elide the private set;.

    public Vector2(int x, int y) : this()
    {
        X = x;
        Y = y;
    }

The this() tells the compiler that it should initialize the hidden fields to zero, which the compiler likes to happen before instance properties are accessed.

    public static bool Equals(Vector2 a, Vector2 b)
    {
        return (a.X == b.X) & (a.Y == b.Y);
    }

Whether you use & or && here doesn't really matter. Remember, checking to see if the previous comparison produced false and skipping the second comparison might be more work than simply doing the second comparison!

The point here is that we now have one-stop shopping for equality.

    public static bool operator== (Vector2 a, Vector2 b) 
    {
        return Equals(a, b);
    }

    public static bool operator !=(Vector2 a, Vector2 b)
    {
        return !Equals(a, b);
    }

    public bool Equals(Vector2 a)
    {
        return Equals(this, a);
    }

You can use this method to implement IEquatable<Vector2> which might be a good idea.

    public override bool Equals(object a)
    {
        return a is Vector2 && Equals(this, (Vector2)a);
    }

    public override int GetHashCode() 
    {
        return X + 17 * Y;
    }

Seems legit.

As a debugging aid:

    public override string ToString() 
    {
        return "(" + X + "," + Y + ")";
    }

I note that all of these methods are a lot more concise in C# 6, without losing comprehensibility:

    public override string ToString() => $"({X},{Y})";

so if you can write these in C# 6, do so.

    public static Vector2 operator+ (Vector2 a, Vector2 b)
    {
        return new Vector2(a.X + b.X, a.Y + b.Y);
    }

Now we could also use unary subtraction and binary subtraction:

    public static Vector2 operator- (Vector2 a, Vector2 b)
    {
        return new Vector2(a.X - b.X, a.Y - b.Y);
    }

    public static Vector2 operator- (Vector2 a)
    {
        return new Vector2(-a.X, -a.Y);
    }

And magnitude:

    public double Magnitude
    {
        get
        {
            return Math.Sqrt(X * X + Y * Y);
        }
    }

Again, much shorter in C# 6:

    public double Magnitude => Sqrt(X * X, Y * Y);

And now we can implement distance:

    public static double Distance(Vector2 a, Vector2 b)
    {
        return (a - b).Magnitude;
    }

    public double Distance (Vector2 a)
    {
        return Distance(this, a);
    }

We could also use an explicit zero:

    public readonly static Vector2 Zero = default(Vector2);

All right. I'll give some thoughts on the rest of your implementation in another answer; this is enough for one answer.

Answer 2

After looking at this code some more I'll expand on my initial comment.

Your stack variable keeps track of the path from start to end. You push the same coordinate value onto it in your foreach loop in findPath multiple times, and remove it after each unsuccessful attempt. You could push it before the loop, and remove it after the loop, to avoid this duplication.

You should keep track of the places you've already been, or that are already in the list of places to check. The way you have it now you're checking the same spot multiple times. For example, your starting location at (1,1) will check from (1,2) and (2,1). A couple of steps later, when checking from (2,2), you'll add (2,1) and check it again. With diagonals this duplication is even worse, which is probably why it takes so much longer. (And only allocating 6 elements in the SortedList instead of 8 won't help.)

Using a loop instead of recursion will help some.

And for completeness, I'll restate my comment: One minor improvement would be to not take the square root in your distance calculation. Since you're comparing distances that works just as well with the square of the distance as it does the square root. It's also a bit confusing to use the * operator for distance, as it is frequently used with vectors for cross product.

Answer 3

Finally, your path finding algorithm itself.

I am not going to criticize the algorithm; others have already talked about how it revisits ground its been over before, backtracks unnecessarily, and so on. I want to talk about the actual quality of the code, not the algorithm.

public bool findPath()

Again follow proper naming conventions etc.

Methods should do what they say on the tin. This method should be named "PathExists" because it returns true if one exists and false otherwise. One questions why this method is useful; I would expect a method called FindPath to return a path, or null if none existed.

You notice something about your program: it is a pathfinding program with types to represent points and maps. Where is the type that represents a path? You said on the tin that this thing finds paths; I would have expected that to be front-and-center in the type system! Instead this thing reads like maps and vectors are the most important things and paths are unimportant.

When I was playing around with path finding algorithms a few years back my FindPath method had this signature:

static public Path<Node> FindPath<Node>(
 Node start, 
 Node destination, 
 Func<Node, Node, double> distance, 
 Func<Node, double> estimate)
 where Node : IHasNeighbours<Node>

My Node is like your Tile except that nodes (1) know their neighbours, and (2) never point you at an impassible neighbour. The metrics for distance and estimating are passed in, and the result is a path of nodes.

Moving on...

    Console.Clear();
    printMap();

I like your debugging aid. I don't like that it modifies the console. If this is a debugging aid, use Debug.Write, not Console.Write, and look at it in the debugger. If it's part of the user interface of the program, then make a public "animate path" method that does that.

But what would be even better is to make a method that produces a second Map with the path filled in. And then make a ToString on the Map type so that you can easily render it. You went to all the work of making a map type. Use it to help you.

    if ((iterations++ & 4) >> 1 == 1)

This is illegible. It modifies a variable in the same expression where it uses its value, which is awful. It treats an integer as a bit field. And is the logic even right? This says to take the iterations, remove all the bits except for bit two, shift bit two to bit one, and then compare that against bit zero. What is going on here?

  var q = new SortedList<double, Tile>(allowDiagonalMovement ? 6 : 4, new DupeKeyComparer<double>());

OK, the goal here as I understand it is: create a list of possible next tiles sorted by which one is shortest Euclidean distance to the goal. This is a good idea; you are well on your way to reinventing the A* algorithm. I note that in A* the Euclidean metric is permissible because it never over-estimates.

However the implementation here could use some work.

First off, I cannot for the life of me figure out where the magic numbers 6 and 4 came from. What do these mean?

Second, let's take a step back. What do we want? A list of tiles. How do we want it sorted? By distance to the end. So let's start with a list of tiles. And let's write a bunch of helper methods along the way.

var q = new List<Tile>();
if (map.IsValid(start.X + 1, start.Y))
    q.Add(map[start.X + 1, start.Y));

Make a method on the map that means "is this square one that I can move onto?" because that's what you need to know. If it is, then add that square to the map. Easy peasy. Now do that seven more times, which I omit. In the original program you put tiles into the list that you can't even move onto. Why bother? Just don't even go there.

OK, now we have a list that only contains valid tiles. You want to iterate over it, ordered by a particular metric. So do that:

foreach(var p in q.OrderBy(tile => tile.Location.Distance(end)))

You want to do something in a particular order, so use OrderBy. You want the order to be the distance from a location to another location? Write a method called Distance and call it inside OrderBy. Always be thinking "this code represents an idea; how can I make the code read like the idea, and not read like its implementation details?" Your code is hard to read because it is full of mechanisms that obscure meaningful code. You want to know if a coordinate is valid; don't compare a coordinate to an array length, ask the map if the coordinate is valid with a method called IsValid. You will find that your code gets so much easier to understand, and bonus, is more likely to be correct.

Answer 4

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;
    }
}

Never ever ever do this! A comparer must obey the rules of comparison:

• Things are equal to themselves.
• Equality is transitive. Things equal to the same are equal to each other.
• Size is transitive: if A is bigger than B and B is bigger than C, then A must be bigger than C.
• Size is antisymmetric: if A is bigger than B then B absolutely must be smaller than A.

The rules are there because the comparer must produce a total order. That is, given any set of things to compare, comparison of them in all pairs must produce a single consistent total order of the set.

The algorithms that are implemented in the sorted collection sets require these to be true for their correct operation. They are permitted to crash, go into infinite loops, or produce an unsorted set if any of these conditions are violated.

If you feel that you need to violate these conditions then you are doing something deeply wrong and you need to step back and re-architect the solution.

Answer 5

class Map

Do you intend to subclass this class? If not, I recommend sealing it.

Tile[,] map;
public Map(Tile[,] t) {
    this.map = t;
}

Seems legit.

public int X { get { return map.GetLength(0); } }
public int Y { get { return map.GetLength(1); } }

Why are these called X and Y, and not Width and Height? I don't think of a map as having properties X and Y.

public Tile this[int x, int y] {
    get { return map[x, y]; }
    set { map[x, y] = value; }
}

Again, seems legit.

public static Map makeMap(string[] map)
{

Again, this should be MakeMap. But why is this not a constructor to begin with? Why choose to use a factory method here?

    Tile[,] t = new Tile[map.Length, map[0].Length];

The assumption is that all the strings are of the same length. Seems brittle. A better choice would be to (1) produce an error if they are not, or (2) make the array the size of the longest string, and fill in the holes appropriately.

    for(int i = 0; i < map.Length; i++)
        for (int j = 0; j < map[0].Length; j++)
        {
            char c = map[i].ToCharArray()[j];

Why on earth are you turning the string into a char array here? Why not just

            char c = map[i][j];

???

            t[i, j] = new Tile { xy = new vec2(i, j), ttype = c == '#' ? Tile.TileType.OBSTACLE : Tile.TileType.FREE };

Again, use the constructor rather than making a blank temporary tile and mutating it.

I note that there is a huge amount of redundancy here. You have an array, indexed by two integers. What does it contain at the position 1, 2? The numbers 1, 2 and a type. Why is the map not simply a two-d array of tile kinds? It seems bizarre to have an array that spends two thirds of its space storing its own coordinates!

Answer 6

Many of the same problems arise in the Tile type as in the vec2 type:

• All the properties should be read-only properties, not public mutable fields.
• Set everything in the constructor and don't change it.
• Follow proper naming conventions. Public things begin with capital letters.
• Implement equality consistently.

But let's consider a few other points.

public enum TileType
{
    FREE, OBSTACLE
}

STOP SHOUTING AT ME, THIS ISN'T JAVA.

I like the use of an enum here. I'm not sure it needs to be a nested enum. Nested public types are considered to be a bit of a code smell in C#, though this is more justified than most.

public TileType ttype

A fun fact that you might not know is in C# it is legal to have a property the same name as its type:

public TileType TileType { get; private set; }

This is called the "Color Color" problem because obviously it is common to have a property called color whose type is a type called color, and that needs to be legal.

But...

I still don't like it. The nested enum is already in a type called Tile; does it really need Tile in the name? That seems redundant. And Type has a very specific meaning already. I would be inclined to do this differently.

public vec2 xy;

Awful. Is this the location of the tile? Then call the property Location.

public static bool operator== (Tile a, Tile b) {
    return a.x == b.x && a.y == b.y && a.ttype == b.ttype;
}

Why are tiles being compared for equality at all? And if they are being compared, why are they being compared by their type? Surely if two tiles are at the same location then they already have the same type. I don't understand why you are not simply comparing the locations when you need to compare two tiles.

Let's put it all together, in C# 6 for brevity:

struct Tile
{
    public enum Kind
    {
        Free, Obstacle
    }
    public Kind Kind { get; }    
    public Vector2 Location { get; }
    public int X => Location.X;
    public int Y => Location.Y;

This seems a little redundant, but also useful, so let it stand.

public Tile(int x, int y, Kind kind) : this(new Location(x, y), kind)
{}
public Tile(Location location, Kind kind) : this()
{
    Location = location;
    Kind = kind;
}

And you can work out how to better write equality.

That'll do it for the tile type. Onward in yet another answer.