# A* pathfinding for integer Vectors

Posted on

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);
while (openList.Any())
{
current = openList.First();
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)
{
}
// neighbor is already in openList, checking if this path is better
else
{
if (neighbor.g < oldNeighbor.g)
{
openList.Remove(oldNeighbor);
}
}
}
}
}
return null;
}

private static Path CreateResultPath(Location result)
{
List<Vector2i> resultPath = new List<Vector2i>();
while (result != null)
{
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);
while (openList.Any())
{
current = openList.First();
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)
{
}
// neighbor is already in openList, checking if this path is better
else
{
if (neighbor.g < oldNeighbor.g)
{
openList.Remove(oldNeighbor);
}
}
}
}
}
return null;
}

private static Path CreateResultPath(Location result)
{
List<Vector2i> resultPath = new List<Vector2i>();
while (result != null)
{
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));
}
}
``````