function plotRRT(tree, path, S, G)
    % Visualizes the tree and the path
    figure;
    plot(tree(1, :), tree(2, :), 'bo', 'MarkerFaceColor', 'b'); % Tree nodes
    hold on;
    plot(S(1), S(2), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); % Start point
    plot(G(1), G(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); % Goal point
    for i = 1:length(tree)-1
        line([tree(1, i), tree(1, i+1)], [tree(2, i), tree(2, i+1)], 'Color', 'b'); % Tree branches
    end
    line(path(1, :), path(2, :), 'Color', 'r', 'LineWidth', 2); % Path
    title('RRT Path Planning');
    xlabel('x');
    ylabel('y');
    grid on;
    axis equal;
end