Newer
Older
TheVengeance-Project-IADE-Unity2D / Assets / Scripts / NPC / Pathfinding / Pathfinding.cs
  1. using System.Collections;
  2. using System.Collections.Generic;
  3. using UnityEngine;
  4. using System.Diagnostics;
  5. using MyCollections.Generic.Heap;
  6. using System;
  7. namespace MyCollections.AI.Pathfinding
  8. {
  9. public class Pathfinding : MonoBehaviour
  10. {
  11. Grid grid;
  12. private void Awake()
  13. {
  14. grid = GetComponent<Grid>();
  15. }
  16. public void FindPath(PathRequest request, Action<PathResult> callback)
  17. {
  18. Stopwatch sw = new Stopwatch();
  19. sw.Start();
  20. Vector2[] waypoints = new Vector2[0];
  21. bool pathSuccess = false;
  22. Node startNode = grid.NodeFromWorldPoint(request.pathStart);
  23. Node targetNode = grid.NodeFromWorldPoint(request.pathEnd);
  24. if (startNode.walkable && targetNode.walkable)
  25. {
  26. Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
  27. HashSet<Node> closeSet = new HashSet<Node>();
  28. openSet.Add(startNode);
  29. while (openSet.Count > 0)
  30. {
  31. Node currentNode = openSet.RemoveFirst();
  32. closeSet.Add(currentNode);
  33. if (currentNode == targetNode)
  34. {
  35. sw.Stop();
  36. //print($"Path found: {sw.ElapsedMilliseconds}ms");
  37. pathSuccess = true;
  38. break;
  39. }
  40. foreach (Node neighbour in grid.GetNeighbours(currentNode))
  41. {
  42. if (!neighbour.walkable || closeSet.Contains(neighbour))
  43. continue;
  44. int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;
  45. if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour))
  46. {
  47. neighbour.gCost = newMovementCostToNeighbour;
  48. neighbour.hCost = GetDistance(neighbour, targetNode);
  49. neighbour.parent = currentNode;
  50. if (!openSet.Contains(neighbour))
  51. openSet.Add(neighbour);
  52. else
  53. openSet.UpdateItem(neighbour);
  54. }
  55. }
  56. }
  57. }
  58. if (pathSuccess)
  59. {
  60. waypoints = RetracePath(startNode, targetNode);
  61. pathSuccess = waypoints.Length > 0;
  62. }
  63. callback(new PathResult(waypoints, pathSuccess, request.callback));
  64. }
  65. private Vector2[] RetracePath(Node startNode, Node endNode)
  66. {
  67. List<Node> path = new List<Node>();
  68. Node currentNode = endNode;
  69. while (currentNode != startNode)
  70. {
  71. path.Add(currentNode);
  72. currentNode = currentNode.parent;
  73. }
  74. Vector2[] waypoints = SimplifyPath(path);
  75. Array.Reverse(waypoints);
  76. return waypoints;
  77. }
  78. private Vector2[] SimplifyPath(List<Node> path)
  79. {
  80. List<Vector2> waypoints = new List<Vector2>();
  81. Vector2 directionOld = Vector2.zero;
  82. for (int i = 1; i < path.Count; i++)
  83. {
  84. Vector2 directionNew = new Vector2(path[i - 1].gridX - path[i].gridX, path[i - 1].gridY - path[i].gridY);
  85. if (directionNew != directionOld)
  86. {
  87. waypoints.Add(path[i].worldPos);
  88. }
  89. directionOld = directionNew;
  90. }
  91. return waypoints.ToArray();
  92. }
  93. private int GetDistance(Node nodeA, Node nodeB)
  94. {
  95. int distX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
  96. int distY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
  97. if (distX > distY)
  98. return 14 * distY + 10 * (distX - distY);
  99. return 14 * distX + 10 * (distY - distX);
  100. }
  101. }
  102. }