void CreateGrid()
{
grid = new Node2D[gridSizeX, gridSizeY];
Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x / 2 - Vector3.up * gridWorldSize.y / 2;
for (int x = 0; x < gridSizeX; x++)
{
for (int y = 0; y < gridSizeY; y++)
{
Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.up * (y * nodeDiameter + nodeRadius);
//bool walkable = !(Physics2D.OverlapCircle(worldPoint, nodeRadius, unwalkableMask));
bool walkable = false;
int movementPenalty = 0;
RaycastHit2D hit = Physics2D.Raycast(worldPoint + Vector3.forward * 50, Vector3.back, 100, walkableMask);
if (hit)
{
walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
walkable = true;
}
Ray ray = new Ray(worldPoint + Vector3.forward * 50, Vector3.back);
RaycastHit hit3D;
if (Physics.Raycast(ray, out hit3D, 100, walkableMask))
{
walkableRegionsDictionary.TryGetValue(hit3D.collider.gameObject.layer, out movementPenalty);
walkable = true;
}
if (!walkable)
{
movementPenalty += obstacleProximityPenalty;
}
grid[x, y] = new Node2D(walkable, worldPoint, x, y, movementPenalty);
}
}
BlurPenaltyMap(3);
}
public Node2D[] NodesFromWorldBound(Vector3 worldPosition, Vector2 worldBounds)
{
int nodesX = Mathf.RoundToInt(worldBounds.x / nodeDiameter);
nodesX = (nodesX < 1) ? 1 : nodesX;
int nodesY = Mathf.RoundToInt(worldBounds.y / nodeDiameter);
nodesY = (nodesY < 1) ? 1 : nodesY;
Vector3 lowerLeftPos = new Vector3(worldPosition.x - worldBounds.x / 2f, worldPosition.y - worldBounds.y / 2f);
Node2D[] nodes = new Node2D[nodesX * nodesY];
int i = 0;
for (int x = 0; x < nodesX; x++)
{
for (int y = 0; y < nodesY; y++)
{
nodes[i] = NodeFromWorldPoint(lowerLeftPos + Vector3.right * x * nodeDiameter + Vector3.up * y * nodeDiameter);
i++;
}
}
return(nodes);
}
public void FindPath(PathRequest request, Action <PathResult> callback)
{
Stopwatch sw = new Stopwatch();
sw.Start();
Vector3[] waypoints = new Vector3[0];
bool pathSuccess = false;
Node2D startNode = grid.NodeFromWorldPoint(request.pathStart);
var endNodes = grid.NodesFromWorldBound(request.pathEnd, request.endBounds);
UnityEngine.Debug.Log("nodes in end node: " + endNodes.Length);
Node2D[] targetNode = endNodes.Where(n => n.walkable).ToArray();
UnityEngine.Debug.Log("usable target nodes: " + targetNode.Length);
Node2D endNode = startNode;
try
{
endNode = targetNode[0];
startNode.parent = startNode;
if (startNode.walkable && targetNode != null && targetNode.Length > 0)
{
Heap <Node2D> openSet = new Heap <Node2D>(grid.MaxSize);
HashSet <Node2D> closedSet = new HashSet <Node2D>();
openSet.Add(startNode);
while (openSet.Count > 0)
{
Node2D currentNode = openSet.RemoveFirst();
closedSet.Add(currentNode);
if (targetNode.Contains(currentNode))
{
sw.Stop();
UnityEngine.Debug.Log("Path found: " + sw.ElapsedMilliseconds + " ms");
pathSuccess = true;
endNode = currentNode;
break;
}
foreach (Node2D neighbour in grid.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);
}
}
}
}
}
}
catch (Exception e)
{
UnityEngine.Debug.Log("No usable end nodes");
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
if (pathSuccess)
{
waypoints = RetracePath(startNode, endNode);
pathSuccess = waypoints.Length > 0;
}
callback(new PathResult(waypoints, pathSuccess, request.callback));
}
int GetDistance(Node2D nodeA, Node2D[] nodesB)
{
return(nodesB.Min <Node2D>(o => GetDistance(nodeA, o)));
}
