导航寻路

源码 地址 http://download.csdn.net/download/u012419410/9037849

NAV导航网格寻路（1）-- 介绍  

2010-04-02 13:23:21|  分类： 游戏 |  标签：寻路  nav  navigation  mesh  导航网格  游戏  |举报|字号 订阅

竹石  http://blianchen.blog.163.com/

这篇是翻译的文章，原文 http://www.ai-blog.net/archives/2008_07.html

WayPoint寻路

下图是一个典型的路点寻路

 

另一种方法是使用多边形来记录路径信息，它可以提供更多的信息给ai角色使用。下图就是一个navigation mesh。

 

以下列出几个WayPoint的不足之处：

1. 一些复杂的游戏地图需要的WayPoint数量过于庞大
2. 有时会使角色走“Z”型路径

如下图A点和B点之间的路径

NAV寻路如下图

下图是路点寻路，如黄线，会产生“Z”字形

下图为文章开始时展示的地图的比较，红线是wayPoint寻路，兰线是nav。

3. 不能进行动态修正，如果地图中有动态障碍，处理起来会很困难

     如要实现即时战略游戏中，一辆在路上行走的坦克会挡住你军队的去路，这个移动的坦克就是一个动态障碍。

4. 不能根据角色的特性（不同宽度、高度等）改变路径

    如一个狭窄的通道，普通的人能够通过，而一辆马车的宽度超过通道宽度，应该不能通过。

 5. 不能保存地形的附加信息，如地表高度、地面特征（水面、沙地等）

     比如一个游戏中的角色在走到沙地上时会降低移动速度，或走在一个斜坡上时人物会发生倾斜等。

NAV导航网格寻路（2） -- 寻路方法

竹石  http://blianchen.blog.163.com/

nav寻路一般包含两部分，首先是使用工具根据地图信息生成寻路用的nav mesh，接下来就是在游戏中根据生成的nav mesh来自动寻路。

一般人首先关心的就是寻路方法，所以这里把顺序颠倒下，先说寻路。

一.  使用A*寻找所经过网格路径

 下图为一个已经生成nav网格的地图，深红色区域为不可行走区域，浅红色区域为可以行走的区域。

如下图，现在如果要寻找从A点到B点的路径，首先要从所有可行走的网格中找到一条最优的网格路径（图中紫色的网格），然后再根据这些网格生成所需要的路径点。

计算最优网格路径的方法可以使用流行的A*，也可以使用其它方法。A*算法网上很多就不说了，至于三角网格的A*实现因为涉及网格的数据结构会在系列的最后给出。

 

二.  生成路径点

 nav寻路最常用的就是光照射线法了，这个在neoragex2002的blog上有讲，这里就不说了

http://www.cnblogs.com/neoragex2002/archive/2007/09/09/887556.html

另一种方法就是拐角点法，如下图 

下图的5个凸多边形是已经生成的导航网格，多边形外部的区域为不可行走区域，current为起点，goal为终点，从图中就可以看出最短路径为图中红线，蓝色圈出的点为我们需要找出的点。所有多边形顶点均按逆时针方向存储（这些均在生成导航网格时处理，以后会讲到）。

（1）下图显示出各区域之间的入口，即多边形的临边。由图中可以看出每个临边均为起点穿出该多边形区域的边，故以下称该边为穿出边。

（2）首先找到起始点所在的多边形和穿出边的两个端点，由起点连接两个端点，形成两个线段lineLeft 和lineRight。如下图。绿色圈表示左点，红色表示右点（左点、右点是根据多边形顶点保存顺序而来）。

（3）继续找到下一个穿出边的两个端点，判断新的左点是否在lineLeft 和lineRigh之间，如果在，则更新lineLeft为起点到新左点的线段。

同样处理新穿出边的右点，如下图

该步最后得到两个新的线段，如下图。

（4） 继续判断下一个穿出边的两个端点，如下图，新的左点在lineLeft和lineRight的外面，则不更新线段。

下图说明新的右点在两条直线之间，更新lineRight。

该步最后得到两个新的线段，如下图。

（5） 继续循环判断下一个穿出边的两个端点，该穿出边的两个端点都在lineRight的右侧，表示lineRight的终点即为路径的一个拐角点。

（6） 循环以上步骤都可以找到从起点到终点的一条完整路径。

NAV导航网格寻路（3） -- 一些必要的计算几何知识

竹石  http://blianchen.blog.163.com/

在继续下面的nav网格生成算法之前，先介绍一下涉及到的计算几何知识。这里只罗列出结论，要详细了解参考相关书籍。 

• 矢量加减法：
  　设二维矢量P = ( x1, y1 )，Q = ( x2 , y2 )，则矢量加法定义为： P + Q = ( x1 + x2 , y1 + y2 )，同样的，矢量减法定义为： P - Q = ( x1 - x2 , y1 - y2 )。显然有性质 P + Q = Q + P，P - Q = - ( Q - P )。
