function path = rtt(map, start, goal)
    maxIterations = 5000;
    stepSize = 5;
    goalThreshold = 5; % 这里用行列坐标距离阈值
    
    mapSize = size(map);
    
    % 初始化树，存储节点和父节点索引
    tree.nodes = start;
    tree.parents = 0;
    
    for i = 1:maxIterations
        % 采样随机点
        randPoint = sampler(mapSize, goal);
        
        % 找到最近树节点
        [nearestIdx, nearestPoint] = find_nearest(tree.nodes, randPoint);
        
        % 局部规划器扩展新节点
        newPoint = local_planner(map, nearestPoint, randPoint, stepSize);
        
        % 如果无法扩展则跳过
        if isempty(newPoint)
            continue;
        end
        
        % 加入树
        tree.nodes = [tree.nodes; newPoint];
        tree.parents = [tree.parents; nearestIdx];
        
        % 判断是否到达目标
        if norm(newPoint - goal) < goalThreshold
            tree.nodes = [tree.nodes; goal];
            tree.parents = [tree.parents; size(tree.nodes, 1) - 1];
            path = make_path(tree);
            return;
        end
    end
    
    % 没有找到路径，返回空数组
    path = [];
end

function point = sampler(mapSize, goal)
    % 10%概率采样目标点，90%概率采样随机点
    if rand() < 0.1
        point = goal;
    else
        point = round([rand()*mapSize(1), rand()*mapSize(2)]);
        % 保证点不越界
        point(1) = max(min(point(1), mapSize(1)), 1);
        point(2) = max(min(point(2), mapSize(2)), 1);
    end
end

function [idx, nearest] = find_nearest(nodes, point)
    % 计算所有节点到point的距离，返回最近节点的索引和坐标
    dists = vecnorm(nodes - point, 2, 2);
    [~, idx] = min(dists);
    nearest = nodes(idx, :);
end

function newPoint = local_planner(map, nearestPoint, randPoint, stepSize)
    % 沿着方向扩展，检查碰撞和越界
    direction = randPoint - nearestPoint;
    if norm(direction) == 0
        newPoint = [];
        return;
    end
    direction = direction / norm(direction);
    newPoint = round(nearestPoint + direction * stepSize);
    
    % 越界检测
    if newPoint(1) < 1 || newPoint(2) < 1 || ...
       newPoint(1) > size(map, 1) || newPoint(2) > size(map, 2)
        newPoint = [];
        return;
    end
    
    % 碰撞检测(用简单连线检测)
    if isCollision(map, nearestPoint, newPoint)
        newPoint = [];
        return;
    end
end

function collision = isCollision(map, p1, p2)
    % 使用Bresenham算法检查两点间是否碰撞
    linePts = bresenham(p1, p2);
    collision = false;
    for i = 1:size(linePts,1)
        pt = linePts(i,:);
        if map(pt(1), pt(2)) == 1
            collision = true;
            return;
        end
    end
end

function pts = bresenham(p1, p2)
    % Bresenham直线算法实现
    x1 = p1(1); y1 = p1(2);
    x2 = p2(1); y2 = p2(2);
    dx = abs(x2 - x1); dy = abs(y2 - y1);
    sx = sign(x2 - x1); sy = sign(y2 - y1);
    err = dx - dy;
    
    pts = [];
    while true
        pts = [pts; x1, y1];
        if x1 == x2 && y1 == y2
            break;
        end
        e2 = 2*err;
        if e2 > -dy
            err = err - dy;
            x1 = x1 + sx;
        end
        if e2 < dx
            err = err + dx;
            y1 = y1 + sy;
        end
    end
end

function path = make_path(tree)
    % 通过父节点回溯路径
    path = tree.nodes(end, :);
    idx = size(tree.nodes, 1);
    while tree.parents(idx) ~= 0
        idx = tree.parents(idx);
        path = [tree.nodes(idx, :); path];
    end
    
    % 打印路径
    if ~isempty(path)
        disp('路径为：');
        for i = 1:size(path, 1)
            fprintf('(%d, %d)\n', path(i, 2), path(i, 1));
        end
    else
        disp('未找到路径。');
    end
end