几何寻路：漏斗算法（Funnel Algorithm）

几何寻路：漏斗算法（Funnel Algorithm）

• 1. 背景介绍
• 2. 算法详解
• 3. 图例解说
• 4. 判断漏斗大小
• 5. 如何确定左右顶点
• • 5.1 左右顶点的概念
  • 5.2 一次遍历确定左右顶点
  • • 5.2.1 算法和证明
    • 5.2.2 图例
• 6. 代码解析
• • 6.1 整体代码结构
  • 6.2 漏斗算法主体代码
  • 6.3 确定左右顶点代码
• 7. 附录：项目代码
• 8. 参考资料
• 9. 免责声明

1. 背景介绍
2. 算法详解
3. 图例解说
4. 判断漏斗大小
5. 如何确定左右顶点
6. 代码解析

1. 背景介绍

如果学过图论的童鞋，应该对寻路，特别是最短路径问题不会感到陌生。比如说最简单的最短路径算法：BFS（广度优先搜索），能解决非负权重边的Dijkstra’s，以及能解决任意权重边的最短路径算法Bellman Ford’s，这些算法都能解决图论中单一源最短路径问题，但是如果现在我们不是在图中了，而是在一个几何图形中，那我们如何计算从给定A点到B点的最短路径问题呢？比如下面这个多边形，我们如何计算从A点到B点的最短路径呢？

既然我们知道图论的最短路径问题，那能不能将多边形转换图，然后使用图论的最短路径算法。问题转化是非常有效解决问题的方法，用已知问题的算法，经过推导来解决未知问题。但是，要将多边形看成图好像不是那么容易，虽然我们可以把多边形看成由无数个点组成的图形，但是因为要处理无数个点，那么没有算法能够中止的，看来我们需要另辟蹊径了。

接下来，我们将会介绍一种非常通用的几何最短路径算法：漏斗算法（Funnel Algorithm），它的应用范围非常的广泛，包括游戏AI寻路，网格寻路，机器人寻路等等。而且它的原理并不复杂，实现也不算特别困难，所以推荐大家好好理解掌握一下哒~

2. 算法详解

首先，我们来看看下面这样一个多边形¹：

绿点表示起点，红点表示终点，蓝色虚线表示两点之间的最短路径。那该如何求得最短路径呢？我们先用内对角线将这个几何图形进行三角剖分：

现在整个多边形都是由一个个三角形构成的，虚线表示内对角线，也可以称之为导航线。同时，起点和终点分别位于三角形ABN和三角形IGH里面。然后我们以起点所在的三角形的两边（AN和AB）为界，与起点（apex）构建一个”漏斗“：

并且我们规定，蓝色边为左边界，红色边为右边界，对应顶点为左顶点和右顶点。那么到此，我们可以引入漏斗算法的伪代码了：

伪代码是不是很短，但到底是什么意思呢？接下来，我们将通过一个图例，让大家明白上面伪代码的含义。

3. 图例解说

我们以上面的图例为例进行讲解，初始状态，我们已经处理了导航边NB，那么接下来需要处理BM（蓝色红色虚线表示新形成的漏斗边界）：

想必通过上面的图例，大家已经明白了漏斗算法的核心思路，我们只需要把经过的导航边的点依次进行处理，然后就可以得到最短路径。这里需要注意一个细节，我们将起点、终点和重置后的起点看成一条导航边来进行处理，这样可以简化代码的逻辑，虽然会有重复处理的情况。那接下来的问题，我们怎们知道漏斗是变大还是变小了呢，还是需要toLeft测试来救场。

4. 判断漏斗大小

同样，我们把左右边界看成有向线段，每条有向线段会将平面分成正负两个平面，那么两条有向线段会将平面分成4个平面，如下图所示：

我们可以观察到：

如果更新的顶点能使漏斗变小，那它相对左边界一定是负，相对右边界一定是正，即平面A；
如果更新的左顶点越过右边界，那它相对左边界一定是负，相对右边界一定也是负，即平面D；
如果更新的右顶点越过左边界，那它相对左边界一定是正，相对右边界一定也是正，即平面B；