• 矢量叉积
  设矢量P = ( x1, y1 )，Q = ( x2, y2 )，则矢量叉积定义为由(0,0)、p1、p2和p1+p2所组成的平行四边形的带符号的面积，即：P × Q = x1*y2 - x2*y1，其结果是一个标量。显然有性质 P × Q = - ( Q × P ) 和 P × ( - Q ) = - ( P × Q )。
• 折线段的拐向判断：
  　　折线段的拐向判断方法可以直接由矢量叉积的性质推出。对于有公共端点的线段p0p1和p1p2，通过计算(p2 - p0) × (p1 - p0)的符号便可以确定折线段的拐向：
  　　若(p2 - p0) × (p1 - p0) > 0,则p0p1在p1点拐向右侧后得到p1p2。
  　　若(p2 - p0) × (p1 - p0) < 0,则p0p1在p1点拐向左侧后得到p1p2。
  　　若(p2 - p0) × (p1 - p0) = 0,则p0、p1、p2三点共线。
• 判断两线段是否相交：
  　　我们分两步确定两条线段是否相交：
  　　(1)快速排斥试验
  　　　　设以线段 P1P2 为对角线的矩形为R， 设以线段 Q1Q2 为对角线的矩形为T，如果R和T不相交，显然两线段不会相交。
  　　(2)跨立试验
  　　　　如果两线段相交，则两线段必然相互跨立对方。若P1P2跨立Q1Q2 ，则矢量 ( P1 - Q1 ) 和( P2 - Q1 )位于矢量( Q2 - Q1 ) 的两侧，即( P1 - Q1 ) × ( Q2 - Q1 ) * ( P2 - Q1 ) × ( Q2 - Q1 ) < 0。上式可改写成( P1 - Q1 ) × ( Q2 - Q1 ) * ( Q2 - Q1 ) × ( P2 - Q1 ) > 0。当 ( P1 - Q1 ) × ( Q2 - Q1 ) = 0 时，说明 ( P1 - Q1 ) 和 ( Q2 - Q1 )共线，但是因为已经通过快速排斥试验，所以 P1 一定在线段 Q1Q2上；同理，( Q2 - Q1 ) ×(P2 - Q1 ) = 0 说明 P2 一定在线段 Q1Q2上。所以判断P1P2跨立Q1Q2的依据是：( P1 - Q1 ) × ( Q2 - Q1 ) * ( Q2 - Q1 ) × ( P2 - Q1 ) >= 0。同理判断Q1Q2跨立P1P2的依据是：( Q1 - P1 ) × ( P2 - P1 ) * ( P2 - P1 ) × ( Q2 - P1 ) >= 0。
  
• 凸多边形
  假设我们在一个多边形上(包括多边形的边界及边界围封的范围)任意取两点并以一条线段连结该两点，如果线段上的每一点均在该多边形上，那么我们便说这个多边形是凸的。
• 凸包
  给定平面上的一个(有限)点集(即一组点)，这个点集的凸包就是包含点集中所有点的最小面积的凸多边形。
  
• 点在凸多边形中的判断
  假设多边形是凸的，而且顶点p0,p1,...,pn按顺时针方向排列，则点在多边形任意一边 pi-1, pi 的右面。
• Voronoi图及对偶图
  
• Delaunay三角剖分（Voronoi对偶图）
  
  在实际中运用的最多的三角剖分是Delaunay三角剖分，它是一种特殊的三角剖分。先从Delaunay边说起：
      【定义】Delaunay边：假设E中的一条边e（两个端点为a,b），e若满足下列条件，则称之为Delaunay边：存在一个圆经过a,b两点，圆内(注意是圆内，圆上最多三点共圆)不含点集V中任何其他的点，这一特性又称空圆特性。
      【定义】Delaunay三角剖分：如果点集V的一个三角剖分T只包含Delaunay边，那么该三角剖分称为Delaunay三角剖分。
  以下是Delaunay剖分所具备的优异特性：
      1.最接近：以最近临的三点形成三角形，且各线段(三角形的边)皆不相交。
      2.唯一性：不论从区域何处开始构建，最终都将得到一致的结果。
      3.最优性：任意两个相邻三角形形成的凸四边形的对角线如果可以互换的话，那么两个三角形六个内角中最小的角度不会变大。
      4.最规则：如果将三角网中的每个三角形的最小角进行升序排列，则Delaunay三角网的排列得到的数值最大。
      5.区域性：新增、删除、移动某一个顶点时只会影响临近的三角形。
      6.具有凸多边形的外壳：三角网最外层的边界形成一个凸多边形的外壳。
• 多边形裁剪
  
  
  Weiler-Athenton算法

–主多边形：被裁剪多边形，记为A 

–裁剪多边形：裁剪窗口，记为B

多边形顶点的排列顺序（使多边形区域位于有向边的左侧 ）外环：逆时针 ；内环：顺时针

主多边形和裁剪多边形把二维平面分成两部分。

内裁剪：A∩B

外裁剪：A-B

裁剪结果区域的边界由A的部分边界和B的部分边界两部分构成，并且在交点处边界发生交替，即由A的边界转至B的边界，或由B的边界转至A的边界。

如果主多边形与裁剪多边形有交点，则交点成对出现，它们被分为如下两类：

