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