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

[工程师阿伟]正在和[机器小伟]1起研究[计算几何]]。

<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)):

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)):

#顶点列表
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])]; #注意不要超过主顶点的阵列个数

#路径描写

#求取路径
dijkstrapath = Dijk.DijkstraPath(node_map);
#路径字符串
dijkstrapath(from_node, to_node);

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>

1. 本站所有资源来源于用户上传和网络，如有侵权请邮件联系站长！
2. 分享目的仅供大家学习和交流，您必须在下载后24小时内删除！
3. 不得使用于非法商业用途，不得违反国家法律。否则后果自负！
4. 本站提供的源码、模板、插件等等其他资源，都不包含技术服务请大家谅解！
5. 如有链接无法下载、失效或广告，请联系管理员处理！
6. 本站资源售价只是赞助，收取费用仅维持本站的日常运营所需！
7. 如遇到加密压缩包，请使用WINRAR解压,如遇到无法解压的请联系管理员！