进点：主多边形边界由此进入裁剪多边形内  如，I1,I3, I5, I7, I9, I11 

出点：主多边形边界由此离开裁剪多边形区域. 如， I0,I2, I4, I6, I8, I10 

算法步骤

(1)建立空的裁剪结果多边形的顶点表．

(2)选取任一没有被跟踪过的交点为始点，将其输出到结果多边形顶点表中．

(3)如果该交点为进点，跟踪主多边形边边界；否则跟踪裁剪多边形边界．

(4) 跟踪多边形边界，每遇到多边形顶点，将其输出到结果多边形顶点表中，直至遇到新的交点．

(5)将该交点输出到结果多边形顶点表中，并通过连接该交点的双向指针改变跟踪方向（如果上一步跟踪的是主多边形边界，现在改为跟踪裁剪多边形边界；如果上一步跟踪裁剪多边形边界，现在改为跟踪主多边形边界）．

(6)重复(4)、(5)直至回到起点

 

NAV导航网格寻路（4） -- 生成nav网格

竹石  http://blianchen.blog.163.com/

假设上图是一个游戏地图，红色的区域是不可行走的区域，浅灰色区域是可行走区域，要想在游戏中实现nav寻路，必须将可行走区域转化为nav网格并保存为一种固定形式的数据，如下图浅红色的三角形。

nav网格必须是凸多边形，这里使用三角型，当然也可以使用4边形。下面介绍一种任意多边形的三角化算法。算法来自论文《平面多边形域的快速约束 三角化》作者：曾薇 孟祥旭 杨承磊 杨义军。详细内容请参考该论文。

先来看几个定义：

A. 我们称点 p3 为直线 p1p2 的可见点，其必须满足下面三个条件：

（1） p3 在边 p1p2 的右侧 (顶点顺序为顺时针)；

（2） p3 与 p1 可见，即 p1p3 不与任何一个约束边相交；

（3） p3 与 p2 可见

B. DT点

在一个约束Delaunay三角形中，其中与一条边相对的顶点称为该边的DT点。

确定 DT 点的过程如下：

Step1.  构造 Δp1p2p3 的外接圆 C（p1，p2，p3）及其网格包围盒 B（C（p1，p2，p3））

Step2. 依次访问网格包围盒内的每个网格单元：

            对未作当前趟数标记的网格单元进行搜索，并将其标记为当前趟数

           若某个网格单元中存在可见点 p, 并且 ∠p1pp2 > ∠p1p3p2，则令 p3=p1，转Step1；否则，转Step3.

Step3.  若当前网格包围盒内所有网格单元都已被标记为当前趟数，也即C（p1，p2，p3）内无可见点，则 p3 为的 p1p2 的 DT 点

生成Delaunay三角网格算法如下：

Step2. 取任意一条外边界边 p1p2 .

Step3.  计算 DT 点 p3，构成约束 Delaunay 三角形 Δp1p2p3 .

Step4. 如果新生成的边 p1p3 不是约束边，若已经在堆栈中，则将其从中删除；否则，将其放入堆栈；类似地，可处理 p3p2 .

Step5. 若堆栈不空，则从中取出一条边，转Step3；否则，算法停止 .

程序实现该算法（AS3语言）

1。数据结构

不难看出，要想实现该算法首先要确定一些基础对象，如点、线、三角型和多边形等对象。（才发现这个blog中竟然没有代码的格式）

二维点的定义 public class Vector2f {}  包括矢量加减、叉积等方法。

线的定义  public class Line2D {} 包括下面方法：

    classifyPoint(point:Vector2f, epsilon:Number = 0.000001):int
                 判断点与直线的关系，假设你站在a点朝向b点， 则输入点与直线的关系分为：Left, Right or Centered on the line
           equals(line:Line2D):Boolean
                 线段是否相等 （忽略方向）
           getDirection():Vector2f
                  直线方向
           intersection(other:Line2D, pIntersectPoint:Vector2f = null):int
                  判断两个直线关系 this line A = x0, y0 and B = x1, y1 other is A = x2, y2 and B = x3, y3
           length():Number
                  直线长度
           signedDistance(point:Vector2f):Number
                  给定点到直线的带符号距离，从a点朝向b点，右向为正，左向为负  

三角型定义 public class Triangle：

    getSide(sideIndex:int):Line2D
                  取得指定索引的边(从0开始，顺时针)
            getVertex(i:int):Vector2f
                   根据i返回顶点
            isPointIn(testPoint:Vector2f):Boolean
                   测试给定点是否在三角型中
            setVertex(i:int, point:Vector2f):void
                   根据i指定的索引设置三角形的顶点  

多边形定义  public class Polygon：

     public vertexV : Vector.<Vector2f>   //顶点列表，按顺时针方向排序 

     cw():void 
                 将多边形的顶点按逆时针排序
            isCW():Boolean
                 将多边形的顶点按顺时针排序
            isSimplicity():Boolean
                 是否是简单多边形
            rectangle():Rectangle
                  返回矩形包围盒  Polygon 
            union(polygon:Polygon):Vector.<Polygon>
                  合并两个多边形(Weiler-Athenton算法)  

