/***************************************************************************/ public List <NodePathfinding> GetNeighbours(NodePathfinding node, bool eightConnectivity) { List <NodePathfinding> neighbours = new List <NodePathfinding>(); for (int x = -1; x <= 1; x++) { for (int y = -1; y <= 1; y++) { if ((x == 0 && y == 0)) { continue; } if (!eightConnectivity && (Mathf.Abs(x) + Mathf.Abs(y) > 1)) { continue; } int checkX = node.mGridX + x; int checkY = node.mGridY + y; if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) { neighbours.Add(grid[checkX, checkY]); } } } return(neighbours); }
/***************************************************************************/ void RetracePath(NodePathfinding startNode, NodePathfinding endNode) { List <NodePathfinding> path = new List <NodePathfinding>(); bool end = false; NodePathfinding newNode = endNode; while (!end) { path.Add(newNode); newNode = newNode.mParent; if (newNode == startNode) { path.Add(newNode); end = true; } } path.Reverse(); #if Smooth SmoothPath(path); #else Grid.path = path; #endif }
/***************************************************************************/ float Heuristic(NodePathfinding nodeA, NodePathfinding nodeB) { // Heuristic function //nodeA.mGridX nodeB.mGridX //nodeA.mGridY nodeB.mGridY return(Vector2.Distance(new Vector2(nodeB.mGridX, nodeB.mGridY), new Vector2(nodeA.mGridX, nodeA.mGridY)) * hMultiplier); }
public NodePathfinding(int gridX, int gridY, float gCost = 0f, float hCost = 0f, NodePathfinding parent = null) { this.GCost = gCost; this.HCost = hCost; this.Parent = parent; this.GridX = gridX; this.GridY = gridY; FCost = gCost + hCost; }
private void CountTiles() { //walkableTiles = new List<Vector2Int>(); //worldWalkableTiles = new List<Vector3>(); //walkableNodes = new List<NodePathfinding>(); BoundsInt bounds = tilemapBase.cellBounds; tilemapGrid = tilemapBase.layoutGrid; //tilemapGrid. TileBase[] allTiles = tilemapBase.GetTilesBlock(bounds); walkableNodesArray = new NodePathfinding[bounds.size.x, bounds.size.y]; worldWalkableTilesArray = new Vector3[bounds.size.x, bounds.size.y]; tilesCount = 0; for (int i = 0; i < bounds.size.x; i++) { for (int j = 0; j < bounds.size.y; j++) { /* * if(tilemapBase.HasTile(new Vector3Int(i, j, 0))) * { * tilesCount++; * tilemapBase.SetColor(new Vector3Int(i, j, 0), Color.green); * } */ TileBase tempTile = allTiles[i + j * bounds.size.x]; if (tempTile != null) { tilesCount++; //walkableTiles.Add(new Vector2Int(i, j)); //Debug.Log("x: " + i + " y: " + j + " tile: " + tempTile.name); walkableNodesArray[i, j] = new NodePathfinding(i, j); worldWalkableTilesArray[i, j] = new Vector3(i + tilemapBase.origin.x, j + tilemapBase.origin.y); //Vector3Int localPlace = (new Vector3Int(i, j, (int)tilemapBase.transform.position.y)); //Vector3 place = tilemapBase.CellToWorld(localPlace); //Debug.Log("place: ") } } } //Debug.Log("Tiles count: " + tilesCount); /* * foreach (Vector2Int tile in walkableTiles) * { * Vector3 tileWorldPos = new Vector3(tile.x + tilemapBase.origin.x, tile.y + tilemapBase.origin.y, 0); * //Instantiate(prefab, tileWorldPos, prefab.transform.rotation); * worldWalkableTiles.Add(tileWorldPos); * walkableNodes.Add(new NodePathfinding(tile.x, tile.y)); * } */ }
private NodePathfinding GetNodeWithLowestF(List <NodePathfinding> nodeList) { NodePathfinding lowest = nodeList[0]; foreach (NodePathfinding node in nodeList) { if (node.FCost < lowest.FCost) { lowest = node; } } return(lowest); }
public bool MyEquals(NodePathfinding other) { if (other != null) { if (this.GridX == other.GridX) { if (this.GridY == other.GridY) { return(true); } } } return(false); }
List <NodePathfinding> GetNeighbours(NodePathfinding curr) { var list = new List <NodePathfinding>(); for (int i = 0; i < curr.neightbourds.Count; i++) { if (curr.neightbourds[i].isTrap) { continue; } list.Add(curr.neightbourds[i]); } return(list); }
private bool NodeExistsInList(List <NodePathfinding> nodeList, NodePathfinding node) { /* * foreach(NodePathfinding n in nodeList) * { * if(node.GridX == n.GridX) * { * if(node.GridY == n.GridY) * { * return true; * } * } * } * return false; */ return(nodeList.Contains(node)); }
/***************************************************************************/ float GetDistance(NodePathfinding nodeA, NodePathfinding nodeB) { // Distance function //nodeA.mGridX nodeB.mGridX //nodeA.mGridY nodeB.mGridY if (EightConnectivity) { int vertDistance = Mathf.Abs(nodeA.mGridY - nodeB.mGridY); int horzDistance = Mathf.Abs(nodeA.mGridX - nodeB.mGridX); float distance = Mathf.Sqrt(Mathf.Pow(vertDistance, 2) + Mathf.Pow(horzDistance, 2)); return(distance); } else { return((Mathf.Abs(nodeA.mGridX - nodeB.mGridX)) + (Mathf.Abs(nodeA.mGridY - nodeB.mGridY))); } }
/***************************************************************************/ /***************************************************************************/ void SmoothPath(List <NodePathfinding> path) { //TODO //Comparamos si el nodo final es el mismo que el inicial //Cuando bresenham sea true, el nodo inicial se cambia al nodo que ha salido true y se vuelve a recalcular //Se añade ese nodo para guardarlo y se vuelve a iterar por el resto de nodos hasta que vuelva a encontrar el nodo inicial List <NodePathfinding> targetNodes = new List <NodePathfinding>(); NodePathfinding newOriginNode = path[0]; NodePathfinding endNode = path[path.Count - 1]; List <NodePathfinding> auxNodeList = new List <NodePathfinding>(); List <NodePathfinding> newNodeList = new List <NodePathfinding>(); newNodeList = path; newNodeList.Reverse(); int a = 0; for (a = 0; a < 10000; a++) { foreach (NodePathfinding n in newNodeList) { if (BresenhamWalkable(newOriginNode.mGridX, newOriginNode.mGridY, n.mGridX, n.mGridY)) { targetNodes.Add(n); newOriginNode = n; newNodeList = new List <NodePathfinding>(auxNodeList); auxNodeList.Clear(); break; } auxNodeList.Add(n); } if (endNode == newOriginNode) { a = 999999; } } targetNodes.Add(path[0]); Grid.path = targetNodes; }
/***************************************************************************/ void CreateGrid() { grid = new NodePathfinding[gridSizeX, gridSizeY]; Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.forward * gridWorldSize.y / 2; 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); bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask)); float costMultiplier = (Physics.CheckSphere(worldPoint, nodeRadius, costMultiplierMask)) ? 1.5f : 1.0f; grid[x, y] = new NodePathfinding(walkable, worldPoint, x, y, costMultiplier); } } }
public bool DiagonalNeighbour(NodePathfinding neighbour) { if ((neighbour.GridX == this.GridX + 1) && (neighbour.GridY == this.GridY + 1)) { return(true); } else if ((neighbour.GridX == this.GridX + 1) && (neighbour.GridY == this.GridY - 1)) { return(true); } else if ((neighbour.GridX == this.GridX - 1) && (neighbour.GridY == this.GridY - 1)) { return(true); } else if ((neighbour.GridX == this.GridX - 1) && (neighbour.GridY == this.GridY + 1)) { return(true); } else { return(false); } }
/***************************************************************************/ public List <NodePathfinding> FindPath(Vector3 startPos, Vector3 targetPos, int iterations) { CurrentStartNode = Grid.NodeFromWorldPoint(startPos); CurrentTargetNode = Grid.NodeFromWorldPoint(targetPos); List <NodePathfinding> openSet = new List <NodePathfinding>(); HashSet <NodePathfinding> closedSet = new HashSet <NodePathfinding>(); openSet.Add(CurrentStartNode); Grid.openSet = openSet; int currentIteration = 0; NodePathfinding node = CurrentStartNode; while (openSet.Count > 0 && node != CurrentTargetNode && (iterations == -1 || currentIteration < iterations)) { // Select best node from open list node = openSet[0]; foreach (NodePathfinding nod in openSet) { if (nod.fCost < node.fCost) { node = nod; } } // Manage open/closed list openSet.Remove(node); closedSet.Add(node); Grid.openSet = openSet; Grid.closedSet = closedSet; // Check destination if (node != CurrentTargetNode) { // Open neighbours foreach (NodePathfinding neighbour in Grid.GetNeighbours(node, EightConnectivity)) { if (!neighbour.mWalkable || closedSet.Contains(neighbour)) { continue; } //Cost of reaching my neighbor = cost of reaching ME + float neighbourGcost = (node.gCost + (GetDistance(node, neighbour) * node.mCostMultiplier)); if (neighbourGcost < node.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = neighbourGcost; neighbour.hCost = Heuristic(neighbour, CurrentTargetNode); neighbour.mParent = node; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } } } currentIteration++; } else { //Path found! //Comment / Uncomment the define Smooth to deactive / active the smoothing with Bresenham. RetracePath(CurrentStartNode, CurrentTargetNode); // Path found totalNodes = openSet.Count + closedSet.Count; openNodes = openSet.Count; closedNodes = closedSet.Count; Debug.Log("Statistics:"); Debug.LogFormat("Total nodes: {0}", totalNodes); Debug.LogFormat("Open nodes: {0}", openNodes); Debug.LogFormat("Closed nodes: {0}", closedNodes); } } return(Grid.path); }
//A* Pathfinding Algorithm Implementation public List <Vector3> ExecutePathfinding(Vector3 startingPosition) { CountTiles(); Vector3 playerPosition = PlayerCharacter.instance.transform.position; List <NodePathfinding> open = new List <NodePathfinding>(); List <NodePathfinding> closed = new List <NodePathfinding>(); Vector2Int startingTile = GetTileFromWorldPosition(startingPosition); Vector3 centerOfStartTile = GetWorldPositionOfCenterOfTile(startingTile); //NodePathfinding startNode = new NodePathfinding(startingTile.x, startingTile.y, 0, Vector3.Distance(startingPosition, playerPosition)); //NodePathfinding startNode = GetNode(startingTile.x, startingTile.y); NodePathfinding startNode = walkableNodesArray[startingTile.x, startingTile.y]; startNode.HCost = Vector3.Distance(startingPosition, playerPosition); open.Add(startNode); Vector2Int targetTile = GetTileFromWorldPosition(playerPosition); //NodePathfinding targetNode = new NodePathfinding(targetTile.x, targetTile.y); NodePathfinding targetNode = walkableNodesArray[targetTile.x, targetTile.y]; NodePathfinding currentNode = null; for (int i = 0; i < tilesCount; i++) { currentNode = GetNodeWithLowestF(open); open.Remove(currentNode); closed.Add(currentNode); if (currentNode.MyEquals(targetNode)) { //break i = tilesCount; } foreach (Vector2Int neighbourTile in currentNode.Neighbours()) { NodePathfinding neighbourNode; if ((neighbourTile.x < walkableNodesArray.GetLength(0)) && (neighbourTile.x >= 0) && (neighbourTile.y < walkableNodesArray.GetLength(1)) && (neighbourTile.y >= 0)) { neighbourNode = walkableNodesArray[neighbourTile.x, neighbourTile.y]; } else { neighbourNode = null; } if (ExistsInWalkableTiles(neighbourTile) && (!NodeExistsInList(closed, neighbourNode))) { float traverseDistance = GetTraverseNeighbourDistance(currentNode, neighbourNode); float newPath = currentNode.GCost + traverseDistance; if ((newPath < neighbourNode.GCost) || (!NodeExistsInList(open, neighbourNode))) { neighbourNode.GCost = newPath; Vector3 nodeWorldCenter = GetWorldPositionOfCenterOfTile(new Vector2Int(neighbourNode.GridX, neighbourNode.GridY)); neighbourNode.HCost = Vector3.Distance(nodeWorldCenter, playerPosition); neighbourNode.FCost = neighbourNode.GCost + neighbourNode.HCost; neighbourNode.Parent = currentNode; if (!NodeExistsInList(open, neighbourNode)) { open.Add(neighbourNode); } } } } } List <Vector3> results = new List <Vector3>(); NodePathfinding currentResultNode = currentNode; while (currentResultNode.Parent != null) { results.Add(GetWorldPositionOfCenterOfTile(new Vector2Int(currentResultNode.GridX, currentResultNode.GridY))); currentResultNode = currentResultNode.Parent; } results.Reverse(); return(results); }
private float GetTraverseNeighbourDistance(NodePathfinding nb1, NodePathfinding nb2) { return(nb1.DiagonalNeighbour(nb2) ? tileDiag : tileSize); }
bool Satisfies(NodePathfinding curr) { return(curr == finit); }
void Start() { pathfinding = transform.GetComponent <NodePathfinding>(); Target = Player.Instance.transform; actor = transform.GetComponent <NPC>(); }