public static Vector3[] ReconstructPath(Dictionary <Node, Node> pathSoFar, Node pathStartNode) { List <Vector3> path = new List <Vector3>(); Node current = pathStartNode; Node next = pathSoFar[pathStartNode]; path.Add(current.WorldPosition); path.Add(next.WorldPosition); while (!current.Equals(next)) { current = next; next = pathSoFar[next]; path.Add(next.WorldPosition); } path.Remove(path[path.Count - 1]); path.Reverse(); return(path.ToArray()); }
//by now it uses a GridDebugger Class. It should have as a paramenter a IAstarSearchableSurface or something like that public static void AStarSearch(PathfindingGrid pathfindingGrid, PathfindingManager.PathRequest request, Action <PathfindingManager.PathResult> callback, bool needsSmoothing, bool usesInfluenceData) { //estos diccionarios se resetean cada vez porque estan dentro de un metodo estatico. Solo hay una instancia de el en memoria //the camefrom path can be reconstructed using the parent field in the node itself Dictionary <Node, Node> pathSoFar = new Dictionary <Node, Node>(); Dictionary <Node, float> costSoFar = new Dictionary <Node, float>(); Stopwatch sw = new Stopwatch(); if (isDebugMode) { Debug.Log("Started search at thread number " + Thread.CurrentThread.ManagedThreadId); sw.Start(); } Node source = pathfindingGrid.GetNodeFromWorldPosition(request.PathStart); Node goal = pathfindingGrid.GetNodeFromWorldPosition(request.PathEnd); if ((source.NodeType == Node.ENodeType.NonWalkable || source.NodeType == Node.ENodeType.Invisible) || (goal.NodeType == Node.ENodeType.NonWalkable || source.NodeType == Node.ENodeType.Invisible)) { if (goal.NodeType == Node.ENodeType.NonWalkable || goal.NodeType == Node.ENodeType.Invisible) { Debug.LogError("No se puede llegar hasta el nodo indicado"); callback(new PathfindingManager.PathResult(null, false, request.Callback, Thread.CurrentThread.ManagedThreadId)); return; } if (source.NodeType == Node.ENodeType.NonWalkable) { Debug.LogError("No se puede inciar un camino desde este nodo"); } } pathSoFar.Clear(); costSoFar.Clear(); var frontier = new SimplePriorityQueue <Node>(); frontier.Enqueue(source, 0); pathSoFar[source] = source; costSoFar[source] = 0; while (frontier.Count > 0) { Node currentNode = frontier.Dequeue(); if (currentNode.Equals(goal)) { if (isDebugMode) { sw.Stop(); mediciones.Add(sw.ElapsedMilliseconds); Debug.Log("Tiempo medio de pathfinding: " + GetAverageSearchTime() + " ms."); Debug.Log("Finished search at thread number " + Thread.CurrentThread.ManagedThreadId + " in " + sw.ElapsedMilliseconds + "ms."); } break; } foreach (var next in pathfindingGrid.GetNeighbors(currentNode)) { float cost = usesInfluenceData ? pathfindingGrid.CostWithInfluences(currentNode, next) : pathfindingGrid.Cost(currentNode, next); var newCost = costSoFar[currentNode] + cost; if (!costSoFar.ContainsKey(next) || newCost < costSoFar[next]) { costSoFar[next] = newCost; float priority = newCost + Heuristic(next, goal); frontier.Enqueue(next, priority); pathSoFar[next] = currentNode; } } } Vector3[] path = ReconstructPath(pathSoFar, goal); if (needsSmoothing) { Vector3[] smoothedWaypoints = pathfindingGrid.SmoothPath(path, request.AgentRadius); callback(new PathfindingManager.PathResult(smoothedWaypoints, true, request.Callback, Thread.CurrentThread.ManagedThreadId)); } else { callback(new PathfindingManager.PathResult(path, true, request.Callback, Thread.CurrentThread.ManagedThreadId)); } }