【路径规划】三维深度矩阵寻路算法

在三维空间中寻路相较于二维空间更为复杂,因为需要处理额外的维度。下面是一个示例,展示了如何使用深度优先搜索(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.")

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

import sys
sys.setrecursionlimit(100000000) #例如这里设置为十万
  • 12
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

神精兵院院长

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值