public List <PF_Node> GetNeighbourNodes(PF_Node node_)
{
List <PF_Node> neighbourNodes = new List <PF_Node>();
// Loop through and get all neighbouring nodes.
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
// Ignore the current node.
if (x == 0 && y == 0)
{
continue;
}
int checkX = node_.gridX + x;
int checkY = node_.gridY + y;
if (checkX >= 0 &&
checkX < gridSizeX &&
checkY >= 0 &&
checkY < gridSizeY)
{
// Add the node to the neighbour nodes.
neighbourNodes.Add(grid[checkX, checkY]);
}
}
}
return(neighbourNodes);
}
private int GetDistance(PF_Node nodeA, PF_Node nodeB)
{
// Get the x and y distances between the two nodes.
int distanceX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
int distanceY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
// Return the distance of the shortest path.
if (distanceX > distanceY)
{
return(14 * distanceY + 10 * (distanceX - distanceY));
}
else
{
return(14 * distanceX + 10 * (distanceY - distanceX));
}
}
private Vector3[] CalculatePath(PF_Node startNode_, PF_Node endNode_)
{
List <PF_Node> path = new List <PF_Node>();
// Begin at the end node.
PF_Node currentNode = endNode_;
// Loop through from the end node to the start node and construct the path.
while (currentNode != startNode_)
{
path.Add(currentNode);
currentNode = currentNode.parentNode;
}
// Simplify the path by removing duplicate commands from adjacent nodes when direction does not change.
Vector3[] waypoints = SimplifyPath(path);
// Reverse the waypoints to get it from start to end.
Array.Reverse(waypoints);
return(waypoints);
}
private void CreateGrid()
{
grid = new PF_Node[gridSizeX, gridSizeY];
// Get the world position for the bottom left of the grid.
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++)
{
// Get the world position for the current node.
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius);
// Determine if the node is walkable or should be considered obstructed.
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
int movementPenalty = 0;
// Raycast
if (walkable)
{
// Start the ray in the sky and fire directly down.
Ray ray = new Ray(worldPoint + Vector3.up * 50, Vector3.down);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, 100, walkableMask))
{
// Get the movement penalty of the terrain type.
walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
}
}
// Update the node information within the grid for this particular node.
grid[x, y] = new PF_Node(walkable, worldPoint, x, y, movementPenalty);
}
}
}
private void OnDrawGizmos()
{
// Wireframe to show grid boundaries in editor.
Gizmos.DrawWireCube(transform.position, new Vector3(gridWorldSize.x, 1, gridWorldSize.y));
if (grid != null && displayGridGizmos)
{
PF_Node playerNode = GetNodeFromWorldPoint(player.position);
foreach (PF_Node n in grid)
{
if (n.walkable)
{
// Walkable.
Gizmos.color = Color.white;
if (n.movementPenalty == 5)
{
Gizmos.color = Color.green;
}
}
else
{
// Obstacle.
Gizmos.color = Color.red;
}
if (playerNode == n)
{
// Player.
Gizmos.color = Color.cyan;
}
// Cubes to represent node positions.
Gizmos.DrawCube(n.worldPos, Vector3.one * (nodeDiameter - 0.1f));
}
}
}
IEnumerator FindPath(Vector3 startPos_, Vector3 endPos_)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
// Get the start and end nodes.
PF_Node startNode = grid.GetNodeFromWorldPoint(startPos_);
PF_Node endNode = grid.GetNodeFromWorldPoint(endPos_);
startNode.parentNode = startNode;
// Only search if start and end are both walkable nodes.
if (startNode.walkable && endNode.walkable)
{
// Create two lists of nodes.
// Open list will contain nodes to be evaluated.
// Closed list will contain nodes that have already been evaluated.
PF_Heap <PF_Node> openSet = new PF_Heap <PF_Node>(grid.MaxSize);
HashSet <PF_Node> closedSet = new HashSet <PF_Node>();
// Add the starting node to our open list.
openSet.Add(startNode);
// Loop through the openSet.
while (openSet.Count > 0)
{
// Begin with the first element in the list.
PF_Node currentNode = openSet.RemoveFirst();
// Add the starting node to the closed set.
closedSet.Add(currentNode);
// Path has been found.
if (currentNode == endNode)
{
sw.Stop();
//UnityEngine.Debug.Log("Path found: " + sw.ElapsedMilliseconds + "ms");
pathSuccess = true;
break;
}
foreach (PF_Node neighbour in grid.GetNeighbourNodes(currentNode))
{
// Check if the neighbour is not walkable, or already in the closed list.
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
// Get the cost to move to the neighbour.
int newNeighbourMoveCost = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
// If the new move cost is smaller than the current, or the open list does not contain this neighbour.
if (newNeighbourMoveCost < neighbour.gCost || !openSet.Contains(neighbour))
{
// Update the gCost and hCost.
neighbour.gCost = newNeighbourMoveCost;
neighbour.hCost = GetDistance(neighbour, endNode);
// Update the parent node.
neighbour.parentNode = currentNode;
// Add it to the open set if it is not currently there.
if (!openSet.Contains(neighbour))
{
openSet.Add(neighbour);
}
// Otherwise update it.
else
{
openSet.UpdateItem(neighbour);
}
}
}
}
}
yield return(null);
if (pathSuccess)
{
waypoints = CalculatePath(startNode, endNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
