三维空间下的rrt程序

%%   RRT* 3D state space
%%  - create a path from a start node to an end node
%%    using the RRT algorithm.
%%  - RRT* = Rapidly-exploring Random Tree star
%% This short programm is writen based other basic RRT code and used for test my hardware work .
%% Apologize no time to add the enough comments 
%%  Size（Adrian） Xiao   DEC 2016  University of Queensland Australia  uqsxiao@uq.edu.au

function RRT_star_3D;

clear all;
% create random worlddetermine the parameters of simulation
Size = 16384;       % the 2D world is a 100 by 100 map

Obs_Cordi = [111;111;111];
NumObstacles = 3; % randomly obstacles distributed on the map
maxObstacleRadius = 0.05; % define the max radius of obstacle
maxturningangle = 1.05;  %75deg
Heading = 0.758;% 45 degree; ccw rotation ve