A* pathfinding for integer Vectors

Question

This class is for calculating the shortest path in a 2D World (without minding obstacles).

I'm using the A* algorithm and 2D Vectors (from the SFML Framework). It is working but takes (a lot of) time for higher amount of coordinates.

It takes 4 seconds to calculate the path from one edge of the map to the other.

Example from my Logs: (so you can see the coordinate range i meant)

Searching from : [Vector2i] X(100) Y(100) to: [Vector2i] X(3945) Y(4046)

Time needed: 4003ms

This is a lot for real time interaction and it's not even checking for obstacles.

I'm probably doing it not very efficient, so i hope you have some advice for me.

EDIT: Changed code to use Sets instead of Lists

 public static class AStar
 {
 protected class Location: IComparable<Location>
 {
 public Vector2i pos;
 public int f;
 public int g;
 public int h;
 public int weight;
 public Location parent;
 public Location(Vector2i pos)
 {
 this.pos = pos;
 f = 0;
 g = 0;
 h = 0;
 weight = 1;
 parent = null;
 }
 public override bool Equals(object obj)
 {
 Location other = obj as Location;
 return this.pos.X == other.pos.X && this.pos.Y == other.pos.Y;
 }
 public int CompareTo([AllowNull] Location other)
 {
 if (this.Equals(other))
 {
 return 0;
 }
 else if (other.f < this.f)
 {
 return 1;
 }
 else
 {
 return -1;
 }
 }
 }
 public static Path Search(Vector2i start, Vector2i target)
 {
 Location current = null;
 SortedSet<Location> openList = new SortedSet<Location>();
 HashSet<Location> closedList = new HashSet<Location>();
 Location targetLocation = new Location(target);
 openList.Add(new Location(start));
 while (openList.Any())
 {
 current = openList.First();
 closedList.Add(current);
 openList.Remove(current);
 if (current.Equals(targetLocation))
 {
 return CreateResultPath(current);
 }
 List<Location> possibleNeighbors = GetPossibleNeighbors(current);
 foreach (Location neighbor in possibleNeighbors)
 {
 // neighbor must not be in closedSet
 if (closedList.FirstOrDefault(location => location.Equals(neighbor)) == null)
 {
 // calculating neighbor
 neighbor.g = current.g + neighbor.weight;
 neighbor.h = CalcDistance(neighbor.pos, target);
 neighbor.f = neighbor.g + neighbor.h;
 neighbor.parent = current;
 Location oldNeighbor = openList.FirstOrDefault(location => location.Equals(neighbor));
 if (oldNeighbor == null)
 {
 openList.Add(neighbor);
 }
 // neighbor is already in openList, checking if this path is better
 else
 {
 if (neighbor.g < oldNeighbor.g)
 {
 openList.Remove(oldNeighbor);
 openList.Add(neighbor);
 }
 }
 }
 }
 }
 return null;
 }
 private static Path CreateResultPath(Location result)
 {
 List<Vector2i> resultPath = new List<Vector2i>();
 while (result != null)
 {
 resultPath.Add(result.pos);
 result = result.parent;
 }
 resultPath.Reverse();
 return new Path(resultPath.ToArray());
 }
 private static List<Location> GetPossibleNeighbors(Location current)
 {
 Vector2i currentPos = current.pos;
 List<Location> possibleNeighbors = new List<Location>();
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X - 1, currentPos.Y + 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X - 1, currentPos.Y - 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X - 1, currentPos.Y)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X, currentPos.Y + 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X, currentPos.Y - 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X + 1, currentPos.Y + 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X + 1, currentPos.Y - 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X + 1, currentPos.Y)));
 return possibleNeighbors;
 }
 private static int CalcDistance(Vector2i current, Vector2i target)
 {
 return Math.Abs(target.X - current.X) + Math.Abs(target.Y - current.Y);
 }
 }

• the default.: You should not use List for openList and closedList: for each processed vertex, you iterate over the entire openList (twice!) to find the minimum element in linear time, and for each neighbour, you iterate over the entire closedList... They should probably be SortedSets instead (HashSet might do for closedList). The line neighbor.f = neighbor.g = neighbor.h seems to have a bug (should use a +).
• Urbs: @mypronounismonicareinstate With Sorted Set it actually takes more time than before even though i can now just say openSet.First() instead of min and then First(minObject). I think adding an element to the SortedList takes more time than it saves.
• the default.: It should be logarithmic (constant-ish for HashSet) in time complexity if used correctly though. Could you show your code with SortedSets instead of Lists? (and have you fixed the neighbor.f = neighbor.g = neighbor.h line?) Priority queues can also be used for the same time complexity with SortedSets, but a noticeably lower constant factor.
• Urbs: @mypronounismonicareinstate i've edited the code in the original post. And yes i did fix the neighbor.f line.
• the default.: I think the guess I had was correct: FirstOrDefault on sets still iterates over the set in linear time, with a higher constant factor than regular Lists. I can't recall what methods are used for searching in sets in C#, but that should be easy to find.

