NAV导航网格寻路（6） -- 寻路实现

这篇是转的文章，原文 http://blianchen.blog.163.com/blog/static/13105629920103911258517/

 
前面已经介绍了寻路的方法，现在给出我的一个实现。
A*寻找网格路径
A*算法就不说了，网上很多，这里只说下三角形网格如何使用A*算法，如下图，绿色直线代表最终路径和方向，路径线进入三角形的边称为穿入边，路径线出去的边称为穿出边。每个三角形的花费（g值）采用穿入边和穿出边的中点的距离（图中红线），至于估价函数（h值）使用该三角形的中心点（3个顶点的平均值）到路径终点的x和y方向的距离。
 



下面只贴出关键代码

private var m_CellVector:Vector.<Cell>;

private var openList:Heap;     //二叉堆
private var closeList:Array;

/**
* 构建网格路径，该方法生成closeList
* @param startCell 起始点所在网格
* @param startPos 起始点坐标
* @param endCell 终点所在网格
* @param endPos 终点坐标
* @return
*/

public function buildPath(startCell:Cell, startPos:Vector2f,
endCell:Cell, endPos:Vector2f):void{
openList.clear();
closeList.length = 0;

openList.put(endCell);
endCell.f = 0;
endCell.h = 0;
endCell.isOpen = false;
endCell.parent = null;
endCell.sessionId = pathSessionId;

var foundPath:Boolean = false;        //是否找到路径
var currNode:Cell;                //当前节点
var adjacentTmp:Cell = null;    //当前节点的邻接三角型
while (openList.size > 0) {
// 1. 把当前节点从开放列表删除, 加入到封闭列表
currNode = openList.pop();
closeList.push(currNode);

//路径是在同一个三角形内
if (currNode == startCell) {
foundPath = true;
break;
}

// 2. 对当前节点相邻的每一个节点依次执行以下步骤:
//所有邻接三角型
var adjacentId:int;
for (var i:int=0; i<3; i++) {
adjacentId = currNode.links[i];
// 3. 如果该相邻节点不可通行或者该相邻节点已经在封闭列表中,
//    则什么操作也不执行,继续检验下一个节点;
if (adjacentId < 0) {                        //不能通过
continue;
} else {
adjacentTmp = m_CellVector[adjacentId];            //m_CellVector 保存所有网格的数组
}

if (adjacentTmp != null) {
if (adjacentTmp.sessionId != pathSessionId) {
// 4. 如果该相邻节点不在开放列表中,则将该节点添加到开放列表中,
//    并将该相邻节点的父节点设为当前节点,同时保存该相邻节点的G和F值;
adjacentTmp.sessionId = pathSessionId;
adjacentTmp.parent = currNode;
adjacentTmp.isOpen = true;

//H和F值
adjacentTmp.computeHeuristic(startPos);

//m_WallDistance 是保存三角形各边中点连线的距离，共3个
adjacentTmp.f = currNode.f + adjacentTmp.m_WallDistance[Math.abs(i - currNode.m_ArrivalWall)];

//放入开放列表并排序
openList.put(adjacentTmp);

// remember the side this caller is entering from
adjacentTmp.setAndGetArrivalWall(currNode.index);
} else {
// 5. 如果该相邻节点在开放列表中,
//    则判断若经由当前节点到达该相邻节点的G值是否小于原来保存的G值,
//    若小于,则将该相邻节点的父节点设为当前节点,并重新设置该相邻节点的G和F值
if (adjacentTmp.isOpen) {//已经在openList中
if (currNode.f + adjacentTmp.m_WallDistance[Math.abs(i - currNode.m_ArrivalWall)] < adjacentTmp.f) {
adjacentTmp.f = currNode.f;
adjacentTmp.parent = currNode;

// remember the side this caller is entering from
adjacentTmp.setAndGetArrivalWall(currNode.index);
}
} else {//已在closeList中
adjacentTmp = null;
continue;
}
}
}
}
}

}

 
 
由close list取出网格路径

/**
* 路径经过的网格
* @return
*/
private function getCellPath():Vector.<Cell> {
var pth:Vector.<Cell> = new Vector.<Cell>();

var st:Cell = closeList[closeList.length-1];
pth.push(st);

while (st.parent != null) {
pth.push(st.parent);
st = st.parent;
}
return pth;
}

 
 
根据网格路径计算路径点
算法前面已经详细说明，以下是代码
 

/**
* 根据经过的三角形返回路径点(下一个拐角点法)
* @param start
* @param end
* @return Point数组
*/
private function getPath(start:Vector2f, end:Vector2f):Array {
//经过的三角形
var cellPath:Vector.<Cell> = getCellPath();
//没有路径
if (cellPath == null || cellPath.length == 0) {
return null;
}

//保存最终的路径（Point数组）
var pathArr:Array = new Array();

//开始点
pathArr.push(start.toPoint());
//起点与终点在同一三角形中
if (cellPath.length == 1) {
pathArr.push(end.toPoint());    //结束点
return pathArr;
}

//获取路点
var wayPoint:WayPoint = new WayPoint(cellPath[0], start);
while (!wayPoint.position.equals(end)) {
wayPoint = this.getFurthestWayPoint(wayPoint, cellPath, end);
pathArr.push(wayPoint.position);
}

return pathArr;
}

/**
* 下一个拐点
* @param wayPoint 当前所在路点
* @param cellPath 网格路径
* @param end 终点
* @return
*/
private function getFurthestWayPoint(wayPoint:WayPoint, cellPath:Vector.<Cell>, end:Vector2f):WayPoint {
var startPt:Vector2f = wayPoint.position;    //当前所在路径点
var cell:Cell = wayPoint.cell;
var lastCell:Cell = cell;
var startIndex:int = cellPath.indexOf(cell);    //开始路点所在的网格索引
var outSide:Line2D = cell.sides[cell.m_ArrivalWall];    //路径线在网格中的穿出边
var lastPtA:Vector2f = outSide.getPointA();
var lastPtB:Vector2f = outSide.getPointB();
var lastLineA:Line2D = new Line2D(startPt, lastPtA);
var lastLineB:Line2D = new Line2D(startPt, lastPtB);
var testPtA:Vector2f, testPtB:Vector2f;        //要测试的点
for (var i:int=startIndex+1; i<cellPath.length; i++) {
cell = cellPath[i];
outSide = cell.sides[cell.m_ArrivalWall];
if (i == cellPath.length-1) {
testPtA = end;
testPtB = end;
} else {
testPtA = outSide.getPointA();
testPtB = outSide.getPointB();
}

if (!lastPtA.equals(testPtA)) {
if (lastLineB.classifyPoint(testPtA, EPSILON) == PointClassification.RIGHT_SIDE) {
//路点
return new WayPoint(lastCell, lastPtB);
} else {
if (lastLineA.classifyPoint(testPtA, EPSILON) != PointClassification.LEFT_SIDE) {
lastPtA = testPtA;
lastCell = cell;
//重设直线
lastLineA.setPointB(lastPtA);
}
}
}

if (!lastPtB.equals(testPtB)) {
if (lastLineA.classifyPoint(testPtB, EPSILON) == PointClassification.LEFT_SIDE) {
//路径点
return new WayPoint(lastCell, lastPtA);
} else {
if (lastLineB.classifyPoint(testPtB, EPSILON) != PointClassification.RIGHT_SIDE) {
lastPtB = testPtB;
lastCell = cell;
//重设直线
lastLineB.setPointB(lastPtB);
}
}
}
}
return new WayPoint(cellPath[cellPath.length-1], end);    //终点
}

 

标签: NAV导航网格寻路