public override void Generate() { pathfinderObject = new GameObject(GraphName); pathfinderObject.transform.parent = GraphContainer; pathfinderObject.transform.position = GraphPosition; pathfinderObject.tag = GraphTag; pathfinder = pathfinderObject.AddComponent<Pathfinder>(); pathfinder.ColorTraversable = ColorTraversable; pathfinder.ColorOutOfReach = ColorOutOfReach; pathfinder.ColorNeutral = ColorNeutral; pathfindingNodes = new PathfindingNode[GraphWidth, GraphHeight]; for (int y = 0; y < GraphHeight; y++) { for (int x = 0; x < GraphWidth; x++) { pathfinderNodeObject = GameObject.Instantiate(NodePrefab) as GameObject; pathfindingNode = pathfinderNodeObject.GetComponent<PathfindingNode>(); pathfindingNode.SetColor(pathfinder.ColorNeutral); pathfinderNodeObject.name = "(" + x + ", " + y + ") " + NodePrefab.name; pathfinderNodeObject.transform.parent = pathfinderObject.transform; pathfinderNodeObject.transform.position = new Vector3(x, y, 0); pathfindingNodes[x, y] = pathfindingNode; } } pathfinder.SetNodes(pathfindingNodes); }
void CreateGrid () { grid = new PathfindingNode[gridSizeX, gridSizeY]; Vector3 worldBottomLeft = transform.position - (Vector3.right * gridWorldSize.x/2) - (Vector3.forward * gridWorldSize.y/2); // center - left - bottom [(0,0,0)-(-1,0,0)-(0,0,-1) = (-1,0,-1)] for (int x = 0; x < gridSizeX; x++) { for (int y = 0; y < gridSizeY; y++) { //Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter /*+ nodeRadius*/) + Vector3.forward * (y * nodeDiameter /*+ nodeRadius*/); // bottome left + current x + current y, current x = x for iterator * node diameter for diatacne between+ node ra Vector3 worldPoint = worldBottomLeft + new Vector3(x * nodeDiameter /*+ nodeRadius*/ , 0, y * nodeDiameter /*+ nodeRadius*/); // allows for gridnodes to be based on unity centered coodinates <-- toggleable if (gridOnNodeCenter) { //worldPoint = worldPoint + (Vector3.one * nodeRadius); worldPoint.x += nodeRadius; worldPoint.z += nodeRadius; } // may need something here to eliminate areas outside of grid from play (past walls bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask)); grid[x,y] = new PathfindingNode(walkable, worldPoint, x, y); } } }
public static List <Node> DijkstraNodes(Node startNode, Node endNode) { float starTime = Time.realtimeSinceStartup; List <Node> waypoints = new List <Node>(); // open list, closed list List <PathfindingNode> openList = new List <PathfindingNode>(), closedList = new List <PathfindingNode>(); Dictionary <Node, PathfindingNode> pathfindingNodes = new Dictionary <Node, PathfindingNode>(); pathfindingNodes.Add(startNode, new PathfindingNode(startNode)); openList.Add(pathfindingNodes[startNode]); while (!DoesListContainNode(endNode, closedList) && openList.Count > 0) { openList.Sort(); PathfindingNode smallestCostSoFar = openList[0]; foreach (Node connectedNode in smallestCostSoFar.graphNode.connections.Keys) { if (smallestCostSoFar.graphNode.connections[connectedNode] != 0) { if (!DoesListContainNode(connectedNode, closedList)) { if (!DoesListContainNode(connectedNode, openList)) { float costToConnected = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode] + Vector3.Distance(connectedNode.transform.position, endNode.transform.position); PathfindingNode predecessor = smallestCostSoFar; pathfindingNodes.Add(connectedNode, new PathfindingNode(connectedNode, costToConnected, predecessor)); openList.Add(pathfindingNodes[connectedNode]); } else { float currentCostToTarget = pathfindingNodes[connectedNode].costSoFar; float costToTargetThroughCurrentNode = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode]; if (costToTargetThroughCurrentNode < currentCostToTarget) { pathfindingNodes[connectedNode].costSoFar = costToTargetThroughCurrentNode; pathfindingNodes[connectedNode].predecessor = smallestCostSoFar; } } } } } closedList.Add(smallestCostSoFar); openList.Remove(smallestCostSoFar); } for (PathfindingNode waypoint = pathfindingNodes[endNode]; waypoint != null; waypoint = waypoint.predecessor) { waypoints.Add(waypoint.graphNode); } waypoints.Reverse(); //Debug.Log("Finding path took: " + (Time.realtimeSinceStartup - starTime) + "\n"); return(waypoints); }
public static List <PathfindingNode> Find(PathfindingNode start, PathfindingNode end, PathfindingNode[,] map, int width, int height) { int x, y, cost = 0, step = 0; map[end.x, end.y].cost = 0; // начало поиска с точки назначения if (start.x - 1 >= 0) { if (map[start.x - 1, start.y].cost == -2) { step++; } } else { step++; } if (start.y - 1 >= 0) { if (map[start.x, start.y - 1].cost == -2) { step++; } } else { step++; } if (start.x + 1 < width) { if (map[start.x + 1, start.y].cost == -2) { step++; } } else { step++; } if (start.y + 1 < height) { if (map[start.x, start.y + 1].cost == -2) { step++; } } else { step++; } // проверка на доступность (например, юнит окружен) if (step == 4) { return(null); } else { step = 0; } while (true) // цикл поиска { for (y = 0; y < height; y++) { for (x = 0; x < width; x++) { if (map[x, y].cost == step) // находим клетку, соответствующую текущему шагу { if (x - 1 >= 0) // если не выходим за границы массива { if (map[x - 1, y].cost == -1) // если клетка еще не проверялась { cost = step + 1; // сохраняем проходимость клетки map[x - 1, y].cost = cost; // назначаем проходимость } } if (y - 1 >= 0) { if (map[x, y - 1].cost == -1) { cost = step + 1; map[x, y - 1].cost = cost; } } if (x + 1 < width) { if (map[x + 1, y].cost == -1) { cost = step + 1; map[x + 1, y].cost = cost; } } if (y + 1 < height) { if (map[x, y + 1].cost == -1) { cost = step + 1; map[x, y + 1].cost = cost; } } } } } step++; // следующий шаг/волна if (map[start.x, start.y].cost != -1) { break; // если путь найден, выходим из цикла } if (step != cost || step > width * height) { return(null); // если путь найти невозможно, возврат } } List <PathfindingNode> result = new List <PathfindingNode>(); // массив пути // начало поиска со старта x = start.x; y = start.y; step = map[x, y].cost; // определяем базовую проходимость while (x != end.x || y != end.y) // прокладка пути { if (x - 1 >= 0 && y - 1 >= 0) // если не выходим за границы массива { if (map[x - 1, y - 1].cost >= 0) // если клетка проходима { if (map[x - 1, y - 1].cost < step) // если эта проходимость меньше, базовой проходимости { step = map[x - 1, y - 1].cost; // новая базовая проходимость result.Add(map[x - 1, y - 1]); // добавляем клетку в массив пути x--; y--; continue; // переходим на следующий цикл } } } if (y - 1 >= 0 && x + 1 < width) { if (map[x + 1, y - 1].cost >= 0) { if (map[x + 1, y - 1].cost < step) { step = map[x + 1, y - 1].cost; result.Add(map[x + 1, y - 1]); x++; y--; continue; } } } if (y + 1 < height && x + 1 < width) { if (map[x + 1, y + 1].cost >= 0) { if (map[x + 1, y + 1].cost < step) { step = map[x + 1, y + 1].cost; result.Add(map[x + 1, y + 1]); x++; y++; continue; } } } if (y + 1 < height && x - 1 >= 0) { if (map[x - 1, y + 1].cost >= 0) { if (map[x - 1, y + 1].cost < step) { step = map[x - 1, y + 1].cost; result.Add(map[x - 1, y + 1]); x--; y++; continue; } } } if (x - 1 >= 0) { if (map[x - 1, y].cost >= 0) { if (map[x - 1, y].cost < step) { step = map[x - 1, y].cost; result.Add(map[x - 1, y]); x--; continue; } } } if (y - 1 >= 0) { if (map[x, y - 1].cost >= 0) { if (map[x, y - 1].cost < step) { step = map[x, y - 1].cost; result.Add(map[x, y - 1]); y--; continue; } } } if (x + 1 < width) { if (map[x + 1, y].cost >= 0) { if (map[x + 1, y].cost < step) { step = map[x + 1, y].cost; result.Add(map[x + 1, y]); x++; continue; } } } if (y + 1 < height) { if (map[x, y + 1].cost >= 0) { if (map[x, y + 1].cost < step) { step = map[x, y + 1].cost; result.Add(map[x, y + 1]); y++; continue; } } } return(null); // текущая клетка не прошла проверку, ошибка пути, возврат } return(result); // возвращаем найденный маршрут }
protected override async Task <Queue <TileRef> > Process() { // VERY similar to A*; main difference is with the neighbor tiles you look for jump nodes instead if (_startNode == null || _endNode == null) { return(null); } // If we couldn't get a nearby node that's good enough if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs)) { return(null); } var openTiles = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer()); var gScores = new Dictionary <PathfindingNode, float>(); var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>(); var closedTiles = new HashSet <PathfindingNode>(); #if DEBUG var jumpNodes = new HashSet <PathfindingNode>(); #endif PathfindingNode currentNode = null; openTiles.Add((0, _startNode)); gScores[_startNode] = 0.0f; var routeFound = false; var count = 0; while (openTiles.Count > 0) { count++; // JPS probably getting a lot fewer nodes than A* is if (count % 5 == 0 && count > 0) { await SuspendIfOutOfTime(); } (_, currentNode) = openTiles.Take(); if (currentNode.Equals(_endNode)) { routeFound = true; break; } foreach (var node in currentNode.GetNeighbors()) { var direction = PathfindingHelpers.RelativeDirection(node, currentNode); var jumpNode = GetJumpPoint(currentNode, direction, _endNode); if (jumpNode != null && !closedTiles.Contains(jumpNode)) { closedTiles.Add(jumpNode); #if DEBUG jumpNodes.Add(jumpNode); #endif // GetJumpPoint should already check if we can traverse to the node var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, jumpNode); if (tileCost == null) { throw new InvalidOperationException(); } var gScore = gScores[currentNode] + tileCost.Value; if (gScores.TryGetValue(jumpNode, out var nextValue) && gScore >= nextValue) { continue; } cameFrom[jumpNode] = currentNode; gScores[jumpNode] = gScore; // pFactor is tie-breaker where the fscore is otherwise equal. // See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties // There's other ways to do it but future consideration var fScore = gScores[jumpNode] + PathfindingHelpers.OctileDistance(_endNode, jumpNode) * (1.0f + 1.0f / 1000.0f); openTiles.Add((fScore, jumpNode)); } } } if (!routeFound) { return(null); } var route = PathfindingHelpers.ReconstructJumpPath(cameFrom, currentNode); if (route.Count == 1) { return(null); } #if DEBUG // Need to get data into an easier format to send to the relevant clients if (DebugRoute != null && route.Count > 0) { var debugJumpNodes = new HashSet <TileRef>(jumpNodes.Count); foreach (var node in jumpNodes) { debugJumpNodes.Add(node.TileRef); } var debugRoute = new SharedAiDebug.JpsRouteDebug( _pathfindingArgs.Uid, route, debugJumpNodes, DebugTime); DebugRoute.Invoke(debugRoute); } #endif return(route); }
private PathfindingNode GetJumpPoint(PathfindingNode currentNode, Direction direction, PathfindingNode endNode) { var count = 0; while (count < 1000) { count++; PathfindingNode nextNode = null; foreach (var node in currentNode.GetNeighbors()) { if (PathfindingHelpers.RelativeDirection(node, currentNode) == direction) { nextNode = node; break; } } // We'll do opposite DirectionTraversable just because of how the method's setup // Nodes should be 2-way anyway. if (nextNode == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode) == null) { return(null); } if (nextNode == endNode) { return(endNode); } // Horizontal and vertical are treated the same i.e. // They only check in their specific direction // (So Going North means you check NorthWest and NorthEast to see if we're a jump point) // Diagonals also check the cardinal directions at the same time at the same time // See https://harablog.wordpress.com/2011/09/07/jump-point-search/ for original description switch (direction) { case Direction.East: if (IsCardinalJumpPoint(direction, nextNode)) { return(nextNode); } break; case Direction.NorthEast: if (IsDiagonalJumpPoint(direction, nextNode)) { return(nextNode); } if (GetJumpPoint(nextNode, Direction.North, endNode) != null || GetJumpPoint(nextNode, Direction.East, endNode) != null) { return(nextNode); } break; case Direction.North: if (IsCardinalJumpPoint(direction, nextNode)) { return(nextNode); } break; case Direction.NorthWest: if (IsDiagonalJumpPoint(direction, nextNode)) { return(nextNode); } if (GetJumpPoint(nextNode, Direction.North, endNode) != null || GetJumpPoint(nextNode, Direction.West, endNode) != null) { return(nextNode); } break; case Direction.West: if (IsCardinalJumpPoint(direction, nextNode)) { return(nextNode); } break; case Direction.SouthWest: if (IsDiagonalJumpPoint(direction, nextNode)) { return(nextNode); } if (GetJumpPoint(nextNode, Direction.South, endNode) != null || GetJumpPoint(nextNode, Direction.West, endNode) != null) { return(nextNode); } break; case Direction.South: if (IsCardinalJumpPoint(direction, nextNode)) { return(nextNode); } break; case Direction.SouthEast: if (IsDiagonalJumpPoint(direction, nextNode)) { return(nextNode); } if (GetJumpPoint(nextNode, Direction.South, endNode) != null || GetJumpPoint(nextNode, Direction.East, endNode) != null) { return(nextNode); } break; default: throw new ArgumentOutOfRangeException(nameof(direction), direction, null); } currentNode = nextNode; } Logger.WarningS("pathfinding", "Recursion found in JPS pathfinder"); return(null); }
void GenerateLevel(int size) { size = Mathf.Clamp(size, minLevelSize, maxLevelSize); if (!gameCamera) { gameCamera = Camera.main; } if (gameCamera) { gameCamera.orthographicSize = size / 2f * 1920 / 1080 / Screen.width * Screen.height; } startPoint = Vector2.one * levelSize / 2 * -1; levelNodes = new PathfindingNode[size, size]; //create playing field for (int vert = 0; vert < size; vert++) { for (int hor = 0; hor < size; hor++) { GameObject.Instantiate(groundTilePrefab, new Vector3(hor, vert, 0), new Quaternion(), groundParentTr); levelNodes[hor, vert] = new PathfindingNode(true, hor, vert); } } // create maze for (int vert = -1; vert <= size; vert++) { // add left and right level border GameObject.Instantiate(wallTilePrefab, new Vector3(-1, vert, 0), new Quaternion(), groundParentTr); GameObject.Instantiate(wallTilePrefab, new Vector3(size, vert, 0), new Quaternion(), groundParentTr); int predefinedHole = Random.Range(0, size - 1); for (int hor = 0; hor < size; hor++) { // full bottom and top border if (vert == -1 || vert == size) { GameObject.Instantiate(wallTilePrefab, new Vector3(hor, vert, 0), new Quaternion(), groundParentTr); continue; } // every second empty if (vert % 2 == 0) { continue; } // ensure there is a hole in every wall if (predefinedHole == hor) { continue; } // generate random walls //todo implement here random destructable walls if (Random.Range(0, 100) < levelDensity) { GameObject.Instantiate(wallTilePrefab, new Vector3(hor, vert, 0), new Quaternion(), groundParentTr); levelNodes[hor, vert].walkable = false; } } } // place base & player int baseLocation = Random.Range(0, size - 1); baseGO.transform.localPosition = new Vector3(baseLocation, 0, 0); playerGO.transform.localPosition = new Vector3(baseLocation, 0, 0); // place enemies enemiesGos = new List <GameObject>(); int ver = size; // upper half of level only for (int iter = 0; iter < enemyCount; iter++) { ver--; int hor = Random.Range(0, size - 1); int startHor = hor; while (levelNodes[hor, ver].walkable == false) { // find empty space for enemy hor++; if (hor == size) { hor = 0; } if (hor == startHor) { continue; } } bool randAlgorithm = Random.Range(0, 100) < 50; // spawn enemy enemiesGos.Add(GameObject.Instantiate(randAlgorithm ? enemyPrefabs[1] : enemyPrefabs[0], new Vector3(hor, ver, 0), new Quaternion(), groundParentTr)); // start enemy pathfinding EnemyAI tempAi = enemiesGos[iter].GetComponent <EnemyAI>(); tempAi.StartPathfinding(randAlgorithm, iter, levelNodes[hor, ver], GetNode(baseGO.transform)); } // center playing field groundParentTr.position = new Vector3(startPoint.x, startPoint.y, 0); }
public static List <Vector3> Pathfind(TileGraph graph, Node fromNode, Node toNode) { List <Vector3> waypoints = new List <Vector3>(); //TODO : implement Dijkstra List <PathfindingNode> openList = new List <PathfindingNode>(); List <PathfindingNode> closedList = new List <PathfindingNode>(); Dictionary <Node, PathfindingNode> pathfindingNodes = new Dictionary <Node, PathfindingNode>(); pathfindingNodes.Add(fromNode, new PathfindingNode(fromNode)); openList.Add(pathfindingNodes[fromNode]); while (openList.Count > 0 && !DoesListContainNode(toNode, closedList)) { //TODO find connections from the lowest cost so far point to all connected points // How do I know what the lowest cost so far is? openList.Sort(); PathfindingNode smallestCostSoFar = openList[0]; // How do we get connections? foreach (Node connectedNode in smallestCostSoFar.graphNode.connections.Keys) { if (!DoesListContainNode(connectedNode, closedList)) { if (!DoesListContainNode(connectedNode, openList)) { float costToConnectedNode = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode]; PathfindingNode predecessor = smallestCostSoFar; pathfindingNodes.Add(connectedNode, new PathfindingNode(connectedNode, costToConnectedNode, predecessor)); openList.Add(pathfindingNodes[connectedNode]); } else { // Is my connection from the current processing node faster than the existing connectino to this node? // If so, update it. float currentCostToTarget = pathfindingNodes[connectedNode].costSoFar; float costToTargetThroughCurrentNode = smallestCostSoFar.costSoFar + smallestCostSoFar.graphNode.connections[connectedNode]; if (costToTargetThroughCurrentNode < currentCostToTarget) { pathfindingNodes[connectedNode].costSoFar = costToTargetThroughCurrentNode; pathfindingNodes[connectedNode].predecessor = smallestCostSoFar; } } } } closedList.Add(smallestCostSoFar); openList.Remove(smallestCostSoFar); }//end of while loop - pathfinding complete //TODO fill out waypoints //Destination node is on closed list //All of its predecessors are on the closed list for (PathfindingNode waypoint = pathfindingNodes[toNode]; waypoint != null; waypoint = waypoint.predecessor) { waypoints.Add(waypoint.graphNode.pos); Debug.Log(waypoint.graphNode.pos); } waypoints.Reverse(); return(waypoints); }
// Gets tiles adjacent to another on this map. // For now includes only the 4 tiles around it but could theoretically include portal jumps public List <PathfindingNode> GetAdjacentTiles(PathfindingNode origin, bool[,] passabilityMask, bool[,] seenBefore) { List <PathfindingNode> ret = new List <PathfindingNode>(); if (origin.x - 1 >= 0) { int newX = origin.x - 1; int newY = origin.y; if (!seenBefore[newY, newX] && passabilityMask[newY, newX]) { PathfindingNode neighbor = new PathfindingNode { x = newX, y = newY, origin = origin, cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space heuristic = 0 }; ret.Add(neighbor); } } if (origin.y - 1 >= 0) { int newX = origin.x; int newY = origin.y - 1; if (!seenBefore[newY, newX] && passabilityMask[newY, newX]) { PathfindingNode neighbor = new PathfindingNode { x = newX, y = newY, origin = origin, cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space heuristic = 0 }; ret.Add(neighbor); } } if (origin.x + 1 < map.width) { int newX = origin.x + 1; int newY = origin.y; if (!seenBefore[newY, newX] && passabilityMask[newY, newX]) { PathfindingNode neighbor = new PathfindingNode { x = newX, y = newY, origin = origin, cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space heuristic = 0 }; ret.Add(neighbor); } } if (origin.y + 1 < map.height) { int newX = origin.x; int newY = origin.y + 1; if (!seenBefore[newY, newX] && passabilityMask[newY, newX]) { PathfindingNode neighbor = new PathfindingNode { x = newX, y = newY, origin = origin, cost = origin.cost + 1, // Pathfinding cost is 1 due to the tiled nature of the space heuristic = 0 }; ret.Add(neighbor); } } return(ret); }
// Main pathfinder. Uses A*. Could get faster but due to the small size of the maps, it is not a high priority. public List <BattleMapTile> GetPath(int startX, int startY, int endX, int endY, bool[,] passabilityMask) { List <BattleMapTile> path = new List <BattleMapTile>(); // Mask to know which tiles we have already checked bool[,] seenBefore = new bool[map.height, map.width]; for (int x = 0; x < map.width; x++) { for (int y = 0; y < map.height; y++) { seenBefore[y, x] = false; } } // A* for getting a specific path // Define the base node PathfindingNode start = new PathfindingNode { x = startX, y = startY, origin = null, cost = 0, heuristic = 0 }; start.heuristic = CalculateHeuristic(start, endX, endY); // Set the start position as seen seenBefore[startY, startX] = true; // TODO: This is a grossly inefficient way of handling this. Should use a proper min heap based priority queue instead List <PathfindingNode> pool = new List <PathfindingNode>(); PathfindingNodeComparer comparer = new PathfindingNodeComparer(); pool.Add(start); PathfindingNode current = null; bool foundPath = false; // Main loop while (pool.Count > 0) { pool.Sort(comparer); current = pool[0]; pool.RemoveAt(0); List <PathfindingNode> adj = GetAdjacentTiles(current, passabilityMask, seenBefore); foreach (PathfindingNode node in adj) { // If we found the path, termiate early if (node.x == endX && node.y == endY) { foundPath = true; current = node; break; } else { node.heuristic = CalculateHeuristic(node, endX, endY); pool.Add(node); seenBefore[node.y, node.x] = true; } } if (foundPath) { break; } } if (foundPath) { while (current.origin != null) { path.Add(map.tiles[current.y][current.x]); current = current.origin; } path.Reverse(); } return(path); }
private float CalculateHeuristic(PathfindingNode node, int endX, int endY) { return(node.cost + Mathf.Abs(endX - node.x) + Mathf.Abs(endY - node.y)); }
bool SetCoverTargetFromPlayerLastKnownPosition() { Vector2 bestCoverPoint; List <PathfindingNode> potentialCoverNodes = new List <PathfindingNode>(); HashSet <Vector2> inProcessQueue = new HashSet <Vector2>(); Queue <PathfindingNode> nodesToProcess = new Queue <PathfindingNode>(); grid.Reset(); PathfindingNode node = grid.NodeAtWorldPosition(rigidbody.position); node.Parent = null; nodesToProcess.Enqueue(node); inProcessQueue.Add(node.WorldPosition); while (nodesToProcess.Count > 0 && Vector2.Distance(rigidbody.position, node.WorldPosition) < maxCoverSearchDistance) { node = nodesToProcess.Dequeue(); Quaternion left = Quaternion.AngleAxis(-7, Vector3.forward); Quaternion right = Quaternion.AngleAxis(7, Vector3.forward); RaycastHit2D leftRaycastHit = Physics2D.Linecast(playerLastKnownPosition, left * node.WorldPosition, sightBlockMask); RaycastHit2D rightRaycastHit = Physics2D.Linecast(playerLastKnownPosition, right * node.WorldPosition, sightBlockMask); if (leftRaycastHit && rightRaycastHit) { potentialCoverNodes.Add(node); } foreach (PathfindingNode neighbor in grid.GetNeighbors(node)) { if (!inProcessQueue.Contains(neighbor.WorldPosition)) { if (neighbor.IsWalkable) { neighbor.Parent = node; nodesToProcess.Enqueue(neighbor); inProcessQueue.Add(neighbor.WorldPosition); } } } } float nodeDistance = 0; float smallestNodeDistance = float.MaxValue; PathfindingNode currNode; bestCoverPoint = NullVector; foreach (PathfindingNode coverNode in potentialCoverNodes) { currNode = coverNode; nodeDistance = 0; while (currNode != null) { if (currNode.Parent != null) { nodeDistance += Vector2.Distance(currNode.WorldPosition, currNode.Parent.WorldPosition); } currNode = currNode.Parent; } if (nodeDistance < 5) { //print("NodePos: " + coverNode.WorldPosition + ", NodeDist: " + nodeDistance); } if (nodeDistance < smallestNodeDistance) { smallestNodeDistance = nodeDistance; bestCoverPoint = coverNode.WorldPosition; } } if (bestCoverPoint != NullVector) { coverDistance = smallestNodeDistance; coverTarget = bestCoverPoint; //print("BEST NodePos: " + coverTarget + ", NodeDist: " + coverDistance); Quaternion left = Quaternion.AngleAxis(-sightAngle, Vector3.forward); Quaternion right = Quaternion.AngleAxis(sightAngle, Vector3.forward); leftP = left * coverTarget; rightP = right * coverTarget; nodeP = coverTarget; return(true); } return(false); }
protected override async Task <Queue <TileRef> > Process() { if (_startNode == null || _endNode == null || Status == JobStatus.Finished) { return(null); } // If we couldn't get a nearby node that's good enough if (!PathfindingHelpers.TryEndNode(ref _endNode, _pathfindingArgs)) { return(null); } var frontier = new PriorityQueue <ValueTuple <float, PathfindingNode> >(new PathfindingComparer()); var costSoFar = new Dictionary <PathfindingNode, float>(); var cameFrom = new Dictionary <PathfindingNode, PathfindingNode>(); PathfindingNode currentNode = null; frontier.Add((0.0f, _startNode)); costSoFar[_startNode] = 0.0f; var routeFound = false; var count = 0; while (frontier.Count > 0) { // Handle whether we need to pause if we've taken too long count++; if (count % 20 == 0 && count > 0) { await SuspendIfOutOfTime(); if (_startNode == null || _endNode == null) { return(null); } } // Actual pathfinding here (_, currentNode) = frontier.Take(); if (currentNode.Equals(_endNode)) { routeFound = true; break; } foreach (var nextNode in currentNode.GetNeighbors()) { // If tile is untraversable it'll be null var tileCost = PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, nextNode); if (tileCost == null) { continue; } // So if we're going NE then that means either N or E needs to be free to actually get there var direction = PathfindingHelpers.RelativeDirection(nextNode, currentNode); if (!PathfindingHelpers.DirectionTraversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, currentNode, direction)) { continue; } // f = g + h // gScore is distance to the start node // hScore is distance to the end node var gScore = costSoFar[currentNode] + tileCost.Value; if (costSoFar.TryGetValue(nextNode, out var nextValue) && gScore >= nextValue) { continue; } cameFrom[nextNode] = currentNode; costSoFar[nextNode] = gScore; // pFactor is tie-breaker where the fscore is otherwise equal. // See http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#breaking-ties // There's other ways to do it but future consideration // The closer the fScore is to the actual distance then the better the pathfinder will be // (i.e. somewhere between 1 and infinite) // Can use hierarchical pathfinder or whatever to improve the heuristic but this is fine for now. var fScore = gScore + PathfindingHelpers.OctileDistance(_endNode, nextNode) * (1.0f + 1.0f / 1000.0f); frontier.Add((fScore, nextNode)); } } if (!routeFound) { return(null); } var route = PathfindingHelpers.ReconstructPath(cameFrom, currentNode); if (route.Count == 1) { return(null); } #if DEBUG // Need to get data into an easier format to send to the relevant clients if (DebugRoute != null && route.Count > 0) { var debugCameFrom = new Dictionary <TileRef, TileRef>(cameFrom.Count); var debugGScores = new Dictionary <TileRef, float>(costSoFar.Count); foreach (var(node, parent) in cameFrom) { debugCameFrom.Add(node.TileRef, parent.TileRef); } foreach (var(node, score) in costSoFar) { debugGScores.Add(node.TileRef, score); } var debugRoute = new SharedAiDebug.AStarRouteDebug( _pathfindingArgs.Uid, route, debugCameFrom, debugGScores, DebugTime); DebugRoute.Invoke(debugRoute); } #endif return(route); }
public override List <PathfindingNode> FindPath(LevelGenerator tempLevelNodes, PathfindingNode tempStartNode, PathfindingNode tempTargetNode) { StartPathfindingSteps(tempLevelNodes, tempStartNode, tempTargetNode); while (openSet.Count > 0) { PathfindingNode node = openSet[0]; for (int i = 1; i < openSet.Count; i++) { if (openSet[i].fCost < node.fCost || openSet[i].fCost == node.fCost) { if (openSet[i].hCost < node.hCost) { node = openSet[i]; } } } openSet.Remove(node); closedSet.Add(node); if (node == targetNode) { return(RetracePath(startNode, targetNode)); } foreach (PathfindingNode neighbour in GetNeighbours(node)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newCostToNeighbour = node.gCost + GetDistance(node, neighbour); if (newCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = node; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } } return(null); }
// this uses Dijkstra's Algorithm to get all the spaces a player can visit. public static List <PathfindingNode> Dijkstras(SpaceModel startSpace, int maxCost, bool ignoreTerrain) { List <PathfindingNode> allNodes = new List <PathfindingNode>(); //List<SpaceModel> shortestPath = new List<SpaceModel> //{ // startSpace //}; PathfindingNode currentnode = new PathfindingNode(startSpace, null, 0, 0, true, null); allNodes.Add(currentnode); bool done = false; while (!done) { foreach (SpaceModel adjacentSpace in currentnode.GetSpace().GetAdjacentSpaces()) { if (adjacentSpace != null) { PathfindingNode nextNode = adjacentSpace.GetNode(); if (nextNode != null) { // not null if (!nextNode.BeenSeen()) { // Next node hasn't been visited yet if (ignoreTerrain) { nextNode.Update(currentnode.GetCost() + 1, currentnode.GetCost() + 1, currentnode); } else { int adjacentSpaceCost = currentnode.GetCost() + adjacentSpace.GetMovementCost(); double adjacentSpacePathfindingCost = adjacentSpaceCost; if (adjacentSpace.IsHazardous()) { adjacentSpacePathfindingCost += 0.001; } nextNode.Update(adjacentSpaceCost, adjacentSpacePathfindingCost, currentnode); } } } else { // Is null, need new node int newNodeCost; double pathfindingCost; if (ignoreTerrain) { newNodeCost = currentnode.GetCost() + 1; pathfindingCost = newNodeCost; } else { newNodeCost = currentnode.GetCost() + adjacentSpace.GetMovementCost(); pathfindingCost = newNodeCost; if (adjacentSpace.IsHazardous()) { pathfindingCost = newNodeCost + 0.001; } if (adjacentSpace.Occupied()) { pathfindingCost += 100; } } if (newNodeCost <= maxCost) { PathfindingNode newNode = new PathfindingNode(adjacentSpace, currentnode, newNodeCost, pathfindingCost, false, null); allNodes.Add(newNode); adjacentSpace.SetNode(newNode); } } } } PathfindingNode lowestNode = null; foreach (PathfindingNode node in allNodes) { if (!node.BeenSeen()) { if (lowestNode == null) { lowestNode = node; } else { if (node.GetPathfindingCost() < lowestNode.GetPathfindingCost()) { lowestNode = node; } } } } if (lowestNode == null) { done = true; } else { lowestNode.Seen(); currentnode = lowestNode; } } foreach (PathfindingNode node in allNodes) { node.GetSpace().SetNode(null); } return(allNodes); }
List<Vector3> RetracePath(PathfindingNode startNode, PathfindingNode endNode) { List<PathfindingNode> path = new List<PathfindingNode>(); PathfindingNode currentNode = endNode; while (currentNode != startNode) { path.Add(currentNode); currentNode = currentNode.parent; } List<Vector3> waypoints = SimplifyPath(path); //List<Vector3> waypoints = FullPath(path); //Array.Reverse(waypoints); waypoints.Reverse(); return waypoints; //path.Reverse(); //return path; }
public abstract List <PathfindingNode> FindPath(LevelGenerator tempLevelNodes, PathfindingNode tempStartNode, PathfindingNode tempTargetNode);
public Vector3 GetWorldPos(PathfindingNode node) { return(new Vector3(node.gridX, node.gridY, 0f)); }
public void StartPathfindingSteps(LevelGenerator tempLevelNodes, PathfindingNode tempStartNode, PathfindingNode tempTargetNode) { LevelPathfindingNodes = new PathfindingNode[tempLevelNodes.LevelSize, tempLevelNodes.LevelSize]; LevelPathfindingNodes = tempLevelNodes.CopyLevelNodes(); startNode = LevelPathfindingNodes[tempStartNode.gridX, tempStartNode.gridY]; targetNode = LevelPathfindingNodes[tempTargetNode.gridX, tempTargetNode.gridY]; openSet = new List <PathfindingNode>(); closedSet = new List <PathfindingNode>(); openSet.Add(startNode); pathfindingActive = true; }
public void MarkPassible(int x, int y) { nodes[x, y] = new PathfindingNode(cellSize, new Rect(x * cellSize.x, y * cellSize.y, cellSize.x, cellSize.y)); }
public bool IsNeighboor(PathfindingNode node) { return((Math.Abs(node.X - X) + Math.Abs(node.Y - Y)) == 1); }
private bool IsDiagonalJumpPoint(Direction direction, PathfindingNode currentNode) { // If we're going diagonally need to check all cardinals. // I tried just casting direction ints and offsets to make it smaller but brain no worky. // From NorthEast we check (Closed / Open) S - SE, W - NW PathfindingNode openNeighborOne = null; PathfindingNode closedNeighborOne = null; PathfindingNode openNeighborTwo = null; PathfindingNode closedNeighborTwo = null; switch (direction) { case Direction.NorthEast: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.SouthEast: openNeighborOne = neighbor; break; case Direction.South: closedNeighborOne = neighbor; break; case Direction.NorthWest: openNeighborTwo = neighbor; break; case Direction.West: closedNeighborTwo = neighbor; break; } } break; case Direction.SouthEast: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.NorthEast: openNeighborOne = neighbor; break; case Direction.North: closedNeighborOne = neighbor; break; case Direction.SouthWest: openNeighborTwo = neighbor; break; case Direction.West: closedNeighborTwo = neighbor; break; } } break; case Direction.SouthWest: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.NorthWest: openNeighborOne = neighbor; break; case Direction.North: closedNeighborOne = neighbor; break; case Direction.SouthEast: openNeighborTwo = neighbor; break; case Direction.East: closedNeighborTwo = neighbor; break; } } break; case Direction.NorthWest: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.SouthWest: openNeighborOne = neighbor; break; case Direction.South: closedNeighborOne = neighbor; break; case Direction.NorthEast: openNeighborTwo = neighbor; break; case Direction.East: closedNeighborTwo = neighbor; break; } } break; default: throw new ArgumentOutOfRangeException(); } if ((closedNeighborOne == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborOne) == null) && openNeighborOne != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborOne) != null) { return(true); } if ((closedNeighborTwo == null || PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, closedNeighborTwo) == null) && openNeighborTwo != null && PathfindingHelpers.GetTileCost(_pathfindingArgs, currentNode, openNeighborTwo) != null) { return(true); } return(false); }
public Vector3Int To(PathfindingNode node) { return(new Vector3Int(node.X - X, node.Y - Y, 0)); }
/// <summary> /// Check to see if the node is a jump point (only works for cardinal directions) /// </summary> private bool IsCardinalJumpPoint(Direction direction, PathfindingNode currentNode) { PathfindingNode openNeighborOne = null; PathfindingNode closedNeighborOne = null; PathfindingNode openNeighborTwo = null; PathfindingNode closedNeighborTwo = null; switch (direction) { case Direction.North: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.NorthEast: openNeighborOne = neighbor; break; case Direction.East: closedNeighborOne = neighbor; break; case Direction.NorthWest: openNeighborTwo = neighbor; break; case Direction.West: closedNeighborTwo = neighbor; break; } } break; case Direction.East: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.NorthEast: openNeighborOne = neighbor; break; case Direction.North: closedNeighborOne = neighbor; break; case Direction.SouthEast: openNeighborTwo = neighbor; break; case Direction.South: closedNeighborTwo = neighbor; break; } } break; case Direction.South: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.SouthEast: openNeighborOne = neighbor; break; case Direction.East: closedNeighborOne = neighbor; break; case Direction.SouthWest: openNeighborTwo = neighbor; break; case Direction.West: closedNeighborTwo = neighbor; break; } } break; case Direction.West: foreach (var neighbor in currentNode.GetNeighbors()) { var neighborDirection = PathfindingHelpers.RelativeDirection(neighbor, currentNode); switch (neighborDirection) { case Direction.NorthWest: openNeighborOne = neighbor; break; case Direction.North: closedNeighborOne = neighbor; break; case Direction.SouthWest: openNeighborTwo = neighbor; break; case Direction.South: closedNeighborTwo = neighbor; break; } } break; default: throw new ArgumentOutOfRangeException(); } if ((closedNeighborOne == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborOne)) && openNeighborOne != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborOne)) { return(true); } if ((closedNeighborTwo == null || !PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, closedNeighborTwo)) && openNeighborTwo != null && PathfindingHelpers.Traversable(_pathfindingArgs.CollisionMask, _pathfindingArgs.Access, openNeighborTwo)) { return(true); } return(false); }
public Path NavigateTo(Vector3Int from, Vector3Int to) { if (!HasNodeAt(from) || !HasNodeAt(to)) { return(null); } List <PathfindingNode> nodes = new List <PathfindingNode>(); PathfindingNode current = new PathfindingNode(from); PathfindingNode toNode = new PathfindingNode(to); for (int x = 0; x < Width; x += 1) { for (int y = 0; y < Height; y += 1) { if (!_nodes[x, y]) { continue; } PathfindingNode node = new PathfindingNode(x, y); nodes.Add(node); node.FScore = node.Heuristic(toNode); if (node.Equals(from)) { current = node; } if (node.Equals(to)) { toNode = node; } } } current.GScore = 0; List <PathfindingNode> closedSet = new List <PathfindingNode>(); List <PathfindingNode> openSet = new List <PathfindingNode> { current }; while (openSet.Count > 0) { current = openSet.OrderBy(n => n.FScore).First(); if (current.Equals(to)) { return(current.ConstructPath()); } openSet.Remove(current); closedSet.Add(current); foreach (PathfindingNode node in nodes.Where(n => n.IsNeighboor(current))) { if (closedSet.Contains(node)) { continue; } if (!openSet.Contains(node)) { openSet.Add(node); } int tentativeGScore = current.GScore + 1; if (tentativeGScore > AStarLimit) { return(null); } if (tentativeGScore >= node.GScore) { continue; } node.GScore = tentativeGScore; node.FScore = node.GScore + node.Heuristic(toNode); if (current.From != null) { node.FScore += current.To(current.From).Equals(node.To(current)) ? 0 : 4; } node.From = current; } } return(null); }
public List <Tuple <int, int> > FindShortestPathBetween(int sx, int sy, int dx, int dy) { // Djikstras var queue = new List <Tuple <int, int> >(); var visited = new HashSet <Tuple <int, int> >(); queue.Add(new Tuple <int, int>(sx, sy)); var pathMap = new PathfindingNode[MAP_WIDTH, MAP_HEIGHT]; for (int x = 0; x < MAP_WIDTH; x++) { for (int y = 0; y < MAP_HEIGHT; y++) { pathMap[x, y] = new PathfindingNode(); pathMap[x, y].distanceToGoal = float.MaxValue; pathMap[x, y].distanceToStart = float.MaxValue; } } GD.Print("pathMap initialized"); GD.Print($"Searching between: ({sx},{sy}) -> ({dx},{dy})"); pathMap[sx, sy].distanceToStart = 0; pathMap[sx, sy].distanceToGoal = (dx - sx) * (dx - sx) + (dy - sy) * (dy - sy); while (queue.Count > 0) { Tuple <int, int> current = null; var min = float.MaxValue; foreach (var i in queue) { if (pathMap[i.Item1, i.Item2].distanceToGoal < min) { current = i; min = pathMap[i.Item1, i.Item2].distanceToGoal; } } if (current.Item1 == dx && current.Item2 == dy) { break; } queue.Remove(current); visited.Add(current); var currentPathMap = pathMap[current.Item1, current.Item2]; var left = new Tuple <int, int>(current.Item1 - 1, current.Item2); if (!visited.Contains(left) && IsWalkable(left.Item1, left.Item2)) { var leftPathMap = pathMap[left.Item1, left.Item2]; var distToNeighbor = 1; var tentativeScore = currentPathMap.distanceToStart + distToNeighbor; if (!queue.Contains(left)) { queue.Add(left); } else if (tentativeScore >= leftPathMap.distanceToStart) { continue; } leftPathMap.prevX = current.Item1; leftPathMap.prevY = current.Item2; leftPathMap.distanceToStart = tentativeScore; var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - left.Item2) * (dy - left.Item2); leftPathMap.distanceToGoal = leftPathMap.distanceToStart + distToEnd; } var right = new Tuple <int, int>(current.Item1 + 1, current.Item2); if (!visited.Contains(right) && IsWalkable(right.Item1, right.Item2)) { var rightPathMap = pathMap[right.Item1, right.Item2]; var distToNeighbor = 1; var tentativeScore = currentPathMap.distanceToStart + distToNeighbor; if (!queue.Contains(right)) { queue.Add(right); } else if (tentativeScore >= rightPathMap.distanceToStart) { continue; } rightPathMap.prevX = current.Item1; rightPathMap.prevY = current.Item2; rightPathMap.distanceToStart = tentativeScore; var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - right.Item2) * (dy - right.Item2); rightPathMap.distanceToGoal = rightPathMap.distanceToStart + distToEnd; } var up = new Tuple <int, int>(current.Item1, current.Item2 - 1); if (!visited.Contains(up) && IsWalkable(up.Item1, up.Item2)) { var upPathMap = pathMap[up.Item1, up.Item2]; var distToNeighbor = 1; var tentativeScore = currentPathMap.distanceToStart + distToNeighbor; if (!queue.Contains(up)) { queue.Add(up); } else if (tentativeScore >= upPathMap.distanceToStart) { continue; } upPathMap.prevX = current.Item1; upPathMap.prevY = current.Item2; upPathMap.distanceToStart = tentativeScore; var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - left.Item2) * (dy - left.Item2); upPathMap.distanceToGoal = upPathMap.distanceToStart + distToEnd; } var down = new Tuple <int, int>(current.Item1, current.Item2 + 1); if (!visited.Contains(down) && IsWalkable(down.Item1, down.Item2)) { var downPathMap = pathMap[down.Item1, down.Item2]; var distToNeighbor = 1; var tentativeScore = currentPathMap.distanceToStart + distToNeighbor; if (!queue.Contains(down)) { queue.Add(down); } else if (tentativeScore >= downPathMap.distanceToStart) { continue; } downPathMap.prevX = current.Item1; downPathMap.prevY = current.Item2; downPathMap.distanceToStart = tentativeScore; var distToEnd = (dx - left.Item1) * (dx - left.Item1) + (dy - down.Item2) * (dy - down.Item2); downPathMap.distanceToGoal = downPathMap.distanceToStart + distToEnd; } } // Now, starting from the destination, walk back a path var path = new List <Tuple <int, int> >(); var cursor = new Tuple <int, int>(dx, dy); path.Add(cursor); while (cursor.Item1 != sx || cursor.Item2 != sy) { var pm = pathMap[cursor.Item1, cursor.Item2]; cursor = new Tuple <int, int>(pm.prevX, pm.prevY); path.Add(cursor); } path.Reverse(); return(path); }
protected override void OnUpdate() { ActiveMap activeMap = MapManager.ActiveMap; if (activeMap == null) { return; } Dictionary <Entity, List <Hex> > Paths = new Dictionary <Entity, List <Hex> >(); //clean buffer and restart waypoint Entities.WithAll <PathWaypoint, TriggerPathfinding>().ForEach((Entity entity, ref PathWaypointIndex waypointIndex) => { DynamicBuffer <PathWaypoint> buffer = World.EntityManager.GetBuffer <PathWaypoint>(entity); buffer.Clear(); waypointIndex = new PathWaypointIndex() { Value = 0 }; }); //shortestPath Entities.ForEach((Entity entity, ref TriggerPathfinding pathSolicitude, ref HexPosition hexPosition) => { var path = new List <Hex>(); Hex startHex = hexPosition.HexCoordinates.Round(); Hex destinationHex = pathSolicitude.Destination; if (!activeMap.map.MovementMapValues.TryGetValue(destinationHex, out bool c)) { destinationHex = MapUtilities.FindClosestOpenHex((FractionalHex)destinationHex, activeMap.map, true); } else if (!c) { destinationHex = MapUtilities.FindClosestOpenHex((FractionalHex)destinationHex, activeMap.map, true); } if (!activeMap.map.MovementMapValues.TryGetValue(startHex, out bool b)) { startHex = MapUtilities.FindClosestOpenHex(hexPosition.HexCoordinates, activeMap.map, true); } else if (!b) { startHex = MapUtilities.FindClosestOpenHex(hexPosition.HexCoordinates, activeMap.map, true); } if (startHex == destinationHex) { path.Add(startHex); Paths.Add(entity, path); return; } var startNode = new PathfindingNode(startHex, 0, 0, startHex); var openList = new NativeList <PathfindingNode>(Allocator.Temp); var closedList = new NativeList <PathfindingNode>(Allocator.Temp); openList.Add(startNode); while (openList.Length > 0) { var currentNode = openList[0]; for (int i = 1; i < openList.Length; i++) { var scanNode = openList[i]; if (currentNode.fCost > scanNode.fCost || currentNode.fCost == scanNode.fCost && currentNode.hCost > scanNode.hCost) { currentNode = scanNode; } } openList.RemoveAtSwapBack(openList.IndexOf(currentNode)); closedList.Add(currentNode); //enters here when the path have been found if (currentNode.Equals(destinationHex)) { var reversePath = new List <Hex>(); //esto no incluye el inicial. while (currentNode != startNode) { reversePath.Add(currentNode.hex); currentNode = closedList[closedList.IndexOf(currentNode.parent)]; } //add starting node. reversePath.Add(currentNode.hex); reversePath.Reverse(); path.AddRange(reversePath); //Debug.Log($"Pathfinding called. start hex: {startHex} dest hex: {destinationHex}."); //for (int i = 0; i < path.Count; i++) //{ // Debug.Log($"waypoint {i}: {path[i]}"); //} var simplifiedPath = new List <Hex>(HexFunnelAlgorithm(path, true)); Paths.Add(entity, simplifiedPath); //var path0 = new List<Hex>(path.GetRange(1, path.Count - 1)); //Paths.Add(entity, path0); openList.Dispose(); closedList.Dispose(); return; } for (int i = 0; i < 6; i++) { var neightbor = currentNode.hex.Neightbor(i); if (activeMap.map.MovementMapValues.ContainsKey(neightbor)) { //Este check debe ser actualizado! if (!MapUtilities.IsTraversable(neightbor, currentNode.hex, MapUtilities.MapType.MOVEMENT) || closedList.Contains(neightbor))//!activeMap.map.MovementMapValues[neightbor] { continue; } if (!openList.Contains(neightbor)) { var neightborNode = new PathfindingNode(neightbor, currentNode.gCost + 1, neightbor.Distance(destinationHex), currentNode.hex); openList.Add(neightborNode); } else { int indexOfNeightbor = openList.IndexOf(neightbor); var neightborNode = openList[indexOfNeightbor]; int newMovementCostToNeighbor = currentNode.gCost + 1; if (newMovementCostToNeighbor < neightborNode.gCost) { openList[indexOfNeightbor] = new PathfindingNode(neightbor, newMovementCostToNeighbor, neightbor.Distance(destinationHex), currentNode.hex); } } } } } //we may end up here if the destinaation isn't recheable from the start //a posible solution is to return the path to the closest reachable hex to the destination Debug.LogError($"The pathfinding was a failure for of index:{entity.Index}. The start node is: {startHex} and is open:{activeMap.map.MovementMapValues[startHex]}. the end node is: {destinationHex} and is open:{activeMap.map.MovementMapValues[destinationHex]}"); openList.Dispose(); closedList.Dispose(); }); //clean the buffer and then add the path there Entities.WithAll <PathWaypoint>().ForEach((Entity entity, ref TriggerPathfinding pathSolicitude) => { DynamicBuffer <PathWaypoint> buffer = World.EntityManager.GetBuffer <PathWaypoint>(entity); buffer.Clear(); List <Hex> path; if (Paths.TryGetValue(entity, out path)) { foreach (Hex waypoint in path) { buffer.Add(new PathWaypoint() { Value = waypoint }); } } else { Debug.LogError("the entity doesn't have a path. this is because the pathfinding failed before"); } }); //remove path solicitude Entities.ForEach((Entity entity, ref TriggerPathfinding pathSolicitude) => { PostUpdateCommands.RemoveComponent <TriggerPathfinding>(entity); }); }
public Transform CreateAPath(Transform start, Transform end) { //Getting closest node to start transform PathfindingNode startingNode = GetClosestNode(start); //Getting closest node to the end transform PathfindingNode endingNode = GetClosestNode(end); //Open list List <PathfindingNode> openList = new List <PathfindingNode>(); foreach (PathfindingNode node in map) { //Set the g cost to infinity node.gCost = Mathf.Infinity; //set the parent to null node.parent = null; //add all the nodes to the open list openList.Add(node); } //Set the starting node g cost to 0 startingNode.gCost = 0; while (openList.Count > 0) { //Get the node with the lowest g cost inside the open list PathfindingNode currentNode = GiveMeAnImportantNode(openList); //Remove it from the open list because it is visited openList.Remove(currentNode); //Check if the node is the ending node if (currentNode == endingNode) { //create new list for the path List <Transform> path = new List <Transform>(); //If the current node is not null while (currentNode != null) { //Add the current node to the path path.Add(currentNode.nodeTransform); //Retrace by going back to the parent of the current node currentNode = currentNode.parent; } //Reverse the list path.Reverse(); //Add the target to the list path.Add(end); //Calculate the distance between the second node in the path, and the start position float distance = Vector3.Distance(path[1].position, start.position); //If the raycast returns false then remove the node from the path as it is not a viable node to be placed inside the path if (!Physics.Raycast(start.position, Vector3.Normalize(path[1].position - start.position), distance, layer)) { path.Remove(path[0]); } //Return the first one in the path return(path[0]); } //Calculate the G costs foreach (PathfindingNode v in currentNode.linkedNodes) { float alt = currentNode.gCost + (currentNode.nodeTransform.position - v.nodeTransform.position).magnitude; if (alt < v.gCost) { v.gCost = alt; v.parent = currentNode; } } } return(null); }
public List <PathfindingNode> FindTarget(Vector2 startPos, Vector2 endPos) { if (grid.GetGridObject(endPos) == null) { return(null); } PathfindingNode startNode = grid.GetGridObject(startPos); PathfindingNode endNode = grid.GetGridObject(endPos); openNodes = new List <PathfindingNode> { startNode }; closedNodes = new List <PathfindingNode>(); for (int i = 0; i < grid.GetGridWidth(); i++) { for (int j = 0; j < grid.GetGridHeight(); j++) { PathfindingNode node = grid.GetGridObject(i, j); node.gCost = int.MaxValue; node.CalculateFCost(); node.parentNode = null; } } startNode.gCost = 0; startNode.hCost = GetDistanceValue(startNode, endNode); startNode.CalculateFCost(); while (openNodes.Count > 0) { PathfindingNode currentNode = GetLowestValueNode(openNodes); if (currentNode == endNode) { return(CalculateFinalPath(endNode)); } openNodes.Remove(currentNode); closedNodes.Add(currentNode); foreach (PathfindingNode neighbor in FindNeighbor(currentNode)) { if (closedNodes.Contains(neighbor)) { continue; } if (!neighbor.isWalkable) { closedNodes.Add(neighbor); /* * for(int i = -2; i < 3; i++) * { * for(int j = -2; j < 3; j++) * { * closedNodes.Add(grid.GetGridObject(neighbor.GetX() + i, neighbor.GetY() + j)); * } * } */ continue; } int newGCost = currentNode.gCost + GetDistanceValue(currentNode, neighbor); if (newGCost < neighbor.gCost) { neighbor.parentNode = currentNode; neighbor.gCost = newGCost; neighbor.hCost = GetDistanceValue(currentNode, endNode); neighbor.CalculateFCost(); } if (!openNodes.Contains(neighbor)) { openNodes.Add(neighbor); } } } return(null); }
public List<PathfindingNode> GetNeighbors (PathfindingNode node) { List<PathfindingNode> neighbors = new List<PathfindingNode>(); for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { if (x ==0 && y == 0) { continue; } else { int checkX = node.gridX + x; int checkY = node.gridY + y; if (checkX >- 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) { neighbors.Add(grid[checkX, checkY]); } } } } return neighbors; }
public List <Point> getPath(Point start, Point goal) { List <Point> path = new List <Point>(); PriorityQueue <PathfindingNode, float> open_set = new PriorityQueue <PathfindingNode, float>(); ResetPathfindingNodeData(); PathfindingNode starting_node = this.pathfindingNodes[pointToIndex(start)]; starting_node.pos = start; starting_node.parent = null; starting_node.givenCost = 0; starting_node.finalCost = 0; starting_node.listStatus = ListStatus.ON_OPEN; open_set.push(starting_node, 0); while (!open_set.isEmpty()) { PathfindingNode curr_node = open_set.pop(); PathfindingNode parent = curr_node.parent; Node jp_node = gridNodes[pointToIndex(curr_node.pos)]; // get jump point info // Check if we've reached the goal if (curr_node.pos.Equals(goal)) { // end and return path return(reconstructPath(curr_node, start)); } // foreach direction from parent foreach (eDirections dir in getAllValidDirections(curr_node)) { PathfindingNode new_successor = null; int given_cost = 0; // goal is closer than wall distance or closer than or equal to jump point distnace if (isCardinal(dir) && goalIsInExactDirection(curr_node.pos, dir, goal) && Point.diff(curr_node.pos, goal) <= Mathf.Abs(jp_node.jpDistances[(int)dir])) { new_successor = this.pathfindingNodes[pointToIndex(goal)]; given_cost = curr_node.givenCost + Point.diff(curr_node.pos, goal); } // Goal is closer or equal in either row or column than wall or jump point distance else if (isDiagonal(dir) && goalIsInGeneralDirection(curr_node.pos, dir, goal) && (Mathf.Abs(goal.column - curr_node.pos.column) <= Mathf.Abs(jp_node.jpDistances[(int)dir]) || Mathf.Abs(goal.row - curr_node.pos.row) <= Mathf.Abs(jp_node.jpDistances[(int)dir]))) { // Create a target jump point // int minDiff = min(RowDiff(curNode, goalNode), // ColDiff(curNode, goalNode)); int min_diff = Mathf.Min(Mathf.Abs(goal.column - curr_node.pos.column), Mathf.Abs(goal.row - curr_node.pos.row)); // newSuccessor = GetNode (curNode, minDiff, direction); new_successor = getNodeDist( curr_node.pos.row, curr_node.pos.column, dir, min_diff); // givenCost = curNode->givenCost + (SQRT2 * DiffNodes(curNode, newSuccessor)); given_cost = curr_node.givenCost + (int)(SQRT_2 * Point.diff(curr_node.pos, new_successor.pos)); } else if (jp_node.jpDistances[(int)dir] > 0) { // Jump Point in this direction // newSuccessor = GetNode(curNode, direction); new_successor = getNodeDist( curr_node.pos.row, curr_node.pos.column, dir, jp_node.jpDistances[(int)dir]); // givenCost = DiffNodes(curNode, newSuccessor); given_cost = Point.diff(curr_node.pos, new_successor.pos); // if (diagonal direction) { givenCost *= SQRT2; } if (isDiagonal(dir)) { given_cost = (int)(given_cost * SQRT_2); } // givenCost += curNode->givenCost; given_cost += curr_node.givenCost; } // Traditional A* from this point if (new_successor != null) { // if (newSuccessor not on OpenList) if (new_successor.listStatus != ListStatus.ON_OPEN) { // newSuccessor->parent = curNode; new_successor.parent = curr_node; // newSuccessor->givenCost = givenCost; new_successor.givenCost = given_cost; new_successor.directionFromParent = dir; // newSuccessor->finalCost = givenCost + // CalculateHeuristic(curNode, goalNode); new_successor.finalCost = given_cost + octileHeuristic(new_successor.pos.column, new_successor.pos.row, goal.column, goal.row); new_successor.listStatus = ListStatus.ON_OPEN; // OpenList.Push(newSuccessor); open_set.push(new_successor, new_successor.finalCost); } // else if(givenCost < newSuccessor->givenCost) else if (given_cost < new_successor.givenCost) { // newSuccessor->parent = curNode; new_successor.parent = curr_node; // newSuccessor->givenCost = givenCost; new_successor.givenCost = given_cost; new_successor.directionFromParent = dir; // newSuccessor->finalCost = givenCost + // CalculateHeuristic(curNode, goalNode); new_successor.finalCost = given_cost + octileHeuristic(new_successor.pos.column, new_successor.pos.row, goal.column, goal.row); new_successor.listStatus = ListStatus.ON_OPEN; // OpenList.Update(newSuccessor); open_set.push(new_successor, new_successor.finalCost); } } } } return(path); }
public ConsideredNode(PathfindingNode nodeInfo, ConsideredNode parent, int directTravelCostToTarget) { this.Info = nodeInfo; this.DirectTravelCostToTarget = directTravelCostToTarget; this.SetParent(parent); }
public void DrawPath(int enemyNum, List <PathfindingNode> myPath, PathfindingNode startNode) { StartCoroutine(DrawPathSequental(enemyNum, myPath, startNode)); }
public int GetDistance (PathfindingNode nodeA, PathfindingNode nodeB) { int distX = Mathf.Abs(nodeA.gridX - nodeB.gridX); int distY = Mathf.Abs(nodeA.gridY - nodeB.gridY); int diagonal = 14; // make this more than double strait if you want to eliminate diagonals int strait = 10; int diagonalWeighting = diagonalWeight; if (distX > distY) { return ((diagonal * distY * diagonalWeighting) + (strait * (distX - distY))); } else { return ((diagonal * distX * diagonalWeighting) + (strait * (distY - distX))); } }
public PathfindingConnection(PathfindingNode a, PathfindingNode b, double c) { From = a; To = b; Cost = c; }