一个点是否在矩形内的算法_detour 寻路算法流程

获取点附近的所有Poly

detour真正计算寻路的时候用的是Poly，即凸多边形。为了计算起点所在的Poly，我们首先需要将起点扩张为一个长方体，因为坐标点可能并不在寻路表面上。扩张为长方体之后，调用findNearestPoly来获取离这个长方体最近的Poly，而这个函数其实就是queryPolygons的一个封装：

dtStatus dtNavMeshQuery::queryPolygons(const float* center, const float* halfExtents,
                                       const dtQueryFilter* filter, dtPolyQuery* query) const
{
    float bmin[3], bmax[3];
    dtVsub(bmin, center, halfExtents);
    dtVadd(bmax, center, halfExtents);

    // Find tiles the query touches. 根据AABB来计算所有可能相交的Tile
    int minx, miny, maxx, maxy;
    m_nav->calcTileLoc(bmin, &minx, &miny);
    m_nav->calcTileLoc(bmax, &maxx, &maxy);

    static const int MAX_NEIS = 32;
    const dtMeshTile* neis[MAX_NEIS];

    for (int y = miny; y <= maxy; ++y)
    {
        for (int x = minx; x <= maxx; ++x)
        {
            // 获取所有以当前tile为中心的多层邻接tile
            const int nneis = m_nav->getTilesAt(x,y,neis,MAX_NEIS);
            for (int j = 0; j < nneis; ++j)
            {
                // 然后查询一个tile里面与当前长方体相交的多边形
                queryPolygonsInTile(neis[j], bmin, bmax, filter, query);
            }
        }
    }

    return DT_SUCCESS;
}

queryPolygonsInTile是查询Tile内特定矩形会被哪些Poly覆盖的接口：

int dtNavMesh::queryPolygonsInTile(const dtMeshTile* tile, const float* qmin, const float* qmax,
                                   dtPolyRef* polys, const int maxPolys) const
{
    if(tile->bvTree)
    {
        //使用bvtree来加速查询
        // 相关知识参考 http://www.pbr-book.org/3ed-2018/Primitives_and_Intersection_Acceleration/Bounding_Volume_Hierarchies.html
    }
    else
    {
        // 暴力遍历所有的poly 计算是否aabb相交
        float bmin[3], bmax[3];
        int n = 0;
        dtPolyRef base = getPolyRefBase(tile);
        for (int i = 0; i < tile->header->polyCount; ++i)
        {
            dtPoly* p = &tile->polys[i];
            // Do not return off-mesh connection polygons.
            if (p->getType() == DT_POLYTYPE_OFFMESH_CONNECTION)
                continue;
            // Calc polygon bounds.
            const float* v = &tile->verts[p->verts[0]*3];
            dtVcopy(bmin, v);
            dtVcopy(bmax, v);
            for (int j = 1; j < p->vertCount; ++j)
            {
                v = &tile->verts[p->verts[j]*3];
                dtVmin(bmin, v);
                dtVmax(bmax, v);
            }
            if (dtOverlapBounds(qmin,qmax, bmin,bmax))
            {
                if (n < maxPolys)
                    polys[n++] = base | (dtPolyRef)i;
            }
        }
        return n;
    }
}

筛选最近的Poly

通过上面两步获取了所有与目标长方体区域相交的Poly之后，选择里面离目标点最短的Poly。这个流程说起来简单，但是还是有很多层的调用：

1. 首先我们需要解决的是一个点到一个Triangle上的投影距离是多少，对应的接口为dtClosestHeightPointTriangle，如果不在这个Triangle的正上方，则返回False。 2. 根据这个投影距离，来获得一个Poly与特定点之间的最短距离，对应接口为getPolyHeight，原理就是遍历这个Poly内的所有Triangle调用投影距离接口来比较，所以不在Poly上方的时候，这个接口也是返回False的

