public static LinkedList <Node> FindPath(Node _startNode, Node _goalNode, GridManager _grid) { if (_startNode.State != BlockState.EnemyPiece) { return(null); } if (_goalNode.State != BlockState.Empty) { return(null); } // RESET ALL NODES. IEnumerator gridEnumurator = _grid.nodes.GetEnumerator(); while (gridEnumurator.MoveNext()) { (gridEnumurator.Current as Node).Reset(); } openBHList.Clear(); openBHList.Insert(_startNode); _startNode.nodePathCost = 0.0f; _startNode.totalCost = _grid.GridAlgorithms.HeuristicEstimatedCost(_startNode, _goalNode); // + _startNode.nodePathCost; closedList.Clear(); Node curNode = null; while (openBHList.Count > 0) { // Check if the closed List contains the _goalNode. if (closedList.Contains(_goalNode)) { return(ConvertToPath(curNode)); } curNode = openBHList.PopRoot(); for (LinkedListNode <Node> curLinkedNode = curNode.neighbours.First; curLinkedNode != null; curLinkedNode = curLinkedNode.Next) { Node curNeighbourNode = (Node)curLinkedNode.Value; if (curNeighbourNode.State != BlockState.Empty) { continue; } if (!closedList.Contains(curNeighbourNode)) { //Cost from current node to this neighbour node float cost = _grid.GridAlgorithms.NeighbourPathCost(curNode, curNeighbourNode); //Total Cost So Far from start to this neighbour node float totalPathCost = curNode.nodePathCost + cost; //Estimated cost for neighbour node to the goal float neighbourNodeEstCost = _grid.GridAlgorithms.HeuristicEstimatedCost(curNeighbourNode, _goalNode); if (openBHList.Contains(curNeighbourNode)) // Calculated before? { if (totalPathCost < curNeighbourNode.nodePathCost) { curNeighbourNode.nodePathCost = totalPathCost; curNeighbourNode.parent = curNode; curNeighbourNode.totalCost = totalPathCost + neighbourNodeEstCost; } } else { curNeighbourNode.nodePathCost = totalPathCost; curNeighbourNode.parent = curNode; curNeighbourNode.totalCost = totalPathCost + neighbourNodeEstCost; //Add the neighbour node to the list if not already existed in the list openBHList.Insert(curNeighbourNode); } } } closedList.Add(curNode); } if (closedList.Contains(_goalNode)) { return(ConvertToPath(curNode)); } return(null); }
public List <Vector2> GetPath(Vector2 startPos, Vector2 endPos) { NodeBinaryHeap openList = new NodeBinaryHeap(); List <Node> closedList = new List <Node> (); int startX = Mathf.RoundToInt(Mathf.Abs(scanArea.x - startPos.x) / sizeWidth); int startY = Mathf.RoundToInt(Mathf.Abs(scanArea.y - startPos.y) / sizeHeight); Debug.Log("StartX: " + startX + " StartY: " + startY); int endX = Mathf.RoundToInt(Mathf.Abs(scanArea.x - endPos.x) / sizeWidth); int endY = Mathf.RoundToInt(Mathf.Abs(scanArea.y - endPos.y) / sizeHeight); Debug.Log("EndX: " + endX + " EndY: " + endY); //Check Path is Solveable if (map[startX, startY].passable == false || map[endX, endY].passable == false) { return(null); } //Set score to 0, could set its parent to itself for path detection if needed. map [startX, startY].FScore = 0; map [startX, startY].SetParent(startX, startY); openList.Insert(map [startX, startY]); while (openList.count > 0) { Node lowestNode = openList.GetAndRemoveRoot(); if (lowestNode.x == endX && lowestNode.y == endY) { List <Vector2> output = new List <Vector2>(); return(CalculatePath(lowestNode, output)); } //openList.RemoveAt (indexForRemove); closedList.Add(lowestNode); //Get Neighbours Node[] primaryNei = new Node[8]; //Default 4 //Left if (lowestNode.x > 0) { primaryNei[0] = map[lowestNode.x - 1, lowestNode.y]; } //Right if (lowestNode.x < width - 1) { primaryNei[1] = map[lowestNode.x + 1, lowestNode.y]; } //Up if (lowestNode.y < height - 1) { primaryNei[2] = map[lowestNode.x, lowestNode.y + 1]; } //Down if (lowestNode.y > 0) { primaryNei[3] = map[lowestNode.x, lowestNode.y - 1]; } //Advanced 4 //Top Left if (lowestNode.y < height - 1 && lowestNode.x > 0) { primaryNei[4] = map[lowestNode.x - 1, lowestNode.y + 1]; } //Bottom Left if (lowestNode.y > 0 && lowestNode.x > 0) { primaryNei[5] = map[lowestNode.x - 1, lowestNode.y - 1]; } //Top Right if (lowestNode.y < height - 1 && lowestNode.x < width - 1) { primaryNei[6] = map[lowestNode.x + 1, lowestNode.y + 1]; } //Bottom Right if (lowestNode.y > 0 && lowestNode.x < width - 1) { primaryNei[7] = map[lowestNode.x + 1, lowestNode.y - 1]; } for (int i = 0; i < 8; i++) { //Out of bounds check. if (primaryNei[i] == null) { continue; } if (primaryNei[i].passable == false) { continue; } if (i >= 4) { if (DiagonalWallCheck(primaryNei[i]) == true) { continue; } } //Checks Neighbour isn't in Closed List if (closedList.Contains(primaryNei[i])) { continue; } if (openList.Contains(primaryNei[i]) == false) { //Set the parent of the neighbour to the current node. primaryNei[i].SetParent(lowestNode.x, lowestNode.y); //primaryNei[i].GScore = CalculateDistance(primaryNei[i].x, primaryNei[i].y, startX, startY); primaryNei[i].HScore = CalculateDistance(primaryNei[i].x, primaryNei[i].y, endX, endY); primaryNei[i].FScore = primaryNei[i].GScore + primaryNei[i].HScore; openList.Insert(primaryNei[i]); } else { //Diagonal optomisation will go here. - Actually add a working G cost. } //Add node back to map. map[primaryNei[i].x, primaryNei[i].y] = primaryNei[i]; } } //Output List <Vector2> finalPath = new List <Vector2> (); return(finalPath); }