public bool Check(NavGraph graph, int sourceNode, int targetNode)
{
graphNodes = graph.Nodes;
nodeCosts[sourceNode] = 0f;
GetAdjacentEdges(sourceNode, graphNodes, graphNodes[targetNode]);
while (edgeQueue.Count() != 0)
{
GraphEdge tempEdge = edgeQueue.Dequeue();
traversedEdges.Add(tempEdge);
if (nodeCosts[tempEdge.To] > nodeCosts[tempEdge.From] + tempEdge.GetCost())
{
nodeRoute[tempEdge.To] = tempEdge.From;
nodeCosts[tempEdge.To] = nodeCosts[tempEdge.From] + tempEdge.GetCost() + manhattanDist(graphNodes[tempEdge.To], graphNodes[tempEdge.From]);
if (tempEdge.To == targetNode)
{
targetNodeFound = true;
continue;
}
}
for (int i = 0; i < graphNodes[tempEdge.To].adjacencyList.Count; i++)
{
bool canAdd = true;
if (traversedEdges.Contains(graphNodes[tempEdge.To].adjacencyList[i]))
{
canAdd = false;
}
else if (edgeQueue.data.Contains(graphNodes[tempEdge.To].adjacencyList[i]))
{
canAdd = false;
}
if (canAdd == true)
{
float priority = manhattanDist(graphNodes[tempEdge.To], graphNodes[targetNode]); //should set the value to be based off of the manhattan distance
graphNodes[tempEdge.To].adjacencyList[i].edgeCost = priority;
nodeCosts[tempEdge.To] = nodeCosts[tempEdge.From] + tempEdge.GetCost();
edgeQueue.Enqueue(graphNodes[tempEdge.To].adjacencyList[i]);
}
}
}
if (targetNodeFound == true)
{
int currentNode = targetNode;
Path.Add(currentNode);
while (currentNode != sourceNode)
{
currentNode = nodeRoute[currentNode];
Path.Add(currentNode);
}
return(true);
}
return(false);
}
