[从头学数学] 第267节 [计算几何] 路径规划

<span style="font-size:18px;"># >>> 28 [Point([⑴0, ⑼]), Point([⑵, ⑼]), Point([2, ⑼]), Point([-0.71, ⑺.45]), Point([0.29, ⑺.29]), Point([2, ⑺]), Point([8, ⑺]), Point([-0.18, ⑹.82]), Point([⑵, ⑸]), Point([2, ⑶]), Point([6, ⑶]), Point([4.3, ⑴.44]), Point([4.58, ⑴.11]), Point([4, ⑴]), Point([4, 1]), Point([2.0, 2.33]), Point([0, 3]), Point([1.5, 3]), Point([2.0, 3]), Point([2, 3]), Point([6, 3]), Point([8, 3]), Point([4.57, 3.29]), Point([0.71, 4.06]), Point([⑷, 5]), Point([0, 5]), Point([⑹, 9]), Point([6, 9])] >>> #生成结点表 def tmp10(): divideSeg = [[[⑴0, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [0.29, ⑺.29]], [[0.29, ⑺.29], [2, ⑺]], [[⑵, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [4.3, ⑴.44]], [[4.3, ⑴.44], [4.58, ⑴.11]], [[4.58, ⑴.11], [8, 3]], [[2, ⑼], [0.29, ⑺.29]], [[0.29, ⑺.29], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [⑵, ⑸]], [[8, ⑺], [4.3, ⑴.44]], [[4.3, ⑴.44], [4, ⑴]], [[2, ⑶], [2.0, 2.33]], [[2.0, 2.33], [2, 3]], [[2, 3], [2.0, 3]], [[6, ⑶], [4.58, ⑴.11]], [[4.58, ⑴.11], [2.0, 2.33]], [[2.0, 2.33], [1.5, 3]], [[1.5, 3], [0.71, 4.06]], [[0.71, 4.06], [0, 5]], [[4, 1], [4.57, 3.29]], [[4.57, 3.29], [6, 9]], [[0, 3], [1.5, 3]], [[1.5, 3], [2, 3]], [[2, 3], [2.0, 3]], [[6, 3], [4.57, 3.29]], [[4.57, 3.29], [0.71, 4.06]], [[0.71, 4.06], [⑷, 5]], [[0, 5], [⑹, 9]]]; vertex = set(); for i in range(len(divideSeg)): vertex.add(Point(divideSeg[i][0])); vertex.add(Point(divideSeg[i][1])); vertexArray = sorted(list(vertex)); print(len(vertexArray)); print(vertexArray); #</span>

