18 lines
650 B
Matlab
18 lines
650 B
Matlab
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
|