I tried implementing Wikipedia's version of the A* pathfinding algorithm, but when I run it in Unity the object which the script is applied to does nothing and Unity ends up taking 15% of my CPU. I feel like this is a stack overflow error, but I can't tell where it breaks down. Here's the entirety of the script:
public class Walker : MonoBehaviour {
public int x { get; set; }
public int y { get; set; }
public MapView core { get; set; }
public Node goal { get; set; }
public float DistanceBetween(Node a, Node b) {
return Mathf.Abs(a.x - b.x) + Mathf.Abs(a.y - b.y);
}
public List<Vector3> FindPath(Node goal) {
Node start = new Node(x, y);
List<Node> open = new List<Node>();
open.Add(start);
List<Node> closed = new List<Node>();
Dictionary<Node, Node> cameFrom = new Dictionary<Node, Node>();
Dictionary<Node, float> gScore = new Dictionary<Node, float>();
gScore[start] = 0;
Dictionary<Node, float> fScore = new Dictionary<Node, float>();
fScore[start] = DistanceBetween(start, goal);
while (open.Count != 0) {
Node current = open[0];
foreach (Node n in fScore.Keys)
if (fScore[n] < fScore[current])
current = n;
if (current == goal)
return ReconstructPath(cameFrom, current);
open.Remove(current);
closed.Add(current);
foreach (Node neighbor in Neighbors(current)) {
if (closed.Contains(neighbor))
continue;
float tempGScore = gScore[current] + DistanceBetween(current, neighbor);
if (!open.Contains(neighbor))
open.Add(neighbor);
else if (tempGScore >= gScore[neighbor])
continue;
cameFrom[neighbor] = current;
gScore[neighbor] = tempGScore;
fScore[neighbor] = gScore[neighbor] + DistanceBetween(neighbor, goal);
}
}
return new List<Vector3>();
}
public List<Vector3> ReconstructPath(Dictionary<Node, Node> cameFrom, Node current) {
List<Vector3> path = new List<Vector3>();
path.Add(current.GetVector());
while (cameFrom.ContainsKey(current)) {
current = cameFrom[current];
path.Add(current.GetVector());
}
return path;
}
public List<Node> Neighbors(Node current) {
int currx = current.x;
int curry = current.y;
List<Node> neighbors = new List<Node>();
neighbors.Add(new Node(currx, curry - 1));
neighbors.Add(new Node(currx, curry + 1));
neighbors.Add(new Node(currx - 1, curry));
neighbors.Add(new Node(currx + 1, curry));
return neighbors;
}
public virtual void SetStartCoords() {
x = (int)transform.position.x;
y = (int)transform.position.z;
}
}
public class Node {
public int x;
public int y;
public Node(int x, int y) {
this.x = x;
this.y = y;
}
public Vector3 GetVector() {
return new Vector3(x, 0, y);
}
}
Could I be misusing the DistanceBetween() function incorrectly, since I'm using it to calculate both the gScore and fScore of each node?
Edit: I tried a new implementation, but Unity is still not even responding. @ratchet-freak @bálint
public class Walker {
public List<Vector3> FindPath(Node goal) {
Node start = new Node(x, y);
SimplePriorityQueue<Node> open = new SimplePriorityQueue<Node>();
open.Enqueue(start,0);
List<Node> closed = new List<Node>();
while (open.Count != 0) {
Node current = open.Dequeue();
foreach (Node neighbor in Neighbors(current)) {
if (neighbor == goal)
return ReconstructPath(neighbor);
neighbor.gScore = DistanceBetween(current, neighbor);
neighbor.hScore = DistanceBetween(neighbor, goal);
neighbor.fScore = neighbor.gScore + neighbor.hScore;
bool skip = false;
foreach (Node n in open)
if (n == neighbor && n.fScore < neighbor.fScore)
skip = true;
foreach (Node n in closed)
if (n == neighbor && n.fScore < neighbor.fScore)
skip = true;
if (!skip)
open.Enqueue(neighbor, neighbor.fScore);
}
closed.Add(current);
}
return new List<Vector3>();
}
public List<Vector3> ReconstructPath(Node current) {
List<Vector3> path = new List<Vector3>();
path.Add(current.GetVector());
while (current.cameFrom != null) {
current = current.cameFrom;
path.Add(current.GetVector());
}
return path;
}
}