private int heuristic_cost_estimate(TerrainNode node1, TerrainNode node2) { int x0 = node1.getX(); int z0 = node1.getZ(); int x1 = node2.getX(); int z1 = node2.getX(); return((int)Mathf.Abs(Mathf.Sqrt((x1 - x0) * (x1 - x0) + (z1 - z0) * (z1 - z0)))); }
//Unfinished - commented out so it compiles public List <TerrainNode> AStar(int x0, int z0, int x1, int z1) { List <TerrainNode> closedset = new List <TerrainNode>(); List <TerrainNode> openset = new List <TerrainNode>(); List <TerrainNode> camefrom = new List <TerrainNode>(); TerrainNode start = grid[x0, z0]; TerrainNode goal = grid[x1, z1]; openset.Add(start); start.setGScore(0); start.setFScore(start.getGScore() + heuristic_cost_estimate(start, goal)); //While not empty while (openset.Count != 0) { TerrainNode current = null; for (int i = 0; i < openset.Count; i++) { TerrainNode tn = openset[i]; if (current == null || tn.getFScore() < current.getFScore()) { current = tn; } } if (current == goal) { return(reconstruct_path(camefrom, goal)); } openset.Remove(current); closedset.Add(current); List <TerrainNode> neighbours = getNeighbours(current.getX(), current.getZ()); for (int i = 0; i < neighbours.Count; i++) { if (closedset.Contains(neighbours[i])) { continue; } int tenative_g_score = current.getGScore() + 1; if (!openset.Contains(neighbours[i]) || tenative_g_score < neighbours[i].getGScore()) { if (!closedset.Contains(neighbours[i])) { openset.Add(neighbours[i]); } neighbours[i].setCameFrom(current); neighbours[i].setGScore(tenative_g_score); neighbours[i].setFScore(neighbours[i].getGScore() + heuristic_cost_estimate(neighbours[i], goal)); } } } return(null); }