Upload New File

Matlab Code/getConstBoundSet.m

+function boundSet = getConstBoundSet(Z,x_area,y_area,bddBox,rho,obs_points)
+
+[maxCols,~] = max(Z);
+
+maxX = 0;
+maxY = 0;
+maxVal = 0;
+r = rho/2;
+
+for i=1:length(x_area)
+    for j=1:length(y_area)
+        if inpolygon(x_area(i), y_area(j), bddBox(:,1), bddBox(:,2))
+            obs_dists = sqrt((obs_points(:,1) - x_area(i)).^2 + (obs_points(:,2) - y_area(j)).^2);
+            if ~any(obs_dists <= 0.2)
+                if Z(j,i) >= maxVal
+                    maxVal = Z(j,i);
+                    maxX = x_area(i);
+                    maxY = y_area(j);
+                end
+            end
+        end
+    end
+end
+%
+% if maxX + sig >= bddBox(2)
+%     maxX = bddBox(2) - sig;
+% elseif maxX - sig <= bddBox(1)
+%     maxX = bddBox(1) + sig;
+% end
+%
+% if maxY + sig >= bddBox(4)
+%     maxY = bddBox(4) - sig;
+% elseif maxX - sig <= bddBox(3)
+%     maxY = bddBox(3) + sig;
+% end
+
+th = 0:0.05:(2*pi);
+
+
+[xSet, ySet] = pol2cart(th,r);
+
+boundSet = [xSet'+maxX,ySet'+maxY];
+
+end
\ No newline at end of file