更新点是不可能在平面C的，因为这样形成的图形不是经过三角剖分后的多边形。那其余情况都是使漏斗变大的，所以不用更新左右边界。

5. 如何确定左右顶点

5.1 左右顶点的概念

到此，漏斗算法的核心思想大家应该都已经理解了，但在进入到代码讲解之前，我们还需要讲解一个细节，或者说漏斗算法的前置条件：

我们需要按照导航边左右顶点的顺序，依次对漏斗的左右边界进行更新，如果发生边界越界，则更新apex，并重置导航边的状态

不知道有童鞋注意到了嘛？之前的讲解，我们从来没有说过怎么求得左右顶点，只是默认它们已知。而且对于左右顶点的定义也没有解释，根据笔者自己的理解，漏斗算法的左右顶点应该是：

从起点所在三角形的导航边的某一个端点开始，沿着多边形的外边（即非内对角线），所经过的顶点都为同一类顶点，但终点所在三角形，且不在导航边的顶点除外。

描述可能听着有点绕口，我们看看上面的例子：

起点所在的三角形为NAB，其导航边为NB，我们以N点为例，规定它为left vertex，那么沿着多边形的外边（即实线边），所经过的顶点都为left vertex，所以left vertex还有：M，K， J，I，但H除外，因为它在终点所在的三角形，且不在导航边上。同理，B，C，D，E，F，G为right vertex。

好啦，左右顶点的概念我们理解了，但是这该怎们用代码来确定呢？计算机怎们知道那些边是外边，那些边是内对角线？如果我们用这样的思路去实现，那会非常复杂，时间复杂度也很高。所以我们需要一种比较讨巧的方法，只需一次遍历所有导航边就能确定所有的左右顶点。

5.2 一次遍历确定左右顶点

5.2.1 算法和证明

确定左右顶点的方法还是需要我们去观察。我们观察一下上图中左右顶点在导航线上的关系：似乎，导航线连接的两个顶点类型是相反的。

但这只是我们的观察，所以我们不能直接就用，万一只是个例呢？其他多边形不是这样的情况呢？所以上面这个结论需要简单证明一下。

Lemma：导航线连接的两个顶点类型一定是相反的。

Proof：证明这个结论，我们需要使用漏斗算法的一个前置条件：

给定的多边形一定是经过三角剖分的，且每个三角形至多和一个三角形相邻。

这就意味着，平面中的图形除了三角形，没有其他图形，也意味着导航边一定是三角形的一条边。首先，我们先来看看初始情况，即起点所在的三角形：

Case 1，CD是导航边，因为CDB是三角形，那BD一定是外边，且每个三角形只有一个相邻的三角形。同理Case 2，BD是导航边，那CD一定是外边。再往后，就是相同过程的递推，我们可以把三角形CDB看成新的起始状态，进行相同的推导，最后结论得证，导航线连接的两个顶点类型一定是相反的。

至此，确定左右顶点的伪代码也比较简单了：

那么，我们就可以把所有顶点存储到一个数组中，并用索引保存它们的顶点类型，之后执行漏斗算法寻找最短路径即可。但是这里需要注意个细节：我们把起点和终点，或之后重置的起点，都看成一种特别的导航边，也就是说只有这些顶点有left 和 right两种属性，其他顶点只有一种属性。

5.2.2 图例

我们以上面相同的图例，列出起始状态下，所有顶点的类型，我们以数组的形式表示：

我们可以注意到，起始点（S）和终点（E）默认算一条导航边，所以数组的开头和结尾都有它们。好啦，漏斗算法的所有思路都已经讲解完毕，接下面我们来解析一下代码吧~

6. 代码解析

6.1 整体代码结构

老惯例，先展示算法的整体代码结构：

