A* Pathfinding implementation not quite working

Hi, all. First note: If you spend the time to read this post, you have my respect. If you take the time to locate the error, you rock. :stuck_out_tongue:

So, after about 20 hours of research and four failed attempts, I managed to produce the below script (you’ll have to copy it into your favorite code editor for it to make any sense, the styling on here is awful):

using UnityEngine;
using System.Collections;

public class SmartMove : MonoBehaviour {
	
	public Transform[] CalculatePath(Transform startingNode, Transform destinationNode) {
		Node startNode = startingNode.GetComponent<Node>();
		Node endNode = destinationNode.GetComponent<Node>();
		
		startNode.G = 0;
		startNode.H = EuclideanHeuristic(startingNode, destinationNode);
		startNode.F = startNode.H;
		
		Hashtable openList = new Hashtable();
		Hashtable closedList = new Hashtable();
		
		openList.Add("startNode", startingNode);
		string currentNodeString;
		while(1 > 0) {
			
			currentNodeString = ReturnLowestF(openList);
			SwitchList(currentNodeString, openList, closedList);
			Transform currentNodeObject = (Transform)closedList[currentNodeString];
			Node currentNode = currentNodeObject.GetComponent<Node>();
			Transform[] children = currentNode.children;
			
			
			foreach(Transform child in children) {
				if(child!=null) {
					Node childNode = child.GetComponent<Node>();
					if(childNode.isWalkable  !closedList.ContainsValue(child)) {
						if(!openList.ContainsValue(child)) {
							openList.Add((Random.value*Random.value + Time.deltaTime).ToString(), child);
							childNode.G = currentNode.G+childNode.movementCost;
							childNode.H = EuclideanHeuristic(child, destinationNode);
							childNode.F = childNode.G + childNode.H;
							childNode.parent = currentNodeObject;
						}
						
						if(openList.ContainsValue(child)) {
							float tempG = currentNode.G + childNode.movementCost;
							if(tempG < currentNode.G) {
								childNode.parent = currentNodeObject;
								childNode.G = currentNode.G + childNode.movementCost;
								childNode.F = childNode.G + childNode.H;
							}
						}
					}
				}
			}
			
			if(closedList.ContainsValue(destinationNode))
				break;
			
			if (openList.Count == 0)
				break;
		}
		
		Hashtable pathNodes = new Hashtable();
		
		Node currentPathNode = endNode;
		int i = 0;
		while(1>0) {
			if (currentPathNode.parent==null)
				break;
			
			pathNodes.Add(i, currentPathNode.GetComponent<Transform>());
			currentPathNode = currentPathNode.parent.GetComponent<Node>();
			
			i++;
		}
		
		Transform[] path = new Transform[pathNodes.Count];
		pathNodes.Values.CopyTo(path, 0);
		
		return path;
	}
	
	string ReturnLowestF(Hashtable list) {
		float lowestF = Mathf.Pow(10,100);
		string lowestKey = "ah";
		foreach(DictionaryEntry currentEntry in list) {
			Transform currentEntryObject = (Transform)list[currentEntry.Key];
			float currentF = currentEntryObject.GetComponent<Node>().F;
			
			if (lowestF == 0)
				lowestF = currentF;
			
			if (currentF < lowestF  lowestF != 0) {
				lowestF = currentF;
				lowestKey = (string)currentEntry.Key;
			}
		}
		return lowestKey;
	}
	
	bool CheckAgainstList(Hashtable list, Transform searchItem) {
		bool isOnList = false;
		foreach(DictionaryEntry currentEntry in list) {
			if((Transform)list[currentEntry.Key]==searchItem) {
				isOnList = true;
			}
		}
		return isOnList;
	}
	
	void RemoveFromList(Hashtable list, string key) {
		list.Remove(key);
	}
	
	void SwitchList(string key, Hashtable listToRemoveFrom, Hashtable listToAddTo) {
		listToAddTo.Add(key, listToRemoveFrom[key]);
		listToRemoveFrom.Remove(key);
	}
	
	float EuclideanHeuristic(Transform start, Transform end) {
		float distance = Mathf.Sqrt(Mathf.Pow((start.transform.position.x - end.transform.position.x),2)
			+ Mathf.Pow((start.transform.position.z - end.transform.position.z),2));
		return distance;
	}
}

Here’s where I call the function from (again, copy into code editor of choice):

