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); //终点 }