A * Pathfinding specific problem with implementation
I am having problems with my A * implementation. He sometimes chooses to do strange things on my grid, like ignore travel costs and move at a high cost or go in the wrong direction before getting back on track.
I have officially spent too many hours on this, so I would like to admit that I am reaching out to you to find a pair of fresh eyes.
private List<Vector2> PathFromTo(Vector2 startID, Vector2 targetID){
List<Vector2> path = new List<Vector2> ();
List<Node> closedList = new List<Node> ();
List<Node> openList = new List<Node> ();
Node startNode = nodeList.Find (tgt => tgt.nodeID == new Vector2 (startID.x, startID.y));
if (startNode == null)
return path;
Node targetNode = nodeList.Find (tgt => tgt.nodeID == new Vector2 (targetID.x, targetID.y));
if (targetNode == null)
return path;
openList.Add (startNode);
while (openList.Count > 0) {
Node current = openList [0];
for (int i = 1; i < openList.Count; i++)
if (openList [i].GetFCost () <= current.GetFCost () && openList [i].GetHCost () < current.GetHCost ())
current = openList [i];
openList.Remove (current);
closedList.Add (current);
if (current == targetNode) {
RetracePath (startNode, targetNode, ref path);
return path;
}
foreach(Vector2 neighbour in current.neighbors) {
Node neighbourNode = nodeList.Find (tgt => tgt.nodeID == new Vector2 (neighbour.x, neighbour.y));
CheckNeighbor(ref neighbourNode, ref current, ref targetNode, ref closedList, ref openList);
}
}
return path;
}
private void CheckNeighbor(ref Node neighborTile, ref Node currentTile, ref Node targetTile, ref List<Node> closedList, ref List<Node> openList){
if (neighborTile != null) {
if (!neighborTile.passable || closedList.Contains (neighborTile)) {
} else {
int newCostToNeighbor = (int)(currentTile.moveCost + CalculateDistance (currentTile.position, neighborTile.position));
if (newCostToNeighbor < neighborTile.GetGCost() || !openList.Contains (neighborTile)) {
neighborTile.SetGCost (newCostToNeighbor);
neighborTile.SetHCost (CalculateDistance (neighborTile.position, targetTile.position));
neighborTile.SetParent (currentTile);
if (!openList.Contains (neighborTile))
openList.Add (neighborTile);
}
}
}
}
public float CalculateDistance(Vector2 tileA_pos, Vector2 tileB_pos){
float dX = Mathf.Abs (tileB_pos.x - tileA_pos.x);
float dY = Mathf.Abs (tileB_pos.y - tileA_pos.y);
float shift1 = -(tileA_pos.x + tileA_pos.y);
float shift2 = -(tileB_pos.x + tileB_pos.y);
float dZ = Mathf.Abs (shift2 - shift1);
return Mathf.Max (dX, dY, dZ);
}
private void RetracePath(Node start, Node end, ref List<Vector2> pathInfo){
pathInfo = new List<Vector2> ();
Node current = end;
while (current != start) {
pathInfo.Add (current.nodeID);
current = current.GetParent ();
}
pathInfo.Reverse ();
}
source to share
After too many hours (and a full night's sleep) I was able to figure it out. The problem is with the CheckNeighbor function. The new method looks like this:
private void CheckNeighbor(ref Node neighborTile, ref Node currentTile, ref Node targetTile, ref List<Node> closedList, ref List<Node> openList, bool ignoreMoveCost = false){
if (neighborTile != null) {
if (!neighborTile.passable || closedList.Contains (neighborTile)) {
} else {
int newCostToNeighbor = (int)((ignoreMoveCost ? 1 : neighborTile.moveCost) + currentTile.GetGCost() + CalculateDistance (currentTile.position, neighborTile.position));
if (!openList.Contains (neighborTile)) {
openList.Add (neighborTile);
} else if (newCostToNeighbor >= neighborTile.GetGCost ()) {
return;
}
neighborTile.SetParent (currentTile);
neighborTile.SetGCost (newCostToNeighbor);
neighborTile.SetHCost (CalculateDistance (currentTile.position, neighborTile.position));
}
}
}
source to share
Considering the method CalculateDistance
you show in the comments, I wrote the following test program: (Assuming yours Mathf
is similar to System.Math
)
for (int y = -4; y < 5; y++)
{
for (int x = -4; x < 5; x++)
{
var dst = CalculateDistance(new Vector2(x, y), new Vector2());
Console.Write($"{((int)dst):D1} ");
}
Console.WriteLine();
}
The test program checks all coordinates between (-4, -4) and (4, 4) and calculates their distance to (0,0). Output:
8 7 6 5 4 4 4 4 4
7 6 5 4 3 3 3 3 4
6 5 4 3 2 2 2 3 4
5 4 3 2 1 1 2 3 4
4 3 2 1 0 1 2 3 4
4 3 2 1 1 2 3 4 5
4 3 2 2 2 3 4 5 6
4 3 3 3 3 4 5 6 7
4 4 4 4 4 5 6 7 8
As you can see, the result is absolutely chubby, you'd expect the bottom right corner to be as far away from (0,0) in the top right corner as possible, but that's not the case. You may need to rewrite your method CalculateDistance
.
You seem to be calculating dX, dY and dZ, which is not possible if you only have 2 coordinates ( Vector2
).
Edit: you can use pythagoras to determine the distance between two points, if the "weights" are not written:
var dist = Math.Sqrt(Math.Pow(point1.x - point2.x, 2) + Math.Pow(point1.y - point2.y, 2));
source to share
You don't specify whether you allow diagonal movements, but one of these two procedures for CalculateDistance should suffice:
public static readonly int D = 1;
public static readonly int D2 = 1;
public static float CalculateDistance(Vector2 tileA_pos, Vector2 tileB_pos)
{
float dX = Math.Abs(tileB_pos.x - tileA_pos.x);
float dY = Math.Abs(tileB_pos.y - tileA_pos.y);
return D * (dX + dY);
}
public static float CalculateDistanceDiagonalsAllowed(Vector2 tileA_pos, Vector2 tileB_pos)
{
float dX = Math.Abs(tileB_pos.x - tileA_pos.x);
float dY = Math.Abs(tileB_pos.y - tileA_pos.y);
return D * (dX + dY) + (D2 - 2 * D) * (dX < dY ? dX : dY);
}
Where D is the cost of the vertical / horizontal move and D2 is the cost of the diagonal move - you can set this to 1 or Sqrt (2) as needed. I am assuming currentTile.moveCost is used to define high / low cost tiles
source to share