Newer
Older
TheVengeance-Project-IADE-Unity2D / Assets / Scripts / NPC / Pathfinding / Grid.cs
  1. using System;
  2. using System.Collections;
  3. using System.Collections.Generic;
  4. using UnityEngine;
  5. using MyCollections.Generic.Heap;
  6. namespace MyCollections.AI.Pathfinding
  7. {
  8. public class Grid : MonoBehaviour
  9. {
  10. //Variables
  11. public bool displayGridGizmos;
  12. //Layer Masks
  13. private LayerMask walkableMask;
  14. public LayerMask unwalkableMask; //Unwalkable Mask (used to determine which areas are walkable)
  15. private Node[,] grid;
  16. public TerrainType[] walkableTerrains;
  17. private Dictionary<int, int> walkableRegionsDictionary = new Dictionary<int, int>();
  18. public Vector2 gridWorldSize;
  19. public float nodeRadius;
  20. private float nodeDiameter;
  21. public int obstacleProxPenalty = 0;
  22. private int gridSizeX, gridSizeY; //The dimensions of the grid
  23. private int penaltyMin = int.MaxValue;
  24. private int penaltyMax = int.MinValue;
  25. public int MaxSize => gridSizeX * gridSizeY;
  26. private void Awake()
  27. {
  28. //Get Diameter
  29. nodeDiameter = nodeRadius * 2;
  30. //Grid Size
  31. gridSizeX = Mathf.RoundToInt(gridWorldSize.x / nodeDiameter);
  32. gridSizeY = Mathf.RoundToInt(gridWorldSize.y / nodeDiameter);
  33. foreach (TerrainType type in walkableTerrains)
  34. {
  35. walkableMask |= type.terrainMask.value;
  36. walkableRegionsDictionary.Add((int)Mathf.Log(type.terrainMask.value, 2), type.terrainPenalty);
  37. }
  38. //Create the grid
  39. CreateGrid();
  40. }
  41. private void OnDrawGizmos()
  42. {
  43. Gizmos.DrawWireCube(transform.position, new Vector2(gridWorldSize.x, gridWorldSize.y));
  44. if (grid != null && displayGridGizmos)
  45. {
  46. foreach (Node node in grid)
  47. {
  48. Gizmos.color = Color.Lerp(Color.white, Color.red, Mathf.InverseLerp(penaltyMin, penaltyMax, node.movementPenalty));
  49. Gizmos.color = (node.walkable) ? Gizmos.color : Color.black;
  50. Gizmos.DrawCube(node.worldPos, Vector2.one * (nodeDiameter - 0.1f));
  51. }
  52. }
  53. }
  54. public Node NodeFromWorldPoint(Vector2 worldPosition)
  55. {
  56. // Calculate the percentage of the world position relative to the grid
  57. float percentX = (worldPosition.x - transform.position.x + gridWorldSize.x / 2) / gridWorldSize.x;
  58. float percentY = (worldPosition.y - transform.position.y + gridWorldSize.y / 2) / gridWorldSize.y;
  59. // Clamp the values before flooring them to avoid going out of bound
  60. int x = Mathf.FloorToInt(Mathf.Clamp(gridSizeX * percentX, 0, gridSizeX - 1));
  61. int y = Mathf.FloorToInt(Mathf.Clamp(gridSizeY * percentY, 0, gridSizeY - 1));
  62. // Return the appropriate node
  63. return grid[x, y];
  64. }
  65. private void BlurPenaltyMap(int blurSize)
  66. {
  67. int kernalSize = blurSize * 2 + 1;
  68. int kernelExtents = blurSize;
  69. int[,] penaltiesHorizontalPass = new int[gridSizeX, gridSizeY];
  70. int[,] penaltiesVerticalPass = new int[gridSizeX, gridSizeY];
  71. for (int y = 0; y < gridSizeY; y++)
  72. {
  73. for (int x = -kernelExtents; x <= kernelExtents; x++)
  74. {
  75. int sampleX = Mathf.Clamp(x, 0, kernelExtents);
  76. penaltiesHorizontalPass[0, y] += grid[sampleX, y].movementPenalty;
  77. }
  78. for (int x = 1; x < gridSizeX; x++)
  79. {
  80. int removeIndex = Mathf.Clamp(x - kernelExtents - 1, 0, gridSizeX);
  81. int addIndex = Mathf.Clamp(x + kernelExtents, 0, gridSizeX - 1);
  82. penaltiesHorizontalPass[x, y] = penaltiesHorizontalPass[x - 1, y] - grid[removeIndex, y].movementPenalty
  83. + grid[addIndex, y].movementPenalty;
  84. }
  85. }
  86. for (int x = 0; x < gridSizeX; x++)
  87. {
  88. for (int y = -kernelExtents; y <= kernelExtents; y++)
  89. {
  90. int sampleY = Mathf.Clamp(x, 0, kernelExtents);
  91. penaltiesVerticalPass[x, 0] += penaltiesHorizontalPass[x, sampleY];
  92. }
  93. int blurredPenalty = Mathf.RoundToInt((float)penaltiesVerticalPass[x, 0] / (kernalSize * kernalSize));
  94. grid[x, 0].movementPenalty = blurredPenalty;
  95. for (int y = 1; y < gridSizeY; y++)
  96. {
  97. int removeIndex = Mathf.Clamp(y - kernelExtents - 1, 0, gridSizeY);
  98. int addIndex = Mathf.Clamp(y + kernelExtents, 0, gridSizeY - 1);
  99. penaltiesVerticalPass[x, y] = penaltiesVerticalPass[x, y - 1] - penaltiesHorizontalPass[x, removeIndex]
  100. + penaltiesHorizontalPass[x, addIndex];
  101. blurredPenalty = Mathf.RoundToInt((float)penaltiesVerticalPass[x, y] / (kernalSize * kernalSize));
  102. grid[x, y].movementPenalty = blurredPenalty;
  103. if (blurredPenalty > penaltyMax)
  104. penaltyMax = blurredPenalty;
  105. if (blurredPenalty < penaltyMin)
  106. penaltyMin = blurredPenalty;
  107. }
  108. }
  109. }
  110. //Returns a list of Neighbours of a node
  111. public List<Node> GetNeighbours(Node node)
  112. {
  113. List<Node> neighbours = new List<Node>();
  114. for (int i = -1; i <= 1; i++)
  115. {
  116. for (int j = -1; j <= 1; j++)
  117. {
  118. if (i == 0 && j == 0) continue;
  119. int checkX = node.gridX + i;
  120. int checkY = node.gridY + j;
  121. if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY)
  122. neighbours.Add(grid[checkX, checkY]);
  123. }
  124. }
  125. return neighbours;
  126. }
  127. //Grid Creation
  128. private void CreateGrid()
  129. {
  130. grid = new Node[gridSizeX, gridSizeY];
  131. Vector2 worldBottomLeftCorner = (Vector2)transform.position - Vector2.right *
  132. gridWorldSize.x / 2 - Vector2.up * gridWorldSize.y / 2;
  133. for (int i = 0; i < gridSizeX; i++)
  134. {
  135. for (int j = 0; j < gridSizeY; j++)
  136. {
  137. Vector2 worldPoint = worldBottomLeftCorner + Vector2.right * (i * nodeDiameter + nodeRadius)
  138. + Vector2.up * (j * nodeDiameter + nodeRadius);
  139. bool walkable = (Physics2D.OverlapCircle(worldPoint, nodeRadius, unwalkableMask) == null);
  140. int movementPenalty = walkable ? 0 : obstacleProxPenalty;
  141. RaycastHit2D hit = Physics2D.CircleCast(worldPoint, nodeRadius, Vector2.up, nodeRadius, walkableMask);
  142. if (hit)
  143. {
  144. walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out int terrainPenalty);
  145. movementPenalty += terrainPenalty;
  146. }
  147. grid[i, j] = new Node(walkable, worldPoint, i, j, movementPenalty);
  148. }
  149. }
  150. BlurPenaltyMap(3);
  151. }
  152. [Serializable]
  153. public class TerrainType
  154. {
  155. public LayerMask terrainMask;
  156. public int terrainPenalty;
  157. }
  158. }
  159. }