系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

---

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

本文先对通过图形学实现在凸多边形内生成覆盖路径做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

---

提示：以下是本篇文章正文内容

通过图形学实现在凸多边形内生成覆盖路径

核心原理

1、先对复杂的障碍物和地图进行精确单元分解（梯形单元分解，牛耕单元分解），得到凸多边形cell
2、在凸多边形cell中，使用图形学计算的方法，依次生成覆盖式路径

生成覆盖式路径的步骤：
（1）先寻找凸多边形cell中最长的一条边，并判断覆盖方向式往该最长边的上方还是下方
（2）根据覆盖式路径的间距，依次计算最长边所在的直线，该直线与凸多边形cell相交的部分就是覆盖式路径，把当前覆盖式路径设置为最长边
（3）重复步骤二，直到凸多边形cell的所有顶点都在最长边所在的直线的一侧，结束覆盖路径规划

代码实现

from helpers.geometry import *
import math
import matplotlib.pyplot as plt
# return list of points
def sweep(vertexes):WIDTH = 2 #覆盖路径的宽度edges = []# 凸多边形cell的边maxLength = -1maxIndex = -1for i in range(len(vertexes)):point0 = vertexes[i]point1 = vertexes[(i + 1) % len(vertexes)]edges.append(Line.getLineFromTwoPoints(point0, point1))if edges[i].length() > maxLength:maxLength = edges[i].length()maxIndex = ilongestEdge = edges.pop(maxIndex)# 计算凸多边形cell的最长的边longestEdge.setToInfinite()intersections = []# intersections.append(vertexes[maxIndex])# intersections.append(vertexes[(maxIndex + 1) % len(vertexes)])# 计算覆盖路径的间距及方向direct = 0for vertex in vertexes:if longestEdge.relativePosition(vertex) != 0:direct = longestEdge.relativePosition(vertex)breakWIDTH = WIDTH * direct#当遍历凸多边形cell的所有顶点是否都在当前最长的覆盖线段的一边时，代表覆盖路径生成完毕while not longestEdge.allOnOneSide(vertexes):newIntersect = []for edge in edges:inter = longestEdge.getIntersection(edge)#计算交线，该交线就是中间的覆盖路径if inter is not None:newIntersect.append(inter)intersections.extend(newIntersect)longestEdge = longestEdge.shift(WIDTH)# 不断根据算覆盖路径的间距及方向进行直线偏移for i in intersections:print(i)reorderIntersections(edges, intersections)print("-----------------------")for i in intersections:print(i)return intersectionsdef reorderIntersections(edges, intersections):interCount = []for i in range(len(edges)):interCount.append(0)for inter in intersections:for i in range(len(edges)):if edges[i].relativePosition(inter) == 0:interCount[i] = interCount[i] + 1breakmajorEdge = Nonefor i in range(len(interCount)):if interCount[i] == len(intersections) / 2:majorEdge = edges[i]breakdirect = 1i = 0# direct == 1, intersection on major edge at the front# direct == -1, intersection on major edge at the backwhile i < len(intersections):if direct == 1 & majorEdge.relativePosition(intersections[i]) != 0:temp = intersections[i]intersections[i] = intersections[i + 1]intersections[i + 1] = tempelif direct == -1 & majorEdge.relativePosition(intersections[i + 1]) != 0:temp = intersections[i]intersections[i] = intersections[i + 1]intersections[i + 1] = tempdirect = 0 - directi = i + 2class Line:def __init__(self, vertical, x, k = -1, b = -1, minX = -1, minY = -1, maxX = -1, maxY = -1):self.vertical = verticalself.x = xself.k = kself.b = bself.minX = minXself.minY = minYself.maxX = maxXself.maxY = maxYdef equals(self, line1):if self.vertical & line1.vertical:return self.x == line1.xelif self.vertical | line1.vertical:return Falseelse:return self.k == line1.k & self.b == line1.b@staticmethoddef getLineFromTwoPoints(point1, point2):x1 = point1.xy1 = point1.yx2 = point2.xy2 = point2.yif x1 == x2:return Line(True, x1, -1, -1, min(x1, x2), min(y1, y2), max(x1, x2), max(y1, y2))else:k = (y1 - y2) / (x1 - x2)b = (x1 * y2 - x2 * y1) / (x1 - x2)return Line(False, -1, k, b, min(x1, x2), min(y1, y2), max(x1, x2), max(y1, y2))# a infinite line call this function to intersect with finite linedef getIntersection(self, line2):k1 = self.kb1 = self.bk2 = line2.kb2 = line2.bif self.vertical & line2.vertical:return Noneelif self.vertical | line2.vertical:if self.vertical:x = self.xy = round(line2.k * x + line2.b, 2)else:x = line2.xy = round(self.k * x + self.b, 2)else:if k1 == k2: return Nonex = round((b2 - b1) / (k1 - k2), 2)y = round((b2 * k1 - b1 * k2) / (k1 - k2), 2)if (x >= line2.minX) & (x <= line2.maxX) & (y >= line2.minY) & (y <= line2.maxY):return point(x, y)else:return None# return 1 if point is above the line# -1 if point is below the line# 0 if point is on the linedef relativePosition(self, point0):if self.vertical:if point0.x > self.x: return 1elif point0.x < self.x: return -1else: return 0else:yOfLine = round(self.k * point0.x + self.b, 2)yOfPoint = point0.yif yOfPoint > yOfLine: return 1elif yOfPoint < yOfLine: return -1else: return 0def allOnOneSide(self, listOfPoints):position0 = self.relativePosition(listOfPoints[0])for i in range(1, len(listOfPoints)):if(self.relativePosition(listOfPoints[i]) != position0):return Falsereturn True# return a new Line# width > 0, shift up or right# width < 0, shift down or leftdef shift(self, width):if self.vertical:return Line(True, self.x + width)else:delta = math.sqrt(1 + self.k * self.k) * widthnewB = self.b + deltareturn Line(False, -1, self.k, newB)def length(self):if self.minX == -1 & self.minY == -1 & self.maxX == -1 & self.maxY == -1:return -1else:return math.sqrt((self.maxX - self.minX) ** 2 + (self.maxY - self.minY) ** 2)def setToInfinite(self):self.minX = -1self.minY = -1self.maxX = -1self.maxY = -1def getLine():
# 输出一个凸多边形cellpoint0 = point(3, 2)point1 = point(9, 8)point2 = point(2, 9)
#     point3 = point(1, 8)p_list = []p_list.append(point0)p_list.append(point1)p_list.append(point2)
#     list.append(point3)# 可视化该凸多边形cellx_bndy = [pnt.x for pnt in p_list]y_bndy = [pnt.y for pnt in p_list]x_bndy.append(x_bndy[0])y_bndy.append(y_bndy[0])plt.plot(x_bndy, y_bndy)# 在凸多边形cell内部生成覆盖路径pnts = sweep(p_list)# 可视化覆盖路径x = [pnt.x for pnt in pnts]y = [pnt.y for pnt in pnts]plt.plot(x, y)getLine()#从一个凸多边形cell中生成等距的覆盖式路径

---