python 多边形内部最短路

需求描述

给定一个多边形的顶点，求出从该多边形内部一点sPos出发，到达该多边形内部一点ePos的最短路径。

基本思想

连接sPos、ePos为一线段，记为Lse。若Lse没有穿过该多边形的任意一条边，则直接返回Lse。否则查找被Lse穿过的边，分别递归查找从该边两侧通过的后续路径，选出最短路径。

细节说明

1.基本定义

A.当一个点处在多边形任意一边上，视为该点在多边形内。

B.路径与多边形边可以重叠。

2.如何判断点在多边形内

理论：从P向任意方向引一条射线，统计该射线与该多边形边的交点数，若为奇数，则P在多边形内；若为偶数，则P在多边形外。

特殊情况1：由于实际编程时，计算交点是将相邻顶点分别连接，而后计算射线与边的交点，而该射线可能正好通过多边形的顶点，这会导致一个交点被两侧的边重复统计。解决方法是：对于多边形边的两端记为A、B，使每条边的A端都接上下一条边的B端，即通过A-BA-BA-B这种方式将边首尾相连，那么只需要统一忽略射线与A端的交点而统计其与B端的交点即可。反之也行。

特殊情况2：当引出的射线正好与某一边重叠时，判断与重叠边相邻的两边相对于重叠边是同侧还是异侧。若为同侧，则视为重叠边与射线有一个交点；反之则视为没有交点（当引出射线与某一边重叠时，要将与其相邻的两边一并纳入考虑：异侧时，三边应视为与射线有奇数个交点；同侧时，三边应视为与射线有偶数个交点。因除去重叠边外，另两边与射线必为一个交点（见特殊情况1），故对于重叠边而言，相邻两边同侧，返回1；异侧，返回0）。具体见下图，左图为同侧，右图为异侧，紫色线为重叠边：

3.如何判断线完全处在多边形内

对于线L，首先判断其两端是否均处于多边形内。然后统计L与多边形的所有交点。若有任意交点不为顶点和L两端点，则L不完全在多边形内。对于L两端均在形内，且有顶点交点数<=1，则L亦在形内。

对于L两端均在形内，且顶点交点数>=2，则L有如下情况可能导致其在形外：

那么需要对这种情况进行判断。将所有交点去重后按x坐标排序（x相等则按y坐标排序），然后按顺序判断相邻的两个交点的中点是否在形内。若有任意一个中点不在形内，则L不完全在形内。反之则L完全在形内。

4.对基本思想的优化

基本思想中，仅对起终点连线所穿过或接触的边进行递归寻路，这是基于尽量减少搜索广度的需要而选择的策略，代价是对于一些极端情况，如最短路实际上需要先向反方向行进时，这种搜索方法是无法得出最优解的。如下图（红色是多边形，绿色是的最短路，而黄箭头是这种搜索方式会得出的解）

那么对于这种情况，需要我们在每层搜索完成返回时，对当前路径进行判断，若有可通过本层起点直接连接到的途经点，则对路径进行剪切，剔除中间多余的路径点。

代码

Class/BaseClass.py：点和线段的基础类

import math
from typing import List

import matplotlib.pyplot as plt


class Point:
    x: float
    y: float

    def __init__(self, x: float, y: float):
        self.x = x
        self.y = y

    def to_str(self) -> str:
        return "Point(" + ("None" if self.x is None else format(self.x, ".4f")) + ',' + (
            "None" if self.y is None else format(self.y, ".4f")) + ')'

    def __eq__(self, other):
        if other is None or other.__class__ != self.__class__:
            return False
        return self.x == other.x and self.y == other.y

    def __hash__(self):
        return hash((self.x, self.y))

    def visualize(self, r, g, b, a=1, size=1):
        plt.scatter(self.x, self.y, c=[(r/255, g/255, b/255, a)], s=size)


def distance(a: Point, b: Point) -> float:
    return math.sqrt((a.x - b.x) ** 2 + (a.y - b.y) ** 2)


