Unity--Floyd画出最短的路径

1，前言

给定一个场景：

• 有两个点：A和B
• 若干个辅助点
• 一些障碍物（层级为WallLayer，标签为Wall）

在辅助点的帮助下，将A和B用线连起来，同时画出来的线不能穿过墙壁。求怎么画才能画出最短的路线

2，脚本实现

• person代表A点
• target代表B点
• secondaryParent代表所有辅助点的父物体
• 所有的障碍物，其层级为WallLayer，且障碍物的标签为Wall

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

/// <summary>
/// 根据Floyd算法画出最短路线
/// </summary>
public class AutoFindRoadByFloyd : MonoBehaviour
{
    //人物
    public Transform person;

    //辅助点集合
    public Transform secondaryParent;

    //目标物体
    public Transform target;

    //所有点的集合
    private List<Transform> list = new List<Transform>();

    //射线监测碰到的层【若不设置这个，就有概率发生 当射线穿过辅助点再穿过墙壁 。
    //              因为射线检测碰撞墙壁会检测第一个碰到的物体,当先穿过辅助点，就会被认为这条射线是合理的，即便后续它会穿过墙壁
    //              注意：这里的辅助点是默认为Cube，是否添加这个，视具体情况而定】
    private int targetLayerMask;

    //距离矩阵，Floyd算法核心之一
    private float[,] dis;

    //路径矩阵，FLoyd算法核心之一
    private int[,] path;

    //所有点的数量
    private int length;

    //当得出最短路径有哪些点之后，将其添加到这里，统一画图
    private List<Transform> result = new List<Transform>();

    //线的宽度
    public float lineWidth;

    // Start is called before the first frame update
    void Start()
    {
        //初始化射线检测的层
        targetLayerMask = LayerMask.GetMask("WallLayer");
        
        //初始化所有点的先渲染其设置
        InitAllPoints();
    }

    // Update is called once per frame
    void Update()
    {
        //寻找最短路径，并画线
        FloydWrap();
    }

    /// <summary>
    /// Floyd的包装方法，在这里进行画线前的参数初始化，并进行画线
    /// </summary>
    public void FloydWrap()
    {
        //初始化矩阵
        InitMatrix();

        //重置所有点的线，防止路径改变后，某些点还在画线
        ResetAllPoints();

        //重置路径数组
        result = new List<Transform>();

        //Floyd算法，得出最短路径
        Floyd();

        //将最短路径加入结果数组中，FindFloydPath有可能发生异常，因为当不形成通路时，会报数组下标越界
        try
        {
            result.Add(person);
            FindFloydPath(0, length - 1);
            result.Add(target);
        }
        catch (Exception e)
        {
            //若无法形成通路，在这里输出，或者执行相应的方法
            Debug.Log("当前没有无法形成通路，请重新再试");
        }

        //执行画线的方法
        DrawLine();
    }


    /// <summary>
    /// 初始化矩阵
    /// </summary>
    public void InitMatrix()
    {
        //获取所有点的数量
        length = list.Count;

        //根据点的数量，创建一个【距离矩阵】，每个矩阵点的数值有三种情况===》
        // 0：代表自身到自身
        //  float.MaxValue：代表两个点之间无法相连（中间有墙壁阻隔）
        // 其他情况：代表两个点可以相连，数值为距离
        dis = new float[length, length];

        //创建一个【路径矩阵】，这个矩阵代表着从A点到B点要经过哪些点
        path = new int[length, length];

        //初始化【距离矩阵】和【路径矩阵】
        for (int i = 0; i < length; i++)
        {
            for (int j = 0; j < length; j++)
            {
                //路径矩阵 每个点初始化为-1
                path[i, j] = -1;

                //当点到自身的时候，值为0
                if (i == j)
                {
                    dis[i, j] = 0;
                    continue;
                }

                //判断两点之间是否隔着墙
                if (CheckCollideWall(list[i], list[j]))
                {
                    //若没有隔墙，则将两点距离进行赋值
                    dis[i, j] = Vector3.Distance(list[i].position, list[j].position);
                }
                else
                {
                    //若隔着墙，则赋予 float.MaxValue
                    dis[i, j] = float.MaxValue;
                }
            }
        }
    }

    /// <summary>
    /// 执行Floyd算法，修改dis和path矩阵
    /// </summary>
    public void Floyd()
    {
        for (int k = 0; k < length; k++)
        {
            for (int i = 0; i < length; i++)
            {
                for (int j = 0; j < length; j++)
                {
                    if (dis[i, j] > dis[i, k] + dis[k, j])
                    {
                        dis[i, j] = dis[i, k] + dis[k, j];
                        path[i, j] = k;
                    }
                }
            }
        }
    }

