/// <summary> /// A* search algorithm /// </summary> /// <returns>The sequential list of coordinates along the path</returns> public static LinkedList<Vector3> Search(Node start, Node goal, float d, float d2, int heapSize) { bool pathFound = false; NodeHeap open = new NodeHeap(heapSize); open.Insert(start); while (open.Filled > 0) { Node current = open.Extract(); if (current == goal) { pathFound = true; break; } current.State = NodeState.Closed; // expand nodes adjacent to the current node foreach (Node adjacent in current.AdjacentNodes) { // ignore nodes marked invalid or part of the closed set, as they can't be traversed if (adjacent.State == NodeState.Invalid || adjacent.State == NodeState.Closed) continue; if (adjacent.State == NodeState.Open) { // if a node is adjacent but already on the open list, it can be checked to see if // the path through the current node is more efficient float g = CalculateMovementCost(current, adjacent, d, d2); if (g < adjacent.G) { // the cost is less, so the path through the current node is better adjacent.G = g; adjacent.F = g + adjacent.H; adjacent.Parent = current; // have to relocate this node in the heap open.HeapifyNode(adjacent); } } else { // the adjacent node is not part of the open set, and it's a valid choice, so add it adjacent.Parent = current; adjacent.G = CalculateMovementCost(current, adjacent, d, d2); adjacent.H = CalculateHeuristic(adjacent, goal); adjacent.F = adjacent.G + adjacent.H; adjacent.State = NodeState.Open; open.Insert(adjacent); } } } // the set of points to travel is found by starting with the goal // and moving backwards until the start is hit if (pathFound) { LinkedList<Vector3> path = new LinkedList<Vector3>(); while (goal != start.Parent) { goal.State = NodeState.OnPath; path.AddFirst(goal.Position); goal = goal.Parent; } return path; } return null; }
/// <summary> /// A* search algorithm /// </summary> /// <returns>The sequential list of coordinates along the path</returns> public static LinkedList <Vector3> Search(Node start, Node goal, float d, float d2, int heapSize) { bool pathFound = false; NodeHeap open = new NodeHeap(heapSize); open.Insert(start); while (open.Filled > 0) { Node current = open.Extract(); if (current == goal) { pathFound = true; break; } current.State = NodeState.Closed; // expand nodes adjacent to the current node foreach (Node adjacent in current.AdjacentNodes) { // ignore nodes marked invalid or part of the closed set, as they can't be traversed if (adjacent.State == NodeState.Invalid || adjacent.State == NodeState.Closed) { continue; } if (adjacent.State == NodeState.Open) { // if a node is adjacent but already on the open list, it can be checked to see if // the path through the current node is more efficient float g = CalculateMovementCost(current, adjacent, d, d2); if (g < adjacent.G) { // the cost is less, so the path through the current node is better adjacent.G = g; adjacent.F = g + adjacent.H; adjacent.Parent = current; // have to relocate this node in the heap open.HeapifyNode(adjacent); } } else { // the adjacent node is not part of the open set, and it's a valid choice, so add it adjacent.Parent = current; adjacent.G = CalculateMovementCost(current, adjacent, d, d2); adjacent.H = CalculateHeuristic(adjacent, goal); adjacent.F = adjacent.G + adjacent.H; adjacent.State = NodeState.Open; open.Insert(adjacent); } } } // the set of points to travel is found by starting with the goal // and moving backwards until the start is hit if (pathFound) { LinkedList <Vector3> path = new LinkedList <Vector3>(); while (goal != start.Parent) { goal.State = NodeState.OnPath; path.AddFirst(goal.Position); goal = goal.Parent; } return(path); } return(null); }