- using System.Collections;
- using System.Collections.Generic;
- using UnityEngine;
- using System.Diagnostics;
- using MyCollections.Generic.Heap;
- using System;
- namespace MyCollections.AI.Pathfinding
- {
- public class Pathfinding : MonoBehaviour
- {
- Grid grid;
- private void Awake()
- {
- grid = GetComponent<Grid>();
- }
- public void FindPath(PathRequest request, Action<PathResult> callback)
- {
- Stopwatch sw = new Stopwatch();
- sw.Start();
- Vector2[] waypoints = new Vector2[0];
- bool pathSuccess = false;
- Node startNode = grid.NodeFromWorldPoint(request.pathStart);
- Node targetNode = grid.NodeFromWorldPoint(request.pathEnd);
- if (startNode.walkable && targetNode.walkable)
- {
- Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
- HashSet<Node> closeSet = new HashSet<Node>();
- openSet.Add(startNode);
- while (openSet.Count > 0)
- {
- Node currentNode = openSet.RemoveFirst();
- closeSet.Add(currentNode);
- if (currentNode == targetNode)
- {
- sw.Stop();
- //print($"Path found: {sw.ElapsedMilliseconds}ms");
- pathSuccess = true;
- break;
- }
- foreach (Node neighbour in grid.GetNeighbours(currentNode))
- {
- if (!neighbour.walkable || closeSet.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);
- }
- }
- }
- }
- if (pathSuccess)
- {
- waypoints = RetracePath(startNode, targetNode);
- pathSuccess = waypoints.Length > 0;
- }
- callback(new PathResult(waypoints, pathSuccess, request.callback));
- }
- private Vector2[] RetracePath(Node startNode, Node endNode)
- {
- List<Node> path = new List<Node>();
- Node currentNode = endNode;
- while (currentNode != startNode)
- {
- path.Add(currentNode);
- currentNode = currentNode.parent;
- }
- Vector2[] waypoints = SimplifyPath(path);
- Array.Reverse(waypoints);
- return waypoints;
- }
- private Vector2[] SimplifyPath(List<Node> path)
- {
- List<Vector2> waypoints = new List<Vector2>();
- Vector2 directionOld = Vector2.zero;
- for (int i = 1; i < path.Count; i++)
- {
- Vector2 directionNew = new Vector2(path[i - 1].gridX - path[i].gridX, path[i - 1].gridY - path[i].gridY);
- if (directionNew != directionOld)
- {
- waypoints.Add(path[i].worldPos);
- }
- directionOld = directionNew;
- }
- return waypoints.ToArray();
- }
- private int GetDistance(Node nodeA, Node nodeB)
- {
- int distX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
- int distY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
- if (distX > distY)
- return 14 * distY + 10 * (distX - distY);
- return 14 * distX + 10 * (distY - distX);
- }
- }
- }