Problem
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);
}
}
Solution
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));
}
}