Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- using UnityEngine;
- using System.Collections;
- using System.Collections.Generic;
- public class Pathfinding : MonoBehaviour {
- public Transform seeker, target;
- Grid grid;
- void Awake() {
- grid = GetComponent<Grid>();
- }
- void Update() {
- FindPath(seeker.position,target.position);
- }
- void FindPath(Vector3 startPos, Vector3 targetPos) {
- Node startNode = grid.NodeFromWorldPoint(startPos);
- Node targetNode = grid.NodeFromWorldPoint(targetPos);
- List<Node> openSet = new List<Node>();
- HashSet<Node> closedSet = new HashSet<Node>();
- openSet.Add(startNode);
- while (openSet.Count > 0) {
- Node currentNode = openSet[0];
- for (int i = 1; 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) {
- RetracePath(startNode,targetNode);
- return;
- }
- foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
- if (!neighbour.walkable || closedSet.Contains(neighbour)) {
- continue;
- }
- int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
- 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);
- }
- }
- }
- }
- void RetracePath(Node startNode, Node endNode) {
- List<Node> path = new List<Node>();
- Node currentNode = endNode;
- while (currentNode != startNode) {
- path.Add(currentNode);
- currentNode = currentNode.parent;
- }
- path.Reverse();
- grid.path = path;
- }
- int GetDistance(Node nodeA, Node nodeB) {
- int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
- int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
- if (dstX > dstY)
- return 14*dstY + 10* (dstX-dstY);
- return 14*dstX + 10 * (dstY-dstX);
- }
- }
- public class Grid : MonoBehaviour {
- public LayerMask unwalkableMask;
- public Vector2 gridWorldSize;
- public float nodeRadius;
- Node[,] grid;
- float nodeDiameter;
- int gridSizeX, gridSizeY;
- void Start() {
- nodeDiameter = nodeRadius*2;
- gridSizeX = Mathf.RoundToInt(gridWorldSize.x/nodeDiameter);
- gridSizeY = Mathf.RoundToInt(gridWorldSize.y/nodeDiameter);
- CreateGrid();
- }
- void CreateGrid() {
- grid = new Node[gridSizeX,gridSizeY];
- Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x/2 - Vector3.forward * 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.forward * (y * nodeDiameter + nodeRadius);
- bool walkable = !(Physics.CheckSphere(worldPoint,nodeRadius,unwalkableMask));
- grid[x,y] = new Node(walkable,worldPoint, x,y);
- }
- }
- }
- public List<Node> GetNeighbours(Node node) {
- List<Node> neighbours = new List<Node>();
- for (int x = -1; x <= 1; x++) {
- for (int y = -1; y <= 1; y++) {
- if (x == 0 && y == 0)
- continue;
- int checkX = node.gridX + x;
- int checkY = node.gridY + y;
- if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) {
- neighbours.Add(grid[checkX,checkY]);
- }
- }
- }
- return neighbours;
- }
- public Node NodeFromWorldPoint(Vector3 worldPosition) {
- float percentX = (worldPosition.x + gridWorldSize.x/2) / gridWorldSize.x;
- float percentY = (worldPosition.z + gridWorldSize.y/2) / gridWorldSize.y;
- percentX = Mathf.Clamp01(percentX);
- percentY = Mathf.Clamp01(percentY);
- int x = Mathf.RoundToInt((gridSizeX-1) * percentX);
- int y = Mathf.RoundToInt((gridSizeY-1) * percentY);
- return grid[x,y];
- }
- public List<Node> path;
- void OnDrawGizmos() {
- Gizmos.DrawWireCube(transform.position,new Vector3(gridWorldSize.x,1,gridWorldSize.y));
- if (grid != null) {
- foreach (Node n in grid) {
- Gizmos.color = (n.walkable)?Color.white:Color.red;
- if (path != null)
- if (path.Contains(n))
- Gizmos.color = Color.black;
- Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter-.1f));
- }
- }
- }
- }
- public class Node {
- public bool walkable;
- public Vector3 worldPosition;
- public int gridX;
- public int gridY;
- public int gCost;
- public int hCost;
- public Node parent;
- public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY) {
- walkable = _walkable;
- worldPosition = _worldPos;
- gridX = _gridX;
- gridY = _gridY;
- }
- public int fCost {
- get {
- return gCost + hCost;
- }
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement