public GraphNodeComponent[] GetPath(Transform transformOrigin, Vector2 positionTarget) { GraphNodeComponent nodeOrigin = GetClosestNodeComponent(transformOrigin.position); GraphNodeComponent nodeTarget = GetClosestNodeComponent(positionTarget); return(FindShortestPath(nodeOrigin, nodeTarget)); }
GraphNodeComponent[] FindShortestPath(GraphNodeComponent nodeOrigin, GraphNodeComponent nodeTarget) { List <GraphNodeComponent> path = new List <GraphNodeComponent>(); SimplePriorityQueue <GraphNodeComponent> openNode = new SimplePriorityQueue <GraphNodeComponent>(); Dictionary <GraphNodeComponent, float> costSoFar = new Dictionary <GraphNodeComponent, float>(); Dictionary <GraphNodeComponent, GraphNodeComponent> cameFrom = new Dictionary <GraphNodeComponent, GraphNodeComponent>(); costSoFar[nodeOrigin] = 0; cameFrom[nodeOrigin] = null; openNode.Enqueue(nodeOrigin, 0); GraphNodeComponent current = null; while (openNode.Count > 0) { current = openNode.Dequeue(); if (current == nodeTarget) { break; } foreach (GameObject neighbor in current.neighbors) { GraphNodeComponent currentNeighbor = neighbor.GetComponent <GraphNodeComponent>(); float distance = Vector2.Distance(current.position.position, currentNeighbor.position.position); float newCost = costSoFar[current] + current.cost + distance; if (!costSoFar.ContainsKey(currentNeighbor) || newCost < costSoFar[currentNeighbor]) { float priority = newCost + Vector2.Distance(currentNeighbor.position.position, nodeTarget.position.position); openNode.Enqueue(currentNeighbor, priority); costSoFar[currentNeighbor] = newCost; cameFrom[currentNeighbor] = current; } } } current = nodeTarget; while (current != nodeOrigin) { path.Add(current); current = cameFrom[current]; } path.Add(nodeOrigin); path.Reverse(); return(path.ToArray()); }