Node:
using UnityEngine;
public class Node {
public bool _walkable;
public Vector3 _worldPosition;
public float _gCost, _hCost;
public int _gridX, _gridY, _gridZ;
public int _parent;
public int _movementPenalty;
public int _index;
public float fCost {
get { return _gCost + _hCost; }
}
public int CompareTo(Node nodeToCompare) {
int compare = fCost.CompareTo(nodeToCompare.fCost);
if (compare == 0) {
compare = _hCost.CompareTo(nodeToCompare._hCost);
}
return -compare;
}
public static bool operator ==(Node x, Node y) {
return x._index == y._index;
}
public static bool operator !=(Node x, Node y) {
return !(x._index == y._index);
}
}
Pathfinding (where Unity is freezing):
using System;
using System.Collections;
using System.Collections.Generic;
using System.Diagnostics;
using Unity.Mathematics;
using UnityEngine;
public class Pathfinding : MonoBehaviour {
public bool debugPath;
PathRequestManager requestManager;
Grid grid;
Stopwatch sw = new Stopwatch();
List<Node> openSet;
List<int> neighbors;
HashSet<Node> closedSet;
Vector3[] waypoints;
void Awake() {
requestManager = GetComponent<PathRequestManager>();
grid = GetComponent<Grid>();
neighbors = new List<int>();
openSet = new List<Node>();
closedSet = new HashSet<Node>();
}
public void FindPath(Vector3 startPos, Vector3 targetPos) {
if (debugPath) {
sw = new Stopwatch();
sw.Start();
}
bool pathSuccess = false;
int startNodeId = grid.NodeFromWorldPoint(startPos);
int endNodeId = grid.NodeFromWorldPoint(targetPos);
Node startNode = grid.grid[startNodeId];
Node targetNode = grid.grid[endNodeId];
openSet.Clear();
closedSet.Clear();
neighbors.Clear();
openSet.Add(startNode);
while (openSet.Count > 0) {
Node currentNode = openSet[0];
for (int i = 0; i < openSet.Count; i++) {
if (openSet[i].fCost < currentNode.fCost || openSet[i].fCost == currentNode.fCost && openSet[i]._hCost < currentNode._hCost) {
currentNode = openSet[i];
}
}
openSet.Remove(currentNode);
closedSet.Add(currentNode);
if (currentNode == targetNode) {
if (debugPath) {
sw.Stop();
print("path found: " + sw.ElapsedMilliseconds + " ms");
}
pathSuccess = true;
break;
}
neighbors = grid.GetNeighbours(currentNode);
ProcessNeighbors(currentNode, targetNode);
}
if (pathSuccess) {
waypoints = RetracePath(startNode, targetNode);
}
requestManager.FinishedProcessingPath(waypoints, pathSuccess);
}
public void ProcessNeighbors(Node currentNode, Node targetNode) {
for (int i = 0; i < neighbors.Count; i++) {
Node neighbor = grid.grid[neighbors[i]];
if (!neighbor._walkable || closedSet.Contains(neighbor)) {
continue;
}
float newMovementCostToNeighbour = currentNode._gCost + GetDistance(currentNode, neighbor) + neighbor._movementPenalty;
if (newMovementCostToNeighbour < neighbor._gCost || !openSet.Contains(neighbor)) {
neighbor._gCost = newMovementCostToNeighbour;
neighbor._hCost = GetDistance(neighbor, targetNode);
neighbor._parent = currentNode._index;
if (!openSet.Contains(neighbor)) {
openSet.Add(neighbor);
}
}
}
}
Vector3[] RetracePath(Node startNode, Node endNode) {
List<Node> path = new List<Node>();
Node currentNode = endNode;
while (currentNode != startNode) {
path.Add(currentNode);
int nodeid = currentNode._parent;
currentNode = grid.grid[nodeid];
}
Vector3[] waypoints = SimplifyPath(path);
Array.Reverse(waypoints);
return waypoints;
}
Vector3[] SimplifyPath(List<Node> path) {
List<Vector3> waypoints = new List<Vector3>();
Vector3 directionOld = Vector3.zero;
for (int i = 1; i < path.Count; i++) {
float x = path[i - 1]._gridX - path[i]._gridX;
float y = path[i - 1]._gridY - path[i]._gridY;
float z = path[i - 1]._gridZ - path[i]._gridZ;
Vector3 directionNew = new Vector3(x, y, z);
if (directionNew != directionOld) {
waypoints.Add(path[i]._worldPosition);
}
directionOld = directionNew;
}
return waypoints.ToArray();
}
float GetDistance(Node nodeA, Node nodeB) {
return math.distance(nodeA._worldPosition, nodeB._worldPosition);
}
}
Grid:
using System.Collections.Generic;
using UnityEngine;
public class Grid : MonoBehaviour {
public Vector3 gridWorldSize;
public float nodeRadius;
public TerrainType[] walkableRegions;
public LayerMask unwalkableMask;
public bool displayGridGizmos, onlyDisplayWalkable, onlyDisplayUnwalkable;
public Node[] grid;
float nodeDiameter;
int gridSizeX, gridSizeY, gridSizeZ;
LayerMask walkableMask;
Dictionary<int, int> walkableRegionsDictionary = new Dictionary<int, int>();
public float refreshRate;
float refreshTimer;
List<int> neighbours;
public int MaxSize {
get {
return gridSizeX * gridSizeY * gridSizeZ;
}
}
void Awake() {
neighbours = new List<int>();
nodeDiameter = nodeRadius * 2f;
gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter);
gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter);
gridSizeZ = Mathf.RoundToInt(gridWorldSize.z / nodeDiameter);
foreach (TerrainType region in walkableRegions) {
walkableMask.value |= region.terrainMask.value;
walkableRegionsDictionary.Add((int)Mathf.Log(region.terrainMask.value, 2), region.terrainPenalty);
}
CreateGrid();
}
void Update() {
if (refreshRate != 0) {
refreshTimer += Time.deltaTime;
if (refreshTimer >= Mathf.Abs(refreshRate)) {
refreshTimer = 0;
CreateGrid();
}
}
}
void CreateGrid() {
grid = new Node[gridSizeX * gridSizeY * gridSizeZ];
Vector3 leftEdge = (Vector3.left * gridWorldSize.x / 2);
Vector3 bottomEdge = (Vector3.down * gridWorldSize.y / 2);
Vector3 backEdge = (Vector3.back * gridWorldSize.z / 2);
Vector3 worldBottomLeft = transform.position + leftEdge + bottomEdge + backEdge;
for (int i = 0; i < grid.Length; i++) {
int idx = i;
int z = idx / (gridSizeX * gridSizeY);
idx -= (z * gridSizeX * gridSizeY);
int y = idx / gridSizeX;
int x = idx % gridSizeX;
Vector3 worldPoint = worldBottomLeft +
Vector3.right * (x * nodeDiameter + nodeRadius) +
Vector3.up * (y * nodeDiameter + nodeRadius) +
Vector3.forward * (z * nodeDiameter + nodeRadius);
bool walkable = !(Physics.CheckSphere(worldPoint, nodeRadius, unwalkableMask));
int movementPenalty = 0;
//raycast
if (walkable) {
Ray ray = new Ray(worldPoint + Vector3.up * 50, Vector3.down);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, 100, walkableMask)) {
walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
}
}
grid[i] = new Node() {
};
grid[i]._walkable = walkable;
grid[i]._worldPosition = worldPoint;
grid[i]._gridX = x;
grid[i]._gridY = y;
grid[i]._gridZ = z;
grid[i]._movementPenalty = movementPenalty;
grid[i]._index = i;
}
}
public int GetIdFromCoord(int x, int y, int z) {
return (z * gridSizeX * gridSizeY) + (y * gridSizeX) + x;
}
public int[] to3D(int idx) {
int z = idx / (gridSizeX * gridSizeY);
idx -= (z * gridSizeX * gridSizeY);
int y = idx / gridSizeX;
int x = idx % gridSizeX;
return new int[] { x, y, z };
}
public List<int> GetNeighbours(Node node) {
neighbours.Clear();
for (int x = -1; x <= 1; x++) {
for (int y = -1; y <= 1; y++) {
for (int z = -1; z <= 1; z++) {
if (x == 0 && y == 0 && z == 0) {
continue;
}
int checkX = node._gridX + x;
int checkY = node._gridY + y;
int checkZ = node._gridZ + z;
if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY && checkZ >= 0 && checkZ < gridSizeZ) {
int id = GetIdFromCoord(checkX, checkY, checkZ);
neighbours.Add(id);
}
}
}
}
return neighbours;
}
public int NodeFromWorldPoint(Vector3 worldPosition) {
float percentX = (worldPosition.x + gridWorldSize.x / 2) / gridWorldSize.x;
float percentY = (worldPosition.y + gridWorldSize.y / 2) / gridWorldSize.y;
float percentZ = (worldPosition.z + gridWorldSize.z / 2) / gridWorldSize.z;
percentX = Mathf.Clamp01(percentX);
percentY = Mathf.Clamp01(percentY);
percentZ = Mathf.Clamp01(percentZ);
int x = Mathf.RoundToInt((gridSizeX - 1) * percentX);
int y = Mathf.RoundToInt((gridSizeY - 1) * percentY);
int z = Mathf.RoundToInt((gridSizeZ - 1) * percentZ);
int id = GetIdFromCoord(x, y, z);
return grid[id]._index;
}
void OnDrawGizmos() {
Gizmos.DrawWireCube(transform.position, gridWorldSize);
if (grid != null && displayGridGizmos) {
foreach (Node n in grid) {
bool walkable = n._walkable;
//only do this if both settings arent turned on. if they are, ignore it.
if (!(onlyDisplayUnwalkable && onlyDisplayWalkable)) {
if ((walkable && onlyDisplayUnwalkable) || (!walkable && onlyDisplayWalkable)) {
continue;
}
}
Gizmos.color = walkable ? Color.white : Color.red;
Gizmos.DrawWireCube(n._worldPosition, Vector3.one * (nodeDiameter - 0.1f));
}
}
}
[System.Serializable]
public class TerrainType {
public LayerMask terrainMask;
public int terrainPenalty;
}
}