public List <aStarNode> GetNeighbours(aStarNode node) { List <aStarNode> neighbours = new List <aStarNode>(); for (int y = -1; y <= 1; y++) { for (int x = -1; x <= 1; x++) { if (x == 0 && y == 0) { continue; } int checkX = node.gridX + x; int checkZ = node.gridZ + y; if (checkX >= 0 && checkX < xSize && checkZ >= 0 && checkZ < zSize) { neighbours.Add(pathmap[checkX, checkZ]); } } } return(neighbours); }
IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) { Vector3[] waypoints = new Vector3[0]; bool pathSuccess = false; aStarNode startNode = pathmap.NodeFromWorldPoint(startPos); aStarNode targetNode = pathmap.NodeFromWorldPoint(targetPos); if (startNode.walkable && targetNode.walkable) { Heap <aStarNode> openSet = new Heap <aStarNode>(pathmap.MaxSize); HashSet <aStarNode> closedSet = new HashSet <aStarNode>(); openSet.Add(startNode); while (openSet.Count > 0) { aStarNode currentNode = openSet.RemoveFirst(); closedSet.Add(currentNode); if (currentNode == targetNode) { pathSuccess = true; break; } foreach (aStarNode neighbour in pathmap.GetNeighbours(currentNode)) { if (!neighbour.walkable || closedSet.Contains(neighbour)) { continue; } int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty; if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) { neighbour.gCost = newMovementCostToNeighbour; neighbour.hCost = GetDistance(neighbour, targetNode); neighbour.parent = currentNode; if (!openSet.Contains(neighbour)) { openSet.Add(neighbour); } else { openSet.UpdateItem(neighbour); } } } } } yield return(null); if (pathSuccess) { waypoints = RetracePath(startNode, targetNode); } requestManager.FinishedProcessingPath(waypoints, pathSuccess); }
public aStarNode(Vector3 position, float f, aStarNode parent, Vector3 movementFromParent, int step) { this.position = position; this.f = f; this.parent = parent; this.movementFromParent = movementFromParent; this.step = step; }
int GetDistance(aStarNode nodeA, aStarNode nodeB) { int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX); int dstY = Mathf.Abs(nodeA.gridZ - nodeB.gridZ); if (dstX > dstY) { return(14 * dstY + 10 * (dstX - dstY)); } return(14 * dstX + 10 * (dstY - dstX)); }
Vector3[] RetracePath(aStarNode startNode, aStarNode endNode) { List <aStarNode> path = new List <aStarNode>(); aStarNode currentNode = endNode; while (currentNode != startNode) { path.Add(currentNode); currentNode = currentNode.parent; } Vector3[] waypoints = SimplifyPath(path); Array.Reverse(waypoints); return(waypoints); }
void CreatePathmap() { pathmap = new aStarNode[xSize, zSize]; GameObject tileMapGO = GameObject.FindGameObjectWithTag("TileMap"); TileMapData map = tileMapGO.GetComponent <TileMapGraphics>().map; for (int z = 0; z < zSize; z++) { for (int x = 0; x < xSize; x++) { pathmap[x, z] = new aStarNode(map.GetTile(x, z).walkable, map.GetTile(x, z).worldPos, x, z, map.GetTile(x, z).penalty); } } BlurPenaltyMap(0); }
private void MakeListOfMovements(Vector3 startingPosition) { List <aStarNode> openList = new List <aStarNode>(); List <Vector2> exploredPositions = new List <Vector2>(); List <aStarNode> stepedList = new List <aStarNode>(); openList.Add(new aStarNode(startingPosition, 0f, null, Vector3.zero, 0)); aStarNode theChosenOne = null; int algorithmSteps = 0; while (openList.Count > 0) { // get the node with BIGEST F on the list int selectedIndex = 0; for (int i = 0; i < openList.Count; i++) { if (openList[i].f > openList[selectedIndex].f) { selectedIndex = i; } } aStarNode selectedNode = openList[selectedIndex]; openList.RemoveAt(selectedIndex); exploredPositions.Add(selectedNode.position); // Debug.Log(selectedNode.position); // if it's not a wall then add the sucessors Collider2D collision = Physics2D.OverlapCircle(selectedNode.position, .1f); Collider2D[] collisions = Physics2D.OverlapCircleAll(transform.position, .1f); bool hasWall = false; bool hasGround = false; foreach (Collider2D t_collision in collisions) { if (t_collision.tag == "Walls") { hasWall = true; } else if (t_collision.tag == "Ground") { hasGround = true; } } if (hasGround && !hasWall) { stepedList.Add(selectedNode); foreach (Vector3 movement in possibleMovements) { if (Physics2D.OverlapCircle(selectedNode.position + movement, .1f) != null) { if (Physics2D.OverlapCircle(selectedNode.position + movement, .1f).tag == "Ground") { if (!exploredPositions.Contains((selectedNode.position + movement))) { // Debug.Log("Pushing to the list: " + (selectedNode.position + movement)); openList.Add(new aStarNode(selectedNode.position + movement, CalculateManhattanDistance(m_dogReference.transform.position, selectedNode.position + movement), selectedNode, movement, selectedNode.step + 1)); } } } } } algorithmSteps++; if (algorithmSteps > 50) { break; } } // CREATING THE MOVEMENT LIST List <Vector3> movements = new List <Vector3>(); bool canDo = true; theChosenOne = stepedList[0]; for (int i = 1; i < stepedList.Count; i++) { if (stepedList[i].f > theChosenOne.f) { theChosenOne = stepedList[i]; } } aStarNode node = theChosenOne; while (canDo) { Vector3 movement = GetOpposite(node.movementFromParent); movements.Insert(0, node.movementFromParent); if (node.parent != null) { node = node.parent; } else { canDo = false; break; } } // // Debug.Log("movements to take"); // for(int i = 0; i < movements.Count; i++) { // // Debug.Log(movements[i]); // } StartCoroutine(MoveWithListRoutine(movements)); }