public static List <Node> BFSWithObstacles(Node currentNode, int k, PathfindingGrid pathfindingGrid) { List <Node> nodeList = new List <Node>(); for (int i = -k; i <= k; i++) { for (int j = -k; j <= k; j++) { if (i == 0 && j == 0) { continue; } var x = currentNode.GridX + i; var z = currentNode.GridZ + j; if ((x >= 0 && x < pathfindingGrid.GridSizeX) && (z >= 0 && z < pathfindingGrid.GridSizeZ)) { nodeList.Add(pathfindingGrid.Grid[currentNode.GridX + i, currentNode.GridZ + j]); } } } //nodeList.Add(currentNode); return(nodeList); }
/* * public IEnumerable<Node> GetNeighbors(Node currentNode) * { * for (int i = -1; i <= 1; i++) * { * for (int j = -1; j <= 1; j++) * { * if (i == 0 && j == 0) continue; * * var x = currentNode.GridX + i; * var z = currentNode.GridZ + j; * * if ((x >= 0 && x < GridSizeX) && (z >= 0 && z < GridSizeZ) && (Grid[x, z].NodeType == Node.ENodeType.Walkable || Grid[x, z].NodeType == Node.ENodeType.NextToWall)) * { * yield return Grid[currentNode.GridX + i, currentNode.GridZ + j]; * } * } * } * } */ public static List <Node> BFS(Node currentNode, int k, PathfindingGrid pathfindingGrid) { List <Node> nodeList = new List <Node>(); for (int i = -k; i <= k; i++) { for (int j = -k; j <= k; j++) { if (i == 0 && j == 0) { continue; } var x = currentNode.GridX + i; var z = currentNode.GridZ + j; if ((x >= 0 && x < pathfindingGrid.GridSizeX) && (z >= 0 && z < pathfindingGrid.GridSizeZ) && (pathfindingGrid.Grid[x, z].NodeType == Node.ENodeType.Walkable || pathfindingGrid.Grid[x, z].NodeType == Node.ENodeType.NextToWall)) { nodeList.Add(pathfindingGrid.Grid[currentNode.GridX + i, currentNode.GridZ + j]); } } } //nodeList.Add(currentNode); return(nodeList); }
//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)); } }