请给matlab一个完整的三维A*算法，地图用建模实现

以下是一个 Matlab 实现的三维 A* 算法，其中地图使用建模实现（即使用三维矩阵表示地图）：

function path = A_star_3D(start, goal, map)
% 输入：
% start：起点坐标，形如 [x, y, z]，其中 x、y、z 分别为起点在三维矩阵中的横、纵、高坐标
% goal：终点坐标，形如 [x, y, z]，其中 x、y、z 分别为终点在三维矩阵中的横、纵、高坐标
% map：三维矩阵型地图，其中 0 表示可通行，1 表示障碍物
% 输出：
% path：路径，形如 [x1, y1, z1; x2, y2, z2; ...; xn, yn, zn]，其中 xi、yi、zi 分别为路径上第 i 个点在三维矩阵中的横、纵、高坐标

% 初始化 open 列表和 closed 列表
open_list = PriorityQueue();
closed_list = zeros(size(map));

% 将起点加入 open 列表
start_node = Node(start, 0, heuristic(start, goal));
open_list.put(start_node);

while ~open_list.isempty()
    % 取出 open 列表中 f 值最小的节点
    curr_node = open_list.get();
    
    % 如果当前节点为目标节点，则返回路径
    if isequal(curr_node.pos, goal)
        path = construct_path(curr_node);
        return;
    end
    
    % 将当前节点加入 closed 列表
    closed_list(curr_node.pos(1), curr_node.pos(2), curr_node.pos(3)) = 1;
    
    % 遍历当前节点的所有邻居
    neighbors = get_neighbors(curr_node.pos, map);
    for i = 1:size(neighbors, 1)
        neighbor_pos = neighbors(i,:);
        
        % 如果邻居节点已在 closed 列表中，则跳过
        if closed_list(neighbor_pos(1), neighbor_pos(2), neighbor_pos(3)) == 1
            continue;
        end
        
        % 计算从起点到邻居节点的 g 值
        g = curr_node.g + dist(curr_node.pos, neighbor_pos);
        
        % 如果邻居节点不在 open 列表中，则将其加入 open 列表
        neighbor_node = Node(neighbor_pos, g, heuristic(neighbor_pos, goal), curr_node);
        if ~open_list.contains(neighbor_node)
            open_list.put(neighbor_node);
        else
            % 如果邻居节点已在 open 列表中，且从起点到该节点的 g 值更小，则更新 open 列表中的节点信息
            open_node = open_list.get_node(neighbor_node);
            if g < open_node.g
                open_node.parent = curr_node;
                open_node.g = g;
                open_list.put(open_node);
            end
        end
    end
end

% 如果 open 列表为空，说明无法到达目标节点
path = [];

end

function neighbors = get_neighbors(pos, map)
% 获取当前节点的所有邻居
[x,y,z] = ndgrid(-1:1,-1:1,-1:1);
neighbors = [x(:),y(:),z(:)];
neighbors(ismember(neighbors,[0,0,0],'rows')|pos+neighbors(:,[1,2,3])<1|pos+neighbors(:,[1,2,3])>size(map,1)) = [];
neighbors = bsxfun(@plus,neighbors,pos);
neighbors(logical(map(sub2ind(size(map),neighbors(:,1),neighbors(:,2),neighbors(:,3))))) = [];

end

function d = dist(pos1, pos2)
% 计算两点之间的曼哈顿距离
d = sum(abs(pos1 - pos2));

end

function h = heuristic(pos, goal)
% 计算启发式函数值（曼哈顿距离）
h = dist(pos, goal);

end

function path = construct_path(node)
% 构造路径
path = [];
while ~isempty(node)
    path = [node.pos; path];
    node = node.parent;
end

end

classdef Node < handle
    % 节点类，存储节点信息
    
    properties
        pos % 坐标
        g % 从起点到该节点的实际代价
        h % 从该节点到目标节点的估计代价
        parent % 父节点
    end
    
    methods
        function obj = Node(pos, g, h, parent)
            obj.pos = pos;
            obj.g = g;
            obj.h = h;
            obj.parent = parent;
        end
        
        function f = f(obj)
            f = obj.g + obj.h;
        end
        
        function eq = eq(obj1, obj2)
            eq = isequal(obj1.pos, obj2.pos);
        end
    end
    
end

classdef PriorityQueue < handle
    % 优先队列类，用于存储节点，并按照 f 值排序
    
    properties
        nodes % 节点数组
        count % 节点数
    end
    
    methods
        function obj = PriorityQueue()
            obj.nodes = Node.empty;
            obj.count = 0;
        end
        
        function put(obj, node)
            % 将节点加入队列
            obj.count = obj.count + 1;
            obj.nodes(obj.count) = node;
            
            % 按照 f 值排序
            for i = obj.count:-1:2
                if obj.nodes(i).f() < obj.nodes(i-1).f()
                    tmp = obj.nodes(i-1);
                    obj.nodes(i-1) = obj.nodes(i);
                    obj.nodes(i) = tmp;
                else
                    break;
                end
            end
        end
        
        function node = get(obj)
            % 取出 f 值最小的节点
            node = obj.nodes(1);
            
            % 将最后一个节点放到第一个节点的位置，并删除最后一个节点
            obj.nodes(1) = obj.nodes(obj.count);
            obj.count = obj.count - 1;
            
            % 按照 f 值排序
            for i = 1:obj.count-1
                if obj.nodes(i).f() > obj.nodes(i+1).f()
                    tmp = obj.nodes(i);
                    obj.nodes(i) = obj.nodes(i+1);
                    obj.nodes(i+1) = tmp;
                else
                    break;
                end
            end
        end
        
        function bool = contains(obj, node)
            % 判断队列中是否包含某个节点
            bool = false;
            for i = 1:obj.count
                if isequal(obj.nodes(i).pos, node.pos)
                    bool = true;
                    break;
                end
            end
        end
        
        function node = get_node(obj, node)
            % 获取队列中的某个节点
            for i = 1:obj.count
                if isequal(obj.nodes(i).pos, node.pos)
                    node = obj.nodes(i);
                    break;
                end
            end
        end
        
        function bool = isempty(obj)
            % 判断队列是否为空
            bool = obj.count == 0;
        end
    end
    
end

使用该算法，可以通过以下代码来生成一个随机地图，并对其进行路径规划：

% 生成随机地图
map = randi([0,1], [10, 10, 10]);

% 设置起点和终点
start = [1,1,1];
goal = [10,10,10];

% 进行路径规划
path = A_star_3D(start, goal, map);
disp(path);