<span style="font-size:18px;"># >>> [(0, 3, 9.42), (3, 4, 1.01), (4, 5, 1.73), (1, 3, 2.02), (3, 7, 0.82), (7, 11, 7.0), (11, 12, 0.43), (12, 21, 5.35), (2, 4, 2.42), (4, 7, 0.66), (7, 8, 2.57), (6, 11, 6.68), (11, 13, 0.53), (9, 15, 5.33), (15, 18, 0.67), (18, 19, 0.0), (10, 12, 2.36), (12, 15, 4.3), (15, 17, 0.84), (17, 23, 1.32), (23, 25, 1.18), (14, 22, 2.36), (22, 27, 5.89), (16, 17, 1.5), (17, 18, 0.5), (18, 19, 0.0), (20, 22, 1.46), (22, 23, 3.94), (23, 24, 4.8), (25, 26, 7.21)] >>> >>> 选择的线路是： [⑴0, ⑼]-->[-0.71, ⑺.45]-->[-0.18, ⑹.82]-->[4.3, ⑴.44]-->[4.58, ⑴.11]-->[2.0, 2.33]-->[1.5, 3]-->[0.71, 4.06]-->[4.57, 3.29]-->[6, 9] ，这条线路总长为33.96米 >>> >>> 选择的线路是： [⑴0, ⑼]-->[-0.71, ⑺.45]-->[-0.18, ⑹.82]-->[4.3, ⑴.44]-->[4.58, ⑴.11]-->[2.0, 2.33]-->[1.5, 3]-->[0.71, 4.06]-->[4.57, 3.29]-->[6, 9] ，这条线路总长为33.96米 [[0, 3], [3, 7], [7, 11], [11, 12], [12, 15], [15, 17], [17, 23], [23, 22], [22, 27]] [[[⑴0, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [4.3, ⑴.44]], [[4.3, ⑴.44], [4.58, ⑴.11]], [[4.58, ⑴.11], [2.0, 2.33]], [[2.0, 2.33], [1.5, 3]], [[1.5, 3], [0.71, 4.06]], [[0.71, 4.06], [4.57, 3.29]], [[4.57, 3.29], [6, 9]]] >>> #生成结点表 def tmp10(): debug = 0; #主端点 mainVert = [[[⑴0, ⑼], 0], [[⑵, ⑼], 1], [[2, ⑼], 2], [[2, ⑺], 3], [[8, ⑺], 4], [[⑵, ⑸], 5], [[2, ⑶], 6], [[6, ⑶], 7], [[4, ⑴], 8], [[4, 1], 9], [[0, 3], 10], [[2, 3], 11], [[6, 3], 12], [[8, 3], 13], [[⑷, 5], 14], [[0, 5], 15], [[⑹, 9], 16], [[6, 9], 17]]; #分线段 divideSeg = [[[⑴0, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [0.29, ⑺.29]], [[0.29, ⑺.29], [2, ⑺]], [[⑵, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [4.3, ⑴.44]], [[4.3, ⑴.44], [4.58, ⑴.11]], [[4.58, ⑴.11], [8, 3]], [[2, ⑼], [0.29, ⑺.29]], [[0.29, ⑺.29], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [⑵, ⑸]], [[8, ⑺], [4.3, ⑴.44]], [[4.3, ⑴.44], [4, ⑴]], [[2, ⑶], [2.0, 2.33]], [[2.0, 2.33], [2, 3]], [[2, 3], [2.0, 3]], [[6, ⑶], [4.58, ⑴.11]], [[4.58, ⑴.11], [2.0, 2.33]], [[2.0, 2.33], [1.5, 3]], [[1.5, 3], [0.71, 4.06]], [[0.71, 4.06], [0, 5]], [[4, 1], [4.57, 3.29]], [[4.57, 3.29], [6, 9]], [[0, 3], [1.5, 3]], [[1.5, 3], [2, 3]], [[2, 3], [2.0, 3]], [[6, 3], [4.57, 3.29]], [[4.57, 3.29], [0.71, 4.06]], [[0.71, 4.06], [⑷, 5]], [[0, 5], [⑹, 9]]]; vertex = set(); for i in range(len(divideSeg)): vertex.add(Point(divideSeg[i][0])); vertex.add(Point(divideSeg[i][1])); #顶点列表 vertexArray = sorted(list(vertex)); if (debug): print(len(vertexArray)); #print(vertexArray); #顶点字典 vertexDict = dict(); for i in range(len(vertexArray)): vertexDict[vertexArray[i]] = i; #node_list #格式[(序号1, 序号2, 距离), (...), ...] node_list = []; for i in range(len(divideSeg)): point_1 = divideSeg[i][0]; point_2 = divideSeg[i][1]; idx1 = vertexDict[Point(point_1)]; idx2 = vertexDict[Point(point_2)]; d = round(geo.distance2D(point_1, point_2), 2); node_list.append((idx1, idx2, d)); #print(node_list); node = list(range(len(vertexArray))); #节点数量的平方 node_map = [[0 for val in range(len(node))] for val in range(len(node))]; node_map = Dijk.set_node_map(node_map, node, node_list); #任务 from_node = vertexDict[Point(mainVert[0][0])]; #改动2维数组的第1个下标 to_node = vertexDict[Point(mainVert[17][0])]; #注意不要超过主顶点的阵列个数 #路径描写 addressString = vertexArray; #求取路径 dijkstrapath = Dijk.DijkstraPath(node_map); #路径字符串 dijkstrapath(from_node, to_node); print(dijkstrapath.pathString(addressString)); path = dijkstrapath.path(); #用顶点序号显示 #print(path); for i in range(len(path)): path[i][0] = vertexArray[path[i][0]].point; path[i][1] = vertexArray[path[i][1]].point; #用顶点值显示 print(path); #</span>