三角化的完整代码，注释比较全，就不再详细解释了 
/*
* @author 白连忱 
* date Jan 22, 2010
*/
package org.blch.geom
{
    import flash.display.Sprite;
    import flash.geom.Rectangle;
    import flash.text.TextField;
    import flash.text.TextFieldAutoSize;
    import flash.text.TextFormat;
    
    /**
     * Delaunay
     * @langversion ActionScript 3.0
     * @playerversion Flash 10.0
     */
    public class Delaunay
    {
        public function Delaunay()
        {
        }
        
        private static const EPSILON:Number = 0.000001;    //精度
        
        private var polygonV:Vector.<Polygon>;        //所有多边形，第0个元素为区域外边界 (输入数据)
        
        private var vertexV:Vector.<Vector2f>;        //所有顶点列表, 前outEdgeVecNmu个为外边界顶点
        private var edgeV:Vector.<Line2D>;            //所有约束边
        
        private var outEdgeVecNmu:int;            //区域外边界顶点数
        
        private  var lineV:Vector.<Line2D>;    //线段堆栈
        
        private var triangleV:Vector.<Triangle>;     //生成的Delaunay三角形
        
        public function createDelaunay(polyV:Vector.<Polygon>):Vector.<Triangle> {
            //Step1.     建立单元大小为 E*E 的均匀网格，并将多边形的顶点和边放入其中.
            //            其中 E=sqrt(w*h/n)，w 和 h 分别为多边形域包围盒的宽度、高度，n 为多边形域的顶点数 .
            initData(polyV);
            
            //Step2.    取任意一条外边界边 p1p2 .
            var initEdge:Line2D = getInitOutEdge();
            lineV.push(initEdge);
            
            var edge:Line2D;
            do {
                //Step3.     计算 DT 点 p3，构成约束 Delaunay 三角形 Δp1p2p3 .
                edge = lineV.pop();
//                trace("开始处理edge###########:", edge);
                var p3:Vector2f = findDT(edge);
                if (p3 == null) continue;
                var line13:Line2D = new Line2D(edge.getPointA(), p3);
                var line32:Line2D = new Line2D(p3, edge.getPointB());
                
                //Delaunay三角形放入输出数组
                var trg:Triangle = new Triangle(edge.getPointA(), edge.getPointB(), p3);
                
//                trace("DT 点p3", p3);
//                trace("Triangle", trg);
                triangleV.push(trg);
                
                //Step4.    如果新生成的边 p1p3 不是约束边，若已经在堆栈中，
                //            则将其从中删除；否则，将其放入堆栈；类似地，可处理 p3p2 .
                var index:int;
                if (indexOfVector(line13, this.edgeV) < 0) {
                    index = indexOfVector(line13, lineV);
                    if (index > -1) {
                        lineV.splice(index, 1);
                    } else {
                        lineV.push(line13);
                    }
                }
                if (indexOfVector(line32, this.edgeV) < 0) {
                    index = indexOfVector(line32, lineV);
                    if (index > -1) {
                        lineV.splice(index, 1);
                    } else {
                        lineV.push(line32);
                    }
                }
            
            //Step5.    若堆栈不空，则从中取出一条边，转Step3；否则，算法停止 .
//                trace("处理结束edge###########\n");
            } while (lineV.length > 0);
                
            return triangleV;
        }
        
        /**
         * 初始化数据
         * @param polyV
         */        
        private function initData(polyV:Vector.<Polygon>):void {
            //填充顶点和线列表
            vertexV = new Vector.<Vector2f>();
            edgeV = new Vector.<Line2D>();
            var poly:Polygon;
            for (var i:int=0; i<polyV.length; i++) {
                poly = polyV[i];
                putVertex(vertexV, poly.vertexV);
                putEdge(edgeV, poly.vertexV);
            }
            
            outEdgeVecNmu = polyV[0].vertexNmu;
            
            lineV = new Vector.<Line2D>();
            triangleV = new Vector.<Triangle>();
        }
        
        /**
         * 获取初始外边界
         * @return 
         */        
        private function getInitOutEdge():Line2D {
            var initEdge:Line2D = edgeV[0];
            //检查是否有顶点p在该边上，如果有则换一个外边界
            var loopSign:Boolean;
            var loopIdx:int = 0;
            do {
                loopSign = false;
                loopIdx++;
                for each (var testV:Vector2f in this.vertexV) {
                    if ( testV.equals(initEdge.getPointA()) || testV.equals(initEdge.getPointB()) ) continue;
                    if (initEdge.classifyPoint(testV, EPSILON) == PointClassification.ON_LINE) {
                        loopSign = true;
                        initEdge = edgeV[loopIdx];
                        break;
                    }
                }
            } while (loopSign && loopIdx<outEdgeVecNmu-1);    //只取外边界
            return initEdge;
        }
        
        /**
         * 将srcV中的点放入dstV
         * @param dstV
         * @param srcV
         */        
        private function putVertex(dstV:Vector.<Vector2f>, srcV:Vector.<Vector2f>):void {
            for each (var item:Vector2f in srcV) {
                dstV.push(item);
            }
        }
        
        /**
         * 根据srcV中的点生成多边形线段，并放入dstV
         * @param dstV
         * @param srcV
         */        
        private function putEdge(dstV:Vector.<Line2D>, srcV:Vector.<Vector2f>):void {
            if (srcV.length < 3) return;    //不是一个多边形
            
            var p1:Vector2f = srcV[0];
            var p2:Vector2f;
            for (var i:int=1; i<srcV.length; i++) {
                p2 = srcV[i];
                dstV.push(new Line2D(p1, p2));
                p1 = p2;
            }
            p2 = srcV[0];
            dstV.push(new Line2D(p1, p2));
        }
        
        /**
         * 判断线段是否是约束边
         * @param line
         * @return 线段的索引，如果没有找到，返回-1
         */        
        private function indexOfVector(line:Line2D, vector:Vector.<Line2D>):int {
            var lt:Line2D;
            for (var i:int=0; i<vector.length; i++) {
                lt = vector[i];
                if (lt.equals(line)) return i;
            }
            return -1;
        }
        
        /**
         * 计算 DT 点
         * @param line
         * @return 
         */        
        private function findDT(line:Line2D):Vector2f {
            var p1:Vector2f = line.getPointA();
            var p2:Vector2f = line.getPointB();
            
            //搜索所有可见点             TODO 按y方向搜索距线段终点最近的点
            var allVPoint:Vector.<Vector2f> = new Vector.<Vector2f>();        // line的所有可见点
            for each (var vt:Vector2f in this.vertexV) {
                if (isVisiblePointOfLine(vt, line)) {
                    allVPoint.push(vt);
                }
            }

            if (allVPoint.length == 0) return null;
            
            var p3:Vector2f = allVPoint[0];    

            var loopSign:Boolean = false;
            do {
                loopSign = false;
                
                //Step1. 构造 Δp1p2p3 的外接圆 C（p1，p2，p3）及其网格包围盒 B（C（p1，p2，p3））
                var circle:Circle = this.circumCircle(p1, p2, p3);
                var boundsBox:Rectangle = this.circleBounds(circle);
                
                //Step2. 依次访问网格包围盒内的每个网格单元：
                //         若某个网格单元中存在可见点 p, 并且 ∠p1pp2 > ∠p1p3p2，则令 p3=p，转Step1；否则，转Step3.
                var angle132:Number = Math.abs(lineAngle(p1, p3, p2));    // ∠p1p3p2
                for each (var vec:Vector2f in allVPoint) {
                    if ( vec.equals(p1) || vec.equals(p2) || vec.equals(p3) ) {
                        continue;
                    }
                    //不在包围盒中
                    if (boundsBox.contains(vec.x, vec.y) == false) {
                        continue;
                    }
                    
                    //夹角
                    var a1:Number = Math.abs(lineAngle(p1, vec, p2));
                    if (a1 > angle132) {
                        /////转Step1
                        p3 = vec;
                        loopSign = true;
                        break;
                    }
                }
                ///////转Step3
            } while (loopSign); 
            
            //Step3. 若当前网格包围盒内所有网格单元都已被处理完，
            //         也即C（p1，p2，p3）内无可见点，则 p3 为的 p1p2 的 DT 点
            return p3;
        }
        
        /**
         * 返回顶角在o点，起始边为os，终止边为oe的夹角, 即∠soe (单位：弧度) 
         * 角度小于pi，返回正值;   角度大于pi，返回负值 
         */        
        private function lineAngle(s:Vector2f, o:Vector2f, e:Vector2f):Number 
        { 
            var cosfi:Number, fi:Number, norm:Number; 
            var dsx:Number = s.x - o.x; 
            var dsy:Number = s.y - o.y; 
            var dex:Number = e.x - o.x; 
            var dey:Number = e.y - o.y; 
            
            cosfi = dsx*dex + dsy*dey; 
            norm = (dsx*dsx + dsy*dsy) * (dex*dex + dey*dey); 
            cosfi /= Math.sqrt( norm ); 
            
            if (cosfi >=  1.0 ) return 0; 
            if (cosfi <= -1.0 ) return -Math.PI; 
            
            fi = Math.acos(cosfi); 
            if (dsx*dey - dsy*dex > 0) return fi;      // 说明矢量os 在矢量 oe的顺时针方向 
            return -fi; 
        } 
        
        /**
         * 返回圆的包围盒
         * @param c
         * @return 
         */        
        private function circleBounds(c:Circle):Rectangle {
            return new Rectangle(c.center.x-c.r, c.center.y-c.r, c.r*2, c.r*2);
        }
        
        /**
         * 返回三角形的外接圆
         * @param p1
         * @param p2
         * @param p3
         * @return 
         */        
        private function circumCircle(p1:Vector2f, p2:Vector2f, p3:Vector2f):Circle {
            var m1:Number,m2:Number,mx1:Number,mx2:Number,my1:Number,my2:Number;
            var dx:Number,dy:Number,rsqr:Number,drsqr:Number;
            var xc:Number, yc:Number, r:Number;
            
            /* Check for coincident points */
            
            if ( Math.abs(p1.y-p2.y) < EPSILON && Math.abs(p2.y-p3.y) < EPSILON )
            {
                trace("CircumCircle: Points are coincident.");
                return null;
            }
            
            m1 = - (p2.x - p1.x) / (p2.y - p1.y);
            m2 = - (p3.x-p2.x) / (p3.y-p2.y);
            mx1 = (p1.x + p2.x) / 2.0;
            mx2 = (p2.x + p3.x) / 2.0;
            my1 = (p1.y + p2.y) / 2.0;
            my2 = (p2.y + p3.y) / 2.0;
            
            if ( Math.abs(p2.y-p1.y) < EPSILON ) {
                xc = (p2.x + p1.x) / 2.0;
                yc = m2 * (xc - mx2) + my2;
            } else if ( Math.abs(p3.y - p2.y) < EPSILON ) {
                xc = (p3.x + p2.x) / 2.0;
                yc = m1 * (xc - mx1) + my1;    
            } else {
                xc = (m1 * mx1 - m2 * mx2 + my2 - my1) / (m1 - m2);
                yc = m1 * (xc - mx1) + my1;
            }
            
            dx = p2.x - xc;
            dy = p2.y - yc;
            rsqr = dx*dx + dy*dy;
            r = Math.sqrt(rsqr);
            
            return new Circle(new Vector2f(xc, yc), r);
        }
        
        /**
         * 判断点vec是否为line的可见点
         * @param vec
         * @param line
         * @return true:vec是line的可见点
         */        
        private function isVisiblePointOfLine(vec:Vector2f, line:Line2D):Boolean {
            if (vec.equals(line.getPointA()) || vec.equals(line.getPointB())) {
                return false;
            }
            
            //（1） p3 在边 p1p2 的右侧 (多边形顶点顺序为顺时针)；
            if (line.classifyPoint(vec, EPSILON) != PointClassification.RIGHT_SIDE)
            {
                return false;
            }
            
            //（2） p3 与 p1 可见，即 p1p3 不与任何一个约束边相交；
            if (isVisibleIn2Point(line.getPointA(), vec) == false) {
                return false;
            }
            
            //（3） p3 与 p2 可见
            if (isVisibleIn2Point(line.getPointB(), vec) == false) {
                return false;
            }
            
            return true;
        }
        
        /**
         * 点pa和pb是否可见(pa和pb构成的线段不与任何约束边相交，不包括顶点)
         * @param pa
         * @param pb
         * @return 
         */
        private function isVisibleIn2Point(pa:Vector2f, pb:Vector2f):Boolean {
            var linepapb:Line2D = new Line2D(pa, pb);
            var interscetVector:Vector2f = new Vector2f();        //线段交点
            for each (var lineTmp:Line2D in this.edgeV) {
                //两线段相交
                if (linepapb.intersection(lineTmp, interscetVector) == LineClassification.SEGMENTS_INTERSECT) {
                    //交点是不是端点
                    if ( !pa.equals(interscetVector) && !pb.equals(interscetVector) ) {
                        return false;
                    }
                }
            }
            return true;
        }
        
    }
}

