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