class Line:
    A: Point
    B: Point
    k: float
    C: float

    def __init__(self, pointA: Point, pointB: Point):
        self.A = pointA
        self.B = pointB
        self.k = self.slope()
        self.C = self.C()

    def __eq__(self, other):
        if other is None or other.__class__ != self.__class__:
            return False
        return (self.A == other.A and self.B == other.B) or (self.A == other.B and self.B == other.A)

    def to_str(self) -> str:
        return "Line=" + str(self.k) + "x + " + str(self.C)

    def length(self) -> float:
        return distance(self.A, self.B)

    def slope(self):
        if self.A.x == self.B.x:
            return None
        return (self.A.y - self.B.y) / (self.A.x - self.B.x)

    def C(self):
        if self.A.x == self.B.x:
            return None
        return self.B.y - self.k * self.B.x

    def footPoint(self, a: Point) -> Point:
        if self.A.x == self.B.x:
            return Point(self.A.x, a.y)
        if self.A.y == self.B.y:
            return Point(a.x, self.A.y)
        targetX = ((a.y - self.C) * self.k + a.x) / (1 + self.k)
        targetY = targetX * self.k + self.C
        return Point(targetX, targetY)

    def inline(self, p: Point) -> bool:
        if p is None:
            return False
        if p.x > max(self.A.x, self.B.x) or p.x < min(self.A.x, self.B.x) or p.y > max(self.A.y, self.B.y) or p.y < min(
                self.A.y, self.B.y):
            return False
        if self.A.x == self.B.x:
            return max(self.A.y, self.B.y) >= p.y >= min(self.A.y, self.B.y)
        return p.y == p.x * self.k + self.C

    def hit(self, l) -> bool:
        if l is None:
            return False
        if l.A == self.A or l.A == self.B or l.B == self.A or l.B == self.B:
            return True
        crossP = crossPoint(self, l)
        if l.inline(crossP) and self.inline(crossP):
            return True
        return False

    def slice(self, sliceNum: int=10) -> List[Point]:
        if sliceNum < 2:
            return []
        avgLen = self.length() / sliceNum
        slicePoints = []
        if self.A.x == self.B.x:
            point = Point(self.A.x, min(self.A.y, self.B.y))
            for i in range(sliceNum):
                point = Point(point.x, point.y + avgLen)
                slicePoints.append(point)
            return slicePoints
        point = Point(min(self.A.x, self.B.x), min(self.A.y, self.B.y))
        D = (2*self.k*self.A.x+2*self.k*self.C)**2-4*(1+self.k**2)*(avgLen**2-self.C**2-self.k**2*self.A.x**2-2*self.k*self.C*self.A.x)
        deltaX1 = (-(2*self.k*self.A.x+2*self.k*self.C)+math.sqrt(D))/(2*(1+self.k**2))
        deltaX2 = (-(2*self.k*self.A.x+2*self.k*self.C)-math.sqrt(D))/(2*(1+self.k**2))
        print(deltaX1, deltaX2)
        return []

    def visualize(self, r, g, b, a=1, width=1):
        plt.plot([self.A.x, self.B.x], [self.A.y, self.B.y], color=(r/255, g/255, b/255, a), linewidth=width)


def crossPoint(a: Line, b: Line):
    if a.A == b.A or a.A == b.B:
        return a.A
    if a.B == b.A or a.B == b.B:
        return a.B
    if (a.A.x == a.B.x and b.A.x == b.B.x) or a.k == b.k:
        return None
    if a.A.x == a.B.x:
        return Point(a.A.x, a.A.x * b.k + b.C)
    if b.A.x == b.B.x:
        return Point(b.A.x, b.A.x * a.k + a.C)
    targetX = (b.C - a.C) / (a.k - b.k)
    targetY = a.k * targetX + a.C
    return Point(targetX, targetY)

Class/Border.py：边类，该类中的属性在本问题中并未用到，故Border可基本等同为基础类中的Line

from .BaseClass import Line, Point


