/// <summary> /// Find the path between start node and goal node using AStar Algorithm /// </summary> public static ArrayList FindPath(Node start, Node goal, ref Floorbase floorbase) { AstarGame.rows = floorbase.b_ROWS; AstarGame.columns = floorbase.c_COLUMNS; #if !ASTAR_TEST_A //Calculate the path based on the final node goal.parent = start; return(CalculatePath(goal)); #endif openList = new PriorityQueue(); openList.Push(start); start.nodeTotalCost = 0.0f; start.estimatedCost = HeuristicEstimateCost(start, goal); closedList = new PriorityQueue(); Node node = null; while (openList.Length != 0) { node = openList.First(); if (node.position == goal.position) { return(CalculatePath(node)); } ArrayList neighbours = new ArrayList(); AstarGame.getNeighbours(node, neighbours, ref floorbase); #region CheckNeighbours //Get the Neighbours for (int i = 0; i < neighbours.Count; i++) { //Cost between neighbour nodes Node neighbourNode = (Node)neighbours[i]; if (!closedList.Contains(neighbourNode)) { //Cost from current node to this neighbour node float cost = HeuristicEstimateCost(node, neighbourNode); //Total Cost So Far from start to this neighbour node float totalCost = node.nodeTotalCost + cost; //Estimated cost for neighbour node to the goal float neighbourNodeEstCost = HeuristicEstimateCost(neighbourNode, goal); //Assign neighbour node properties neighbourNode.nodeTotalCost = totalCost; neighbourNode.parent = node; neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost; //Add the neighbour node to the list if not already existed in the list if (!openList.Contains(neighbourNode)) { openList.Push(neighbourNode); } } } #endregion closedList.Push(node); openList.Remove(node); } //If finished looping and cannot find the goal then return null if (node.position != goal.position) { Debug.LogError("Goal Not Found"); return(null); } //Calculate the path based on the final node return(CalculatePath(node)); #if !BLOCK_COMMEND //Start Finding the path openList = new PriorityQueue(); openList.Push(start); start.nodeTotalCost = 0.0f; start.estimatedCost = HeuristicEstimateCost(start, goal); closedList = new PriorityQueue(); while (openList.Length != 0) { node = openList.First(); if (node.position == goal.position) { return(CalculatePath(node)); } ArrayList neighbours = new ArrayList(); GridManager.instance.GetNeighbours(node, neighbours); #region CheckNeighbours //Get the Neighbours for (int i = 0; i < neighbours.Count; i++) { //Cost between neighbour nodes Node neighbourNode = (Node)neighbours[i]; if (!closedList.Contains(neighbourNode)) { //Cost from current node to this neighbour node float cost = HeuristicEstimateCost(node, neighbourNode); //Total Cost So Far from start to this neighbour node float totalCost = node.nodeTotalCost + cost; //Estimated cost for neighbour node to the goal float neighbourNodeEstCost = HeuristicEstimateCost(neighbourNode, goal); //Assign neighbour node properties neighbourNode.nodeTotalCost = totalCost; neighbourNode.parent = node; neighbourNode.estimatedCost = totalCost + neighbourNodeEstCost; //Add the neighbour node to the list if not already existed in the list if (!openList.Contains(neighbourNode)) { openList.Push(neighbourNode); } } } #endregion closedList.Push(node); openList.Remove(node); } //If finished looping and cannot find the goal then return null if (node.position != goal.position) { Debug.LogError("Goal Not Found"); return(null); } //Calculate the path based on the final node return(CalculatePath(node)); #endif }
public static void getNeighbours(Node node, ArrayList neighbors, ref Floorbase floorbase) { int row = node.tile.I; int column = node.tile.J; bool obstacle = false; //Bottom obstacle = node.tile.gameObject.GetComponent <RandomScene>().obstacle; int leftNodeRow = row - 1; int leftNodeColumn = column; AstarGame.AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors, ref floorbase); //Top leftNodeRow = row + 1; leftNodeColumn = column; AstarGame.AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors, ref floorbase); //Right leftNodeRow = row; leftNodeColumn = column + 1; AstarGame.AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors, ref floorbase); //Left leftNodeRow = row; leftNodeColumn = column - 1; AstarGame.AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors, ref floorbase); /* * Vector3 neighborPos = node.position; * int neighborIndex = GetGridIndex(neighborPos); * * int row = GetRow(neighborIndex); * int column = GetColumn(neighborIndex); * * //Bottom * int leftNodeRow = row - 1; * int leftNodeColumn = column; * AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors); * * //Top * leftNodeRow = row + 1; * leftNodeColumn = column; * AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors); * * //Right * leftNodeRow = row; * leftNodeColumn = column + 1; * AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors); * * //Left * leftNodeRow = row; * leftNodeColumn = column - 1; * AssignNeighbour(leftNodeRow, leftNodeColumn, neighbors); */ }