public SmartMove smartMove;
	public Transform pathEndNode;
	public Transform pathStartNode;
	
	public Transform[] path;
	
	void Update () {
		if(Input.GetButtonDown("Fire1")) {
			Ray ray = Camera.main.ScreenPointToRay (Input.mousePosition);
			RaycastHit hit;
			if (Physics.Raycast (ray, out hit)) {
				pathStartNode = hit.collider.transform;
				path = new Transform[0];
			}
		}
		
		if(Input.GetButtonUp("Fire1")) {
			Ray ray = Camera.main.ScreenPointToRay (Input.mousePosition);
			RaycastHit hit;
			if (Physics.Raycast (ray, out hit)) {
				pathEndNode = hit.collider.transform;
				path = smartMove.CalculatePath(pathStartNode, pathEndNode);
			}
		}
	}

Now, when I run my program, everything works fine. The first time I calculate a path (click, drag, release) it calculates fine and I get my path. The second time, however, Unity crashes. I’ve spent so long on this script, I feel like I just need somebody else to scan it really quick and point out the error. It’s unoptimized, messy, and uncommented, so if you don’t feel like reading it that’s fine. If you do, though, you’ll have my eternal gratitude.

Thanks a lot, Havoc

Hehe, when you get it working mind posting it up here? I’m working on implementing the same path finding algorithm.

Sure. :slight_smile:

Maybe some arrays are not cleared, or some variables are not reset.

Here is mine.

public class Tile
{
	public var id:int;
	public var xvalue:int;
	public var yvalue:int;
	public var parentid:int;
	public var movementcost:int;
	public var gvalue:int;
	public var tempgvalue:int;
	public var fvalue:int;
}

//set up a 20X20 map consists of square tiles.
var mapwidth:int = 20;
var mapheight:int = 20;
var map = new Tile[mapwidth,mapheight];
var pathlist=new Array();
var openlist = new Array();
var closedlist = new Array();


