/// <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);
}
