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