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