三维空间中的寻路算法两则 RRT与A*
三维空间中的寻路算法两则
之一：RRT 名字应该叫做快速随机生长树
算法简介：（你应该可以在网上找到更好的描述）首先起点算作树根，然后随机选取范围内的一个点或者终点，树上离这个点最近的树枝，生长出一个新点，直到可以到达终点。
核心代码：
配合这个网页中的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：无父节点
// this.reachEnd = false;
// this.cost = new Map();
// let randPoint;
// let nearPoint;
// let newPoint;
// for (let iter = 0; iter < this.maxIter; iter++) {
// //产生随机点randPoint
// randPoint = this.getRandPoint(env, endPoint, this.randToGoal);
// //找到最近已知点nearPoint
// let nearPointIndex = this.getNearPointIndex(this.result.tree, randPoint, endPoint);
// nearPoint = this.result.tree.nodeList[nearPointIndex];
// //产生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。设置父节点
// 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;
// }
// }
在网上找到的一些rrt效果
http://lavalle.pl/rrt/gallery_rigid.html
在实际的三维空间中的效果
上面的结果路径如下
换一对起点终点
上面的结果路径如下
对上面的结果“拉直”（不影响寻路过程，仅对结果效果优化）（这个操作是自己想的，要是有前辈发表了类似操作，请告知我这个操作书面的名字）
之二：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)) {
//终点和其父节点加入探索过的点
that.recordNode(that.endNode, nearestNode, that);
that.findAns = true;
//这个停止事件会产生ansLineScene所需要的数据，所以要在绘图之前
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);
//除去已经关闭的点
let neighborsNotClosed: AlgoNode[] = that.getNeighborsNotClosed(neighbors, that);
//除去无法到达的点
let neighborsCanReach = that.getNeighborsCanReach(nearestNode, neighborsNotClosed, that);
//依次处理 无碰撞且未探索过的点
for (let neighbor of neighborsCanReach) {
/**
* 可优化的值，大于0表示有得优化
*/
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);
}
}
//因为超过设定的探索次数而结束
if (iter > that.dm.config.maxIter) {
that.stopSituation("探索次数用尽", 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);
}
/**
* 新路线是否有优化
* @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;
}
/**
* 寻路的关键函数
* 计算未来消耗 曼哈顿距离
* 在z上乘50是考虑到业务场景的上下楼都不方便
* @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;
}
/**
* 寻路的关键函数
* 计算消耗值
* 总消耗F = 已经消耗的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个方向试探，后续改为6个方向，感觉6个方向就够了。
