public List <Node> CalculatePath(Node start, Node goal)
{
AstarStack openSet;
List <Node> result = new List <Node>();
HashSet <Node> closedSet = new HashSet <Node>();
openSet = new AstarStack(start);
while (!openSet.IsEmpty())
{
Node currentNode = openSet.Retrieve();
closedSet.Add(currentNode);
if (currentNode.NodeState != Node.State.TargetNode && currentNode.NodeState != Node.State.StartNode)
{
currentNode.NodeState = Node.State.Closed;
}
if (currentNode == goal)
{
return(PathResult(start, goal));
}
foreach (Node n in currentNode.GetNeighbors())
{
if (n.NodeState == Node.State.Obstacle || closedSet.Contains(n))
{
continue;
}
int newDistanceCost = currentNode.GCost + GetDistance(currentNode, n);
if (newDistanceCost < n.GCost || !openSet.Search(n))
{
n.GCost = newDistanceCost;
n.HCost = GetDistance(n, goal);
n.Parent = currentNode;
if (!openSet.Search(n))
{
openSet.Insert(n);
if (n.NodeState != Node.State.StartNode && n.NodeState != Node.State.TargetNode)
{
n.NodeState = Node.State.Open;
}
}
}
}
}
return(result);
}
public void Insert(Node temp)
{
if (IsEmpty())
{
Head = temp;
}
else if (temp.FCost < Head.FCost)
{
if (Next == null)
{
Next = new AstarStack(Head);
}
else
{
Next.Insert(Head);
}
Head = temp;
}
else if (temp.FCost == Head.FCost)
{
if (temp.HCost < Head.HCost)
{
if (Next == null)
{
Next = new AstarStack(Head);
}
else
{
Next.Insert(Head);
}
Head = temp;
}
else if (temp.HCost == Head.HCost)
{
if (temp.GCost < Head.GCost)
{
if (Next == null)
{
Next = new AstarStack(Head);
}
else
{
Next.Insert(Head);
}
Head = temp;
}
else
{
if (Next == null)
{
Next = new AstarStack(temp);
}
else
{
Next.Insert(temp);
}
}
}
else
{
if (Next == null)
{
Next = new AstarStack(temp);
}
else
{
Next.Insert(temp);
}
}
}
else
{
if (Next == null)
{
Next = new AstarStack(temp);
}
else
{
Next.Insert(temp);
}
}
}