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

//格子类型
public enum E_Type_Node
{
    //可行走区域
    walk,
    //不可走区域
    stop
}


//格子类
public class AstarNode
{
    //格子坐标
    public int x, y;

    public float f;//寻路消耗
    
    public float g;//起点距离消耗
    
    public float h;//终点距离消耗

    public AstarNode father;//父对象

    public E_Type_Node type;//格子类型

    public AstarNode(int x,int y,E_Type_Node type)
    {
        this. x = x;
        this.y = y;
        this.type = type;
    }
}

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
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
using System.Collections;
using System.Collections.Generic;
using UnityEngine;


public class Singletion<T> where T : class, new()
{
    static T ins;
    public static T Ins
    {
        get
        {
            if (ins == null)
            {
                ins = new T();
            }

            return ins;
        }
    }
}


// A星寻路管理器
public class AstarMgr : Singletion<AstarMgr>
{
    //所有格子对象
    public AstarNode[,] nodes;
    //打开
    private List<AstarNode> OpenList=new List<AstarNode>();
    //关闭
    private List<AstarNode> CloseList= new List<AstarNode>();

    //地图的 x,y 宽高
    private int Mapw;
    private int Maph;
    public void InitMapInfo(int w, int h)
    {
        this.Mapw = w;
        this.Maph = h;

        //声明容器 的长度
        nodes = new AstarNode[w, h];

        AstarNode node;
        for (int i = 0; i < w; i++)
        {
            for (int j = 0; j < h; j++)
            {
                //只是为了测试
                node = new AstarNode(i, j, Random.Range(0, 100) < 20 ? E_Type_Node.stop : E_Type_Node.walk);
                nodes[i, j] = node;
            }
        }
    }

    /// <summary>
    /// 寻找路径
    /// </summary>
    /// <param name="startPos">开始坐标</param>
    /// <param name="endPos">结束坐标 </param>
    /// <returns></returns>
    public List<AstarNode> FindPath(Vector2 startPos, Vector2 endPos)
    {
        //1.是否在地图范围内
        if (startPos.x < 0 || startPos.x >= Mapw || startPos.y < 0 || startPos.y >= Maph || endPos.x < 0 || endPos.x >= Mapw || endPos.y < 0 || endPos.y >= Maph)
        {
            Debug.Log("不在地图范围内");
            return null;
        }

        AstarNode start = nodes[(int)startPos.x, (int)startPos.y];
        AstarNode end = nodes[(int)endPos.x, (int)endPos.y];
        //2.判断格子是否阻挡
        if (start.type == E_Type_Node.stop || end.type == E_Type_Node.stop)
        {
            Debug.Log("开始或结束点是阻挡");
            return null;
        }

        //清空上一次寻路数据
        // 清空关闭和寻路列表
        OpenList.Clear();
        CloseList.Clear();

        // 把开始点放入关闭列表中
        start.father = null;
        start.f = 0;
        start.g = 0;
        start.h = 0;
        CloseList.Add(start);

        while (true)
        {
            //从起点开始找周围的点并放入开启列表中
            ///左上x -1 y - 1
            FindNearNodeToOpenList(start.x - 1, start.y - 1, 1.4f, start, end);
            //上 x y-1
            FindNearNodeToOpenList(start.x, start.y - 1, 1, start, end);
            //右上 x+1,y-11
            FindNearNodeToOpenList(start.x + 1, start.y - 1, 1.4f, start, end);
            //左 x-1,y
            FindNearNodeToOpenList(start.x - 1, start.y, 1f, start, end);
            //右 x,y-1;
            FindNearNodeToOpenList(start.x + 1, start.y, 1f, start, end);
            //左下 x-1,y+1
            FindNearNodeToOpenList(start.x - 1, start.y + 1, 1.4f, start, end);
            //下 x,y+1
            FindNearNodeToOpenList(start.x, start.y + 1, 1, start, end);
            //右下 x+1,y+1
            FindNearNodeToOpenList(start.x + 1, start.y + 1, 1.4f, start, end);

            //死路判断 开启列表为空 就是死路
            if (OpenList.Count == 0)
            {
                Debug.Log("死路");
                return null;
            }

            // 选择 寻路消耗最小的点
            OpenList.Sort(SotrOpenList);

            //放入关闭列表中
            CloseList.Add(OpenList[0]);
            start = OpenList[0];
            OpenList.RemoveAt(0);

            if (start == end)
            {
                //找到重点
                List<AstarNode> path = new List<AstarNode>();
                path.Add(end);
                while (end.father!=null)
                {
                    path.Add(end.father);
                    end = end.father;
                }

                //反转
                path.Reverse();
                return path;
            }
          
        }


    }

