【路径规划】三维深度矩阵寻路算法_三维坐标的深度排序算法

在三维空间中寻路相较于二维空间更为复杂，因为需要处理额外的维度。下面是一个示例，展示了如何使用深度优先搜索（DFS）算法在三维矩阵中寻找路径。

首先，我们需要定义三维矩阵，并编写一个递归的DFS函数来寻找路径。注意，这个示例仅考虑了相邻的格子（上下左右前后）之间的移动，并且假设障碍物用1表示，可通行的格子用0表示。

import numpy as np


def dfs(matrix, start, end, visited, path):
    # 获取当前位置的坐标
    x, y, z = start

    # 如果当前位置是终点，则添加到路径并返回True
    if (x, y, z) == end:
        path.append(end)
        return True

    # 标记当前位置为已访问
    visited.add((x, y, z))
    path.append(start)

    # 定义六个方向的偏移量
    directions = [
        (1, 0, 0),  # 下
        (-1, 0, 0),  # 上
        (0, 1, 0),  # 右
        (0, -1, 0),  # 左
        (0, 0, 1),  # 前
        (0, 0, -1)  # 后
    ]

    # 尝试所有可能的方向
    for dx, dy, dz in directions:
        nx, ny, nz = x + dx, y + dy, z + dz

        # 检查新位置是否在矩阵范围内且未访问过，且不是障碍物
        if (
                0 <= nx < matrix.shape[0] and
                0 <= ny < matrix.shape[1] and
                0 <= nz < matrix.shape[2] and
                (nx, ny, nz) not in visited and
                matrix[nx, ny, nz] == 0
        ):
            # 递归调用DFS
            if dfs(matrix, (nx, ny, nz), end, visited, path):
                return True

    # 如果所有方向都不可行，则回溯
    path.pop()
    return False


def find_path_3d(matrix, start, end):
    visited = set()
    path = []

    # 执行DFS算法
    if dfs(matrix, start, end, visited, path):
        return path
    else:
        return None  # 如果没有找到路径，则返回None

def makegraph(x,y,z,f=[],size = (1,1,1)):
    '''
    创建场景
    :param x: x轴长
    :param y: y轴长
    :param z: z轴长
    :param f: 障碍物点位
    :param size: 单元格尺寸
    :return: 返回基于单元压缩的场景矩阵
    '''
    if isinstance(size,int):
        size = (size,size,size)
    if size[0]>x or size[1]>y or size[2]>z:
        raise ValueError('网格尺寸，大于最小分割条件')
    w = np.zeros((x//size[0],y//size[1],z//size[2]),dtype=int)
    if f:
        for i in f:
            try:
                w[i[0]][i[1]][i[2]] = 1
            except:
                pass
    return w

# 示例使用
# 创建一个3x3x3的三维矩阵，其中0表示可通过的空格，1表示障碍物
# matrix = np.zeros((3, 3, 3), dtype=int)
# matrix[1, 1, 1] = 1  # 设置中心位置为障碍物

import sys
sys.setrecursionlimit(100000000) #例如这里设置为十万

matrix = makegraph(600,30,30,[[1,1,1],[3,2,1],[4,2,1],[10,1,1]],[1,1,1])
print(f'当前场景有个网格{matrix.size}单元,场景尺寸：{matrix.shape[0]}x{matrix.shape[1]}x{matrix.shape[2]}')

# 定义起点和终点
start = (0, 1, 2)
end = (20,5,1)

# 寻找路径
path = find_path_3d(matrix, start, end)

# 输出路径
if path:
    print("Path found:")
    for pos in path:
        print(pos)
else:
    print("No path found.")
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105

【注意】如果遇到最大递归深度报错，请使用

import sys
sys.setrecursionlimit(100000000) #例如这里设置为十万
1
2