import org.blch.geom.Vector2f;
/**
 * 圆
 * @author blc
 */
class Circle {
    public var center:Vector2f;        //圆心
    public var r:Number;            //半径
        
    public function Circle(cen:Vector2f, r:Number) {
        this.center = cen;
        this.r = r;
    }
}

NAV导航网格寻路（5） -- 生成网格的一些补充

竹石  http://blianchen.blog.163.com/

如果你也实现了上一章提到的代码，不难发现对下图的两种情况会出现问题

左面的是两个区域有相交的情况，右面的是多边形本身有自交，在这两种情况下，前面给出的代码均会产生错误的结果。

对于两个多边形相交，可以在生成网格之前先合并多边形，合并后如图

合并算法在前面多边形剪裁处已给出一个，这里只贴上代码：

         /**
         * 合并两个多边形(Weiler-Athenton算法)
         * @param polygon
         * @return 
         *             null--两个多边形不相交，合并前后两个多边形不变
         *             Polygon--一个新的多边形
         */        
        public function union(polygon:Polygon):Vector.<Polygon> {
            //包围盒不相交
            if (rectangle().intersection(polygon.rectangle()) == false) {
                return null;
            }
            
            //所有顶点和交点
            var cv0:Vector.<Node> = new Vector.<Node>();//主多边形
            var cv1:Vector.<Node> = new Vector.<Node>();//合并多边形
            //初始化
            var node:Node;
            for (var i:int=0; i<this.vertexV.length; i++) {
                node = new Node(this.vertexV[i], false, true);
                if (i > 0) {
                    cv0[i-1].next = node;
                }
                cv0.push(node);
            }
            for (var j:int=0; j<polygon.vertexV.length; j++) {
                node = new Node(polygon.vertexV[j], false, false);
                if (j > 0) {
                    cv1[j-1].next = node;
                }
                cv1.push(node);
            }
            
            //插入交点
            var insCnt:int = this.intersectPoint(cv0, cv1);
            
            //生成多边形
            if (insCnt > 0) {
                //顺时针序
                return linkToPolygon(cv0, cv1);
            } else {
                return null;
            }
            
            return null;
        }
        
        /**
         * 生成多边形，顺时针序； 生成的内部孔洞多边形为逆时针序
         * @param cv0
         * @param cv1
         * @return 合并后的结果多边形数组(可能有多个多边形)
         */        
        private function linkToPolygon(cv0:Vector.<Node>, cv1:Vector.<Node>):Vector.<Polygon> {
            trace("linkToPolygon***linkToPolygon");
            //保存合并后的多边形数组
            var rtV:Vector.<Polygon> = new Vector.<Polygon>();
            
            //1. 选取任一没有被跟踪过的交点为始点，将其输出到结果多边形顶点表中．
            for each (var testNode:Node in cv0) {
                if (testNode.i == true && testNode.p == false) {
                    var rcNodes:Vector.<Vector2f> = new Vector.<Vector2f>();
                    while (testNode != null) {
                        
                        testNode.p = true;
                        
                        // 如果是交点
                        if (testNode.i == true) {
                            testNode.other.p = true;
                            
                            if (testNode.o == false) {        //该交点为进点（跟踪裁剪多边形边界）
                                if (testNode.isMain == true) {        //当前点在主多边形中
                                    testNode = testNode.other;        //切换到裁剪多边形中
                                }
                            } else {                    //该交点为出点（跟踪主多边形边界）
                                if (testNode.isMain == false) {        //当前点在裁剪多边形中
                                    testNode = testNode.other;        //切换到主多边形中
                                }
                            }
                        }
                        
                        rcNodes.push(testNode.v);          ////// 如果是多边形顶点，将其输出到结果多边形顶点表中
                        
                        if (testNode.next == null) {    //末尾点返回到开始点
                            if (testNode.isMain) {
                                testNode = cv0[0];
                            } else {
                                testNode = cv1[0];
                            }
                        } else {
                            testNode = testNode.next;
                        }
                        
                        //与首点相同，生成一个多边形
                        if (testNode.v.equals(rcNodes[0])) break;
                    }
                    //提取
                    rtV.push(new Polygon(rcNodes.length, rcNodes));
                }
            }
            
            trace("rtV", rtV);
            return rtV;
        }
        
        /**
         * 生成交点，并按顺时针序插入到顶点表中
         * @param cv0 （in/out）主多边形顶点表，并返回插入交点后的顶点表
         * @param cv1 （in/out）合并多边形顶点表，并返回插入交点后的顶点表
         * @return 交点数
         */        
        private function intersectPoint(cv0:Vector.<Node>, cv1:Vector.<Node>):int {
            var insCnt:int = 0;        //交点数
            
            var findEnd:Boolean = false;
            var startNode0:Node = cv0[0];
            var startNode1:Node;
            var line0:Line2D;
            var line1:Line2D;
            var ins:Vector2f;
            var hasIns:Boolean;
            var result:int;        //进出点判断结果
            while (startNode0 != null) {        //主多边形
                if (startNode0.next == null) {  //最后一个点，跟首点相连
                    line0 = new Line2D(startNode0.v, cv0[0].v);
                } else {
                    line0 = new Line2D(startNode0.v, startNode0.next.v);
                }
                
                startNode1 = cv1[0];
                hasIns = false;
                
                while (startNode1 != null) {        //合并多边形
                    if (startNode1.next == null) {
                        line1 = new Line2D(startNode1.v, cv1[0].v);
                    } else {
                        line1 = new Line2D(startNode1.v, startNode1.next.v);
                    }
                    ins = new Vector2f();    //接受返回的交点
                    //有交点
                    if (line0.intersection(line1, ins) == LineClassification.SEGMENTS_INTERSECT) {
                        //忽略交点已在顶点列表中的
                        if (this.getNodeIndex(cv0, ins) == -1) {
                            insCnt++;
                            
                            ///////// 插入交点
                            var node0:Node = new Node(ins, true, true);
                            var node1:Node = new Node(ins, true, false);
                            cv0.push(node0);
                            cv1.push(node1);
                            //双向引用
                            node0.other = node1;
                            node1.other = node0;
                            //插入
                            node0.next = startNode0.next;
                            startNode0.next = node0;
                            node1.next = startNode1.next;
                            startNode1.next = node1;
                            //出点
                            if (line0.classifyPoint(line1.getPointB()) == PointClassification.RIGHT_SIDE) {
                                node0.o = true;
                                node1.o = true;
                            }
                            //TODO 线段重合
//                            trace("交点****", node0);
                            
                            hasIns = true;        //有交点
                            
                            //有交点，返回重新处理
                            break;
                        }
                    }
                    startNode1 = startNode1.next;
                }
                //如果没有交点继续处理下一个边，否则重新处理该点与插入的交点所形成的线段
                if (hasIns == false) {
                    startNode0 = startNode0.next;
                }
            }
            return insCnt;
        }
        
        /**
         * 取得节点的索引(合并多边形用)
         * @param cv
         * @param node
         * @return 
         */        
        private function getNodeIndex(cv:Vector.<Node>, node:Vector2f):int {
            for (var i:int=0; i<cv.length; i++) {
                if (cv[i].v.equals(node)) {
                    return i;
                }
            }
            return -1;
        }

