In this post we will discuss the implementation of A* pathfinding using SteerLite. The projects were created by a team of 3 as part of a class project for Introduction to Computer Graphics at Rutgers University.
Implementation
We implemented A* using two different heuristic methods: Euclidean distance and Manhattan distance. Euclidean distance is the straight line distance between any two points. Manhattan distance on the other hand is based only on horizontal and vertical paths on a grid.
Manhattan distance
1
2
3
4
5
6
7
double AStarPlanner::Manhattan(Util::Point FirstPoint, Util::Point SecondPoint)
{
Util::Vector diff = FirstPoint - SecondPoint;
return abs(diff.x) + abs(diff.y) + abs(diff.z);
}
}
Euclidean distance
1
2
3
4
5
6
7
8
9
10
11
12
13
double AStarPlanner::Heuristic(Util::Point a, Util::Point b)
{
...
else {
return (double)distanceBetween(a,b);
}
...
}
Example Videos
A* Manhattan First Test Case
A* Manhattan Second Test Case
A* Euclidiean First Test Case
A* Euclidiean Second Test Case
Handling Ties in F value
Ties in F values when searching for the lowest F value were handled by either selecting the node with the higher or lower G value.
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
if(NodeMap.at(OpenSet[i]).f < lowestFScore)
{
lowestFScore = NodeMap.at(OpenSet[i]).f;
GScore = NodeMap.at(OpenSet[i]).g;
lowestFIndex = i;
}
else if(NodeMap.at(OpenSet[i]).f == lowestFScore)
{
if(NextExpandingNodeTie == 1)
{
if(NodeMap.at(OpenSet[i]).f > GScore)
{
lowestFScore = NodeMap.at(OpenSet[i]).f;
GScore = NodeMap.at(OpenSet[i]).g;
lowestFIndex = i;
}
}
else if(NextExpandingNodeTie == -1)
{
if(NodeMap.at(OpenSet[i]).f < GScore)
{
lowestFScore = NodeMap.at(OpenSet[i]).f;
GScore = NodeMap.at(OpenSet[i]).g;
lowestFIndex = i;
}
}
}
Example Videos
A* Big G Favoring Manhattan First Test Case
A* Big G Favoring Manhattan Second Test Case
A* Low G Favoring Manhattan First Test Case
A* Low G Favoring Manhattan Second Test Case
A* Big G Favoring Euclidiean First Test Case
A* Big G Favoring Euclidiean Second Test Case
A* Low G Favoring Euclidiean First Test Case
A* Low G Favoring Euclidiean Second Test Case
Breaking the ties for smaller or bigger G values does not affect the length of the solution path at all as would be expected. It does, however, change the number of nodes that are expanded. The number of nodes that are expanded will be less than or equal to the number of nodes expanded in the original A Star algorithm. The exact value depends on the structure of the environment, but performance will never be worse than A Star.
Increasing Diagonal Cost
The cost of taking Diagonal paths were increased.
1
2
3
4
5
6
7
8
9
TentativeScore = FromNode.g + distanceBetween(FromNode.point,CurrentPoint);
if(PART3)
{
if(CurrentPoint.x != FromNode.point.x && CurrentPoint.z != FromNode.point.z) //if diagonal
{
TentativeScore = FromNode.g + 2*distanceBetween(FromNode.point,CurrentPoint);
}
}
Example Videos
A* Diagonally Weighted Manhattan First Test Case
A* Diagonally Weighted Manhattan Second Test Case
A* Diagonally Weighted Euclidiean First Test Case
A* Diagonally Weighted Euclidiean Second Test Case
Increasing the costs for diagonals worsens performance for Euclidean test cases. The agent is less likely to take a diagonal step over a horizontal or vertical step, so the length of the solution path larger than the one for A Star. It is usually faster for the agent to travel diagonally to reach the goal, so performance is worsened. For Manhattan distance, the paths are unaffected. This is expected because the agent will never travel diagonally when the heuristic is set to Manhattan distance.
Weighted Heuristic
Both the Manhattan and the Euclidean Heuristic were weighted with weights of 2,4,and 8.
1
2
3
4
5
6
7
8
9
10
11
double AStarPlanner::Heuristic(Util::Point a, Util::Point b)
{
if(USE_MANHATTAN_DISTANCE)
{
return WEIGHT*Manhattan(a,b);
}
else
{
return WEIGHT*(double)distanceBetween(a,b);
}
}
A* Heuristic of Weight 2 Manhattan First Test Case
A* Heuristic of Weight 2 Manhattan Second Test Case
A* Heuristic of Weight 4 Manhattan First Test Case
A* Heuristic of Weight 4 Manhattan Second Test Case
A* Heuristic of Weight 8 Manhattan First Test Case
A* Heuristic of Weight 8 Manhattan Second Test Case
A* Heuristic of Weight 2 Euclidean First Test Case
A* Heuristic of Weight 2 Euclidean Second Test Case
A* Heuristic of Weight 4 Euclidean First Test Case
A* Heuristic of Weight 4 Euclidean Second Test Case
A* Heuristic of Weight 8 Euclidean First Test Case
A* Heuristic of Weight 8 Euclidean Second Test Case
Using weighted A Star increases performance by significantly reducing the number of expanded nodes. For example, in Search-2 Euclidean, adding a weight of 2 reduces the number of expanded nodes from 2206 to 1026 while still finding the optimal path as in A Star. However, while increasing the weights decreases the number of nodes you need to expand, it makes the path found less likely to be the optimal one. Therefore, weighted A Star is not guaranteed to find the optimal path. This can be seen in Search-2 Euclidean when the weight is 8. The optimal path is 79. 799, but adding a weight of 8 finds a path of length 82.3848 although the number of expanded nodes is greatly decreased.
Github Link: Here