private static final boolean LEFT_POINT = true;
private static final boolean RIGHT_POINT = false;private static
void addPoint( List<Vector> points, List<Boolean> leftOrRight,int ID, HalfEdge edge, boolean LEFT_POINT ) {edge.origin.mappingID = ID;points.add( edge.origin );leftOrRight.add( LEFT_POINT );
}private static
void addPoint( List<Vector> points, List<Boolean> leftOrRight,int ID, Vector point, boolean LEFT_POINT ) {point.mappingID = ID;points.add( point );leftOrRight.add( LEFT_POINT );
}/*** go through portal edges' points*/private static
int getLeftAndRightPoints( DualVertex endTriangle, List<Vector> points,List<Boolean> leftOrRight, int ID ) {// identify the first left and right pointHalfEdge current = endTriangle.shortestNeighbourEdge;addPoint( points, leftOrRight, ID++, current, LEFT_POINT );addPoint( points, leftOrRight, ID++, current.twin, RIGHT_POINT );endTriangle = ( DualVertex ) endTriangle.parent;// step through the shortest path formed by triangleswhile ( endTriangle.shortestNeighbourEdge != null ) {current = endTriangle.shortestNeighbourEdge;// first see this vertex// the face that is is a left or right point is// based on the opposite point connected by the portal edge// i.e. reverse the direction of the opposite pointif ( current.origin.mappingID == -1 ) {assert current.twin.origin.mappingID > -1;addPoint( points, leftOrRight, ID++, current, !leftOrRight.get( current.twin.origin.mappingID ) );}else {// this is a special case,// where several portal edges have one common vertexassert current.origin.mappingID > -1;addPoint( points, leftOrRight, ID++, current.twin, !leftOrRight.get( current.origin.mappingID ) );}endTriangle = ( DualVertex ) endTriangle.parent;}return ID;
}/*** get Left And Right Points for funnel algorithm*/// funnel algorithm
private static
List<Boolean> getLeftAndRightPoints( DualVertex endTriangle, Vector startPoint,Vector endPoint, List<Vector> points ) {List<Boolean> leftOrRight = new ArrayList<>();if ( endTriangle == null ) return leftOrRight;int ID = 0;// add start pointaddPoint( points, leftOrRight, ID++, startPoint, LEFT_POINT );addPoint( points, leftOrRight, ID++, startPoint, RIGHT_POINT );if ( endTriangle.shortestNeighbourEdge != null )ID = getLeftAndRightPoints( endTriangle, points, leftOrRight, ID );// add end pointaddPoint( points, leftOrRight, ID++, endPoint, LEFT_POINT );addPoint( points, leftOrRight, ID++, endPoint, RIGHT_POINT );return leftOrRight;
}/*** add a distinct corner to the path*/private static
void addCorner( List<Vector> visitedVertices, Vector apex ) {if ( !visitedVertices.get( visitedVertices.size() - 1 ).equals( apex ) )visitedVertices.add( apex );
}/*** Funnel Algorithm** Reference resource:* http://digestingduck.blogspot.com/2010/03/simple-stupid-funnel-algorithm.html*/// TODO: 7/14/2021 not support complex polygons
public static
List<Vector> Funnel( DualVertex startTriangle,Vector startPoint, Vector endPoint ) {List<Vector> visitedVertices = new LinkedList<>();if ( startTriangle == null ) return visitedVertices;// add start pointvisitedVertices.add( startPoint );// get "left" and "right" points,// presented as a boolean array// from the shortest path in a dual graphList<Vector> points = new ArrayList<>();List<Boolean> leftOrRight = getLeftAndRightPoints( startTriangle, startPoint, endPoint, points );assert points.size() == leftOrRight.size();// initialize the first funnel,// with the apex and two points// associated with the internal diagonal of endTriangleint left = 0;int right = 1;Vector apex = startPoint;// while endTriangle is not null,// i.e. we have internal diagonals to step throughfor ( int i = 2; i < points.size(); i++ ) {int mappingID = i == points.size() - 2 ?points.get( i ).mappingID - 1 : points.get( i ).mappingID;// do if the new funnel, to say,// the one formed with the apex and the current internal diagonal,// is smaller than or equal to the previous one,// and move it to the current diagonal.// more precisely speaking,// if current point is left point,// and it is on the right side of left boundary of the funnel,// left point ---> startsif ( leftOrRight.get( mappingID ) &&Triangles.areaTwo( apex, points.get( left ), points.get( i ) ) <= 0 ) {// as well as on the left side of the right boundary,// or on the left boundary// then set left boundary of the funnel to this pointif ( apex.equals( points.get( right ) ) ||Triangles.areaTwo( apex, points.get( right ), points.get( i ) ) > 0 ) {left = i;}// else if current point is "left" point,// and it is on the right side of left boundary of the funnel,// but on the right side of the right boundary,// meaning the left boundary crossing the right boundary,// add the apex to the list,// and then set the apex to the left pointelse {visitedVertices.add( apex = points.get( right ) );i = left = right;}}// left point ---> ends// similar steps when current point is "right" point.// but in this case,// we flip directions for the following steps.// right point ---> startsif ( !leftOrRight.get( mappingID ) &&Triangles.areaTwo( apex, points.get( right ), points.get( i ) ) >= 0 ) {if ( apex.equals( points.get( left ) ) ||Triangles.areaTwo( apex, points.get( left ), points.get( i ) ) < 0 ) {right = i;}else {visitedVertices.add( apex = points.get( left ) );i = right = left;}}// right point ---> ends}// add end pointaddCorner( visitedVertices, endPoint );// reset mapping ID to -1Node.resetMappingID( points );// return the corners we've gong though,// including the start point,// but not the end point.//        System.out.println( visitedVertices );return visitedVertices;
}

