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);
}