<span style="font-size:18px;"># ### # @usage 求两个给定地点，所有路径中的最短值 # @author mw # @date 2016年01月13日 星期3 09:50:23 # @param # @return # ### class DijkstraPath(): #初始化 def __init__(self, node_map): self.node_map = node_map; self.node_length = len(node_map); self.used_node_list = []; self.collected_node_dict = {}; self.step = 0; #调用函数 def __call__(self, from_node, to_node): self.from_node = from_node; self.to_node = to_node; self._init_dijkstra(); return self._format_path(); def _init_dijkstra(self): self.used_node_list.append(self.from_node); self.collected_node_dict[self.from_node] = [0, ⑴]; for index1, node1 in enumerate(self.node_map[self.from_node]): if node1: self.collected_node_dict[index1] = [node1, self.from_node]; self._foreach_dijkstra(); def _foreach_dijkstra(self): #保证每一个点最多只到1次 if len(self.used_node_list) == self.node_length - 1: return; #由于items()方法会改变原字典内容，所以此处拷贝副本 collected_node_dict = dict(self.collected_node_dict); # 遍历已有权值节点 for key, val in collected_node_dict.items(): if key not in self.used_node_list and key != self.to_node: self.used_node_list.append(key); else: continue; # 对节点进行遍历 for index1, node1 in enumerate(self.node_map[key]): # 如果节点在权值节点中并且权值大于新权值 if node1 and index1 in self.collected_node_dict \ and self.collected_node_dict[index1][0] > node1 + val[0]: # 更新权值 self.collected_node_dict[index1][0] = node1 + val[0] self.collected_node_dict[index1][1] = key; elif node1 and index1 not in self.collected_node_dict: self.collected_node_dict[index1] = [node1 + val[0], key]; self.step += 1; if self.step > self.node_length*10: #print('步数太多'); return; #递归 self._foreach_dijkstra(); def _format_path(self): node_list = []; temp_node = self.to_node; node_list.append((temp_node, self.collected_node_dict[temp_node][0])); while self.collected_node_dict[temp_node][1] != ⑴: temp_node = self.collected_node_dict[temp_node][1]; node_list.append((temp_node, self.collected_node_dict[temp_node][0])); node_list.reverse(); return node_list; def path(self): node_list = self._format_path(); size = len(node_list); result = []; for i in range(size⑴): result.append([node_list[i][0], node_list[i+1][0]]); return result; def pathString(self, node): node_list = self._format_path(); size = len(node_list); s = '选择的线路是：\n'; for i in range(size): if i < size⑴: s += str(node[node_list[i][0]])+'-->'; else: s += str(node[node_list[i][0]]); s+= ' ，这条线路总长为{0}米'.format(round(node_list[size⑴][1], 2)); return s; def set_node_map(node_map, node, node_list): for x, y, val in node_list: node_map[node.index(x)][node.index(y)] = node_map[node.index(y)][node.index(x)] = val return node_map; #</span>