class Border(Line):
    passable: bool
    passCost: float
    link: None
    # preBorder: None
    # nxtBorder: None

    def __init__(self, pointA: Point, pointB: Point, passable: bool, passCost: float, link):
        super().__init__(pointA, pointB)
        self.passable = passable
        self.passCost = passCost
        self.link = link

    def neighbour(self, a) -> bool:
        if a is None or a.__class__ != self.__class__:
            return False
        return (self.A == a.A and not self.B == a.B) or \
            (self.A == a.B and not self.B == a.A) or \
            (self.B == a.A and not self.A == a.B) or \
            (self.B == a.B and not self.A == a.A)

Class/Poly.py：多边形类，包括判断点、线是否在形内的方法、递归求解形内最短路的方法

import math

from .Border import Border
from .BaseClass import *
from typing import List


class Poly:
    n: int
    borders: List[Border]
    '''borders mustn't has equal A point, which means A point must be the previous border's B point'''
    moveCost: float
    innerPoly: []

    def __init__(self, borders: List[Border], moveCost: float):
        self.n = len(borders)
        self.borders = borders
        self.moveCost = moveCost

    def pointInside(self, p: Point) -> bool:
        def rightHit(l: Line, index: int) -> int:
            if l.A.y == l.B.y == p.y:
                if p.x > l.A.x and p.x > l.B.x:
                    return 0

                def circleIndex(index: int) -> int:
                    if 0 <= index < self.n:
                        return index
                    if index < 0:
                        return self.n - 1
                    return 0

                preIndex = circleIndex(index - 1)
                nextIndex = circleIndex(index + 1)
                while self.borders[preIndex].A.y == self.borders[preIndex].B.y:
                    preIndex = circleIndex(preIndex - 1)
                while self.borders[nextIndex].A.y == self.borders[nextIndex].B.y:
                    nextIndex = circleIndex(nextIndex + 1)
                preBorder = self.borders[preIndex]
                nextBorder = self.borders[nextIndex]
                if math.copysign(1, preBorder.A.y - preBorder.B.y) == math.copysign(1, nextBorder.B.y - nextBorder.A.y):
                    return 1
                else:
                    return 0

            hitPoint = crossPoint(l, ray)
            if hitPoint is None:
                return 0
            return 1 if hitPoint.x >= p.x and l.inline(hitPoint) and not hitPoint == l.A else 0

        ray = Line(p, Point(p.x + 1, p.y))
        hitNum = 0
        for i in range(len(self.borders)):
            border = self.borders[i]
            if border.inline(p):
                return True
            hitNum += rightHit(border, i)
        return hitNum % 2 == 1

    def lineInside(self, l: Line) -> bool:
        if not self.pointInside(l.A) or not self.pointInside(l.B):
            return False
        hitPoints = []
        for border in self.borders:
            if border.hit(l):
                hitPoint = crossPoint(l, border)
                if not border.inline(l.A) and not border.inline(
                        l.B) and not hitPoint == border.A and not hitPoint == border.B:
                    return False
                hitPoints.append(hitPoint)
        hitPoints = list(set(hitPoints))
        if len(hitPoints) >= 2:
            hitPoints = sorted(hitPoints, key=lambda p: (p.x, p.y))
            for i in range(len(hitPoints) - 1):
                if not self.pointInside(
                        Point((hitPoints[i].x + hitPoints[i + 1].x) / 2, (hitPoints[i].y + hitPoints[i + 1].y) / 2)):
                    return False
        return True

    ''' sPos ePos must inside Poly; return pathLength and waypoints which will inside Poly too '''
    def findInnerPath(self, sPos: Point, ePos: Point, passedBorders: List[Border] = []) -> (float, List[Point]):
        if self.n < 3:
            return 0, []
        bestPathLength, bestWaypoints = None, None
        Lse = Line(sPos, ePos)
        if self.lineInside(Lse):
            # print("from " + sPos.to_str() + " to " + ePos.to_str() + ": direct")
            return Lse.length(), [sPos, ePos]

        for border in self.borders:
            if border not in passedBorders and Lse.hit(border):
                if not sPos == border.A and self.lineInside(Line(sPos, border.A)):
                    followPathLength, followWaypoints = self.findInnerPath(border.A, ePos, passedBorders + [border])
                    if followPathLength is not None:
                        currentPathLength = followPathLength + distance(sPos, border.A)
                        currentWaypoints = [sPos] + followWaypoints
                        if bestPathLength is None or bestPathLength > currentPathLength:
                            bestPathLength = currentPathLength
                            bestWaypoints = currentWaypoints
                if not sPos == border.B and self.lineInside(Line(sPos, border.B)):
                    followPathLength, followWaypoints = self.findInnerPath(border.B, ePos, passedBorders + [border])
                    if followPathLength is not None:
                        currentPathLength = followPathLength + distance(sPos, border.B)
                        currentWaypoints = [sPos] + followWaypoints
                        if bestPathLength is None or bestPathLength > currentPathLength:
                            bestPathLength = currentPathLength
                            bestWaypoints = currentWaypoints

        if bestWaypoints is not None and len(bestWaypoints) > 3:
            for i in range(len(bestWaypoints) - 2, 1, -1):
                if self.lineInside(Line(sPos, bestWaypoints[i])):
                    bestWaypoints = [sPos] + bestWaypoints[i:]
                    bestPathLength = 0
                    for j in range(len(bestWaypoints) - 1):
                        bestPathLength += distance(bestWaypoints[j], bestWaypoints[j + 1])
                    break

        # print("from " + sPos.to_str() + " to " + ePos.to_str() + ": " + str(bestWaypoints))
        return bestPathLength, bestWaypoints

    def visualize(self, r, g, b, a=1):
        for border in self.borders:
            border.visualize(r, g, b, a)

