RRT* (matlab)

%%------------------------
%%2022/06/05
%%matlab
%%rrt star
%%------------------------

clc;
clear all;
close all;

%加载地图
ImpRgb = imread("map.png");
Imp = rgb2gray(ImpRgb);
imshow(Imp);
hold on;
x_l = size(Imp,1)  %row
y_l = size(Imp,2)  %col

% Imp(120,129)
% plot(120,120,'r*');
% hold on;

%参数初始化
start = [1,1];
goal = [450,800];
threshold = 30;
step = 30;
r = 70;
MaxIter = 3000;

T.v(1).x = start(1);
T.v(1).y = start(2);
T.v(1).xPre = start(1);
T.v(1).yPre = start(2);
T.v(1).cost = 0;
T.v(1).indPre = -1;

plot(start(2),start(1),'mo','MarkerSize',10,'MarkerFaceColor','m');
plot(goal(2),goal(1),'go','MarkerSize',10,'MarkerFaceColor','g');

%开始主循环
for iter = 1:MaxIter
   %step1.生成随机点
   n = rand();
   if n < 0.5
        Prand = [unifrnd(0,x_l),unifrnd(0,y_l)];
   else
       Prand = goal;
   en