<span style="font-size:18px;">// $vertex = [[[⑴0, ⑼], 0], [[⑵, ⑼], 1], [[2, ⑼], 2], [[2, ⑺], 3], [[8, ⑺], 4], [[⑵, ⑸], 5], [[2, ⑶], 6], [[6, ⑶], 7], [[4, ⑴], 8], [[4, 1], 9], [[0, 3], 10], [[2, 3], 11], [[6, 3], 12], [[8, 3], 13], [[⑷, 5], 14], [[0, 5], 15], [[⑹, 9], 16], [[6, 9], 17]]; $seg = [[10, 11, 2.0], [15, 16, 7.211], [2, 5, 5.657], [1, 13, 15.62], [4, 8, 7.211], [7, 15, 10.0], [0, 3, 12.166], [6, 11, 6.0], [9, 17, 8.246], [12, 14, 10.198]]; $divideSeg = [[[⑴0, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [0.29, ⑺.29]], [[0.29, ⑺.29], [2, ⑺]], [[⑵, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [4.3, ⑴.44]], [[4.3, ⑴.44], [4.58, ⑴.11]], [[4.58, ⑴.11], [8, 3]], [[2, ⑼], [0.29, ⑺.29]], [[0.29, ⑺.29], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [⑵, ⑸]], [[8, ⑺], [4.3, ⑴.44]], [[4.3, ⑴.44], [4, ⑴]], [[2, ⑶], [2.0, 2.33]], [[2.0, 2.33], [2, 3]], [[2, 3], [2.0, 3]], [[6, ⑶], [4.58, ⑴.11]], [[4.58, ⑴.11], [2.0, 2.33]], [[2.0, 2.33], [1.5, 3]], [[1.5, 3], [0.71, 4.06]], [[0.71, 4.06], [0, 5]], [[4, 1], [4.57, 3.29]], [[4.57, 3.29], [6, 9]], [[0, 3], [1.5, 3]], [[1.5, 3], [2, 3]], [[2, 3], [2.0, 3]], [[6, 3], [4.57, 3.29]], [[4.57, 3.29], [0.71, 4.06]], [[0.71, 4.06], [⑷, 5]], [[0, 5], [⑹, 9]]]; $path = [[[⑴0, ⑼], [-0.71, ⑺.45]], [[-0.71, ⑺.45], [-0.18, ⑹.82]], [[-0.18, ⑹.82], [4.3, ⑴.44]], [[4.3, ⑴.44], [4.58, ⑴.11]], [[4.58, ⑴.11], [2.0, 2.33]], [[2.0, 2.33], [1.5, 3]], [[1.5, 3], [0.71, 4.06]], [[0.71, 4.06], [4.57, 3.29]], [[4.57, 3.29], [6, 9]]]; if (1) { var r = 20; config.setSector(1,1,1,1); config.graphPaper2D(0, 0, r); config.axis2D(0, 0, 250, 1.2); //坐标轴设定 var scaleX = 2*r, scaleY = 2*r; var spaceX = 2, spaceY = 2; var xS = ⑴0, xE = 10; var yS = ⑴0, yE = 10; config.axisSpacing(xS, xE, spaceX, scaleX, 'X'); config.axisSpacing(yS, yE, spaceY, scaleY, 'Y'); var transform = new Transform(); //顶点 var a = []; for (var i = 0; i < $vertex.length; i++) { a.push($vertex[i][0]); } //显示变换 if (a.length > 0) { a = transform.scale(transform.translate(a, 0, 0), scaleX/spaceX, scaleY/spaceY); } var lable = []; for (var i = 0; i < 100; i++) { lable.push(i.toFixed(0)); } //边集 var b = []; for (var i = 0; i < $seg.length; i++) { b.push([a[$seg[i][0]], a[$seg[i][1]]]); } var edges = b.length; for (var i = 0; i < edges; i++) { shape.multiLineDraw([].concat(b[i]), 'red'); } var colorArray = ['red', 'orange', 'yellow', 'green', 'cyan', 'blue', 'purple', ]; var seg = []; var nSeg = $divideSeg.length; for (var i = 0; i < nSeg; i++) { seg = transform.scale(transform.translate($divideSeg[i], 0, 0), scaleX/spaceX, scaleY/spaceY); shape.multiLineDraw([].concat(seg), colorArray[i%7]); } nSeg = $path.length; plot.setLineWidth(5); for (var i = 0; i < nSeg; i++) { seg = transform.scale(transform.translate($path[i], 0, 0), scaleX/spaceX, scaleY/spaceY); shape.multiLineDraw([].concat(seg), 'pink'); } shape.pointDraw([].concat(a), 'blue', 1, 1, lable); } //</span>