3D RRT* algorithm with obstacle original code is from https://www.mathworks.com/matlabcentral/fileexchange/60993-2d-3d-rrt-algorithm add an obstacle in 3D