public PathNode TryGetOpenNode()
 {
     if (OpenHeap.Count > 0)
     {
         return(OpenHeap.Dequeue());
     }
     return(null);
 }
    public void Clear()
    {
        Path.Clear();
        while (OpenHeap.Count > 0)
        {
            var node = OpenHeap.RemoveAtEnd();
            node.Clear();
        }

        for (int i = 0; i < CloseList.Count; i++)
        {
            CloseList[i].Clear();
        }
        CloseList.Clear();
    }