-
Notifications
You must be signed in to change notification settings - Fork 2
/
Pathfinding.cs
145 lines (126 loc) · 5.47 KB
/
Pathfinding.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
using UnityEngine;
using System.Collections.Generic;
namespace StartFramework.GamePlay.Astar
{
public class Pathfinding : MonoBehaviour
{
public Transform seeker, target;
IGrid grid;
//点击寻路
private int currentPathIndex;
void Awake()
{
grid = GetComponent<IGrid>();
}
private void Start()
{
//FindPath(seeker.position, target.position);
}
void Update()
{
if (Input.GetMouseButtonDown(1))
{
//还原
currentPathIndex = 0;
FindPath(seeker.position, Camera.main.ScreenToWorldPoint(Input.mousePosition));
}
if (grid.path != null && grid.path.Count > 0)
{
Vector3 targetPosition = grid.path[currentPathIndex].worldPosition;
if (Vector3.Distance(seeker.transform.position, targetPosition) > 0.1f)
{
Vector3 moveDir = (targetPosition - seeker.transform.position).normalized;
seeker.transform.position = seeker.transform.position + moveDir * 6f * Time.deltaTime;
}
else
{
currentPathIndex++;
if (currentPathIndex >= grid.path.Count)
{
//校准和最后一个点的位置
seeker.transform.position = grid.path[currentPathIndex - 1].worldPosition;
//还原
grid.path.Clear();
grid.path = null;
currentPathIndex = 0;
}
}
}
}
void FindPath(Vector3 startPos, Vector3 targetPos)
{
Node startNode = grid.NodeFromWorldPoint(startPos);
Node targetNode = grid.NodeFromWorldPoint(targetPos);
List<Node> openSet = new List<Node>(); //open列表:等待评估
//哈希集
//优点:提供高性能的set操作。
//特点:不包含重复元素,无特定顺序,无索引。
//ps:如果顺序或元素复制比应用程序的性能更重要,请考虑结合使用 List<T> 类和 Sort 方法。
HashSet<Node> closedSet = new HashSet<Node>(); //close列表:评估完毕
openSet.Add(startNode);
while (openSet.Count > 0)
{
//open列表中的成员进行比对,找出一个最佳节点
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);
return;
}
//找邻居 → 根据当前节点重新计算邻居的代价 → 将所有邻居放入open列表
foreach (Node neighbour in grid.GetNeighbours(node))
{
if (!neighbour.walkable || closedSet.Contains(neighbour))
{
continue;
}
//***根据当前节点刷新邻居的g代价
//只有新的代价小于之前的代价 才刷新邻居的g代价值
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);
}
}
}
}
//回溯:根据终点向回找父节点
//如何得出的这个最佳路径:
//--找邻居→评估代价→更新当前节点→找邻居刷新值→循环,直到找到了终点,然后依据终点的父节点回溯出路径。
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);
}
}
}