clc
clear
tic
map3D = occupancyMap3D;
[xGround,yGround,zGround] = meshgrid(0:240,0:350, 0);
xyzGround = [xGround(:) yGround(:) zGround(:)];
occval = 0;
setOccupancy(map3D,xyzGround,occval)
[xB1, yB1, zB1] = meshgrid(50:130, 60:110, 0:20);
[xB2, yB2, zB2] = meshgrid(80:250, 160:240, 0:30);
[xB3, yB3, zB3] = meshgrid(220:250, 290:340, 0:60);
[xB4, yB4, zB4] = meshgrid(0:30, 10:40, 0:80);
[xB5, yB5, zB5] = meshgrid(150:170, 80:100, 0:10);
[xB6, yB6, zB6] = meshgrid(180:200, 80:100, 0:10);
[xB7, yB7, zB7] = meshgrid(210:230, 80:100, 0:10);
[xB11, yB11, zB11] = meshgrid(240:270, 80:100, 0:10);
[xB8, yB8, zB8] = meshgrid(150:170, 50:70, 0:10);
[xB9, yB9, zB9] = meshgrid(180:200, 50:70, 0:10);
[xB10, yB10, zB10] = meshgrid(210:230, 50:70, 0:10);
[xB12, yB12, zB12] = meshgrid(240:270, 50:70, 0:10);
[xB13, yB13, zB13] = meshgrid(200:260, 0:30, 0:13);
xyzB = [xB1(:), yB1(:), zB1(:);
xB2(:), yB2(:), zB2(:);
xB3(:), yB3(:), zB3(:);
xB4(:), yB4(:), zB4(:);
xB5(:), yB5(:), zB5(:);
xB6(:), yB6(:), zB6(:);
xB7(:), yB7(:), zB7(:);
xB8(:), yB8(:), zB8(:);
xB9(:), yB9(:), zB9(:);
xB10(:), yB10(:), zB10(:);
xB11(:), yB11(:), zB11(:);
xB12(:), yB12(:), zB12(:);
xB13(:), yB13(:), zB13(:);];
obs = 0.65;
my3Dmap = map3D;
updateOccupancy(my3Dmap,xyzB,obs);
startPose = [50 60 25 pi/2;
250 310 65 pi/2;
100 170 35 pi/2;
100 30 85 pi/2;
155 90 15 pi/2;
250 60 15 pi/2;
160 60 15 pi/2
220 15 20 pi/2;];
goalPose = startPose;
ss = ExampleHelperUAVStateSpace("MaxRollAngle",pi/3,...
"AirSpeed",5,...
"FlightPathAngleLimit",[-0.5 0.5],...
"Bounds",[-10 400;-10 4000; -10 100; -pi pi]);
sv = validatorOccupancyMap3D(ss,"Map",my3Dmap);
sv.ValidationDistance = 0.5;
planner = plannerBiRRT(ss,sv);
planner.MaxConnectionDistance = 100;
planner.MaxIterations = 100000;
planner.MaxNumTreeNodes = 10000;
numPose = length(startPose);
distances = zeros(numPose, numPose);
for i = 1:numPose
for j = 1:numPose
if i ~= j
rng(100, "twister");
start = [startPose(i,1), startPose(i,2), startPose(i,3), startPose(i,4)];
goal = [goalPose(j,1), goalPose(j,2), goalPose(j,3), goalPose(j,4)];
[pthObj, solnInfo] = plan(planner, start, goal);
smoothWaypointsObj = exampleHelperUAVPathSmoothing(ss, sv, pthObj);
distances(i, j) = pathLength(smoothWaypointsObj);
end
end
end
toc