Answer 1

Solution suggested by @mypronounismonicareinstate.

The time save is a bit more than factor 100. The average calculation time was ~6s now it's ~50-60ms.

First of all using Sets instead of Lists. In terms of iterating and removing items, Sets are way more faster.

Second: Using SortedSet to save the check for the minimum f in OpenList. And use HashSet for closedList.

Im my original solution this took 2 times iterating the whole list.

Third: Not using System.Linq in Combination with Sets if you want to iterate. System.Linq will always iterate through the whole set.

The final code:

public static class AStar
 {
 protected class Location: IComparable<Location>
 {
 public Vector2i pos;
 public int f;
 public int g;
 public int h;
 public int weight;
 public Location parent;
 public Location(Vector2i pos)
 {
 this.pos = pos;
 f = 0;
 g = 0;
 h = 0;
 weight = 1;
 parent = null;
 }
 public override bool Equals(object obj)
 {
 Location other = obj as Location;
 return this.pos.X == other.pos.X && this.pos.Y == other.pos.Y;
 }
 public int CompareTo([AllowNull] Location other)
 {
 if (this.Equals(other))
 {
 return 0;
 }
 else if (other.f < this.f)
 {
 return 1;
 }
 else
 {
 return -1;
 }
 }
 }
 public static Path Search(Vector2i start, Vector2i target)
 {
 Location current = null;
 SortedSet<Location> openList = new SortedSet<Location>();
 HashSet<Location> closedList = new HashSet<Location>();
 Location targetLocation = new Location(target);
 openList.Add(new Location(start));
 while (openList.Any())
 {
 current = openList.First();
 closedList.Add(current);
 openList.Remove(current);
 if (current.Equals(targetLocation))
 {
 return CreateResultPath(current);
 }
 List<Location> possibleNeighbors = GetPossibleNeighbors(current);
 foreach (Location neighbor in possibleNeighbors)
 {
 // neighbor must not be in closedSet
 if (!closedList.Contains(neighbor)) 
 {
 // calculating neighbor
 neighbor.g = current.g + neighbor.weight;
 neighbor.h = CalcDistance(neighbor.pos, target);
 neighbor.f = neighbor.g + neighbor.h;
 neighbor.parent = current;
 openList.TryGetValue(neighbor, out Location oldNeighbor);
 if (oldNeighbor == null)
 {
 openList.Add(neighbor);
 }
 // neighbor is already in openList, checking if this path is better
 else
 {
 if (neighbor.g < oldNeighbor.g)
 {
 openList.Remove(oldNeighbor);
 openList.Add(neighbor);
 }
 }
 }
 }
 }
 return null;
 }
 private static Path CreateResultPath(Location result)
 {
 List<Vector2i> resultPath = new List<Vector2i>();
 while (result != null)
 {
 resultPath.Add(result.pos);
 result = result.parent;
 }
 resultPath.Reverse();
 return new Path(resultPath.ToArray());
 }
 private static List<Location> GetPossibleNeighbors(Location current)
 {
 Vector2i currentPos = current.pos;
 List<Location> possibleNeighbors = new List<Location>();
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X - 1, currentPos.Y + 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X - 1, currentPos.Y - 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X - 1, currentPos.Y)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X, currentPos.Y + 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X, currentPos.Y - 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X + 1, currentPos.Y + 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X + 1, currentPos.Y - 1)));
 possibleNeighbors.Add(new Location(new Vector2i(currentPos.X + 1, currentPos.Y)));
 return possibleNeighbors;
 }
 // Chebyshev Distance
 private static int CalcDistance(Vector2i current, Vector2i target)
 {
 return Math.Max(Math.Abs(target.X - current.X), Math.Abs(target.Y - current.Y));
 }
 }

• the default.: I have noticed what seems to be another bug: your GetPossibleNeighbors function seems to generate neighbours on a grid with Chebyshev distance (as in, moving like a chess king; \$\max(\Delta x, \Delta y)\$), but the heuristic uses the rectilinear/taxicab/Manhattan distance (as in, moving to any of the 4 neighbours with a common side; \$\Delta x + \Delta y\$). This makes the heuristic non-admissible (it overestimates diagonal paths) and voids your warranty and you don't necessarily get shortest paths.
• Urbs: @mypronounismonicareinstate yes you are right. I'm going to adjust the CalcDistance Function then.
• Urbs: @mypronounismonicareinstate the CalcDistance Function should be correct now.