    private int SotrOpenList(AstarNode a, AstarNode b)
    {
        if (a.f > b.f)
        {
            return 1;
        }
        else
         return -1; 
    }


    /// <summary>
    /// 计算临近点 是否可以放入Open List
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <param name="g"></param>
    /// <param name="father"></param>
    private void FindNearNodeToOpenList(int x, int y, float g, AstarNode father, AstarNode end)
    {
        //判断边界
        if (x < 0 || x >= Mapw || y < 0 || y >= Maph)
        {
            Debug.Log("不在地图范围内");
            return;
        }

        //在范围内 取点
        AstarNode node = nodes[x, y];
        if (node == null || node.type == E_Type_Node.stop||CloseList.Contains(node)||OpenList.Contains(node))
        {
            return;
        }
        //计算寻路消耗


        //记录父对象
        node.father = father;

        //计算g 我的开始距离= 我父亲的距离 +我的距离
        node.g = father.g = g;

        node.h = Mathf.Abs(end.x - node.x) + Mathf.Abs(end.y - node.y);
        node.f = node.g + node.h;
        //通过了上面 的验证，存入开启列表
        OpenList.Add(node);
    }
}

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
	using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEngine.Events;

public class NavAgent : MonoBehaviour
{
    //速度
    private float speed;
    //路径的集合
    private Vector3[] roads;
    // 回掉
    private UnityAction endFunc;
    //当前路径点下标
    private int nextIndex = 0;


    private float totalTime;
    private float passedTime;
    private float vx, vy, vz;
    private bool isWalking;


    private Quaternion dstRot;
    public void Init() {
        
    }

    public void NavOnRoad(float speed, Vector3[] roads, UnityAction endFunc) {
        // step1: 准备数据，验证数据的合法性;
        this.speed = speed;
        this.roads = roads;
        this.endFunc = endFunc;

        if (this.roads.Length < 2) { 
            return;
        }

        // step1: 将我们的角色放到起点，调整好初始方向
        this.transform.position = this.roads[0];
        this.transform.LookAt(this.roads[1]);
        // end

        // step2: 下一个点数据相关
        this.nextIndex = 1;
        this.WalkToNext();
        // end
    }

    // 走向nextIndex
    private void WalkToNext(){
        if (this.nextIndex >= this.roads.Length) { // 走到了尽头;
            this.isWalking = false;
            if (this.endFunc != null) {
                this.endFunc();
            }
            return;
        }

        // 从当前点走到下一个点
        Vector3 src = this.transform.position;
        Vector3 dst = this.roads[this.nextIndex];
        Vector3 dir = dst - src;
        float len = dir.magnitude;
        if (len <= 0) {
            this.nextIndex++;
            this.WalkToNext();
            return;
        }

        this.totalTime = len / this.speed;
        this.passedTime = 0;
        // end

        // 分解速度
        this.vx = this.speed * dir.x / len;
        this.vy = this.speed * dir.y / len;
        this.vz = this.speed * dir.z / len;
        // end

        // 调整角色的朝向
        Quaternion old = this.transform.rotation;
        this.transform.LookAt(dst);
        this.dstRot = this.transform.rotation;
        this.transform.rotation = old;
        // end

        this.isWalking = true;
    }
    public void StopNav() { 
    }

    public void GoAheadOnRoad() { 
    }

    private void Update() {
        if (this.isWalking == false) {
            return;
        }

        // 更新移动的时间
        float dt = Time.deltaTime;
        this.passedTime += dt;
        if(this.passedTime > this.totalTime) {
            dt -= (this.passedTime - this.totalTime);
        }
        // end

        // 每次Update更新位置
        Vector3 pos = this.transform.position;
        pos.x += this.vx * dt;
        pos.y += this.vy * dt;
        pos.z += this.vz * dt;
        this.transform.position = pos;

        // 转向插值
        this.transform.rotation = Quaternion.Slerp(this.transform.rotation, this.dstRot, 8.0f * dt);
        // end

        if (this.passedTime >= this.totalTime) {
            this.nextIndex++;
            this.WalkToNext();
        }
    }
}