请给matlab一个完整的三维A*算法

以下是一个基本的三维A*算法的Matlab代码实现，其中地图使用三维数组表示，0表示可通行的区域，1表示障碍物。

function [path, cost] = Astar3D(start, goal, map)
% 三维A*算法
% 输入参数：
% start: 起点坐标，格式为[x,y,z]
% goal: 终点坐标，格式为[x,y,z]
% map: 地图，三维数组，0表示可通行的区域，1表示障碍物
% 输出参数：
% path: 找到的路径，格式为nx3的矩阵，每行表示路径上的一个点的坐标[x,y,z]
% cost: 找到的路径的总代价

% 初始化起点和终点节点
start_node = Node(start, 0, 0, 0, 0, 0, 0);
goal_node = Node(goal, 0, 0, 0, 0, 0, 0);

% 初始化open列表和close列表
open_list = [start_node];
close_list = [];

% 开始搜索
while ~isempty(open_list)
    % 从open列表中选取f值最小的节点
    current_node = open_list(1);
    for i = 1:length(open_list)
        if open_list(i).f < current_node.f
            current_node = open_list(i);
        end
    end
    
    % 判断是否到达终点
    if current_node == goal_node
        % 生成路径
        path = generate_path(current_node);
        cost = current_node.f;
        return;
    end
    
    % 将当前节点从open列表中删除，加入close列表中
    open_list = open_list([1:end] ~= find(open_list == current_node));
    close_list = [close_list, current_node];
    
    % 扩展当前节点的邻居
    for i = -1:1
        for j = -1:1
            for k = -1:1
                % 排除当前节点和超出地图范围的节点
                if i == 0 && j == 0 && k == 0
                    continue;
                end
                neighbor_pos = current_node.pos + [i,j,k];
                if any(neighbor_pos < 1) || any(neighbor_pos > size(map))
                    continue;
                end
                % 排除障碍物
                if map(neighbor_pos(1), neighbor_pos(2), neighbor_pos(3)) == 1
                    continue;
                end
                % 计算邻居节点的代价
                neighbor_g = current_node.g + sqrt(i^2 + j^2 + k^2);
                neighbor_h = heuristic(neighbor_pos, goal_node.pos);
                neighbor_f = neighbor_g + neighbor_h;
                % 判断邻居节点是否已经在close列表或open列表中，并更新其代价
                neighbor_node = Node(neighbor_pos, neighbor_f, neighbor_g, neighbor_h, ...
                    current_node.pos(1), current_node.pos(2), current_node.pos(3));
                if any(neighbor_node == close_list)
                    continue;
                end
                idx = find(open_list == neighbor_node);
                if isempty(idx)
                    open_list = [open_list, neighbor_node];
                elseif neighbor_node.g < open_list(idx).g
                    open_list(idx) = neighbor_node;
                end
            end
        end
    end
end

% 没有找到路径
path = [];
cost = inf;

end

function path = generate_path(node)
% 从终点开始回溯路径
path = [node.pos];
while ~isempty(node.parent)
    node = node.parent;
    path = [node.pos; path];
end
end

function h = heuristic(pos1, pos2)
% 计算启发函数的值（欧式距离）
h = sqrt(sum((pos1 - pos2) .^ 2));
end

classdef Node
% 节点类，保存节点的坐标和代价信息，及其父节点
properties
    pos % 坐标
    f % 代价f = g + h
    g % 实际代价
    h % 启发函数的值
    parent % 父节点
end

methods
    function obj = Node(pos, f, g, h, parent_x, parent_y, parent_z)
        % 构造函数
        obj.pos = pos;
        obj.f = f;
        obj.g = g;
        obj.h = h;
        obj.parent = [];
        if nargin == 7
            obj.parent = Node([parent_x, parent_y, parent_z], 0, 0, 0, [], [], []);
        end
    end
    
    function eq = eq(obj1, obj2)
        % 重载相等运算符
        eq = all(obj1.pos == obj2.pos);
    end
end

end

使用示例：

map = zeros(10, 10, 10); % 创建一个10x10x10的地图
map(1:5,6,5) = 1; % 在地图中添加障碍物
start = [1,1,1]; % 设置起点
goal = [10,10,10]; % 设置终点
[path, cost] = Astar3D(start, goal, map); % 进行搜索
disp(path); % 输出路径
disp(cost); % 输出代价