Scriptum/Assets/Scripts/Pathfinding/Pathfinding.cs

89 lines
2.7 KiB
C#
Raw Normal View History

2022-11-07 22:18:28 +01:00
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;
}
}