三維空間中的尋路算法兩則 RRT與A*
?
三維空間中的尋路算法兩則
之一：RRT 名字應(yīng)該叫做快速隨機(jī)生長(zhǎng)樹(shù)
算法簡(jiǎn)介：（你應(yīng)該可以在網(wǎng)上找到更好的描述）首先起點(diǎn)算作樹(shù)根，然后隨機(jī)選取范圍內(nèi)的一個(gè)點(diǎn)或者終點(diǎn)，樹(shù)上離這個(gè)點(diǎn)最近的樹(shù)枝，生長(zhǎng)出一個(gè)新點(diǎn)，直到可以到達(dá)終點(diǎn)。
核心代碼：
配合這個(gè)網(wǎng)頁(yè)中的3 1 1食用最佳 https://zhuanlan.zhihu.com/p/51087819
?
//?????????this.result?=?new?Result();
//?????????this.result.tree.nodeList.push(startPoint);?//?0:開(kāi)始點(diǎn)
//?????????this.result.tree.parentNodeIndex.set(0,?-1);?//?0：無(wú)父節(jié)點(diǎn)
//?????????this.reachEnd?=?false;
//?????????this.cost?=?new?Map();
?
//?????????let?randPoint;
//?????????let?nearPoint;
//?????????let?newPoint;
//?????????for?(let?iter?=?0;?iter?<?this.maxIter;?iter++)?{
//?????????????//產(chǎn)生隨機(jī)點(diǎn)randPoint
//?????????????randPoint?=?this.getRandPoint(env,?endPoint,?this.randToGoal);
?
//?????????????//找到最近已知點(diǎn)nearPoint
//?????????????let?nearPointIndex?=?this.getNearPointIndex(this.result.tree,?randPoint,?endPoint);
//?????????????nearPoint?=?this.result.tree.nodeList[nearPointIndex];
?
//?????????????//產(chǎn)生newPoint
//?????????????newPoint?=?this.getNewPoint(nearPoint,?randPoint,?this.stepLen);
?
//?????????????//判斷newPoint合法
//?????????????if?(env.free(nearPoint,?newPoint))?{
//?????????????????//newPoint加入到樹(shù)中
//?????????????????this.result.tree.nodeList.push(newPoint.clone());
//?????????????????let?newPointIndex?=?this.result.tree.nodeList.length?-?1;
//?????????????????this.result.tree.parentNodeIndex.set(newPointIndex,?nearPointIndex);?//邊：newPoint和nearPoint。設(shè)置父節(jié)點(diǎn)
//?????????????????EventDispatcher.dispatchEvent(EVENT_gotANewFreePoint,?nearPoint.clone(),?newPoint.clone());
?
//?????????????????//如果newPoint和終點(diǎn)距離比較小?就進(jìn)行終點(diǎn)測(cè)試
//?????????????????if?(newPoint.distanceTo(endPoint)?<?3?*?this.stepLen)?{
//?????????????????????if?(!this.reachEnd?&&?env.free(newPoint,?endPoint))?{
//?????????????????????????this.result.tree.nodeList.push(endPoint);
//?????????????????????????this.result.tree.parentNodeIndex.set(this.result.tree.nodeList.length?-?1,?this.result.tree.nodeList.length?-?2);
//?????????????????????????this.reachEnd?=?true;
//?????????????????????????EventDispatcher.dispatchEvent(EVENT_findThePath,?this.result);
?
//?????????????????????}
//?????????????????}
//?????????????}
?
//?????????????if?(iter?%?Math.round(this.maxIter?*?0.01)?==?0)?{
//?????????????????console.log([iter,?this.maxIter,?this.result.tree.nodeList.length]);
//?????????????}
//?????????????if?(this.reachEnd)?{
//?????????????????break;
//?????????????}
//?????????}
?
?
在網(wǎng)上找到的一些rrt效果
http://lavalle.pl/rrt/gallery_rigid.html
在實(shí)際的三維空間中的效果
上面的結(jié)果路徑如下
?
換一對(duì)起點(diǎn)終點(diǎn)
上面的結(jié)果路徑如下
對(duì)上面的結(jié)果“拉直”（不影響尋路過(guò)程，僅對(duì)結(jié)果效果優(yōu)化）（這個(gè)操作是自己想的，要是有前輩發(fā)表了類似操作，請(qǐng)告知我這個(gè)操作書(shū)面的名字）
?
?
之二：A*，算法名就叫做A星
算法簡(jiǎn)介：帶有預(yù)估的貪心。每次尋找一個(gè)估價(jià)最好的點(diǎn)前進(jìn)，估價(jià)包括已消耗和未來(lái)預(yù)計(jì)消耗。
核心代碼：
配合??http://theory.stanford.edu/~amitp/GameProgramming/??
?https://www.redblobgames.com/pathfinding/a-star/introduction.html?
使用最佳。
that.endNode?=?that.calcNode(that.dm.config._endPoint,?that.dm.config._startPoint,?that.dm.config.stepLen,?that);
????????//開(kāi)始點(diǎn)
????????that.startNode?=?new?AlgoNode();?//默認(rèn)xCount=0?所以不用設(shè)置了
????????that.recordNode(that.startNode,?undefined,?that);
?
????????let?iter:?number?=?0;
????????while?((that.openList.length?>?0)?&&?(!that.findAns)?&&?++iter)?{
????????????let?nearestNode:?AlgoNode?=?that.findNearestInOpenlist(that.startNode,?that);
????????????//當(dāng)前點(diǎn)可以直達(dá)終點(diǎn)
????????????if?(nearestNode._point.distanceTo(that.endNode._point)?<?5?*?that.dm.config.stepLen)?{
????????????????if?(that.reachEndPoint(nearestNode,?that))?{
????????????????????//終點(diǎn)和其父節(jié)點(diǎn)加入探索過(guò)的點(diǎn)
????????????????????that.recordNode(that.endNode,?nearestNode,?that);
????????????????????that.findAns?=?true;
????????????????????//這個(gè)停止事件會(huì)產(chǎn)生ansLineScene所需要的數(shù)據(jù)，所以要在繪圖之前
????????????????????that.stopSituation("成功探索到目的地",?that);
????????????????????EventDispatcher.dispatchEvent(EVENT_processChanged,?1);
????????????????????if?(that.dm.onProcess)?that.dm.onProcess(1);
????????????????????continue;
????????????????}
????????????}
????????????//從open取出，放到close
????????????that.setCloseMap(nearestNode,?that);
????????????that.setOpenMap(nearestNode,?that);
????????????that.removeFromOpenlist(nearestNode,?that);
????????????let?neighbors:?AlgoNode[]?=?that.getNeighbors(nearestNode,?that);
????????????//除去已經(jīng)關(guān)閉的點(diǎn)
????????????let?neighborsNotClosed:?AlgoNode[]?=?that.getNeighborsNotClosed(neighbors,?that);
????????????//除去無(wú)法到達(dá)的點(diǎn)
????????????let?neighborsCanReach?=?that.getNeighborsCanReach(nearestNode,?neighborsNotClosed,?that);
????????????//依次處理?無(wú)碰撞且未探索過(guò)的點(diǎn)
????????????for?(let?neighbor?of?neighborsCanReach)?{
????????????????/**
?????????????????*?可優(yōu)化的值，大于0表示有得優(yōu)化
?????????????????*/
????????????????let?BetterG:?number?=?that.currentGBetter(neighbor,?nearestNode,?that);
????????????????if?(that.getMap(that.openMap,?neighbor.xCount,?neighbor.yCount,?neighbor.zCount)?&&?BetterG?>?0)?{
????????????????????that.refreshFG(neighbor,?BetterG);
????????????????}
????????????????if?(!that.getMap(that.openMap,?neighbor.xCount,?neighbor.yCount,?neighbor.zCount))?{
????????????????????that.recordNode(neighbor,?nearestNode,?that);
????????????????}
????????????}
????????????//因?yàn)槌^(guò)設(shè)定的探索次數(shù)而結(jié)束
????????????if?(iter?>?that.dm.config.maxIter)?{
????????????????that.stopSituation("探索次數(shù)用盡",?that);
????????????????return;
????????????}
????????????EventDispatcher.dispatchEvent(EVENT_processChanged,?that.nowSteps?/?that.maxSteps);
????????????if?(that.dm.onProcess)?{
????????????????that.dm.onProcess(that.nowSteps?/?that.maxSteps);
????????????}
????????????yield?iter;
?
????????}
????????if?(!that.findAns)?{
????????????that.stopSituation("沒(méi)有路了",?that);
????????}
?
????/**
?????*?新路線是否有優(yōu)化
?????*?@param?neighbor?
?????*/
????private?currentGBetter(neighbor:?AlgoNode,?nearestNode:?AlgoNode,?that:?A_star2):?number?{
????????let?old:?number?=?neighbor.g;
????????let?now:?number?=?nearestNode.g?+?that.costG(nearestNode,?neighbor);
????????return?old?-?now;
????}
?
?
????/**
?????*?尋路的關(guān)鍵函數(shù)
?????*?計(jì)算未來(lái)消耗?曼哈頓距離
?????*?在z上乘50是考慮到業(yè)務(wù)場(chǎng)景的上下樓都不方便
?????*?@param?p?
?????*/
????private?getH(p:?AlgoNode,?that:?A_star2):?number?{
????????let?ansMH:?number?=?0;
????????ansMH?+=?Math.abs(p.xCount?-?that.endNode.xCount);
????????ansMH?+=?Math.abs(p.yCount?-?that.endNode.yCount);
????????ansMH?+=?Math.abs(p.zCount?-?that.endNode.zCount)?*?10;
????????return?ansMH;
????}
?
????/**
?????*?尋路的關(guān)鍵函數(shù)
?????*?計(jì)算消耗值
?????*?總消耗F?=?已經(jīng)消耗的G?+?預(yù)計(jì)消耗H
?????*?1.5表示為未來(lái)多做打算，傾向于選取未來(lái)變化小的點(diǎn)即離目標(biāo)近的
?????*?@param?g?
?????*?@param?h?
?????*/
????private?calcF(g:?number,?h:?number):?number?{
????????return?g?+?h?*?1.5;
????}
?
?
一般來(lái)說(shuō)A星算法的F值H值計(jì)算都是個(gè)性化的，這里給出的方法僅供參考。
?
A星在實(shí)際三維中的效果
上面的圖是每步向26個(gè)方向試探，后續(xù)改為6個(gè)方向，感覺(jué)6個(gè)方向就夠了。
?
?
?
