recast navigation navmesh導航網格算法源碼分析
Author: 林紹川
本文為了方便,引用了一些網上的相關圖片
圖片出處:Recast源碼解析（二）：NavMesh導航網格生成原理（上） - 程序員大本營 特此致謝
1 加載.obj文件
InputGeom::load--->InputGeom::loadMesh負責加載所有三角形
取obj文件中頂點數據,和頂點索引數據
1.1 obj的格式:
v 6.5 0.07999992 20.5
...(每行以v開頭,連着三個浮點數,為一個頂點坐標)
f 1 2 3
...(每行以f開頭,連着三個整數,為頂點索引)
1.2 三角形空間分割
rcCreateChunkyTriMesh,這個函數為之后建立高度場時,能快速索引出對應的三角形
按如下步驟:
1 此函數會為加載的三角形計算aabb盒(其實是xz坐標的包圍矩形)
2 以aabb哪個軸長(x軸方向長),就按哪個軸進行排序,然按三角形數量1分為2
3 如此循環,直到划分的節點里三角形個數小於某個閥值(256)
2 建立高度場
結束加載obj,就要開始建立高度場
1 Recast navmesh是將場景中的三角形光柵光,形成高度場體素,
2 然后根據高度場計算layer,還生成導航多邊形數據
為何不直接利用obj中的三角形數據,原因可能如下:
1 obj中的三角形數據可能細節更多,而導航尋路可以適當簡化多邊形細節,提高效率
2 尋路有對象寬度限制,體素化利於處理
3 obj中沒有包含layer信息,需要處理
layer是recast navmesh里的一個概念,是導入obj里,一些獨立,不連通的區域,或者不同的層,彼此之間尋路上不能互通,
或者通過port互通
此處以 Sample_TempObstacles::handleBuild為例,說明這一過程
這個sample支持動態創建障礙物,具體的處理方向如下步驟:
1 將高度場數據划分成tile,
2 障礙物改變與其相交的高度場體素,將處於障礙物(包圍盒/或者圓柱體)中的體素標記為不可走
3 然后將受影響的tile重新計算,局部重新生成導航多邊形數據
2.1 rasterizeTileLayers光柵化tile內三角形
光柵化場景中的三角形,即用一個正方形單元(xz平面),對場景中三角形數據進行采樣高度數據,此處借用網上的一個圖,形象表示如下:
圖1
對圖中的每個小方格,以下會以span這一稱呼來代替,
一個span是xz大小固定的單元格,在y方向不固定
(根據光柵光時,截取到的多邊形高范圍,記錄ymax ymin)
2.2 划分tile
Sample_TempObstacles為了動態重新開銷減少,需要划分tile,
即將整個場景算出一個aabb,在xz平面上按固定大小分tile
每個tile內包含一個span體素數組
即tile是一個塊, span則是一個最小的體素點,
二維的span組成一個tile
二維的tile組成一個場景
2.3 rasterizeTileLayers的參數 rcConfig
這個參數部分重要成員變量含義:
cs:
體素化的單位xz的量化(每個span在xz平面的長寬,為一個正方向)
ch:
y方向的量化單位
walkableSlopeAngle:
可攀爬的角度(0-90度),用來判斷一個span是否可以行走(太斜了,就不能行走)
walkableHeight:
尋路單位的高度,高度不夠,不能走, 例如:一個身高兩米的人,不能通過只有1米高的門框之類的
walkableClimb:
同上,高度差小於此值,認為兩個span是可以行走的
walkableHeight walkableClimb會被ch量化,例如:
ch=0.5 walkableHeight=2.0 那么 walkableHeight = walkableHeight / ch,
最終walkableHeight =4;
walkableRadius:
可行走的寬度,即一個太窄,狹小的地方認為不能走
walkableRadius會被cs量化
例如:
cs = 0.5 walkableRadius = 2.0
walkableRadius = walkableRadius / cs 即walkableRadius = 4
borderSize:
為每個tile多准備一些span數據,相當於邊框,值會設成比walkableRadius大,避免把一些span錯誤置成不可行走
2.4 rcGetChunksOverlappingRect
此函數比較簡單,計算當前tile范圍,將tile包含的三角形找出來
2.5 rcMarkWalkableTriangles
依據walkableSlopeAngle,根據法線,判斷哪些三角形不能走
2.6 rcRasterizeTriangles
對每個三角形調用rasterizeTri光柵化(或者說體素化),
形成一個高度場,(x,z)單元格,一個格子對應一個鏈表,鏈表里的數據按高度從小到大排列
2.7 rasterizeTri
對單個三角形進行光柵化(見圖1)
在xz平面,對三角形進行分割,
循環調用dividePoly,先平行x軸模向切割, 再平行z軸縱切
2.7.1 dividePoly
分割多邊形,以沿z軸平行分割為例,有如下 圖2 圖3兩種情況
圖2
圖3
假設藍色的那條邊是要被分割的線,該函數會計算線段兩個端點A,B的z坐標與分割線的z坐標差值
如果是圖2的情況:
1 如果處理的點是A記入上半個多邊形頂點,是B記入下半個
(根據d[i]的符號判斷點是在A位置,還是在B位置)
2 交點分別計下上下兩個多邊形頂點
如果是圖3的情況:
點A記入上半個多邊形頂點(或者下半個多邊形頂點,根據d[i]的符號)
代碼片段,記算d[i]
體素化分割截取形象表示如下網圖
三角形被x軸z軸的平行線切割后形成的多邊形,取該多邊形在y軸的最大值與最小值,生成span,
即上面所提到的圖1,再次貼如下:
所對應的代碼:
2.7.2 addSpan
這個函數會做一個處理,所有的span鏈表都是按高度從低到高排序的, span有多個鄰居時,取高度最高的,
原因如下圖:
2.7.3 最終高度場
到此,完成了每個tile的高度場建立
參考網上找到的示意圖,高度場形象表示如下:
每個span xz長度固定, y方向包含一個最大值最小值, 同一個xz坐標會有多個span,
同一個xz坐標處的所有span形成一個鏈表,並且按高度排序(從小到大)
2.7.4 rcFilterLowHangingWalkableObstacles
根據同一個xz坐標不同span之間,如果它們的距離不walkableClimb,並且下面的span可走,則把上面的span也標記成可走, 即處理台階樓梯之類的情形
2.7.5 rcFilterLedgeSpans
和周圍的span比較,"空洞"的高度差是否大於閥值,不大於閥值,說明不夠高,
4個方向都不滿足,該span也會被判斷定為不能走,
例如:該span周圍被牆包圍,
如圖所示的紅線部分,rcFilterLedgeSpans這個函數,判斷空洞交疊部分(紅線長度)是否大開於路單位的高度walkableHeight
2.7.6 rcFilterWalkableLowHeightSpans
判斷當前格子所形成的空間,是否足夠高(>walkableHeight),不夠高也判斷為不可走
如下圖所示,同一個xz坐標上,一系列的span,它們之間的高度差要足夠
致此,完成了所有span能不能行走的標記
2.8 rcBuildCompactHeightfield
與周圍的span進行比較,是否高度足夠,並且高度差是可以走的,形成連接信息
即每個span會記錄與周圍前后左右四個鄰是否連通
如果當前坐標為xz鄰居方向dir值
0:表示x-1的span
1:表示z+1的span
2:表示x+1的span
3:表示z-1的span
Recast navmesh鄰居定義圖示如下:
2.9 rcErodeWalkableArea
Erode的中文含義為侵蝕,即將障礙物周圍可行走區域按radius值適當擴散不可行走區域,目的是尋路目標貼牆走時,留出一定的寬度
在不可行走的區域周圍,標記格子g(格子的鄰居存在不可行走區域)
格子g與障礙的距離dis標記為0(dis默認為0xff)
繼續處理,標記出所有格子與障礙物的距離,根據格子的可連通鄰居dis值
根據dis值與尋路物體寬度值radius比較,小則將該區域也標記成不可行走
如下圖所示,可行走區域要收縮一些,為尋路單位留出walkableRadius
2.10 rcMarkConvexPolyArea
這個是動態障礙用的,把一些span標記成不可行走
函數很簡單,不影響對主要代碼的理解,不詳細說明了
2.11 rcBuildHeightfieldLayers
完成了高度場"扁平化",
每個layer
rcHeightfieldLayer::area： 保存是否可行走
rcHeightfieldLayer::heights：高度
rcHeightfieldLayer::cons：連通性(低4bit)+是否為layer之間的通道(高4bit)
以下說明該函數的執行過程：
2.11.1 找出連通區域
互相連接的span會形成一個區域,形成區域的規則如下圖所示: (*每個代表一個span)
對應的部分代碼片段截圖
2.11.2 相鄰的連通區域合並成一個區域
2.11.2.1 根據區域內span的鄰居的區域id,為每個區域添加鄰居
部分代碼片段如下
2.11.2.2 同時根據span的鏈表,可以查當該span同一個xz坐標處的其它layer,記錄下
形象圖示如下
對應的代碼片段
2.11.2.3 把所有互相連接的連通區域的打上同一個layerId, 排除掉自身的"不同層" 鄰居的"不同層"
即:區域之間根據鄰接關系互聯的時候, 不同的層不能是同一個區域, 鄰居的不同層也不能進入同一個區域
這樣處理是為了扁平化高度場,區分出不同的層,
將3d的場景,轉化成不同的層級的2d場
同時對於不連接的區域, 為了減少區域數量,對於高度差小於閥值的區域之間,也會進入合並處理
(不理解可以跳過此段代碼處理,對整個邏輯理解不影響,是一個優化操作)
該優化部分代碼片段如下:
2.11.2.4 扁平化的高度場數據保存
見2.11處
每個layer保存
rcHeightfieldLayer::area： 保存是否可行走
rcHeightfieldLayer::heights：高度
rcHeightfieldLayer::cons：連通性(低4bit)+是否為layer之間的通道(高4bit)
3 buildNavMeshTilesAt從壓縮的高度場中,重建導航數據
大致過程如下:
1 扁平化高度場數據會被壓縮,解壓高度場數據
2 重建導航數據是逐個tile進行,
3 tile先按區域進行邊緣檢測,得到多邊形
4 將多邊形分解成三角形
5 將三角形進行適當合並(保證合並后的多邊形是凸的)
6 記錄tile多邊形鄰接信息
7 tile之間檢測多邊形邊交疊情況,記錄連接信息
下面進行詳細說明
3.1 標記動態障礙
dtMarkCylinderArea
dtMarkBoxArea
dtMarkBoxArea
動態障礙物標記,改變與障礙物相交的area(可以理解為前文提到的span)標記為不可行走
對代碼理解不影響,可以跳過
3.2 dtBuildTileCacheRegions 再次生成區域數據
部分邏輯參考 2.11 rcBuildHeightfieldLayers
1 將已經扁平化的高度場數據重新生成區域,
2 相信的區域組合成一個區域
原因:因為動態障礙支持,不能從解壓后的數據直接取結果
canMerge函數比較難以理解,做個簡要說明:
1 oldRegId鄰居中有一個regId為newRegId,可以合並
2 count=0:表時newRegId並不在oldRegId鄰居
3 count>=2:說明oldRegId已與別的區域合並,不能再newRegId合並,否則會導致合並好的區域又分裂了
4 這樣處理,保證合並區域不斷變大
可以自行畫三個相鄰的區域,按代碼來理解canMerge的含義
因為數據已經扁平化,這里就沒有扁平化處理了
3.3 dtBuildTileCacheContours 輪廓處理
dtTileCacheContour::verts:記錄的輪廓 (即多邊形的頂點)
dtTileCacheContour::reg:區域id
dtTileCacheContour::area可行走標識
多邊形的點的順時針記錄的
以下進行詳細說明
3.3.1 walkContour
遍歷tile中所有xz坐標,
從每個合法的點area(即span)查找邊緣
每個點與周圍4個鄰點,如果所處的reg不一樣,則認為是邊界點
4個鄰點標號如下圖所示,從3號位開始比對
#表示其它區域的點 1.2.3.4表示當前要查找的輪廓邊緣點, 如果出現x(或z)坐標相同的點,則會把中間的點舍棄
以上圖為例從1點開始
1. 點1的三號位,即點1的下方提#所以點1是邊緣點
2. 按代碼邏輯,x,z不變,然后順時針旋轉,即dir=0,代碼片段如下,可以看出當處理3號位時,只改變dir
<代碼片段1>
3. 此時判斷點1與點2,發現它們是同一個區域的,發現它們是同一區域,此時會把dir逆時針轉回去
即dir=0,同時x,z坐標變成點2,代碼片段如下
4. 點2的下方是#,說明點2是邊緣點,dir這時又順時針轉,即dir=0,同時x,z還是點2的,見<代碼片段1>的邏輯
5. 點2的左邊(dir=0)是不同區域,將z坐標加1(見<代碼片段1>),到點3,同時dir=1,
因為左下角是不同區域點3也算邊緣點
6. 點3的上方(dir=1)是同一個區域,把xz坐標切到點4,並逆時針處理dir=0
7. 點4的左邊(dir=1)是同不區域,點4是邊緣點
8. 就這樣不斷地旋,切坐標,把輪廓找出來,當回到點1 時,說明找到了完整的輪廓,判斷代碼如下
9. 可以看出點是按順時針存儲的
以下4圖片形象說明了這一過程:
3.3.2 simplifyContour簡化輪廓
將輪廓分段,如果該段內的點偏離段小於閥值,合並這些點
采用Douglas-Peucker算法
1 按順序存儲的一系列頂點,將首尾兩點(記為V0和Vn)連成線段
2 計算中間各點與該線段的距離,如果距離小於某個閥值忽略,大於閥值的記錄下來,這個點我們記錄Vmaxd
3 Vmaxd與V0 Vn分別組成兩條線段 V0-Vmaxd Vmaxd-Vn,分別對這兩條線段重復第2步驟,
4 得到一系列線段,並且沒有中間點與對應線段距離大於閥值,結束處理過程
形象圖示如下: 此圖出處 Douglas-Peucker算法 - qingsun_ny - 博客園
3.3.3 getCornerHeight
如圖所示的藍點,如果輪廓中有這樣的占,要去除,是一種容錯機制,
不好理解可以跳過,不影響后面的代碼理解
3.3.4 工具函數介紹
distancePtSeg 計算點到線段的距離,根據向量點乘算出垂足坐標,再計算點與垂足的距離即可
代碼片段截取如下:
3.4 dtBuildTileCachePolyMesh 生成鄰接多邊形
navmesh導航算法需要確保多邊形是凸的,此函數負責生成凸多邊形,並生成鄰接信息
因為高度場已經扁平化,所以這個函數的操作都是在xz平面上行進的
1 先把之前得到的多邊形輪廓(有可能是凹的),分割成三角形
2 再把三角形組合成多邊形(確保是凸的)
3 處理多邊形之間的鄰接關系
以下詳細說膽此函數
3.4.1 triangulate
將多邊形(有可能是凹的)分割成三角形
diagonal
bool diagonal(int i, int j, int n, const unsigned char* verts, const unsigned short* indices)
i和j分別為多邊形的頂點(索引),此函數判斷頂點i 頂點j的連線能否作為多邊形的對角線
用圖表示如下:
diagonal調用了一些工具函數,以下做一些簡要說明
這四個函數大同小異
利用向量的叉積(即兩個向量的夾角sin值),判斷兩個向量的方向關系
以leftOn為例向量V1 =b-a V2=c-a
V1 V2的叉積:
=0時 表示夾角為零(用於共線判斷)
>0時 表示V1轉到V2是順時針,如下圖:
<0時 表示V1轉到V2是逆時針, 此時V1 V2與上圖相反
leftOn代碼片段截取如下:
返回0:表示 a b c 三點共線
返回<0 表示 c 在向量ab的左邊
返回>0 表示 c 在向量ab的右邊
用右手,繞序abc,
大拇指朝上: c在ab右
大拇指朝下: c在ab左
工具函數inCone
此函數利用工具函數leftOn left保證一系列4個點是凸的
代碼片段如下:
工具函數diagonalie
此函數排除與多邊線相交的對角線,如下圖
工具函數intersect
此函數判斷兩個線段是否相交
bool intersectProp(const unsigned char* a, const unsigned char* b, const unsigned char* c, const unsigned char* d)
這個函數利用left工具函數判斷
1 點a和點b在線段cd的兩邊
2 點c和點d在線段ab的兩邊
如果滿足1,2這兩個條件,則說明線相交
bool between(const unsigned char* a, const unsigned char* b, const unsigned char* c)
利用area2判斷是否共線,即叉積為0
在共線的情況下,判斷端點是否交疊,交疊才能算相交
如果下圖所示,要排除紅藍這種共線的方式,因為沒有交疊
致此,triangulate函數中找合法對角線相關的函數大部分介紹完畢
切割三角形
根據合法的對象線,從多邊形中切割出三角形
每次循環找出符合條件的對角線(程序中是長度最小)
根據對角線,把對應的頂點分離出去,形成一個三角形, 剩余的點形成新的多邊形
多邊形改變了,對受影響的點重新調用diagonal,計算對角線的合法性
如下圖所示,沿着綠色虛線,切出三角形,
反復循環之后,多邊形變成三角形,如下
致此,完成多邊形三角化
3.4.2 合並三角形為凸多邊形
好像不合並也行,三角形一定是凸的,合並后數據更少,執行效率更優
getPolyMergeValue
判斷兩個多邊形是否有共同的邊,
並且保證合並后是凸的,
返回值是公共邊的長度
利用uleft來判斷合並后是否是凸的,公共邊的兩個端點處都要判斷,如下圖:
代碼片段如下:
mergePolys
合並多點邊形,這個函數比較簡單,復制數據,保證點的繞序,代碼與相應的注釋說明如下:
3.4.3 buildMeshAdjacency
生成多邊形鄰接信息
1 首先將所有的邊,按頂點索引為key建立類哈希表
2 對具有相同頂點的邊進行判斷,是否為公共邊
3 邊連接
4 port連接
簡單說一下有port連接,其實和普通的邊連接是一樣的
如下圖, 根據扁平化規則, 黑色線處(水平的黑色和斜的黑色)的span和藍色線處的span會被划分至不同layer
可回看addSpan 假設黑色斜的那一部分,在藍色處形成投影,
投影邊緣的那些點(假設是藍色面的邊線),
如果藍色面周圍有其它span鄰居(假設是圖中的紅色,並且紅色與黑色layer是同一個layer)
藍色面的邊緣span會變成port,並形成port連接
即port和 普通邊連接可以認為不共存
部分代碼片段如下:
如果是邊連接,存放對應多邊形的數組下標
如果是port,則要額外打個0x8000的標記,存port的方向(0,1,2,3)
這些數據會dtCreateNavMeshData里做轉換
Port(0,1,2,3,4)會轉成 4,2,0,6
3.4.4 dtNavMesh::addTile
將數據添加進tile,這一步要處理tile之間的連接
即不同的tile,要處理連緣處的多邊形的連通情況,便於tile之間導航尋路
如果只是想了解Sample_SoloMesh的業務邏輯,可以跳過本段
connectIntLinks
這個函數負責生成tile之內多邊形鄰居信息生成(主要是相鄰多邊形的公共邊)
findConnectingPolys
1 此函數負責查找tile之間的多邊形連接情況
這種情況,記錄的不是公共邊,而是多邊形相互靠近的兩條邊,它們的交疊部分記錄下來
2 此函數也負責tile之間,不同層之間的連通情況,即因為port產生的連接
代碼片段如下:
Port可以認為是高一層的layer在低上層的layer上投影的邊緣的那些點
記錄相鄰邊交疊部分的代碼
致此,導航需要多邊形鄰接信息生成完畢,
之后就是利用這些信息進行尋路
4 尋路
4.1 dtNavMeshQuery::findPath 網格尋路
此函數沒有太多難懂的地方
按網格鄰接查找網格路徑,類A*算法,按多邊形中心點的距離估算cost
m_openList:是一個堆(極小堆),每個彈出一個cost最優的節點進行循環迭代
4.2 dtNavMeshQuery::findStraightPath 尋找拐點
得到一系列相鄰的多邊形路徑, 如何穿過這些多邊形, 需要此函數處理,即需要知道要在哪些地方拐彎
4.2.1 尋路初始
如下圖所示,
從紅點出發,要走到藍點
用紅色標出每個多邊形的鄰接邊:
4.2.2 射線包絡
對第一條鄰邊,從起始的紅點,與鄰接邊的端點,發出一條射線(圖中的紫線)
對第二條鄰邊也做同樣的兩條射線(圖中的黃線)
兩條紫射線為一組,兩條黃射線為一組,取它們的交集,
以下圖為例,交集顯示是兩條黃射線包絡的那部分,
如下圖,處理到第三條鄰邊時的最小包絡射線
4.2.3 拐點出現的情況
對之后的每條鄰邊重復上面的循環,直到出現沒有交集的情況
檢測到拐點(粉紅的那個點),
如下圖,此例在紅色鄰邊的兩個端點,此時與紫色的包絡線已經沒有交集了
4.2.4 尋路結束
然后以粉色點為新的起點,重復以上過程,就可以找到一條在多邊形內部的路徑,
即最終的路徑會是從紅點走到粉點,再從粉點走到藍色
4.2.5 結束語
致此,完成了導航網格生成與尋路功能說明
====================== 分割線 ============================
4.3 一些工具函數簡介
尋路過程,使用了一些工具函數,以下做一個簡要說明
dtDistancePtPolyEdgesSqr / dtPointInPolygon
利用射線與多邊形的交點是奇偶判斷是否在多邊形內,
看代碼射線應該是平行x軸往x軸正方向
代碼片段如下:
dtDistancePtSegSqr2D
求點到線段的距離平方
dtTriArea2D
recast里有很多函數功能相似,此函數參考 leftOn
利用差乘判斷線段與點的位置關系
getPortalPoints
獲多邊形鄰邊的兩個端點(如果是tile間的連接或者port,取出交疊的首尾)
dtClosestHeightPointTriangle
利用射線(特殊射線,垂與xz平面的)與三角形的交點計算方法,求出射線與三角形的交點
三角形(p1 p2 p3)內部的頂點可以看到是
向量v1 = p3 - p1;
向量v2 = p2 - p1;
內部點 px = p1 + u*v1 + v*v2;
等同於: px = p1*(1-u-v) + p3*u + p2*v;
另外 px = O + d*t O:射線起點 d:射線方向 t:射線"長度"
O + d*t = p1*(1-u-v) + p3*u + p2*v; 這樣可以得到一個三元一次方程組 未知數為(t u v)
根據克萊默法則解這個方組即可t u v 代入px = O + d*t即可得到對應的px坐標
同時,這個函數dtClosestHeightPointTriangle的射線是特殊的,垂直於xz平面