RecastNavigation-线段或射线与三角形相交原理

说到寻路，主流的地形建模方法有三种：grid（方格）、waypoint（路点）和navmesh（导航网格），而RecastNavigation就是使用navmesh作为模型的一个应用广泛、功能强大的开源项目（​​项目地址​​），它支持建网格、寻路、添加动态障碍、群体寻路等诸多特性，并在unity和unreal等著名引擎上都有应用。

 

核心问题：射线与网格求交点

recast主要分为两部分：recast（建网格）和detour（寻路）。这里只针对其中一个特性：射线与navmesh求交点，探讨其实现原理。

源码解析
首先，将射线的起点取成屏幕点击点，终点取成起点的深度（z坐标）加1，这样射线与navmesh的交点就认为是要设置的起点或终点。然后，在main.cpp中通过opengl的方法将鼠标点击的屏幕坐标转成世界坐标：

// Get hit ray position and direction.
GLdouble x, y, z;
gluUnProject(mousePos[0], mousePos[1], 0.0f, modelviewMatrix, projectionMatrix, viewport, &x, &y, &z);
rayStart[0] = (float)x;
rayStart[1] = (float)y;
rayStart[2] = (float)z;
gluUnProject(mousePos[0], mousePos[1], 1.0f, modelviewMatrix, projectionMatrix, viewport, &x, &y, &z);
rayEnd[0] = (float)x;
rayEnd[1] = (float)y;
rayEnd[2] = (float)z;

接下来将起点、终点坐标rayStart和rayEnd传入如下函数：

bool InputGeom::raycastMesh(float* src, float* dst, float& tmin)
{
    float dir[3];
    rcVsub(dir, dst, src);

    // Prune hit ray.
    float btmin, btmax;
    if (!isectSegAABB(src, dst, m_meshBMin, m_meshBMax, btmin, btmax))
        return false;
    float p[2], q[2];
    p[0] = src[0] + (dst[0]-src[0])*btmin;
    p[1] = src[2] + (dst[2]-src[2])*btmin;
    q[0] = src[0] + (dst[0]-src[0])*btmax;
    q[1] = src[2] + (dst[2]-src[2])*btmax;

    int cid[512];
    const int ncid = rcGetChunksOverlappingSegment(m_chunkyMesh, p, q, cid, 512);
    if (!ncid)
        return false;

    tmin = 1.0f;
    bool hit = false;
    const float* verts = m_mesh->getVerts();

    for (int i = 0; i < ncid; ++i)
    {
        const rcChunkyTriMeshNode& node = m_chunkyMesh->nodes[cid[i]];
        const int* tris = &m_chunkyMesh->tris[node.i*3];
        const int ntris = node.n;

        for (int j = 0; j < ntris*3; j += 3)
        {
            float t = 1;
            if (intersectSegmentTriangle(src, dst,
                                         &verts[tris[j]*3],
                                         &verts[tris[j+1]*3],
                                         &verts[tris[j+2]*3], t))
            {
                if (t < tmin)
                    tmin = t;
                hit = true;
            }
        }
    }

    return hit;
}

isectSegAABB函数的作用是修剪射线：它将整个navmesh看成是一个AABB包围盒，判断射线和包围盒是否有交集；若没有则直接return；否则将射线不在盒内的部分修剪掉。 
将下来通过rcGetChunksOverlappingSegment函数求取二维平面下与射线有交集的所有trimesh node（三角网格节点）（只考虑x、z坐标）。这一步算是粗筛，因为不涉及点乘差乘等耗时运算，执行效率较高。相关代码如下：

int rcGetChunksOverlappingSegment(const rcChunkyTriMesh* cm,
                                  float p[2], float q[2],
                                  int* ids, const int maxIds)
{
    // Traverse tree
    int i = 0;
    int n = 0;
    while (i < cm->nnodes)
    {
        const rcChunkyTriMeshNode* node = &cm->nodes[i];
        const bool overlap = checkOverlapSegment(p, q, node->bmin, node->bmax);
        const bool isLeafNode = node->i >= 0;

        if (isLeafNode && overlap)
        {
            if (n < maxIds)
            {
                ids[n] = i;
                n++;
            }
        }

        if (overlap || isLeafNode)
            i++;
        else
        {
            const int escapeIndex = -node->i;
            i += escapeIndex;
        }
    }

    return n;
}