function Findpath(startx:int,starty:int,endx:int,endy:int)  : boolean {
	//setup some variables. clear open list, closedlist, and pathlist. pathlist is used to store path
	var found:boolean = false; //whether destination has been found or not
	var currenttile = new Tile();
	var sorttempvalue = new Tile();  //used when sorting open list
	openlist.Clear();
	closedlist.Clear();
	pathlist.Clear();
	
	//initialize map
	var tileid:int = 1;
	for (var i:int = 0; i < mapwidth; i++)
	{
		for (var j:int = 0;j < mapheight; j++)
		{
			map[i,j] = new Tile();
			map[i,j].id = tileid;
			map[i,j].xvalue = i;
			map[i,j].yvalue = j;
			map[i,j].parentid = 0;
			map[i,j].gvalue = 0;
			map[i,j].tempgvalue = 0;
			map[i,j].fvalue = 0;
			map[i,j].movementcost = 0;
			
			if (i == 0 || j == 0 || i == mapwidth-1|| j == mapheight-1)
			{
				map[i,j].movementcost = 9999;   //value of 9999 means unpassible. This set the edge of map to be unpassible.
			}
			tileid++;
		}
	}
	
	//check movementcost of destination. if unpassible, exit function and return "false".
	if (map[endx,endy].movementcost == 9999)
	{
		return false;
	}
	else
	{
		//add the starting node to the open list
		openlist.Add(map[startx,starty]);
	}
	
	while (openlist.length > 0)
	{
		//sort open list based on f value using bubble sort
		var n:int = openlist.length;
		var swapped:boolean = true;
		if (n > 1)
		{
			while (swapped == true)
			{
				swapped = false;
				for (i = 0; i<n-1; i++)
				{
					if (openlist[i].fvalue > openlist[i+1].fvalue)
					{
						sorttempvalue = openlist[i];
						openlist[i] = openlist[i+1];
						openlist[i+1] = sorttempvalue;
						swapped = true;
					}
				}
			}
		}
		
		//current node is the one with the smallest f cost in open list, also remove that node from open list.
		currenttile = openlist.Shift();
		
		//if current node is destination, exit while loop.
		if (currenttile.xvalue == endx  currenttile.yvalue == endy)
		{
			//path complete
			found = true;	
			break;
		}
		else
		{
			//add current node to closed list
			closedlist.Add(currenttile);
			
			//examine surrounding tiles
			var currenttilex:int = currenttile.xvalue;
			var currenttiley:int = currenttile.yvalue;
			
			var westtile = new Tile();
			var northwesttile = new Tile();
			var northtile = new Tile();
			var northeasttile = new Tile();
			var easttile = new Tile();
			var southeasttile = new Tile();
			var southtile = new Tile();
			var southwesttile = new Tile();
			
			westtile = map[currenttilex-1,currenttiley];
			northwesttile = map[currenttilex-1,currenttiley+1];
			northtile = map[currenttilex,currenttiley+1];
			northeasttile = map[currenttilex+1,currenttiley+1];
			easttile = map[currenttilex+1,currenttiley];
			southeasttile = map[currenttilex+1,currenttiley-1];
			southtile = map[currenttilex,currenttiley-1];
			southwesttile = map[currenttilex-1,currenttiley-1];
			
			//assign movement cost based on position. Diagonal or not.
			westtile.tempgvalue = 10;
			northwesttile.tempgvalue = 14;
			northtile.tempgvalue = 10;
			northeasttile.tempgvalue = 14;
			easttile.tempgvalue = 10;
			southeasttile.tempgvalue = 14;
			southtile.tempgvalue = 10;
			southwesttile.tempgvalue = 14;
			
			//add surrounding tiles to an array
			var surroundingtiles = new Array(westtile,northwesttile,northtile,northeasttile,easttile,southeasttile,southtile,southwesttile);
			
			//loop throught that array
			for (var surroundingtile in surroundingtiles)
			{
				//check if tile passible
				if (surroundingtile.movementcost != 9999)
				{
					//check if tile is already in closed list
					var isinclosedlist:boolean = false;
					for (var closedlistitem in closedlist)
					{
						if (closedlistitem.id == surroundingtile.id)
						{
							isinclosedlist = true;
							break;
						}
					}					
					if (isinclosedlist == false)
					{
						//check if tile is already in open list
						var isinopenlist:boolean = false;
						for (var openlisttile in openlist)
						{
							if (openlisttile.id == surroundingtile.id)
							{
								isinopenlist = true;
								break;
							}
						}
						if (isinopenlist == true)
						{
							//if tile is in open list
							var cost:int = currenttile.gvalue + surroundingtile.tempgvalue;
							if (cost < surroundingtile.gvalue)
							{
								surroundingtile.gvalue = cost;
								surroundingtile.parentid = currenttile.id;
								surroundingtile.fvalue = surroundingtile.gvalue + Heuristic(surroundingtile.xvalue,surroundingtile.yvalue,endx,endy);
							}
						}
						//if tile is not in open list
						else if (isinopenlist == false)
						{
							surroundingtile.gvalue = currenttile.gvalue + surroundingtile.tempgvalue;
							surroundingtile.parentid = currenttile.id;
							surroundingtile.fvalue = surroundingtile.gvalue + Heuristic(surroundingtile.xvalue,surroundingtile.yvalue,endx,endy);
							openlist.Add(surroundingtile);

						}
					}
				}
			}
		}
	}
	//check if path exists
	if (found == true)
	{
		pathlist.Add(currenttile);
		//back track path using parentid
		while (pathlist[pathlist.length-1].id != map[startx,starty].id)
		{
			for (var closedlistitem1 in closedlist)
			{
				if (closedlistitem1.id == pathlist[pathlist.length-1].parentid)
				{
					pathlist.Add(closedlistitem1);
					break;
				}
			}
		}
		return true;
	}
	else
	{
		return false;
	}
}

function Heuristic(startx:int,starty:int,endx:int,endy:int):int {
	var h:int;
	//Manhattan distance
	h = 10 * (Mathf.Abs(startx - endx) + Mathf.Abs(starty - endy));
	
	//Diagonal distance
	//if (Mathf.Abs(startx-endx) >= Mathf.Abs(starty-endy))
	//{
	//	h = 10 * Mathf.Abs(startx - endx);
	//}
	//else
	//{
	//	h = 10 * Mathf.Abs(starty - endy);
	//}
	
	//Euclidean distance
	//h =  10 * Mathf.Sqrt((startx-endx) * (startx-endx)+(starty-endy) * (starty-endy));
	
	return h;
}

Ah, very neat. Thanks!

For any that want it, here’s the completed A* Pathfinding code: (Disclaimer: It’s unoptimized, uncommented, and still pretty messy, but if you want it to modify or fool around with, it’s yours)

using UnityEngine;
using System.Collections;

public class SmartMove : MonoBehaviour {
	
