RecastNavigation-線段或射線與三角形相交原理

說到尋路，主流的地形建模方法有三種：grid（方格）、waypoint（路點）和navmesh（導航網格），而RecastNavigation就是使用navmesh作爲模型的一個應用廣泛、功能強大的開源項目（項目地址），它支持建網格、尋路、添加動態障礙、羣體尋路等諸多特性，並在unity和unreal等著名引擎上都有應用。

轉載自：https://blog.csdn.net/needmorecode/article/details/81416553

核心問題：射線與網格求交點

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求交點；它本質上等價爲一個數學問題：線段與三角形求交點