对于一个给定的多边形数组polygonV，可以象下面这样在三角化以前做预处理

        /**
         * 合并
         */        
        private function unionAll():void {
            for (var n:int=1; n<polygonV.length; n++) {
                var p0:Polygon = polygonV[n];
                for (var m:int=1; m<polygonV.length; m++) {
                    var p1:Polygon = polygonV[m];
                    if (p0 != p1 && p0.isCW() && p1.isCW()) {
                        var v:Vector.<Polygon> = p0.union(p1);    //合并
                        
                        if (v != null && v.length > 0) {
                            trace("delete");
                            polygonV.splice(polygonV.indexOf(p0), 1);
                            polygonV.splice(polygonV.indexOf(p1), 1);

                            for each (var pv:Polygon in v) {
                                polygonV.push(pv);
                            }
                            
                            n = 1;    //重新开始
                            break;
                        }
                    }
                }
            }
        }

对于多边形自交，可以采用类似多边形剪裁的方法将一个多边形拆分成多个，也可以直接禁止用户绘制这种多边形。我是采用后一种方法所以这里没有代码。

多边形的顶点顺序，上面多处代码都要求多边形顶点按顺时针或逆时针方向保存，但是我们不可能要求用户按哪个固定方向绘制图形，那么怎么判断多边形的顶点顺序。方法的基本思路就是取一个多边形的凸点，然后判断这个点的下一个点的方向，代码如下：

        /**
         * 将多边形的顶点按顺时针排序
         */        
        public function cw():void {
            if (this.isCW() == false) {    //如果为逆时针顺序， 反转为顺时针
                this.vertexV.reverse();    //反转数组
            }
        }
        
        /**
         * clockwise
         * @return true -- clockwise; false -- counter-clockwise
         */        
        public function isCW():Boolean {
            if (vertexV == null || vertexV.length < 0) return false;
            
            //最上（y最小）最左（x最小）点， 肯定是一个凸点
            //寻找最上点
            var topPt:Vector2f = this.vertexV[0];
            var topPtId:int = 0;    //点的索引
            for (var i:int=1; i<vertexV.length; i++) {
                if (topPt.y > vertexV[i].y) {
                    topPt = vertexV[i];
                    topPtId = i;
                } else if (topPt.y == vertexV[i].y) { //y相等时取x最小
                    if (topPt.x > vertexV[i].x) {
                        topPt = vertexV[i];
                        topPtId = i;
                    }
                }
            }
            
            //凸点的邻点
            var lastId:int = topPtId-1>=0 ? topPtId-1 : vertexV.length-1;
            var nextId:int = topPtId+1>=vertexV.length ? 0 : topPtId+1;
            var last:Vector2f = vertexV[lastId];
            var next:Vector2f = vertexV[nextId];
            
            //判断
            var r:Number = multiply(last, next, topPt);
            if (r < 0) {
                return true;
            //三点共线情况不存在，若三点共线则说明必有一点的y（斜线）或x（水平线）小于topPt
            }
            
            return false;
        }
        
        /**
         * r=multiply(sp,ep,op),得到(sp-op)*(ep-op)的叉积 
         * r>0:ep在矢量opsp的逆时针方向； 
         * r=0：opspep三点共线； 
         * r<0:ep在矢量opsp的顺时针方向 
         * @param sp 
         * @param ep 
         * @param op 
         * @return 
         */        
        private function multiply(sp:Vector2f, ep:Vector2f, op:Vector2f):Number { 
            return((sp.x-op.x)*(ep.y-op.y)-(ep.x-op.x)*(sp.y-op.y)); 
        } 