3. 如果不在正上方，则判断是否是OffMeshLink，如果是的话则直接计算当前点到这个Link的最短点，否则计算当前点与当前Poly里所有边的最短距离点，这里的距离都是线段最短距离，调用的都是dtDistancePtSegSqr2D

4. 计算好一个Poly到一个点的最短距离点之后，根据前面计算出来的所有相交Poly筛选出距离最短的，作为这个点的归属Poly。

获取Poly路径

获取了来源Poly和目标Poly之后，我们需要获取一条消耗较少的Poly路径来联通两头的Poly，这里我们可以做两个额外的标记：

1. 给特定类型的Area赋予不同类型的cost，例如平地为1， 游泳为2， 泥巴路为20等等

2. 标记特定类型的Poly不能选中，或者只能选中特定类型的Poly

这些标记信息会组合成一个dtQueryFilter作为参数传入到所有的寻路请求里， 这个类型提供了两个接口来实现对应的功能：

1. getCost 计算同一个Poly里的两点直线距离的cost

2. passFilter 判断一个Poly是否通过类型限制过滤，作为待选路径

在得到了效用函数之后然后，我们开始做最短路径的查找，相应的代码在dtNavMeshQuery::findPath, 采用的算法是A*，启发函数为三维距离乘以一个启发因子，源码里设置为0.999，如果设置为0，则就退回到了基础的单源最短路径算法。而这里的cost计算则，需要ParentNode、CurrentNode、NextNode三个Node的信息。首先取ParentNode和CurrentNode连接的边，计算边的中点，然后取CurrentNode和NextNode连接的边，计算边的中点，两条边的中点的距离乘以AreaCost，作为当前路径经过CurrentNode的cost。这样的计算就带来了一个问题，从一个Poly的不同边进出的cost是不同的，因此不能把一个Poly当作路径图里的一个节点，而应该把一个Poly及进入这个Poly的边组合起来当作一个节点，所以源码里生成新节点是这么计算的， 里面的crossSide就是进入的边编号：

dtNode* neighbourNode = m_nodePool->getNode(neighbourRef, crossSide);

对于OffMeshLink来说，这个其实并不是一个完整的Poly，他其实算是一条线段，所以计算cost的时候直接计算前一条边与当前Link的一个端点距离即可。

生成寻路路点

在上面的过程中，完成了Poly列表的生成之后，我们需要构造一条最优的通过特定Poly列表的线段列表，来让寻路agent按照这个线段列表开始寻路，这段代码在dtNavMeshQuery::findStraightPath里，作者称这个为String Pulling方法。从下面的图中就可以大概了解所谓的拉绳过程了：

这个算法执行的是一个贪心过程，算法目标是计算出路径中所有的拐点，也就是线段之间的连接点。我们的目的是尽可能的减少拐点，以达到路径平滑同时路径最短的目的。 下图就是算法的贪心流程：

初始的时候我们需要如下参数：

1. start 作为寻路的起点

2. left right 作为中间临时变量 他们与start围成的三角形记录可行走区域

3. polys记录剩下的还未探索的Poly

初始时可以把left right 设置为第一个Poly边上的两点， 即A图。接下来，需要依次移动左右两条表，按顺序处理路径上的Poly。每次遇到一条新的边，考察这个边的left2 right2与start之间的连线与原来的left right线段之间的关系。其实left2 right2总是会有一个点与left right重合，所以只需要考虑新加入的节点即可。

1. 如果新加入的节点与start之间的线段与原来的left->right线段相交，则更新left right为left2, right2，即缩小可行走三角形区域 如上图中的B, C,D

2. 不相交的话，我们需要将left right里与left2 right2重合的点设置为拐点，加入到拐点路径里， 然后将这个点当作新的start，下一条Poly边的left right作为新的left right，一路迭代下去， 如图F G

如果算法过程中遇到的Poly是一个OffMeshLink，则将这个Link的端点作为拐点保存，然后以Link的另外一个端点作为新的start开始流程。