三維空間中的尋路算法兩則 RRT與A*
?
三維空間中的尋路算法兩則
之一：RRT 名字應該叫做快速隨機生長樹
算法簡介：（你應該可以在網(wǎng)上找到更好的描述）首先起點算作樹根，然后隨機選取范圍內(nèi)的一個點或者終點，樹上離這個點最近的樹枝，生長出一個新點，直到可以到達終點。
核心代碼：
配合這個網(wǎng)頁中的3 1 1食用最佳 https://zhuanlan.zhihu.com/p/51087819
?
//?????????this.result?=?new?Result();
//?????????this.result.tree.nodeList.push(startPoint);?//?0:開始點
//?????????this.result.tree.parentNodeIndex.set(0,?-1);?//?0：無父節(jié)點
//?????????this.reachEnd?=?false;
//?????????this.cost?=?new?Map();
?
//?????????let?randPoint;
//?????????let?nearPoint;
//?????????let?newPoint;
//?????????for?(let?iter?=?0;?iter?<?this.maxIter;?iter++)?{
//?????????????//產(chǎn)生隨機點randPoint
//?????????????randPoint?=?this.getRandPoint(env,?endPoint,?this.randToGoal);
?
//?????????????//找到最近已知點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加入到樹中
//?????????????????this.result.tree.nodeList.push(newPoint.clone());
//?????????????????let?newPointIndex?=?this.result.tree.nodeList.length?-?1;
//?????????????????this.result.tree.parentNodeIndex.set(newPointIndex,?nearPointIndex);?//邊：newPoint和nearPoint。設置父節(jié)點
//?????????????????EventDispatcher.dispatchEvent(EVENT_gotANewFreePoint,?nearPoint.clone(),?newPoint.clone());
?
//?????????????????//如果newPoint和終點距離比較小?就進行終點測試
//?????????????????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
在實際的三維空間中的效果
上面的結(jié)果路徑如下
?
換一對起點終點
上面的結(jié)果路徑如下
對上面的結(jié)果“拉直”（不影響尋路過程，僅對結(jié)果效果優(yōu)化）（這個操作是自己想的，要是有前輩發(fā)表了類似操作，請告知我這個操作書面的名字）
?
?
之二：A*，算法名就叫做A星
算法簡介：帶有預估的貪心。每次尋找一個估價最好的點前進，估價包括已消耗和未來預計消耗。
核心代碼：
配合??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);
????????//開始點
????????that.startNode?=?new?AlgoNode();?//默認xCount=0?所以不用設置了
????????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);
????????????//當前點可以直達終點
????????????if?(nearestNode._point.distanceTo(that.endNode._point)?<?5?*?that.dm.config.stepLen)?{
????????????????if?(that.reachEndPoint(nearestNode,?that))?{
????????????????????//終點和其父節(jié)點加入探索過的點
????????????????????that.recordNode(that.endNode,?nearestNode,?that);
????????????????????that.findAns?=?true;
????????????????????//這個停止事件會產(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)閉的點
????????????let?neighborsNotClosed:?AlgoNode[]?=?that.getNeighborsNotClosed(neighbors,?that);
????????????//除去無法到達的點
????????????let?neighborsCanReach?=?that.getNeighborsCanReach(nearestNode,?neighborsNotClosed,?that);
????????????//依次處理?無碰撞且未探索過的點
????????????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);
????????????????}
????????????}
????????????//因為超過設定的探索次數(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("沒有路了",?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ù)
?????*?計算未來消耗?曼哈頓距離
?????*?在z上乘50是考慮到業(yè)務場景的上下樓都不方便
?????*?@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ù)
?????*?計算消耗值
?????*?總消耗F?=?已經(jīng)消耗的G?+?預計消耗H
?????*?1.5表示為未來多做打算，傾向于選取未來變化小的點即離目標近的
?????*?@param?g?
?????*?@param?h?
?????*/
????private?calcF(g:?number,?h:?number):?number?{
????????return?g?+?h?*?1.5;
????}
?
?
一般來說A星算法的F值H值計算都是個性化的，這里給出的方法僅供參考。
?
A星在實際三維中的效果
上面的圖是每步向26個方向試探，后續(xù)改為6個方向，感覺6個方向就夠了。
?
?
?