检查方法checkOverlapSegment是将node看成AABB包围盒，通过比较射线起止点p、q与包围盒的x、z坐标的相对位置。若存在overlap，则还要判断node是否为叶子节点。这里recast为trimesh node建立的模型是一个树状结构，从根节点出发管理到大的区块，再到小的区块，直至一个基础node作为叶子节点。叶子节点是通过node的属性i来判断，若i小于0代表叶子节点，可以将这个node加入返回数组中；否则判断下一个。注意这里选取下一个的时候有个分支优化：若既没有overlap，又不是叶节点，则放弃当前节点下面的所有子孙节点，直接跳转到通过属性i计算出的下一个节点索引处。 
通过上面这一步可以排除掉绝大多数节点。下面只需要对剩余的若干个trimesh node做精选，判断射线是否与它们存在交点。这实际是分两步：一是求射线与三角形所在平面的交点，二是判断交点是否在三角形内部。这是在如下函数中处理的：

// 空间点 sp 起点 sq终点
// 三角形空间点 a b c 
// 输出参数 t
static bool intersectSegmentTriangle(const float* sp, const float* sq,
                                     const float* a, const float* b, const float* c,
                                     float &t)
{
    float v, w;
    float ab[3], ac[3], qp[3], ap[3], norm[3], e[3];
    rcVsub(ab, b, a);
    rcVsub(ac, c, a);
    rcVsub(qp, sp, sq);

    // 求三角形所在平面的法向量norm
    rcVcross(norm, ab, ac);

    // 计算将QP映射到norm方向
    // 如果d=0,表示QP和norm是垂直的，QP和三角形平行，不可能相交，舍弃
    // 如果d<0,表示QP和norm是钝角的，QP是从三角形背面进入和三角形相交的，应该是锐角，舍弃
    float d = rcVdot(qp, norm);
    if (d <= 0.0f) return false;

    // 将AP映射到norm方向
    // 如果t<0,表示AP和norm是钝角的，QP是从三角形背面进入和三角形相交的，应该是锐角，舍弃
    rcVsub(ap, sp, a);
    t = rcVdot(ap, norm);
    if (t < 0.0f) return false;
    if (t > d) return false; // 此处仅用于QP是线段时，当QP是射线时，需要删掉这行代码

    // 计算重心坐标分量并测试是否在界限内
    rcVcross(e, qp, ap);
    v = rcVdot(ac, e);
    if (v < 0.0f || v > d) return false;
    w = -rcVdot(ab, e);
    if (w < 0.0f || v + w > d) return false;

    // 线段或射线与三角形相交，延迟除法
    t /= d;

    return true;
}

这是一个纯粹的数学问题：设P、Q为射线的起止点，三角形的三个顶点分别为A、B、C，我们得到如下的几何模型：

程序先求三角形所在平面的法向量

，再用叉乘将

，

分别映射到

所在方向，分别得到高度t和d，若t>d，则射线PQ肯定与平面没有交点，直接return。 

接下来再判断交点是否在三角形内部。这里要用到的一个概念叫做质心坐标系。大概意思就是三角形ABC所在平面的点可以表示成： 

而三角形内部的点必定满足：

和

都在(0,1)范围内。 

通过这个性质，再加一系列的方程计算和矩阵变换可以判断出交点是否在三角形内部（演算过程这里略过，具体可看​​《空间中直线段和三角形的相交算法》​​，说得很详细了）。

这里直接引用结论：

若不在（0,1）范围内，直接return，否则将t/d作为返回值传出，后面会用来求取最终的交点坐标。 

接下来重新回到InputGeom::raycastMesh函数中，可以看到若射线与多个trimesh node相交，会选择最先遇到的交点：

for (int j = 0; j < ntris*3; j += 3)
{
    float t = 1;
    if (intersectSegmentTriangle(src, dst,
                                &verts[tris[j]*3],
                                &verts[tris[j+1]*3],
                                &verts[tris[j+2]*3], t))
    {
        if (t < tmin)
           tmin = t;
        hit = true;
    }
}

最后回到main.cpp中，根据上面return的t与d的比例关系，求取最终的交点坐标：

float pos[3];
pos[0] = rayStart[0] + (rayEnd[0] - rayStart[0]) * hitTime;
pos[1] = rayStart[1] + (rayEnd[1] - rayStart[1]) * hitTime;
pos[2] = rayStart[2] + (rayEnd[2] - rayStart[2]) * hitTime;

至此大功告成。

小结：射线与mesh求交点；它本质上等价为一个数学问题：线段与三角形求交点