public IList <INode> Dijkstra(MyAbstractGraph <INode, IEdge> i_Graph, INode i_StartNode, INode i_TargetNode)
{
bool isTargetFound = false;
if (!i_Graph.GetAllNodes().Contains(i_StartNode))
{
Console.WriteLine("starting node is not in the graph");
return(null);
}
PriorityQueueCopied <INode> priorityQueue = new PriorityQueueCopied <INode>(true);
Dictionary <INode, int> distances = new Dictionary <INode, int>();
Dictionary <INode, INode> parents = new Dictionary <INode, INode>();
foreach (INode node in i_Graph.GetAllNodes())
{
priorityQueue.Enqueue(int.MaxValue / 3, node);
distances[node] = int.MaxValue / 3;
parents[node] = null;
}
distances[i_StartNode] = 0;
priorityQueue.UpdatePriority(i_StartNode, 0);
while (priorityQueue.Count != 0)
{
INode minNode = priorityQueue.Dequeue();
foreach (IEdge neighbor in i_Graph.GetNeighbors(minNode))
{
if (priorityQueue.IsInQueue(neighbor.V))
{
int newDistance = distances[minNode] + neighbor.GetWeight();
if (newDistance < distances[neighbor.V])
{
{
distances[neighbor.V] = newDistance;
priorityQueue.UpdatePriority(neighbor.V, newDistance);
parents[neighbor.V] = minNode;
}
}
}
}
}
//UnityEngine.Debug.Log("distance: " + distances[i_TargetNode]);
if (distances[i_TargetNode] == int.MaxValue)
{
return(null);
}
else
{
List <INode> resPath = new List <INode>();
INode currentNode = i_TargetNode;
while (currentNode != null)
{
resPath.Insert(0, currentNode);
currentNode = parents[currentNode];
}
return(resPath);
}
}
private void visit(MyAbstractGraph <INode, IEdge> i_Graph, INode i_Root, Dictionary <INode, NodeStatus> i_VisitStatus, List <Tuple <INode, NodeStatus> > i_VisitedOrder)
{
i_VisitStatus[i_Root] = NodeStatus.Visited;
i_VisitedOrder.Add(new Tuple <INode, NodeStatus>(i_Root, NodeStatus.Visited));
foreach (IEdge edge in i_Graph.GetNeighbors(i_Root))
{
if (i_VisitStatus[edge.V] == NodeStatus.Unvisited && !edge.V.IsObstacle)
{
visit(i_Graph, edge.V, i_VisitStatus, i_VisitedOrder);
}
}
i_VisitStatus[i_Root] = NodeStatus.Done;
i_VisitedOrder.Add(new Tuple <INode, NodeStatus>(i_Root, NodeStatus.Done));
}
//returns
//1. list of tuples - (node, distance)
//2. list of the result path
public Tuple <IList <Tuple <INode, int> >, IList <INode> > AstarSearch(MyAbstractGraph <INode, IEdge> i_Graph, INode i_StartNode, INode i_TargetNode)
{
if (!i_Graph.GetAllNodes().Contains(i_StartNode))
{
Console.WriteLine("starting node is not in the graph");
return(null);
}
List <Tuple <INode, int> > distancesHistory = new List <Tuple <INode, int> >();
PriorityQueueCopied <INode> fScorePriorityQueue = new PriorityQueueCopied <INode>(true); // For node n, fScore[n] := gScore[n] + h(n).
Dictionary <INode, int> gScore = new Dictionary <INode, int>(); // For node n, gScore[n] is the cost of the cheapest path from start to n currently known.
Dictionary <INode, INode> parents = new Dictionary <INode, INode>();
foreach (INode node in i_Graph.GetAllNodes())
{
if (node.Id != i_StartNode.Id)
{
fScorePriorityQueue.Enqueue(int.MaxValue / 3, node);
gScore[node] = int.MaxValue / 3;
}
parents[node] = null;
}
gScore[i_StartNode] = 0;
fScorePriorityQueue.Enqueue(manhattanDistance(i_Graph, (MyUnityNode)i_StartNode, (MyUnityNode)i_TargetNode), i_StartNode);
distancesHistory.Add(new Tuple <INode, int>(i_StartNode, 0));
while (fScorePriorityQueue.Count != 0)
{
INode minNode = fScorePriorityQueue.Dequeue();
if (minNode.Id == i_TargetNode.Id)
{
break;
}
foreach (IEdge neighbor in i_Graph.GetNeighbors(minNode))
{
if (fScorePriorityQueue.IsInQueue(neighbor.V))// not sure if i need this line!
{
int tentative_gScore = gScore[minNode] + neighbor.GetWeight();
if (tentative_gScore < gScore[neighbor.V])
{
{
parents[neighbor.V] = minNode;
gScore[neighbor.V] = tentative_gScore;
fScorePriorityQueue.UpdatePriority(neighbor.V, tentative_gScore + manhattanDistance(i_Graph, (MyUnityNode)neighbor.V, (MyUnityNode)i_TargetNode));
if (neighbor.V.Id != i_TargetNode.Id)
{
distancesHistory.Add(new Tuple <INode, int>(neighbor.V, tentative_gScore));
}
}
}
}
}
}
//UnityEngine.Debug.Log("distance: " + distances[i_TargetNode]);
if (gScore[i_TargetNode] == int.MaxValue)
{
return(null);
}
else
{
List <INode> resPath = new List <INode>();
INode currentNode = i_TargetNode;
while (currentNode != null)
{
resPath.Insert(0, currentNode);
currentNode = parents[currentNode];
}
return(Tuple.Create <IList <Tuple <INode, int> >, IList <INode> >(distancesHistory, resPath));
}
}