【unity】navmesh很远的点寻路不到的处理（迭代寻路）

项目里，有个场景比较绕，比较远的目标寻路只会寻路到一半的地方就停下来， 终点是可达的，但unity的navmesh寻路算法本身因素，遍历深度有限制造成的。 
（c++实现的recastnavigation 也有这个问题， dtNavMeshQuery::init(const dtNavMesh* nav, const int maxNodes) maxNodes 决定了寻路算法时最多的寻路缓存节点数，太多会慢，太少有些绕路的大寻不到。）

1. 一种思路是寻路对象那边处理， 遇到寻路有结果但不能到达终点(寻路得到的NavMeshPath status一般是 PathPartial的情况）， 可以定时或者快到本次路径的终点时再触发一次寻路。

2.方案2，改一些逻辑比较多或者比较麻烦的时候，且不介意中途绕路的情况，则可采用迭代寻路的方案， 就是寻路的接口自己再封装一下： 没法直接寻路到终点，把本次得到的最后一个路点当起点，继续寻路，然后把若干段路径拼接起来。
这种方案的问题就是 中间的点很可能走到一个沟沟里，然后又跑出来。。。

unity 这边目前实现的代码：
 

/// 

/// 可以拼接的寻路路点
/// 
public class NavMeshLongPath
{
    public NavMeshLongPath()
    {
        m_pcorners = m_listCorners.ToArray();
    }
    ~NavMeshLongPath() { }

    /// 

    /// Erase all corner points from path.
    /// 
    public void ClearCorners()
    {
        m_listCorners.Clear();
        m_pcorners = m_listCorners.ToArray();
    }

    public void MergeFrom(Vector3[] otherCorners, int startIndex = 0)
    {
        if (startIndex > 0)
        {
            for (int i = startIndex; i < otherCorners.Length; ++i)
            {
                m_listCorners.Add(otherCorners[i]);
            }
        }
        else
        {
            m_listCorners.AddRange(otherCorners);
        }
        m_pcorners = m_listCorners.ToArray();
    }

    public Vector3[] corners
    {
        get
        {
            return m_pcorners;
        }
    }
    public NavMeshPathStatus status = NavMeshPathStatus.PathInvalid;
    protected List m_listCorners = new List();
    protected Vector3[] m_pcorners = null;
}






// 另外封装一个函数替换原本调用 NavMesh.CalculatePath的地方
    public static bool CalculateLongPath(Vector3 sourcePosition, Vector3 targetPosition, int areaMask, NavMeshLongPath path)
    {
        //if(null == path)
        //{
        //    path = new NavMeshLongPath();
        //}
        NavMeshPath subPath = new NavMeshPath();
        // 需要把终点作为起点再次寻路
        Vector3 newStart = sourcePosition;
        int loopCount = 0;  // 防止意外死循环
        while (loopCount++ < 5)
        {
            subPath.ClearCorners();
            if (NavMesh.CalculatePath(newStart, targetPosition, areaMask, subPath))
            {
                path.status = subPath.status;   // 状态用最后一个
                if (subPath.corners.Length > 1)
                {
                    if (subPath.corners[0] == subPath.corners[subPath.corners.Length - 1])
                    {
                        // 寻路到的还是这个点本身，则应该结束
                        if (1 == loopCount)
                        {
                            path.MergeFrom(subPath.corners);
                        }
                        break;
                    }
                    else
                    {
                        // 把剩余的点合并到path里
                        path.MergeFrom(subPath.corners, (1 == loopCount ? 0 : 1));
                        if (NavMeshPathStatus.PathComplete == subPath.status)
                        {
                            // 寻路结束
                            break;
                        }
                        else
                        {
                            // 还是没寻路到终点则继续找一遍
                            newStart = subPath.corners[subPath.corners.Length - 1];
                        }
                    }
                }
                else
                {
                    // 寻路到的还是这个点本身，则应该结束
                    if (1 == loopCount)
                    {
                        path.MergeFrom(subPath.corners);
                    }
                    break;
                }
            }
            else
            {
                if (1 == loopCount)
                    // 第一次就寻路失败则返回false，否则按理不需要
                    return false;
                else
                    break;
            }
        }
        return true;
    }

C++ 另外一个项目的实现，仅供参考， 思路差不多的，寻路发现未达终点，尝试若干次迭代， 如果得到的路点只有1个 或者 终点就是起点 或者 寻路失败，则说明没法继续寻路停止迭代。

 

bool CPathFinding::findPath(dtNavMeshQuery* navQuery, const MapPos &posStart, const MapPos &posEnd, vecPbClientPath &vecPbPath) {
    if(nullptr == navQuery) {
        LOG_E("navQuery is null");
        return false;
    }

    float straight_path[(MAX_POLYS + 1) * 3];
    static unsigned char straight_pathflags[MAX_POLYS];
    static dtPolyRef straight_path_polys[MAX_POLYS];
    int straight_path_num = 0;
    static PbcPosition clientPos;
    // 查询的次数
    int32 n32Count = 0;
    auto _posStart = posStart;
    bool bNeedReverse = false;
    const int32 N32_MAX_TRY_NUM = 8;
    // 是否需要循环继续下一个查询
    bool bNotEnd = false;
    while(n32Count < N32_MAX_TRY_NUM) {
        ++n32Count;
        bNotEnd = false;
        if (!CPathFinding::findPathInner(navQuery, _posStart, posEnd, straight_path_num, straight_path, straight_pathflags, straight_path_polys, n32Count, bNotEnd)) {
            return false;
        }

        if (straight_path_num > 0) {
            for (int32_t i = 0; i < straight_path_num; ++i) {
                if (n32Count > 1 && 0 == i) {
                    // 重新寻路的第一个点跳过
                    continue;
                }
                clientPos.set_x((int32_t)(straight_path[i * 3] * 100));
                clientPos.set_y((int32_t)(straight_path[i * 3 + 2] * 100));
                *vecPbPath.Add() = clientPos;
            }

            if (bNotEnd) {
                if (1 == straight_path_num) {
                    bNeedReverse = true;            // 有可能虽然两个点很近但绕了很远的路， 可以尝试反向寻路，或许能成功。 如果都不行, 则就认命了。
                    break;
                }
                // 一次没有寻路完，则多段寻路
                MapPos posLast = MapPos((int32_t)(straight_path[(straight_path_num - 1) * 3] * 100), (int32_t)(straight_path[(straight_path_num - 1) * 3 + 2] * 100));
                _posStart = posLast;
            } else {
                break;
            }
        } else {
            break;
        }
    }

    if (bNotEnd && N32_MAX_TRY_NUM <= n32Count) {
        // 没找到最终的点？尝试反向
        bNeedReverse = true;
    }

    if (bNeedReverse) {
        bNeedReverse = false;
        _posStart = posEnd;
        auto _posEnd = posStart;
        vector vecRPos;
        straight_path_num = 0;
        n32Count = 0;
        while (n32Count < N32_MAX_TRY_NUM) {
            ++n32Count;
            bNotEnd = false;
            if (!CPathFinding::findPathInner(navQuery, _posStart, _posEnd, straight_path_num, straight_path, straight_pathflags, straight_path_polys, n32Count, bNotEnd)) {
                return false;
            }

            if (straight_path_num > 0) {
                for (int32_t i = 0; i < straight_path_num; ++i) {
                    if (n32Count > 1 && 0 == i) {
                        // 重新寻路的第一个点跳过
                        continue;
                    }
                    clientPos.set_x((int32_t)(straight_path[i * 3] * 100));
                    clientPos.set_y((int32_t)(straight_path[i * 3 + 2] * 100));
                    vecRPos.emplace_back(clientPos);
                }

                if (bNotEnd) {
                    if (1 == straight_path_num) {
                        bNeedReverse = true;            // 还是寻不到，则按失败处理， vecPbPath 仍旧存了之前寻路结果。
                        break;
                    }
                    // 一次没有寻路完，则多段寻路
                    MapPos posLast = MapPos((int32_t)(straight_path[(straight_path_num - 1) * 3] * 100), (int32_t)(straight_path[(straight_path_num - 1) * 3 + 2] * 100));
                    _posStart = posLast;
                }
                else {
                    break;
                }
            }
            else {
                break;
            }
        }

        if (!bNeedReverse && vecRPos.size() > 0) {
            if (bNotEnd && N32_MAX_TRY_NUM <= n32Count) {
                // 还是没有找到完整路径，则不覆盖
            } else {
                // 没有出现问题， 用反向寻路出来的数据，返回
                vecPbPath.Clear();
                int n32Size = static_cast(vecRPos.size());
                vecPbPath.Reserve(n32Size);
                for(int i = n32Size - 1; i >= 0; --i) {
                    *vecPbPath.Add() = vecRPos[i];
                }
            }
        }

    }

    return true;

}



bool CPathFinding::findPathInner(dtNavMeshQuery* navQuery
    , const MapPos& start
    , const MapPos& end
    , int& m_nstraightPath
    , float* m_straightPath
    , unsigned char* m_straightPathFlags
    , dtPolyRef* m_straightPathPolys
    , int32 n32Round
    , bool& bNotEnd) {

    if(nullptr == navQuery) {
        LOG_E("navQuery is null");
        return false;
    }
    if(nullptr == m_straightPath) {
        LOG_E("m_straightPath is null");
        return false;
    }

    if(nullptr == m_straightPathFlags) {
        LOG_E("m_straightPathFlags is null");
        return false;

    }
    if(nullptr == m_straightPathPolys) {
        LOG_E("m_straim_straightPathPolysghtPath is null");
        return false;
    }

    bNotEnd = false;

    dtPolyRef m_startRef;
    dtPolyRef m_endRef;

    float m_spos[3];
    float m_epos[3];
    m_spos[0] = start.n32X / 100.0f;
    m_spos[1] = 0.0f;  // 0 在 0.0f 1 在10.0f
    m_spos[2] = start.n32Y / 100.0f;
    m_epos[0] = end.n32X / 100.0f;
    m_epos[1] = 0.0f;
    m_epos[2] = end.n32Y / 100.0f;

    dtQueryFilter m_filter;
    // m_filter.setIncludeFlags(0xffff^0x10);
    // m_filter.setIncludeFlags(m_scene->get_nav_query_filter_include_flags());
    m_filter.setExcludeFlags(SAMPLE_POLYFLAGS_DISABLED);

    float m_polyPickExt[3];
    m_polyPickExt[0] = 25.6f;       // 不可走区域 容错 10x10格子左右
    m_polyPickExt[1] = 1;
    m_polyPickExt[2] = 25.6f;

    dtPolyRef m_polys[MAX_POLYS];
    int m_npolys = 0;


    static float nearestPt[3];
    bool isOverPoly = false;
    navQuery->findNearestPoly(m_epos, m_polyPickExt, &m_filter, &m_endRef, nearestPt, &isOverPoly);
    if (!isOverPoly) {
        // 终点在障碍物里， 则Ref 必须取靠近spos的
        float fDist = dtVdist2D(m_epos, m_spos);
        if (fDist > 0) {
            float npos[3];
            float t = std::fmin(1.0f, 0.64f / fDist);   // 1.28 偏移可能会穿墙 所以改0.64偏移
            dtVlerp(npos, m_epos, m_spos, t);
            navQuery->findNearestPoly(npos, m_polyPickExt, &m_filter, &m_endRef, m_epos);   // 刷新 m_epos点
        }
    }

    bool isStartOverPoly = false;     // 点是否在多边形里，否的话说明点在障碍物里
    navQuery->findNearestPoly(m_spos, m_polyPickExt, &m_filter, &m_startRef, nearestPt, &isStartOverPoly);
    if (!isStartOverPoly && 1 == n32Round) {
        // 起点在障碍物里， 则Ref 必须取靠近epos的. 注：只有初始时要这么处理，若循环的第二次就不需要
        float fDist = dtVdist2D(m_epos, m_spos);
        if (fDist > 0) {
            float npos[3];
            float t = std::fmin(1.0f, 0.64f / fDist);   // 1.28 偏移可能会穿墙 所以改0.64偏移
            dtVlerp(npos, m_spos, m_epos, t);
            navQuery->findNearestPoly(npos, m_polyPickExt, &m_filter, &m_startRef, m_spos); // 刷新 m_spos点
        }
    }

    {
        // 直线可达的优化
        float t = 0;
        float hitNormal[3];// , hitPos[3];
        navQuery->raycast(m_startRef, m_spos, m_epos, &m_filter, &t, hitNormal, m_polys, &m_npolys, MAX_POLYS);
        if (t > 1) {
            // No hit
            // dtVcopy(hitPos, m_epos);
            m_nstraightPath = 2;
            m_straightPath[0] = m_spos[0];
            m_straightPath[1] = m_spos[1];
            m_straightPath[2] = m_spos[2];
            m_straightPath[3] = m_epos[0];
            m_straightPath[4] = m_epos[1];
            m_straightPath[5] = m_epos[2];
            return true;
        }
        else {
            // Hit, 但是射线的终点就是目标的polyid, 则说明也能直线到，
            if (m_npolys > 1 && m_endRef == m_polys[m_npolys - 1]) {
                m_nstraightPath = 2;
                m_straightPath[0] = m_spos[0];
                m_straightPath[1] = m_spos[1];
                m_straightPath[2] = m_spos[2];
                dtVlerp(&m_straightPath[3], m_spos, m_epos, t);
                return true;
            }
        }
    }

    auto nDtStatus = navQuery->findPath(m_startRef, m_endRef, m_spos, m_epos, &m_filter, m_polys, &m_npolys, MAX_POLYS);
    m_nstraightPath = 0;
    if (m_npolys)
    {
        // In case of partial path, make sure the end point is clamped to the last polygon.
        float epos[3];
        dtVcopy(epos, m_epos);
        if (m_polys[m_npolys - 1] != m_endRef && m_npolys > 1) {
            // 寻路出来的路径没到终点，  且 非 (DT_SUCCESS & DT_PARTIAL_RESULT)  而是  (DT_SUCCESS & DT_PARTIAL_RESULT & DT_OUT_OF_NODES) 则后面需接上寻路
            bNotEnd = true;
            navQuery->closestPointOnPoly(m_polys[m_npolys - 1], m_epos, epos, 0);
        }
        auto pStraightPath = m_straightPath;
        navQuery->findStraightPath(m_spos, epos, m_polys, m_npolys, pStraightPath, m_straightPathFlags,
            m_straightPathPolys, &m_nstraightPath, MAX_POLYS, DT_STRAIGHTPATH_AREA_CROSSINGS);

        m_nstraightPath = FixedPath(m_nstraightPath, pStraightPath, m_straightPathFlags, m_straightPathPolys);
        if (1 == m_nstraightPath && m_startRef == m_endRef) {
            m_nstraightPath = 2;
            m_straightPath[3] = m_straightPath[0];
            m_straightPath[4] = m_straightPath[1];
            m_straightPath[5] = m_straightPath[2];
        }
    }
    else
    {
        return false;
    }

    return true;
}



// 修复Recast导航折返问题
static int FixedPath(
    int ncorners,
    float* cornerVerts,
    unsigned char* cornerFlags,
    dtPolyRef* cornerPolys) {

    if (ncorners <= 2) {
        return ncorners;
    }
    // 过滤共线折返路点
    const float* p0 = cornerVerts;
    const float* p1 = cornerVerts + 3;
    const float* p2 = cornerVerts + 6;
    // 起点不覆盖
    for (int i = 1; i < ncorners - 1;)
    {
        if (cornerFlags[i] & DT_STRAIGHTPATH_OFFMESH_CONNECTION) {
            break;
        }
        // 共线
        if (dtAbs(dtTriArea2D(p0, p1, p2)) < dtSqr(0.1f)) {
            // 反向
            if ((p1[0] - p0[1]) * (p2[0] - p1[0]) < 0
                || (p1[2] - p0[2]) * (p2[2] - p1[2]) < 0) {
                --ncorners;
                memmove(cornerFlags + i, cornerFlags + i + 1, sizeof(unsigned char) * (ncorners - i));
                memmove(cornerPolys + i, cornerPolys + i + 1, sizeof(dtPolyRef) * (ncorners - i));
                memmove(cornerVerts + 3 * i, cornerVerts + 3 * (i + 1), sizeof(float) * 3 * (ncorners - i));
                continue;
            }
        }
        ++i;
        p0 = p1;
        p1 = p2;
        p2 += 3;
    }
    return ncorners;
}