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