6.2 漏斗算法主体代码

漏斗算法主体代码比较好理解，基本就是上面思路的直接实现，首先，我们先确定左右顶点：

    List<Vector> visitedVertices = new LinkedList<>();if ( startTriangle == null ) return visitedVertices;// add start pointvisitedVertices.add( startPoint );// get "left" and "right" points,// presented as a boolean array// from the shortest path in a dual graphList<Vector> points = new ArrayList<>();List<Boolean> leftOrRight = getLeftAndRightPoints( startTriangle, startPoint, endPoint, points );

然后依次进行处理：

// initialize the first funnel,
// with the apex and two points
// associated with the internal diagonal of endTriangle
int left = 0;
int right = 1;
Vector apex = startPoint;
// while endTriangle is not null,
// i.e. we have internal diagonals to step through
for ( int i = 2; i < points.size(); i++ ) {int mappingID = i == points.size() - 2 ?points.get( i ).mappingID - 1 : points.get( i ).mappingID;// do if the new funnel, to say,// the one formed with the apex and the current internal diagonal,// is smaller than or equal to the previous one,// and move it to the current diagonal.// more precisely speaking,// if current point is left point,// and it is on the right side of left boundary of the funnel,// left point ---> startsif ( leftOrRight.get( mappingID ) &&Triangles.areaTwo( apex, points.get( left ), points.get( i ) ) <= 0 ) {// as well as on the left side of the right boundary,// or on the left boundary// then set left boundary of the funnel to this pointif ( apex.equals( points.get( right ) ) ||Triangles.areaTwo( apex, points.get( right ), points.get( i ) ) > 0 ) {left = i;}// else if current point is "left" point,// and it is on the right side of left boundary of the funnel,// but on the right side of the right boundary,// meaning the left boundary crossing the right boundary,// add the apex to the list,// and then set the apex to the left pointelse {visitedVertices.add( apex = points.get( right ) );i = left = right;}}// left point ---> ends// similar steps when current point is "right" point.// but in this case,// we flip directions for the following steps.// right point ---> startsif ( !leftOrRight.get( mappingID ) &&Triangles.areaTwo( apex, points.get( right ), points.get( i ) ) >= 0 ) {if ( apex.equals( points.get( left ) ) ||Triangles.areaTwo( apex, points.get( left ), points.get( i ) ) < 0 ) {right = i;}else {visitedVertices.add( apex = points.get( left ) );i = right = left;}}// right point ---> ends
}

如果是左顶点，且在左边界的负平面：

// more precisely speaking,
// if current point is left point,
// and it is on the right side of left boundary of the funnel,
// left point ---> starts
if ( leftOrRight.get( mappingID ) &&Triangles.areaTwo( apex, points.get( left ), points.get( i ) ) <= 0 ) 