到此生成网格的关键代码都发出来了，下一章放上寻路的代码

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

竹石  http://blianchen.blog.163.com/

前面已经介绍了寻路的方法，现在给出我的一个实现。

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导航网格寻路（7） -- 代码和一些优化

竹石  http://blianchen.blog.163.com/

这里发不了源码，本系列完整源码可以到http://bbs.9ria.com/thread-49841-1-1.html下。

       看下图，最优路径应该是从上面绕过中间阻挡区，而实际寻路产生的路径确是下面。这是由于，在网格面积过大或有某边长度过长时，由于a*中的花费是计算网格的两边中点距离而不实际的路径长度，所以产生的路径偏差较大。所以在一般的网格生成算法中都会加入网格最小面积和最大面积的限制，如果生成的网格小于最小面积时则抛弃，如果大于最大面积则拆分。

一般地图中都会产生孤岛，如下图，两个阻挡区域交叉，合并后在中间深红色区域产生孤岛。按照上面介绍的剪裁算法，中间孤岛多边形会以逆时针方向存储（其他顺时针），因此在这一步能够判断出是否生成孤岛。对于生成的孤岛，如果游戏允许人物可以通过瞬移等方式进入孤岛，那么孤岛也需要生成nav网格，不过孤岛的nav网格和其他区域的网格最好能分成不同区域，这样在寻路时就可以优化（起点和终点不在同区域，可直接确定为不能通过）。