	public Transform[] CalculatePath(Transform startingNode, Transform destinationNode) {
		Node startNode = startingNode.GetComponent<Node>();
		Node endNode = destinationNode.GetComponent<Node>();
		
		startNode.G = 0;
		startNode.H = EuclideanHeuristic(startingNode, destinationNode);
		startNode.F = startNode.H;
		
		Hashtable openList = new Hashtable();
		Hashtable closedList = new Hashtable();
		
		openList.Add("startNode", startingNode);
		string currentNodeString;
		int currentNodeCount = 0;
		while(1 > 0) {
			
			currentNodeString = ReturnLowestF(openList);
			SwitchList(currentNodeString, openList, closedList);
			Transform currentNodeObject = (Transform)closedList[currentNodeString];
			Node currentNode = currentNodeObject.GetComponent<Node>();
			Transform[] children = currentNode.children;
			
			
			foreach(Transform child in children) {
				currentNodeCount++;
				if(child!=null) {
					Node childNode = child.GetComponent<Node>();
					if(childNode.isWalkable  !closedList.ContainsValue(child)) {
						if(!openList.ContainsValue(child)) {
							openList.Add(currentNodeCount.ToString(), child);
							childNode.G = currentNode.G+childNode.movementCost;
							childNode.H = EuclideanHeuristic(child, destinationNode);
							childNode.F = childNode.G + childNode.H;
							childNode.parent = currentNodeObject;
						}
						
						if(openList.ContainsValue(child)) {
							float tempG = currentNode.G + childNode.movementCost;
							if(tempG < currentNode.G) {
								childNode.parent = currentNodeObject;
								childNode.G = currentNode.G + childNode.movementCost;
								childNode.F = childNode.G + childNode.H;
							}
						}
					}
				}
			}
			
			if(closedList.ContainsValue(destinationNode))
				break;
			
			if (openList.Count == 0)
				break;
		}
		
		Hashtable pathNodes = new Hashtable();
		
		Node currentPathNode = endNode;
		int i = 0;
		while(1>0) {
			if (currentPathNode.parent==null)
				break;
			
			pathNodes.Add(i, currentPathNode.GetComponent<Transform>());
			currentPathNode = currentPathNode.parent.GetComponent<Node>();
			
			i++;
		}
		
		Transform[] path = new Transform[pathNodes.Count];
		pathNodes.Values.CopyTo(path, 0);
		
		foreach(DictionaryEntry currentEntry in closedList) {
			Transform currentObject = (Transform)closedList[currentEntry.Key];
			Node currentObjectNode = currentObject.GetComponent<Node>();
			currentObjectNode.parent = null;
		}
		
		foreach(DictionaryEntry currentEntry in openList) {
			Transform currentObject = (Transform)openList[currentEntry.Key];
			Node currentObjectNode = currentObject.GetComponent<Node>();
			currentObjectNode.parent = null;
		}
		
		return path;
	}
	
	string ReturnLowestF(Hashtable list) {
		float lowestF = Mathf.Pow(10,100);
		string lowestKey = "ah";
		foreach(DictionaryEntry currentEntry in list) {
			Transform currentEntryObject = (Transform)list[currentEntry.Key];
			float currentF = currentEntryObject.GetComponent<Node>().F;
			
			if (lowestF == 0)
				lowestF = currentF;
			
			if (currentF < lowestF  lowestF != 0) {
				lowestF = currentF;
				lowestKey = (string)currentEntry.Key;
			}
		}
		return lowestKey;
	}

	void RemoveFromList(Hashtable list, string key) {
		list.Remove(key);
	}
	
	void SwitchList(string key, Hashtable listToRemoveFrom, Hashtable listToAddTo) {
		listToAddTo.Add(key, listToRemoveFrom[key]);
		listToRemoveFrom.Remove(key);
	}
	
	float EuclideanHeuristic(Transform start, Transform end) {
		float distance = Mathf.Sqrt(Mathf.Pow((start.transform.position.x - end.transform.position.x),2)
			+ Mathf.Pow((start.transform.position.z - end.transform.position.z),2));
		return distance;
	}
}

The reason it was crashing is because I needed to clear the parents of the nodes within the openList and closedList – otherwise it went into an infinite loop as more parents were added, because at some point, every node would end up with a parent, and one of my loops only broke when there were no parents on the grid. :slight_smile:

Holy cow, thanks for pointing out the crashing solution.
I had a smiliar pathfinding using waypoints and i had the exact same crash which i couldnt find the reason.

Cool, glad I was able to help. :slight_smile:

That’s a little frustrating because most A* explanations don’t tell you to do this – probably because they assume you aren’t using saved variables, but recreate the list every time, which would clear the parents automatically.