bool PathSearch(NavNode current, NavNode finish)
{
current.state = NodeState.Closed;
if (current.Equals(finish))
{
return(true);
}
foreach (var item in current.neighbours)
{
if (item.node.state == NodeState.Closed)
{
continue;
}
if (item.node.state == NodeState.Open)
{
if (item.distance < item.node.G) //if the new g is smaller then the old one
{
item.node.parent = current;
item.node.G = item.distance;
}
}
else
{
item.node.state = NodeState.Open;
item.node.parent = current;
item.node.G = item.distance;
}
}
current.neighbours.Sort((x, y) => x.node.F.CompareTo(y.node.F));
foreach (var node in current.neighbours.Where(x => x.node.state != NodeState.Closed))
{
if (PathSearch(node.node, finish))
{
return(true);
}
}
return(false);
}