public List <Node> CostSearch(Node startPoint, Node goalPoint)
{
if (goalPoint.Successors.Count == 0 && startPoint.Successors.Count == 0)
{
return(null);
}
costQueue = new PriorityQueue();
costQueue.Enqueue(startPoint);
costQueue[0].Ancestor = new Node("Beginner");
while (costQueue.Count != 0)
{
Node current = costQueue.Dequeue();
if (current.Equals(goalPoint))
{
Node beginning = new Node(" ");
Queue <Node> myRoute = new Queue <Node>();
List <Node> route = new List <Node>();
myRoute.Enqueue(current);
route.Add(current);
while (!beginning.Equals(startPoint))
{
beginning = myRoute.Dequeue().Ancestor;
if (beginning == null)
{
break;
}
myRoute.Enqueue(beginning);
route.Add(beginning);
}
costQueue = null;
return(route);
}
// await Task.Run(() => //Wyłączyć przyciski wczytywania/zapisywania kiedy liczona jest droga
//{
for (int i = 0; i < current.Successors.Count; i++)
{
tempCost = 0;
if (current.Successors[i].Ancestor == null)
{
costQueue.Enqueue(current.Successors[i]);
current.Successors[i].Ancestor = current;
foreach (Edge e in current.Successors[i].Ancestor.Edges)
{
if (e.LocB == current.Successors[i])
{
tempCost = e.Cost;
break;
}
}
current.Successors[i].TotalCost = current.TotalCost + tempCost;
}
else
{
Node clones = CloneNode(current.Successors[i]);
costQueue.Enqueue(clones);
if (clones.Equals(current.Ancestor) || current.Successors[i].Equals(startPoint))
{
costQueue.Remove(clones);
clones = null;
}
else
{
clones.Ancestor = current;
if (costQueue.Contains(clones))
{
foreach (Edge e in clones.Ancestor.Edges)
{
if (e.LocB == current.Successors[i])
{
tempCost = e.Cost;
break;
}
}
clones.TotalCost = current.TotalCost + tempCost;
}
}
}
costQueue.CheckPriority();
}
//});
}
return(null);
}
public List <Node> CostSearch(Node startPoint, Node goalPoint, bool useHeuristic)
{
if (startPoint.Successors.Count == 0)
{
return(null);
}
costQueue = new PriorityQueue();
closedList = new List <Node>();
costQueue.Enqueue(startPoint);
costQueue[0].Ancestor = new Node("Beginner");
while (costQueue.Count != 0)
{
Node current = costQueue.Dequeue();
closedList.Add(current);
if (current.Equals(goalPoint))
{
Node beginning = new Node(" ");
Queue <Node> myRoute = new Queue <Node>();
List <Node> route = new List <Node>();
myRoute.Enqueue(current);
route.Add(current);
while (!beginning.Equals(startPoint))
{
beginning = myRoute.Dequeue().Ancestor;
if (beginning == null)
{
break;
}
myRoute.Enqueue(beginning);
route.Add(beginning);
}
costQueue = null;
return(route);
}
for (int i = 0; i < current.Successors.Count; i++)
{
tempCost = 0;
if (!closedList.Contains(current.Successors[i]))
{
if (!costQueue.Contains(current.Successors[i]))
{
costQueue.Enqueue(current.Successors[i]);
current.Successors[i].Ancestor = current;
foreach (Edge e in current.Successors[i].Ancestor.Edges)
{
if (e.LocB == current.Successors[i])
{
tempCost = e.Cost;
break;
}
}
current.Successors[i].TotalCost = current.TotalCost + tempCost;
current.Successors[i].FullValue = current.Successors[i].TotalCost
+ Heuristic.HeuristicValue(current.Successors[i], goalPoint);
if (!useHeuristic)
{
current.Successors[i].FullValue = current.Successors[i].TotalCost;
}
}
else
{
Node potentiallyBetterNeighbor = CloneNode(current.Successors[i]);
potentiallyBetterNeighbor.Ancestor = current;
foreach (Edge e in potentiallyBetterNeighbor.Ancestor.Edges)
{
if (e.LocB == current.Successors[i])
{
tempCost = e.Cost;
break;
}
}
potentiallyBetterNeighbor.TotalCost = current.TotalCost + tempCost;
if (potentiallyBetterNeighbor.TotalCost < current.Successors[i].TotalCost)
{
current.Successors[i].Ancestor = potentiallyBetterNeighbor.Ancestor;
current.Successors[i].TotalCost = potentiallyBetterNeighbor.TotalCost;
current.Successors[i].FullValue = current.Successors[i].TotalCost
+ Heuristic.HeuristicValue(current.Successors[i], goalPoint);
if (!useHeuristic)
{
current.Successors[i].FullValue = current.Successors[i].TotalCost;
}
}
}
}
costQueue.CheckPriority();
}
}
return(null);
}