    /// <summary>
    /// 通过【路径矩阵】获取最短路径
    ///  path矩阵
    ///     (0) (1) (2) (3) (4)
    /// (0) -1   2  -1   2   3   
    /// (1) -1  -1  -1  -1   3
    /// (2) -1  -1  -1   1   3
    /// (3) -1  -1  -1  -1  -1
    /// (4) -1  -1  -1  -1  -1 
    ///      
    ///     以找 0-4 之间的最短路径为例：
    ///         先找path[0,4],其值为3，代表0与4之间存在一个中间节点3
    ///             接下来就分为两部分：【0-3】 与 【3-4】，查看这两部分之间是否存在中间节点
    ///                 找path[0,3],其值为2，代表存在一个中间节点 2
    ///                     接下来分为两部分：【0-2】 与 【2-3】，查看这两部分之间是否存在中间节点
    ///                         找path[0,2],其值为-1，代表不存在中间节点
    ///                         找path[2.3],其值为1，代表存在一个中间节点1
    ///                             接下来分为两部分：【2，1】 与 【1，3】，查看这两部分之间是否存在中间节点
    ///                                 找path[2,1],其值为-1，代表不存在中间节点
    ///                                 找path[1,3],其值为-1.代表不存在中间节点
    ///                 找path[3,4],其值为-1，代表不存在中间节点
    ///     综上所述，可以得出0-4之间的最短路径序列为：
    ///         0-》2-》1-》3-》4
    /// </summary>
    /// <param name="st"></param>
    /// <param name="ed"></param>
    void FindPath(int st, int ed)
    {
        if (dis[st, ed] < float.MaxValue && path[st, ed] == (-1))
        {
            result.Add(list[ed]);
            // Debug.Log("->>" + list[ed].name);
        }
        else
        {
            int mid = path[st, ed];
            FindPath(st, mid);
            FindPath(mid, ed);
        }
    }

    /// <summary>
    /// FindPath的包装方法，可以通过这，递归调用FindPath()
    /// </summary>
    /// <param name="st"></param>
    /// <param name="ed"></param>
    void FindFloydPath(int st, int ed)
    {
        // Debug.Log("最短路径" + st + "-" + ed + "-" + st);
        FindPath(st, ed);
    }

    /// <summary>
    /// 根据结果数组进行画线
    /// </summary>
    public void DrawLine()
    {
        LineRenderer component;
        for (int i = 0; i < result.Count - 1; i++)
        {
            component = result[i].GetComponent<LineRenderer>();
            component.SetPosition(0, result[i].position);
            component.SetPosition(1, result[i + 1].position);
        }
    }

    public void InitAllPoints()
    {
        LineRenderer component;
        
        //初始化所有点集合，并添加线渲染器
        person.gameObject.AddComponent<LineRenderer>();
        component = person.GetComponent<LineRenderer>();
        component.startWidth = lineWidth;
        component.endWidth = lineWidth;
        component.material = lineMaterial;
        component.textureMode = LineTextureMode.Tile;
        
        list.Add(person);
        foreach (Transform o in secondaryParent)
        {
            o.gameObject.AddComponent<LineRenderer>();
            component = o.GetComponent<LineRenderer>();
            component.startWidth = lineWidth;
            component.endWidth = lineWidth;
            component.material = lineMaterial;
            component.textureMode = LineTextureMode.Tile;
            list.Add(o);
        }

        target.gameObject.AddComponent<LineRenderer>();
        component = target.GetComponent<LineRenderer>();
        component.startWidth = lineWidth;
        component.endWidth = lineWidth;
        component.material = lineMaterial;
        component.textureMode = LineTextureMode.Tile;
        
        list.Add(target);
    }

    /// <summary>
    /// 重置所有点的线渲染器的位置数据，以达到删除线的效果
    /// </summary>
    public void ResetAllPoints()
    {
        foreach (Transform o in list)
        {
            LineRenderer lineRenderer = o.GetComponent<LineRenderer>();
            lineRenderer.SetPosition(0, o.position);
            lineRenderer.SetPosition(1, o.position);
        }
    }

    /// <summary>
    /// 判断是否撞墙了
    /// </summary>
    /// <param name="from"></param>
    /// <param name="to"></param>
    /// <returns></returns>
    public bool CheckCollideWall(Transform from, Transform to)
    {
        //初始化射线设置
        Ray ray = new Ray(@from.position, (to.position - @from.position).normalized);
        RaycastHit hit;
        //射线只会与 targetLayerMask指定的Layer发生碰撞检测
        if (Physics.Raycast(ray, out hit, Vector3.Distance(from.position, to.position), targetLayerMask))
        {
            //碰到墙
            if (hit.transform.tag.Equals("Wall"))
            {
                return false;
            }
        }

        //未碰到墙
        return true;
    }

}