如果该顶点在右边界在正平面，或者apex和原右顶点是同一个顶点，则更新漏斗大小，反之，则更新apex，并重置导航边状态：

// as well as on the left side of the right boundary,
// or on the left boundary
// then set left boundary of the funnel to this point
if ( apex.equals( points.get( right ) ) ||Triangles.areaTwo( apex, points.get( right ), points.get( i ) ) > 0 ) {left = i;
}
// else if current point is "left" point,
// and it is on the right side of left boundary of the funnel,
// but on the right side of the right boundary,
// meaning the left boundary crossing the right boundary,
// add the apex to the list,
// and then set the apex to the left point
else {visitedVertices.add( apex = points.get( right ) );i = left = right;
}

实现方法大致和之前我们讲的都是一致的，只有“或者apex和原右顶点是同一个顶点，则更新漏斗大小”没有提到，这里是一个特殊情况，即左右顶点和apex共点的情况，如下图所示：

对于这些共点情况，我们只更新漏斗大小，不更新apex。

6.3 确定左右顶点代码

确定左右顶点的实现代码思路和我们上面讲解的基本一致，把起点和终点看成特殊的导航边，然后处理中间的顶点：

int ID = 0;
// add start point
addPoint( points, leftOrRight, ID++, startPoint, LEFT_POINT );
addPoint( points, leftOrRight, ID++, startPoint, RIGHT_POINT );if ( endTriangle.shortestNeighbourEdge != null )ID = getLeftAndRightPoints( endTriangle, points, leftOrRight, ID );// add end point
addPoint( points, leftOrRight, ID++, endPoint, LEFT_POINT );
addPoint( points, leftOrRight, ID++, endPoint, RIGHT_POINT );

中间顶点的处理方法也是先确定初始左右顶点：

// identify the first left and right point
HalfEdge current = endTriangle.shortestNeighbourEdge;
addPoint( points, leftOrRight, ID++, current, LEFT_POINT );
addPoint( points, leftOrRight, ID++, current.twin, RIGHT_POINT );

然后确定没有定义顶点类型的端点，其类型为另一个端点类型取反：

// step through the shortest path formed by triangles
while ( endTriangle.shortestNeighbourEdge != null ) {current = endTriangle.shortestNeighbourEdge;// first see this vertex// the face that is is a left or right point is// based on the opposite point connected by the portal edge// i.e. reverse the direction of the opposite pointif ( current.origin.mappingID == -1 ) {assert current.twin.origin.mappingID > -1;addPoint( points, leftOrRight, ID++, current, !leftOrRight.get( current.twin.origin.mappingID ) );}else {// this is a special case,// where several portal edges have one common vertexassert current.origin.mappingID > -1;addPoint( points, leftOrRight, ID++, current.twin, !leftOrRight.get( current.origin.mappingID ) );}endTriangle = ( DualVertex ) endTriangle.parent;
}

只是如何用索引建立顶点和类型数组的关系，我的方法比较讨巧和复杂，用的全是ID索引，不是很好理解，有兴趣童鞋可以看看完整代码理解一下，嫌麻烦的童鞋也可以用HashTable无脑建立映射关系。

最后就是while循环里面一级级向上找最短路径经过的三角形，来遍历最短路径经过的内对角线，这个方法和算法是dual graph和BFS，对于本篇文章来说，完全超纲了，同样，有兴趣的童鞋，可以看看这里的系列讲解视频：计算几何课堂：几何寻路之旅，这里就不再赘述。

更详细的代码，大家可以参考项目中的完整代码，有什么不懂的可以随时问我哒~

拓展阅读：

1）单调多边形拆分：如何处理水平线

2）DCEL：如何连接和添加边

7. 附录：项目代码

个人项目代码：Algorithm

8. 参考资料

1. Simple Stupid Funnel Algorithm

9. 免责声明

※ 本文之中如有错误和不准确的地方，欢迎大家指正哒~
※ 此项目仅用于学习交流，请不要用于任何形式的商用用途，谢谢呢；

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！