90 lines
2.7 KiB
C#
90 lines
2.7 KiB
C#
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
public class Pathfinding : MonoBehaviour
|
|
{
|
|
// public Transform Player;
|
|
|
|
public NodeMap Map;
|
|
// Start is called before the first frame update
|
|
void Start()
|
|
{
|
|
//Player = FindObjectOfType<Player>().transform;
|
|
}
|
|
|
|
// Update is called once per frame
|
|
void Update()
|
|
{
|
|
|
|
}
|
|
|
|
public void FindPath(Vector3 startPos, Vector3 targetPos, AStarPathfindingAgent _agent) {
|
|
Node startNode = Map.NodeFromWorldPoint(startPos);
|
|
Node targetNode = Map.NodeFromWorldPoint(targetPos);
|
|
|
|
List<Node> openSet = new List<Node>();
|
|
HashSet<Node> closedSet = new HashSet<Node>();
|
|
openSet.Add(startNode);
|
|
|
|
while (openSet.Count > 0) {
|
|
Node node = openSet[0];
|
|
|
|
for (int i = 1; i < openSet.Count; i ++) {
|
|
if (openSet[i].FCost < node.FCost || openSet[i].FCost == node.FCost) {
|
|
if (openSet[i].hCost < node.hCost)
|
|
node = openSet[i];
|
|
}
|
|
}
|
|
|
|
openSet.Remove(node);
|
|
closedSet.Add(node);
|
|
|
|
if (node == targetNode) {
|
|
RetracePath(startNode, targetNode, _agent);
|
|
return;
|
|
}
|
|
|
|
foreach (Node neighbour in Map.GetNeighbours(node)) {
|
|
if (!neighbour.walkable || closedSet.Contains(neighbour)) {
|
|
continue;
|
|
}
|
|
|
|
int newCostToNeighbour = node.GCost + GetDistance(node, neighbour);
|
|
if (newCostToNeighbour < neighbour.GCost || !openSet.Contains(neighbour)) {
|
|
neighbour.GCost = newCostToNeighbour;
|
|
neighbour.hCost = GetDistance(neighbour, targetNode);
|
|
neighbour.parent = node;
|
|
|
|
if (!openSet.Contains(neighbour))
|
|
openSet.Add(neighbour);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int GetDistance(Node nodeA, Node nodeB) {
|
|
int dstX = Mathf.Abs(nodeA.gridPosition.x - nodeB.gridPosition.x);
|
|
int dstY = Mathf.Abs(nodeA.gridPosition.y - nodeB.gridPosition.y);
|
|
|
|
if (dstX > dstY)
|
|
return 14*dstY + 10* (dstX-dstY);
|
|
return 14*dstX + 10 * (dstY-dstX);
|
|
}
|
|
|
|
void RetracePath(Node startNode, Node endNode, AStarPathfindingAgent agent) {
|
|
List<Node> path = new List<Node>();
|
|
Node currentNode = endNode;
|
|
|
|
while (currentNode != startNode) {
|
|
path.Add(currentNode);
|
|
currentNode = currentNode.parent;
|
|
}
|
|
path.Reverse();
|
|
//Debug.Log("I am retracing the path");
|
|
agent.path = path;
|
|
|
|
}
|
|
|
|
}
|