【已解决】Python Bresenham 3D算法

放一段使用Python实现Bresenham 3D 算法的代码,并通过Matplot可视化

python 复制代码
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from numba import njit

@njit
def bresenham_safe(grid, x0, y0, z0, x1, y1, z1, value_to_fill):
    start_point = [int(x0), int(y0), int(z0)]
    end_point = [int(x1), int(y1), int(z1)]

    steep_xy = (abs(end_point[1] - start_point[1]) > abs(end_point[0] - start_point[0]))
    if steep_xy:
        start_point[0], start_point[1] = start_point[1], start_point[0]
        end_point[0], end_point[1] = end_point[1], end_point[0]

    steep_xz = (abs(end_point[2] - start_point[2]) > abs(end_point[0] - start_point[0]))
    if steep_xz:
        start_point[0], start_point[2] = start_point[2], start_point[0]
        end_point[0], end_point[2] = end_point[2], end_point[0]

    delta = [abs(end_point[0] - start_point[0]), abs(end_point[1] - start_point[1]), abs(end_point[2] - start_point[2])]

    error_xy = delta[0] / 2
    error_xz = delta[0] / 2

    step = [
        -1 if start_point[0] > end_point[0] else 1,
        -1 if start_point[1] > end_point[1] else 1,
        -1 if start_point[2] > end_point[2] else 1
    ]

    y = start_point[1]
    z = start_point[2]

    for x in range(start_point[0], end_point[0], step[0]):
        point = [x, y, z]

        if steep_xz:
            point[0], point[2] = point[2], point[0]
        if steep_xy:
            point[0], point[1] = point[1], point[0]

        if 0 <= point[0] < grid.shape[0] and 0 <= point[1] < grid.shape[1] and 0 <= point[2] < grid.shape[2]:
            grid[point[0], point[1], point[2]] = value_to_fill

        error_xy -= delta[1]
        error_xz -= delta[2]

        if error_xy < 0:
            y += step[1]
            error_xy += delta[0]

        if error_xz < 0:
            z += step[2]
            error_xz += delta[0]

@njit
def get_free_area(obstacle, x, y, z):
    free = np.zeros_like(obstacle)
    obstacle = obstacle > 0
    xs, ys, zs = np.where(obstacle)
    for ox, oy, oz in zip(xs, ys, zs):
        bresenham_safe(free, ox, oy, oz, x, y, z, 1)
    free -= obstacle
    return free

# 创建三维网格和障碍物示例
grid = np.zeros((65, 65, 12))
obstacle = np.zeros_like(grid)
start = np.zeros_like(grid)
obstacle[20:30, 4:60, 2:9] = 1

# 获取自由区域
x = 8
y = 8
z = 11
import time
start_time = time.time()
free_area = get_free_area(obstacle, x, y, z)
end_time = time.time()
execution_time = end_time - start_time
print("□□□□□□□□□□程序执行时间为:", execution_time, "秒") 
start[8,8,11]=1

# 翻转颜色映射
cmap = plt.cm.gray
cmap_inverted = cmap.reversed()
# # 可视化
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')

# 设置坐标轴范围和比例,让显示比例正常
x_dim, y_dim, z_dim = obstacle.shape
max_dim = max(x_dim, y_dim, z_dim)
ax.set_xlim(0, max_dim)
ax.set_ylim(0, max_dim)
ax.set_zlim(0, max_dim)


# ------------------------------------------------------------------------------
# x_indices, y_indices, z_indices = np.where(free_area)
ax.voxels(free_area, facecolors='green',)
ax.voxels(obstacle, facecolors='red',)
ax.voxels(start, facecolors='blue')
# print(grid)
ax.set_xlabel('X')
ax.set_ylabel('Y')
ax.set_zlabel('Z')
plt.show()
相关推荐
菜鸟~noob23333 分钟前
[AI]间章:系列走到这里,聊聊后面的规划、造卡需要的软件平台,以及为什么我要先暂停更新
人工智能·python·fpga开发·django·pygame
TomEval10 小时前
【测AI】第26篇:线上模型监控与告警体系 — 测试不是终点
人工智能·python·功能测试·ai
prince_zxill11 小时前
PEP 751 pylock.toml 实测:pip 26.3 装 6 个包版本全钉死,但 [web] 选不中任何东西
python·pip·依赖管理·pep 751
Thanks_ks12 小时前
Python 的垃圾回收——引用计数撑起日常,分代回收收拾残局
python·内存管理·垃圾回收·循环引用·引用计数·gc 模块·weakref
CyrusYiVPP12 小时前
实测 uv 0.13.7:同一套包,pip 装 94 秒,它只用 19 秒
人工智能·python
罗西的思考13 小时前
【轻量化复刻大模型 / KIMI K3】Muon 优化器及 训练系统
人工智能·算法·机器学习
yiangL_13 小时前
Python 学习笔记:递归、turtle绘图、字符串与函数
笔记·python·学习
ashiho14 小时前
【LeetCode hot 100】3. 无重复字符的最长子串 (三种解法)
java·数据结构·后端·算法·leetcode·滑动窗口
数据工匠老o14 小时前
数据库没死,业务卡住了:行锁等待、元数据锁、长事务的排查路径
数据库·算法·架构
lula88814 小时前
PVC-U、PVC-C、PVC-O 差一个字母,差的是三种增强路线
python