2DPathFinding.py：主函数，含正确初始化Poly类的方式

import matplotlib.pyplot as plt
import time
import json

from Class.BaseClass import *
from Class.Border import Border
from Class.Poly import Poly


def innerPathFindingTest():
    with open("Data/innerPathFindingTest.dat") as f:
        dat = json.loads(f.read())
    sPos = Point(dat['sPos'][0], dat['sPos'][1])
    ePos = Point(dat['ePos'][0], dat['ePos'][1])
    pointList = []
    for i in range(len(dat['vertices'])):
        pointList.append(Point(dat['vertices'][i][0], dat['vertices'][i][1]))
    borders = [Border(pointList[len(pointList)-1], pointList[0], True, 0, None)]
    for i in range(len(pointList) - 1):
        borders.append(Border(pointList[i], pointList[i + 1], True, 0, None))
    poly = Poly(borders, 1.0)

    startTime = time.time()
    #print(poly.lineInside(Line(Point(4,0), Point(6,0))))
    cost, way = poly.findInnerPath(sPos, ePos)
    endTime = time.time()

    print(startTime, endTime)
    print("duringTime:" + str(endTime - startTime) + "s")
    poly.visualize(100,0,0)
    plt.title("cost" + str(cost))
    for i in range(len(way)-1):
        l = Line(way[i], way[i+1])
        l.visualize(0,100,0)
    plt.show()


if __name__ == '__main__':
    innerPathFindingTest()

Data/innerPathFindingTest.dat：输入文件

样例1：

{
    "vertices": [[0,0], [4,0], [4,4], [2,4], [2,8], [8,8], [8,4],[6,4], [6,0], [10,0], [10,10], [0,10]],
    "sPos": [3,2],
    "ePos": [7,2]
}

样例2：

{
    "vertices": [[0,0], [10,0], [5,5], [10,10], [0,10]],
    "sPos": [10,0],
    "ePos": [10,10]
}

样例3：

{
    "vertices": [[0,0], [10,0], [10,4], [5,4], [5,6], [10,6],[10,10], [0,10]],
    "sPos": [10,0],
    "ePos": [10,10]
}