private bool FindPath(int2 startPosition, int2 endPosition, out NativeList <int2> pathList) { int2 gridSize = new int2(59, 42); NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp); for (int x = 0; x < gridSize.x; x++) { for (int y = 0; y < gridSize.y; y++) { PathNode pathNode = new PathNode { x = x, y = y, index = CalculateIndex(x, y, gridSize.x), gCost = int.MaxValue, hCost = CalculateDistanceCost(new int2(x, y), endPosition) }; pathNode.CalculateFCost(); pathNode.isWalkable = GridManager.Instance.IsWalkable(x, y); pathNode.cameFromNodeIndex = -1; pathNodeArray[pathNode.index] = pathNode; } } NativeArray <int2> neighbourOffsetArray = new NativeArray <int2>(new int2[] { new int2(-1, 0), new int2(1, 0), new int2(0, 1), new int2(0, -1), new int2(-1, -1), new int2(-1, 1), new int2(1, -1), new int2(1, 1), }, Allocator.Temp); int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x); PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); NativeList <int> closedList = new NativeList <int>(Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray); PathNode currentNode = pathNodeArray[currentNodeIndex]; if (currentNodeIndex == endNodeIndex) { break; } for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closedList.Add(currentNodeIndex); for (int i = 0; i < neighbourOffsetArray.Length; i++) { int2 neighbourOffest = neighbourOffsetArray[i]; int2 neighbourPosition = new int2(currentNode.x + neighbourOffest.x, currentNode.y + neighbourOffest.y); if (!isInsideGrid(neighbourPosition, gridSize)) { continue; } int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x); if (closedList.Contains(neighbourNodeIndex)) { continue; } PathNode neighbourNode = pathNodeArray[neighbourNodeIndex]; if (neighbourNode.isWalkable == 1) { continue; } int2 currentNodePosition = new int2(currentNode.x, currentNode.y); int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNodeIndex = currentNodeIndex; neighbourNode.gCost = tentativeGCost; neighbourNode.CalculateFCost(); pathNodeArray[neighbourNodeIndex] = neighbourNode; if (!openList.Contains(neighbourNodeIndex)) { openList.Add(neighbourNodeIndex); } } } } PathNode endNode = pathNodeArray[endNodeIndex]; if (endNode.cameFromNodeIndex == -1) { pathNodeArray.Dispose(); openList.Dispose(); closedList.Dispose(); neighbourOffsetArray.Dispose(); Debug.Log("No Paths Available"); pathList = new NativeList <int2>(); return(false); } else { NativeList <int2> path = CalculatePath(pathNodeArray, endNode); // for (int i = 0; i < path.Length; i++) // { // Debug.Log(path[i]); // } pathList = path; pathNodeArray.Dispose(); openList.Dispose(); closedList.Dispose(); neighbourOffsetArray.Dispose(); return(true); } }
private void FindPath(int2 startPositon, int2 endPosition) { int2 gridSize = new int2(4, 4); NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp); for (int x = 0; x < gridSize.x; x++) { for (int y = 0; x < gridSize.y; y++) { PathNode pathNode = new PathNode(); pathNode.x = x; pathNode.y = y; pathNode.index = CalculateIndex(x, y, gridSize.x); pathNode.gCost = int.MaxValue; pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition); pathNode.CalculateFCost(); pathNode.isWalkable = true; pathNode.cameFromNodeIndex = -1; pathNodeArray[pathNode.index] = pathNode; } } NativeArray <int2> neighbourOffseArray = new NativeArray <int2>(new int2[] { new int2(-1, 0), // Left new int2(+1, 0), // Right new int2(0, +1), // Up new int2(0, -1), // Down new int2(-1, -1), // Left Down new int2(-1, +1), // Left Up new int2(+1, -1), // Right Down new int2(+1, +1), // Right UP }, Allocator.Temp); int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x); PathNode startNode = pathNodeArray[CalculateIndex(startPositon.x, startPositon.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); NativeList <int> closedList = new NativeList <int>(Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray); PathNode currentNode = pathNodeArray[currentNodeIndex]; if (currentNodeIndex == endNodeIndex) { // Destination reached! Congrats!! I Hope you have a good travel! break; } // Remove Current node from the OPEN LIST for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closedList.Add(currentNodeIndex); for (int i = 0; i < neighbourOffseArray.Length; i++) { int2 neighbourOffset = neighbourOffseArray[i]; int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y); if (!IsPositionInsideGrid(neighbourPosition, gridSize)) { // This neighbour is not a valid position continue; } int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x); if (closedList.Contains(neighbourNodeIndex)) { // Node Already Searched continue; } PathNode neighbourNode = pathNodeArray[neighbourNodeIndex]; if (!neighbourNode.isWalkable) { // Ops... you can´t walk here. continue; } int2 currentNodePosition = new int2(currentNode.x, currentNode.y); int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNodeIndex = currentNodeIndex; neighbourNode.gCost = tentativeGCost; neighbourNode.CalculateFCost(); pathNodeArray[neighbourNodeIndex] = neighbourNode; if (!openList.Contains(neighbourNode.index)) { openList.Add(neighbourNode.index); } } } } PathNode endNode = pathNodeArray[endNodeIndex]; if (endNode.cameFromNodeIndex == -1) { // NO WAY!! TO BAD... } else { // FOUND THE PERFECT PATCH!!! LET´S GO MF NativeList <int2> path = CalculatePath(pathNodeArray, endNode); path.Dispose(); } // DISPOSE DE TRASH // Skype this and wacth the show... pathNodeArray.Dispose(); neighbourOffseArray.Dispose(); openList.Dispose(); closedList.Dispose(); }
// finds a path between two positions in the graph public List <PathNode> FindPath(Vector2Int startPos, Vector2Int endPos) { // get the first Node (startPos) and add it to open list PathNode startNode = GetNode(startPos.x, startPos.y); PathNode endNode = GetNode(endPos.x, endPos.y); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = GetNode(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); // CYCLE: while (openList.Count > 0) { PathNode currNode = GetLowestFCostNode(openList); if (currNode == endNode) { // reached final node return(CalculatePath(endNode)); } openList.Remove(currNode); closedList.Add(currNode); foreach (PathNode neighborNode in GetNeighbors(currNode)) { if (closedList.Contains(neighborNode)) { continue; } // ignores nodes that are not walkable if (!neighborNode.isWalkable) { closedList.Add(neighborNode); continue; } int tentCost = currNode.gCost + CalculateDistanceCost(currNode, neighborNode); if (tentCost < neighborNode.gCost) { neighborNode.cameFromNode = currNode; neighborNode.gCost = tentCost; neighborNode.hCost = CalculateDistanceCost(neighborNode, endNode); neighborNode.CalculateFCost(); if (!openList.Contains(neighborNode)) { openList.Add(neighborNode); } } } } // Out of nodes in open list! return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = pathGrid.GetGridObject(startX, startY); PathNode endNode = pathGrid.GetGridObject(endX, endY); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < pathGrid.GetWidth(); x++) { for (int y = 0; y < pathGrid.GetHeight(); y++) { PathNode pathNode = pathGrid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCost(openList); if (currentNode == endNode) { //End reached return(CalculatePath(endNode)); } openList.Remove(currentNode); //current node already searched closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { Debug.DrawLine(pathGrid.GetTilePosition(currentNode.x, currentNode.y) + 0.45f * Vector3.one, pathGrid.GetTilePosition(neighbourNode.x, neighbourNode.y) + 0.45f * Vector3.one, Color.yellow, 200f, false); neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); // PROBLEM IF MULTIPLE SOLUTIONS??? } } } } // out of node in the open list return(null); }
public List <PathNode> FindNodePath(GridLocation startGridLocation, GridLocation endGridLocation) { PathNode startNode = GameManager.Instance.CurrentGameLevel.TilesByLocation[startGridLocation].PathNode; PathNode endNode = GameManager.Instance.CurrentGameLevel.TilesByLocation[endGridLocation].PathNode; if (startNode == null || endNode == null) { // Invalid Path Logger.Log("path is invalid. Could not find a node."); return(new List <PathNode>()); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); // go through all nodes. LATER: go through all nodes of a particular area, to filter out number of available nodes. // Because it seems like currently a path from tile A in accessible zone to tile B in an accessible zone could be calculated outside of the area for (int x = 0; x < AllPathNodes.Count; x++) { PathNode pathNode = AllPathNodes[x]; pathNode.gCost = 99999999; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { // Reached final node return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (KeyValuePair <ObjectDirection, PathNode> item in GetNeighbourList(currentNode)) { PathNode neighbourNode = item.Value; if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.Tile.Walkable) { closedList.Add(neighbourNode); continue; } // restrictions for Enemies if (_character is EnemyCharacter) { if (neighbourNode.Tile.TryGetPlayerOnly()) { closedList.Add(neighbourNode); continue; } //restrictions for walking on bridges in the right direction if (!ValidateForBridge(currentNode, neighbourNode, item.Key)) { continue; } } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList return(new List <PathNode>()); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); // if (startNode == null || endNode == null) { // // Invalid Path // return null; // } openList = new List <PathNode> { startNode }; closeList = new List <PathNode>(); for (var x = 0; x < grid.GetWidth(); x++) { for (var y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { //reached final node return(ClaculatePath(endNode)); } openList.Remove(currentNode); // if current node isnt end node remove from list closeList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closeList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closeList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) // cycle through different option when ever neighbour node become comes current node due to its gcost being lower than the current node { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); if (endNode == default) { return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); GenerateGrid(); startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode.Equals(endNode)) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (var neighbourNode in GetNeighboursList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tentaiveGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentaiveGCost < neighbourNode.gCost) { neighbourNode.previousNode = currentNode; neighbourNode.gCost = tentaiveGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList return(null); }
//Execute public void Execute() { for (int i = 0; i < pathNodeArray.Length; i++) { PathNode pathNode = pathNodeArray[i]; pathNode.hCost = CalculateDistanceCost(new int2(pathNode.x, pathNode.y), endPosition); pathNode.cameFromNodeIndex = -1; pathNodeArray[i] = pathNode; } //Neighbour positions NativeArray <int2> neighbourOffsetArray = new NativeArray <int2>(8, Allocator.Temp); neighbourOffsetArray[0] = new int2(-1, 0); //Left neighbourOffsetArray[1] = new int2(+1, 0); //Right neighbourOffsetArray[2] = new int2(0, +1); //Up neighbourOffsetArray[3] = new int2(0, -1); //Down neighbourOffsetArray[4] = new int2(-1, -1); //Left Down neighbourOffsetArray[5] = new int2(-1, +1); //Left Up neighbourOffsetArray[6] = new int2(+1, -1); //Right Down neighbourOffsetArray[7] = new int2(+1, +1); //Right up int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x); PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); NativeList <int> closedList = new NativeList <int>(Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { int currentNodeIndex = GetLowestFCostNodeIndex(openList, pathNodeArray); PathNode currentNode = pathNodeArray[currentNodeIndex]; //Has the destination been reached? if (currentNodeIndex == endNodeIndex) { break; } //Remove node from open list for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closedList.Add(currentNodeIndex); //Cycle through neighbours for (int i = 0; i < neighbourOffsetArray.Length; i++) { int2 neighbourOffset = neighbourOffsetArray[i]; int2 neighbourPos = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y); if (!IsPositionInsideGrid(neighbourPos, gridSize)) { //Neighbour not in valid pos continue; } int neighbourNodeIndex = CalculateIndex(neighbourPos.x, neighbourPos.y, gridSize.x); if (closedList.Contains(neighbourNodeIndex)) { //Already searched this node continue; } PathNode neighbourNode = pathNodeArray[neighbourNodeIndex]; if (!neighbourNode.isWalkable) { //Not walkable continue; } int2 currentNodePosition = new int2(currentNode.x, currentNode.y); int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPos); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNodeIndex = currentNodeIndex; neighbourNode.gCost = tentativeGCost; neighbourNode.CalculateFCost(); pathNodeArray[neighbourNodeIndex] = neighbourNode; if (!openList.Contains(neighbourNode.index)) { openList.Add(neighbourNode.index); } } } } //Array cleanup openList.Dispose(); closedList.Dispose(); neighbourOffsetArray.Dispose(); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetValue(startX, startY); PathNode endNode = grid.GetValue(endX, endY); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.width; x++) { for (int y = 0; y < grid.height; y++) { PathNode pathNode = grid.GetValue(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.previousNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.previousNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } //Out of node in the openList return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = Grid.GetGridObjet(startX, startY); PathNode endNode = Grid.GetGridObjet(endX, endY); if (isInvalidePath(startNode, endNode)) { // Invalid Path return(null); } _openList = new List <PathNode> { startNode }; _closedList = new List <PathNode>(); for (int x = 0; x < Grid.GetWidth(); x++) { for (int y = 0; y < Grid.GetHeight(); y++) { PathNode pathNode = Grid.GetGridObjet(x, y); pathNode.GCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.CameFromeNode = null; } } startNode.GCost = 0; startNode.HCost = CalculateDistance(startNode, endNode); startNode.CalculateFCost(); while (_openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(_openList); if (currentNode == endNode) { // Reached finalnode //PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList); //PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(grid, CalculatePath(endNode)); return(CalculatePath(endNode)); } _openList.Remove(currentNode); _closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (_closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { _closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.GCost + CalculateDistance(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.GCost) { neighbourNode.CameFromeNode = currentNode; neighbourNode.GCost = tentativeGCost; neighbourNode.HCost = CalculateDistance(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!_openList.Contains(neighbourNode)) { _openList.Add(neighbourNode); } } } } return(null); }
public int FindShortestDistance(Point startPos, Point endPos) { var distanceCounter = 0; for (int i = 0; i < mapArray.GetLength(0); i++) { for (int j = 0; j < mapArray.GetLength(1); j++) { PathNode pathNode = new PathNode(j, i); pathNode.GCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.CameFrom = null; if (mapArray[i, j].MapObjectType == MapObjectType.Block) { pathNode.IsBlock = true; } mapArray[i, j].Node = pathNode; } } PathNode startNode = GetNode(startPos.X, startPos.Y); PathNode endNode = GetNode(endPos.X, endPos.Y); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); startNode.GCost = 0; startNode.HCost = CalculateDistance(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { return(CalculatePath(currentNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (var neighbour in GetNeighbours(currentNode)) { if (closedList.Contains(neighbour)) { continue; } if (neighbour.IsBlock) { closedList.Add(neighbour); continue; } int tempGCost = currentNode.GCost + CalculateDistance(currentNode, neighbour); if (tempGCost < neighbour.GCost) { neighbour.CameFrom = currentNode; neighbour.GCost = tempGCost; neighbour.HCost = CalculateDistance(neighbour, endNode); neighbour.CalculateFCost(); if (!openList.Contains(neighbour)) { openList.Add(neighbour); } } } distanceCounter++; } return(0); }
//Main function to calculate path public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { //Validating start position if (startX < 0 || startX >= grid.GetWidth() || startY < 0 || startY >= grid.GetHeight()) { return(null); } //Validating end position if (endX < 0 || endX >= grid.GetWidth() || endY < 0 || endY >= grid.GetHeight()) { return(null); } PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.previousNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); //Main algorithm while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); //Check every neighbour for path foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.CheckWalkable(walkableCheckRadius)) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.previousNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } //No path was found, return null return(null); }
public void Execute() { for (int i = 0; i < pathNodeArray.Length; i++) { PathNode pathNode = pathNodeArray[i]; pathNode.hCost = CalculateHCost(new int2(pathNode.x, pathNode.y), endPosition); pathNode.cameFromNodeIndex = -1; } NativeArray <int2> neighborOffsetArray = new NativeArray <int2>(8, Allocator.Temp); neighborOffsetArray[0] = new int2(-1, 0); // left neighborOffsetArray[1] = new int2(1, 0); //right neighborOffsetArray[2] = new int2(0, 1); // up neighborOffsetArray[3] = new int2(0, -1); //down neighborOffsetArray[4] = new int2(-1, 1); // left up neighborOffsetArray[5] = new int2(-1, -1); // left down neighborOffsetArray[6] = new int2(1, 1); // right up neighborOffsetArray[7] = new int2(1, -1); // right down NativeArray <bool> areStraightNeighborsWalkableArray = new NativeArray <bool>(4, Allocator.Temp); int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x); PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); //NativeList<int> closedList = new NativeList<int>(Allocator.Temp); // 4 instances of closed list including this one ^ NativeArray <bool> checkedArray = new NativeArray <bool>(gridSize.x * gridSize.y, Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { int currentNodeIndex = GetLowestFCostNodeIndex(openList, pathNodeArray); PathNode currentNode = pathNodeArray[currentNodeIndex]; if (currentNode.index == endNodeIndex) { // YIPEE we reached our destination break; } // remove current node from open list for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } //closedList.Add(currentNodeIndex); checkedArray[currentNodeIndex] = true; for (int i = 0; i < neighborOffsetArray.Length; i++) { int2 neighborOffset = neighborOffsetArray[i]; int2 neighborPosition = new int2(currentNode.x + neighborOffset.x, currentNode.y + neighborOffset.y); // if neighbor is a diagonal, check to make sure appropriate straight cells are walkable if (i >= 4) { if (neighborOffset.x == -1) { if (areStraightNeighborsWalkableArray[0] == false) { continue; } } else { if (areStraightNeighborsWalkableArray[1] == false) { continue; } } if (neighborOffset.y == -1) { if (areStraightNeighborsWalkableArray[3] == false) { continue; } } else { if (areStraightNeighborsWalkableArray[2] == false) { continue; } } } if (IsPositionInsideGrid(neighborPosition, gridSize) == false) { // neighbor not valid position continue; } int neighborNodeIndex = CalculateIndex(neighborPosition.x, neighborPosition.y, gridSize.x); //if (closedList.Contains(neighborNodeIndex)) //{ // // already searched this node // continue; //} if (checkedArray[neighborNodeIndex] == true) { // already searched this node continue; } PathNode neighborNode = pathNodeArray[neighborNodeIndex]; if (neighborNode.isWalkable == false) { // not walkable if (i < 4) { areStraightNeighborsWalkableArray[i] = false; } continue; } else { // cache the fact that this straight neighbor is walkable if (i < 4) { areStraightNeighborsWalkableArray[i] = true; } } int2 currentNodePosition = new int2(currentNode.x, currentNode.y); // tentativeG is current g plus distance cost to get to neighbor node int tentativeGCost = currentNode.gCost + CalculateHCost(currentNodePosition, neighborPosition); if (tentativeGCost < neighborNode.gCost) { neighborNode.cameFromNodeIndex = currentNodeIndex; neighborNode.gCost = tentativeGCost; neighborNode.CalculateFCost(); pathNodeArray[neighborNodeIndex] = neighborNode; if (openList.Contains(neighborNode.index) == false) { openList.Add(neighborNode.index); } } } } pathPositionBufferFromEntity[entity].Clear(); PathNode endNode = pathNodeArray[endNodeIndex]; if (endNode.cameFromNodeIndex == -1) { // Did not find path Debug.Log("Failed to find path"); pathFollowComponentDataFromEntity[entity] = new PathFollowComp { pathIndex = -1 }; } else { // Found path! Set pathIndex to buffer length - 1 because we read path from end to start CalculatePathVoid(pathNodeArray, endNode, pathPositionBufferFromEntity[entity]); pathFollowComponentDataFromEntity[entity] = new PathFollowComp { pathIndex = pathPositionBufferFromEntity[entity].Length - 1 }; } areStraightNeighborsWalkableArray.Dispose(); neighborOffsetArray.Dispose(); openList.Dispose(); //closedList.Dispose(); checkedArray.Dispose(); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFrom = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode.x == endNode.x && currentNode.y == endNode.y) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbour in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbour)) { continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbour); if (tentativeGCost < neighbour.gCost) { neighbour.cameFrom = currentNode; neighbour.gCost = tentativeGCost; neighbour.hCost = CalculateDistanceCost(neighbour, endNode); neighbour.CalculateFCost(); } if (!openList.Contains(neighbour)) { openList.Add(neighbour); } } } //out of nodes return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY, bool reverse = true) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); if (startNode == null || endNode == null) { // Invalid Path return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = 99999999; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); if (PathfindingDebugStepVisual.Instance != null) { PathfindingDebugStepVisual.Instance.ClearSnapshots(); PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, startNode, openList, closedList); } while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { // Reached final node if (PathfindingDebugStepVisual.Instance != null) { PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList); PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(grid, CalculatePath(endNode, reverse)); } return(CalculatePath(endNode, reverse)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } if (PathfindingDebugStepVisual.Instance != null) { PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList); } } } // Out of nodes on the openList return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); openList = new List <PathNode>() { startNode }; closeList = new List <PathNode>(); // Initialize Grid for (int x = 0; x < grid.Width; x++) { for (int y = 0; y < grid.Height; y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.PriorNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode.GetPosition(), endNode.GetPosition()); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closeList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closeList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closeList.Add(neighbourNode); continue; } //neighbourNode.gCost = CalculateDistanceCost(currentNode.GetPosition(), neighbourNode.GetPosition()); int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode.GetPosition(), neighbourNode.GetPosition()); if (neighbourNode.gCost > tentativeGCost) { neighbourNode.PriorNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode.GetPosition(), endNode.GetPosition()); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList return(null); }
public void Execute() { PrepareNodes(); int startPositionIndex = CalculateIndex(StartPosition); PathNode startPathNode = Grid[startPositionIndex]; int endPositionIndex = CalculateIndex(EndPosition); startPathNode.GCost = 0; startPathNode.CalculateFCost(); Grid[startPositionIndex] = startPathNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); NativeList <int> closedList = new NativeList <int>(Allocator.Temp); openList.Add(startPositionIndex); while (openList.Length > 0) { int currentNodeIndex = GetLowestCostFNodeIndex(openList); PathNode currentNode = Grid[currentNodeIndex]; if (currentNodeIndex == endPositionIndex) { break; } for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closedList.Add(currentNodeIndex); for (int i = 0; i < NeighboursOffset.Length; i++) { int2 neighbourOffset = NeighboursOffset[i]; int2 neighbourPosition = new int2(currentNode.Position.x + neighbourOffset.x, currentNode.Position.y + neighbourOffset.y); if (!IsPositionInsideGrid(neighbourPosition)) { continue; } int neighbourNodeIndex = CalculateIndex(neighbourPosition); if (closedList.Contains(neighbourNodeIndex)) { continue; } PathNode neighbourNode = Grid[neighbourNodeIndex]; if (!neighbourNode.IsWalkable) { continue; } int tentativeGCost = currentNode.GCost + CalculateDistanceCost(currentNode.Position, neighbourPosition); if (tentativeGCost < neighbourNode.GCost) { neighbourNode.CameFromNodeIndex = currentNodeIndex; neighbourNode.GCost = tentativeGCost; neighbourNode.CalculateFCost(); Grid[neighbourNodeIndex] = neighbourNode; if (!openList.Contains(neighbourNode.Index)) { openList.Add(neighbourNode.Index); } } } } PathNode endNode = Grid[endPositionIndex]; if (endNode.CameFromNodeIndex == -1) { PathResult[0] = (int)PathQueryResult.Invalid; } else { ResultPath = CalculatePath(endNode); PathResult[0] = (int)PathQueryResult.Complete; } openList.Dispose(); closedList.Dispose(); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY, bool forced = false) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); var possibleFinalNodes = GetNeighbourList(endNode); if (possibleFinalNodes == null || possibleFinalNodes.Count == 0) { return(null); } for (int i = 0; i < possibleFinalNodes.Count; i++) { if (!possibleFinalNodes[i].isWalkable) { possibleFinalNodes.Remove(possibleFinalNodes[i]); } } if (possibleFinalNodes.Count == 0) { return(null); } DebugDrawer.DeleteCreateWorldText(); if (startNode == null || endNode == null) { // Invalid Path return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = 99999999; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode.Equals(endNode) || possibleFinalNodes.Contains(currentNode)) { // Reached final node return(CalculatePath(currentNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } /* Code for optimizing pathfinding with multiblocks; * if (neighbourNode.structure is IMultipleNodesStructure) //Adding all other occupied nodes from the multiblock to the closedlist; * { * List<PathNode> disposableNodes = (neighbourNode.structure as IMultipleNodesStructure).getPathNodeList(); * foreach (var node in disposableNodes) * { * closedList.Add(node); * } * } */ if (!neighbourNode.isWalkable && !forced) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); DebugDrawer.CreateWorldText(neighbourNode.hCost.ToString(), null, grid.GetWorldPosition(neighbourNode.x, neighbourNode.y) - Vector3.one * 0.2f + new Vector3(grid.GetCellSize(), grid.GetCellSize()) * .5f, 20, neighbourNode.isWalkable ? Color.white : Color.red, TextAnchor.MiddleCenter); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } //PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList); } } // Out of nodes on the openList return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { //Debug.Log("FindPath: start" + startX + "," + startY + " end" + endX + "," + endY); PathNode startNode = GetNode(startX, startY); PathNode endNode = GetNode(endX, endY); if (startNode == null || endNode == null) { // Invalid Path Debug.LogWarning("Invalid Path!"); return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); // Search while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if ((currentNode.Vector2Int() - endNode.Vector2Int()).magnitude < 15) { // Reached final node return(CalculatePath(currentNode)); //not endnode because not exact hit } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } /*if (!neighbourNode.isWalkable) { * closedList.Add(neighbourNode); * continue; * }*/ float tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode) + InclineCost(currentNode, neighbourNode) + WaterCost(neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList Debug.Log("Failed to find path to " + endNode); return(null); }
// Calculate and return path from startNode to endNode public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = nodeGrid.GetGridObject(startX, startY); PathNode endNode = nodeGrid.GetGridObject(endX, endY); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); // Set all node to make gCost for (int x = 0; x < nodeGrid.Width; x++) { for (int y = 0; y < nodeGrid.Height; y++) { PathNode pathNode = nodeGrid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.prevNode = null; } } // Calculate cost for each nodes startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); // Search loop while (openList.Count > 0) { PathNode currentNode = getLowestCostNode(openList); // Reached final node if (currentNode == endNode) { return(CalculatePath(endNode)); } // Assigned current node to "searched" // Remove current node from openList and add to closedList openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { // Continue if node is in closed list or not walkable if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } // Continue if node adjacent to current and neighbor node is not walkable if (!AvoidWall(currentNode, neighbourNode)) { continue; } // Calculate cost of current neighbour node int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.prevNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); // Add to open list if not on open list if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes, cannot find path return(null); }
private void FindPath(int2 startPos, int2 endPos) { var gridSize = new int2(20, 20); var pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp); for (int i = 0; i < gridSize.x; i++) { for (int j = 0; j < gridSize.y; j++) { var pathNode = new PathNode { x = i, y = j, index = CalculateIndex(i, j, gridSize.x), gCost = int.MaxValue, hCost = CalculateDistanceCost(new int2(i, j), endPos), isWalkable = true, cameFromNodeIndex = -1, }; pathNode.CalculateFCost(); pathNodeArray[pathNode.index] = pathNode; } } var neighbourOffsetArray = new NativeArray <int2>(new int2[] { new int2(-1, 0), new int2(1, 0), new int2(0, 1), new int2(0, -1), new int2(-1, -1), new int2(-1, 1), new int2(1, -1), new int2(1, 1), }, Allocator.Temp); var endPosIndex = CalculateIndex(endPos.x, endPos.y, gridSize.x); var startNode = pathNodeArray[CalculateIndex(startPos.x, startPos.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; var openList = new NativeList <int>(Allocator.Temp); var closeList = new NativeList <int>(Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { var currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray); var currentNode = pathNodeArray[currentNodeIndex]; if (currentNodeIndex == endPosIndex) { // reach end point break; } for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closeList.Add(currentNodeIndex); for (int i = 0; i < neighbourOffsetArray.Length; i++) { var neighbourOffset = neighbourOffsetArray[i]; var neighbourNodeIndex = CalculateIndex(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y, gridSize.x); if (neighbourNodeIndex >= pathNodeArray.Length || neighbourNodeIndex < 0) { continue; } if (closeList.Contains(neighbourNodeIndex)) { continue; } var neighbourNode = pathNodeArray[ CalculateIndex(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y, gridSize.x)]; if (!neighbourNode.isWalkable) { closeList.Add(neighbourNodeIndex); continue; } var currentNodePos = new int2(currentNode.x, currentNode.y); var neighbourNodePos = new int2(neighbourNode.x, neighbourNode.y); var tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePos, neighbourNodePos); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNodeIndex = currentNodeIndex; neighbourNode.gCost = tentativeGCost; neighbourNode.CalculateFCost(); pathNodeArray[neighbourNodeIndex] = neighbourNode; if (!openList.Contains(neighbourNode.index)) { openList.Add(neighbourNode.index); } } } } var endNode = pathNodeArray[endPosIndex]; if (endNode.cameFromNodeIndex == -1) { //didn't find the path Debug.Log("Did't find a path"); } else { //find a path var path = CalculatePath(pathNodeArray, endNode); foreach (var p in path) { Debug.Log(p); } path.Dispose(); } pathNodeArray.Dispose(); openList.Dispose(); closeList.Dispose(); neighbourOffsetArray.Dispose(); }
private List <PathNode> FindPath(int startX, int startY, int endX, int endY, int xCount, int yCount) { allPathNodes.Clear(); for (int i = 0; i < xCount; i++) { for (int j = 0; j < yCount; j++) { allPathNodes.Add(new PathNode(i, j, RefHolder.instance.worldGen.GetGridGroundObject(i, j).GetComponent <GroundScript>().isEmpty)); } } PathNode startNode = GetNode(startX, startY); PathNode endNode = GetNode(endX, endY); if (startNode == null || endNode == null) { // Invalid Path return(null); } List <PathNode> openList = new List <PathNode>(); List <PathNode> closedList = new List <PathNode>(); openList.Add(startNode); for (int x = 0; x < xCount; x++) { for (int y = 0; y < yCount; y++) { PathNode pathNode = GetNode(x, y); pathNode.gCost = 99999999; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { // Reached final node return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList return(null); }
private List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth; x++) { for (int y = 0; y < grid.GetHeight; y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDis(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowfCostNode(openList); if (currentNode == endNode) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tempGCost = currentNode.gCost + CalculateDis(currentNode, neighbourNode); if (tempGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tempGCost; neighbourNode.hCost = CalculateDis(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } return(null); }
public List <PathNode> FindPath(int startX, int startZ, int endX, int endZ) { PathNode startNode = GetNode(startX, startZ); PathNode endNode = GetNode(endX, endZ); openList = new List <PathNode>() { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int z = 0; z < grid.GetHeight(); z++) { PathNode pathNode = GetNode(x, z); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { // Reached final node return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isWalkable) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Could not find the path return(null); }
//public BufferFromEntity<PathPosition> pathPositionBuffer; public void Execute() { for (int i = 0; i < pathNodeArray.Length; i++) { PathNode pathNode = pathNodeArray[i]; pathNode.hCost = CalculateDistanceCost(new int2(pathNode.x, pathNode.y), endPosition); pathNode.cameFromNodeIndex = -1; pathNodeArray[i] = pathNode; } NativeArray <int2> neighbourOffsetArray = new NativeArray <int2>(4, Allocator.Temp); neighbourOffsetArray[0] = new int2(-1, 0); // Left neighbourOffsetArray[1] = new int2(+1, 0); // Right neighbourOffsetArray[2] = new int2(0, +1); // Up neighbourOffsetArray[3] = new int2(0, -1); // Down //neighbourOffsetArray[4] = new int2(-1, -1); // Left Down // neighbourOffsetArray[5] = new int2(-1, +1); // Left Up //neighbourOffsetArray[6] = new int2(+1, -1); // Right Down //neighbourOffsetArray[7] = new int2(+1, +1); // Right Up int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x); PathNode startNode = pathNodeArray[CalculateIndex(startPosition.x, startPosition.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); NativeList <int> closedList = new NativeList <int>(Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray); PathNode currentNode = pathNodeArray[currentNodeIndex]; if (currentNodeIndex == endNodeIndex) { // Reached our destination! break; } // Remove current node from Open List for (int i = 0; i < openList.Length; i++) { if (openList[i] == currentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closedList.Add(currentNodeIndex); for (int i = 0; i < neighbourOffsetArray.Length; i++) { int2 neighbourOffset = neighbourOffsetArray[i]; int2 neighbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y); if (!IsPositionInsideGrid(neighbourPosition, gridSize)) { // Neighbour not valid position continue; } int neighbourNodeIndex = CalculateIndex(neighbourPosition.x, neighbourPosition.y, gridSize.x); if (closedList.Contains(neighbourNodeIndex)) { // Already searched this node continue; } PathNode neighbourNode = pathNodeArray[neighbourNodeIndex]; if (!neighbourNode.isWalkable) { // Not walkable continue; } int2 currentNodePosition = new int2(currentNode.x, currentNode.y); int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neighbourPosition); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNodeIndex = currentNodeIndex; neighbourNode.gCost = tentativeGCost; neighbourNode.CalculateFCost(); pathNodeArray[neighbourNodeIndex] = neighbourNode; if (!openList.Contains(neighbourNode.index)) { openList.Add(neighbourNode.index); } } } } //pathPositionBuffer.Clear(); /* * PathNode endNode = pathNodeArray[endNodeIndex]; * if (endNode.cameFromNodeIndex == -1) { * // Didn't find a path! * //Debug.Log("Didn't find a path!"); * pathFollowComponentDataFromEntity[entity] = new PathFollow { pathIndex = -1 }; * } else { * // Found a path * //CalculatePath(pathNodeArray, endNode, pathPositionBuffer); * //pathFollowComponentDataFromEntity[entity] = new PathFollow { pathIndex = pathPositionBuffer.Length - 1 }; * } */ neighbourOffsetArray.Dispose(); openList.Dispose(); closedList.Dispose(); }
public List <PathNode> FindPathNodes(Vector2Int startPos, Vector2Int endPos, Cell[,] grid) { myGrid = new Grid <PathNode>(grid.GetLength(0), grid.GetLength(1), 10f, Vector3.zero, (Grid <PathNode> g, int x, int y) => new PathNode(g, x, y)); PathNode startNode = myGrid.GetGridObject(startPos.x, startPos.y); PathNode endNode = myGrid.GetGridObject(endPos.x, endPos.y); openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < myGrid.GetWidth(); x++) { for (int y = 0; y < myGrid.GetHeight(); y++) { PathNode pathNode = myGrid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; //Heuristic startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { //Reached final node return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neibhourNode in GetNeighbourList(currentNode, grid)) { if (closedList.Contains(neibhourNode)) { continue; } if (!neibhourNode.isWalkable) { closedList.Add(neibhourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neibhourNode); if (tentativeGCost < neibhourNode.gCost) { neibhourNode.cameFromNode = currentNode; neibhourNode.gCost = tentativeGCost; neibhourNode.hCost = CalculateDistanceCost(neibhourNode, endNode); neibhourNode.CalculateFCost(); if (!openList.Contains(neibhourNode)) { openList.Add(neibhourNode); } } } } //Out of nodes on the openlist return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); if (startNode == null || endNode == null) { // Invalid Path return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { // Reached final node return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighborNode in GetNeighborList(currentNode)) { if (closedList.Contains(neighborNode)) { continue; } if (!neighborNode.isWalkable) { closedList.Add(neighborNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighborNode); if (tentativeGCost < neighborNode.gCost) { neighborNode.cameFromNode = currentNode; neighborNode.gCost = tentativeGCost; neighborNode.hCost = CalculateDistanceCost(neighborNode, endNode); neighborNode.CalculateFCost(); if (!openList.Contains(neighborNode)) { openList.Add(neighborNode); } } } } // Out of nodes on the openList return(null); }
public List <PathNode> FindPath(int startX, int startY, int endX, int endY, bool safePathOnly) { if (!GridCoords.IsInGrid(startX, startY)) { return(null); } if (!GridCoords.IsInGrid(endX, endY)) { return(null); } PathNode startNode = grid[startX, startY].PathNode; PathNode endNode = grid[endX, endY].PathNode; openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < currentGridInfo.gameGridSize.x; x++) { for (int y = 0; y < currentGridInfo.gameGridSize.y; y++) { PathNode pathNode = grid[x, y].PathNode; pathNode.gCost = int.MaxValue; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetValidNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } if (!neighbourNode.isTraversable) { closedList.Add(neighbourNode); continue; } else if (!neighbourNode.isSafe && safePathOnly) { closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } } } // Out of nodes on the openList return(null); }
public List <PathNode> FindPathOnLoop(int startX, int startY, int endX, int endY) { PathNode startNode = grid.GetGridObject(startX, startY); PathNode endNode = grid.GetGridObject(endX, endY); if (startNode == null || endNode == null) { // Invalid Path return(null); } openList = new List <PathNode> { startNode }; closedList = new List <PathNode>(); for (int x = 0; x < grid.GetWidth(); x++) { for (int y = 0; y < grid.GetHeight(); y++) { PathNode pathNode = grid.GetGridObject(x, y); pathNode.gCost = 99999999; pathNode.CalculateFCost(); pathNode.cameFromNode = null; } } startNode.gCost = 0; startNode.hCost = CalculateDistanceCost(startNode, endNode); startNode.CalculateFCost(); //PathfindingDebugStepVisual.Instance.ClearSnapshots(); //PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, startNode, openList, closedList); while (openList.Count > 0) { PathNode currentNode = GetLowestFCostNode(openList); if (currentNode == endNode) { // Reached final node //PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList); //PathfindingDebugStepVisual.Instance.TakeSnapshotFinalPath(grid, CalculatePath(endNode)); return(CalculatePath(endNode)); } openList.Remove(currentNode); closedList.Add(currentNode); foreach (PathNode neighbourNode in GetNeighbourList(currentNode)) { if (closedList.Contains(neighbourNode)) { continue; } Grid <WorldTileObject> worldTileGrid = DelegateController.getWorldTileGrid.Invoke(); PlacedObject_WorldTile currentTileObject = worldTileGrid.GetGridObject(neighbourNode.x, neighbourNode.y).GetPlacedObject(); if (!currentTileObject.MyAspects.Contains(LoopPathAspect)) { //Debug.Log("Closed: " + neighbourNode.x + ", " + neighbourNode.y); closedList.Add(neighbourNode); continue; } int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNode, neighbourNode); if (tentativeGCost < neighbourNode.gCost) { neighbourNode.cameFromNode = currentNode; neighbourNode.gCost = tentativeGCost; neighbourNode.hCost = CalculateDistanceCost(neighbourNode, endNode); neighbourNode.CalculateFCost(); if (!openList.Contains(neighbourNode)) { openList.Add(neighbourNode); } } //PathfindingDebugStepVisual.Instance.TakeSnapshot(grid, currentNode, openList, closedList); } } // Out of nodes on the openList return(null); }
public void Execute() { int2 gridSize = new int2(20, 20); NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp); for (int x = 0; x < gridSize.x; x++) { for (int y = 0; y < gridSize.y; y++) { PathNode pathNode = new PathNode(); pathNode.x = x; pathNode.y = y; pathNode.index = CalculateIndex(x, y, gridSize.x); pathNode.gCost = int.MaxValue; pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPosition); pathNode.CalculateFCost(); pathNode.isWalkable = true; pathNode.cameFronNodeIndex = -1; pathNodeArray[pathNode.index] = pathNode; } } // place walls // { // PathNode walkablePathNode = pathNodeArray[CalculateIndex(1, 0, gridSize.x)]; // walkablePathNode.SetIsWalkable(false); // pathNodeArray[CalculateIndex(1, 0, gridSize.x)] = walkablePathNode; // walkablePathNode = pathNodeArray[CalculateIndex(1, 1, gridSize.x)]; // walkablePathNode.SetIsWalkable(false); // pathNodeArray[CalculateIndex(1, 1, gridSize.x)] = walkablePathNode; // walkablePathNode = pathNodeArray[CalculateIndex(1, 2, gridSize.x)]; // walkablePathNode.SetIsWalkable(false); // pathNodeArray[CalculateIndex(1, 2, gridSize.x)] = walkablePathNode; // } NativeArray <int2> neighbourOffsetsArray = new NativeArray <int2>(8, Allocator.Temp); neighbourOffsetsArray[0] = new int2(-1, 0); // left neighbourOffsetsArray[1] = new int2(+1, 0); // right neighbourOffsetsArray[2] = new int2(0, +1); // up neighbourOffsetsArray[3] = new int2(0, -1); // down neighbourOffsetsArray[4] = new int2(-1, -1); // left down neighbourOffsetsArray[5] = new int2(-1, +1); // left up neighbourOffsetsArray[6] = new int2(+1, -1); // right down neighbourOffsetsArray[7] = new int2(+1, +1); // right up int endNodeIndex = CalculateIndex(endPosition.x, endPosition.y, gridSize.x); PathNode startNode = pathNodeArray[CalculateIndex(startingPosition.x, startingPosition.y, gridSize.x)]; startNode.gCost = 0; startNode.CalculateFCost(); pathNodeArray[startNode.index] = startNode; NativeList <int> openList = new NativeList <int>(Allocator.Temp); NativeList <int> closedList = new NativeList <int>(Allocator.Temp); openList.Add(startNode.index); while (openList.Length > 0) { int currrentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray); PathNode currentNode = pathNodeArray[currrentNodeIndex]; if (currrentNodeIndex == endNodeIndex) { // reached dest break; } for (int i = 0; i < openList.Length; i++) { if (openList[i] == currrentNodeIndex) { openList.RemoveAtSwapBack(i); break; } } closedList.Add(currrentNodeIndex); for (int i = 0; i < neighbourOffsetsArray.Length; i++) { int2 neighbourOffset = neighbourOffsetsArray[i]; int2 neightbourPosition = new int2(currentNode.x + neighbourOffset.x, currentNode.y + neighbourOffset.y); if (!IsPositionInsideGrid(neightbourPosition, gridSize)) { // Neighbour not valid position continue; } int neightbourNodeIndex = CalculateIndex(neightbourPosition.x, neightbourPosition.y, gridSize.x); if (closedList.Contains(neightbourNodeIndex)) { // Allready searched this one continue; } PathNode neightbourNode = pathNodeArray[neightbourNodeIndex]; if (!neightbourNode.isWalkable) { // Not walkable continue; } int2 currentNodePosition = new int2(currentNode.x, currentNode.y); int tentativeGCost = currentNode.gCost + CalculateDistanceCost(currentNodePosition, neightbourPosition); if (tentativeGCost < neightbourNode.gCost) { neightbourNode.cameFronNodeIndex = currrentNodeIndex; neightbourNode.gCost = tentativeGCost; neightbourNode.CalculateFCost(); pathNodeArray[neightbourNodeIndex] = neightbourNode; if (!openList.Contains(neightbourNode.index)) { openList.Add(neightbourNode.index); } } } } PathNode endNode = pathNodeArray[endNodeIndex]; if (endNode.cameFronNodeIndex != -1) { // Found a path NativeList <int2> path = CalculatePath(pathNodeArray, endNode); // foreach (int2 pathPosition in path) { // Debug.Log(pathPosition); // } path.Dispose(); } pathNodeArray.Dispose(); neighbourOffsetsArray.Dispose(); openList.Dispose(); closedList.Dispose(); }