refactored sensing objective into domain, random inits

.gitignore

@@ -41,10 +41,3 @@ codegen/
 
 # Sandbox contents
 sandbox/*
-
-# Videos
-*.mp4
-*.avi
-
-# Figures
-*.fig

@agent/initialize.m

@@ -29,5 +29,5 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tan(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
+function [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
         domain (1, 1) {mustBeGeometry};
@@ -11,11 +11,12 @@ function obj = initialize(obj, domain, objective, agents, timestep, partitoningF
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Define simulation time parameters
     obj.timestep = timestep;
-    obj.maxIter = maxIter - 1;
+    obj.maxIter = maxIter;
 
     % Define domain
     obj.domain = domain;
@@ -33,17 +34,9 @@ function obj = initialize(obj, domain, objective, agents, timestep, partitoningF
     % Compute adjacency matrix
     obj = obj.updateAdjacency();
 
-    % Set up times to iterate over
-    obj.times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
-    obj.partitioningTimes = obj.times(obj.partitioningFreq:obj.partitioningFreq:size(obj.times, 1));
-
-    % Prepare performance data store (at t = 0, all have 0 performance)
-    obj.fPerf = figure;
-    obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
-
     % Create initial partitioning
     obj = obj.partition();
 
     % Set up plots showing initialized state
-    obj = obj.plot();
+    [obj, f] = obj.plot();
 end

@miSim/miSim.m

@@ -11,29 +11,15 @@ classdef miSim
         obstacles = cell(0, 1); % geometries that define obstacles within the domain
         agents = cell(0, 1); % agents that move within the domain
         adjacency = NaN; % Adjacency matrix representing communications network graph
-        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
-        performance = NaN; % current cumulative sensor performance
-        oldMeanTotalPerf = 0;
-
-        fPerf; % performance plot figure
     end
 
     properties (Access = private)
-        % Sim
-        t = NaN; % current sim time
-        perf; % sensor performance timeseries array
-        times;
-        partitioningTimes;
-
         % Plot objects
-        f = firstPlotSetup(); % main plotting tiled layout figure
         connectionsPlot; % objects for lines connecting agents in spatial plots
         graphPlot; % objects for abstract network graph plot
         partitionPlot; % objects for partition plot
 
-        performancePlot; % objects for sensor performance plot
-
         % Indicies for various plot types in the main tiled layout figure
         spatialPlotIndices = [6, 4, 3, 2];
         objectivePlotIndices = [6, 4];
@@ -42,15 +28,15 @@ classdef miSim
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
-        [obj] = run(obj);
+        [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
+        [obj, f] = run(obj, f);
         [obj]    = partition(obj);
         [obj]    = updateAdjacency(obj);
-        [obj] = plot(obj);
-        [obj] = plotConnections(obj);
-        [obj] = plotPartitions(obj);
-        [obj] = plotGraph(obj);
-        [obj] = updatePlots(obj, updatePartitions);
+        [obj, f] = plot(obj);
+        [obj, f] = plotConnections(obj, ind, f);
+        [obj, f] = plotPartitions(obj, ind, f);
+        [obj, f] = plotGraph(obj, ind, f);
+        [obj, f] = updatePlots(obj, f, updatePartitions);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/partition.m

@@ -9,33 +9,17 @@ function obj = partition(obj)
     % Assess sensing performance of each agent at each sample point
     % in the domain
     agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentPerformances{end + 1} = obj.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
     agentPerformances = cat(3, agentPerformances{:});
     
     % Get highest performance value at each point
     [~, idx] = max(agentPerformances, [], 3);
 
-    % Collect agent indices in the same way as performance
+    % Collect agent indices in the same way
     agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
     agentInds = cat(3, agentInds{:});
 
-    % Use highest performing agent's index to form partitions
+    % Get highest performing agent's index
     [m,n,~] = size(agentInds);
-    [jj, kk] = ndgrid(1:m, 1:n);
-    obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
-
-    % Get individual agent sensor performance
-    nowIdx = [0; obj.partitioningTimes] == obj.t;
-    if isnan(obj.t)
-        nowIdx = 1;
-    end
-    for ii = 1:size(obj.agents, 1)
-        idx = obj.partitioning == ii;
-        agentPerformance = squeeze(agentPerformances(:, :, ii));
-        obj.perf(ii, nowIdx) = sum(agentPerformance(idx) .* obj.objective.values(idx));
-    end
-
-    % Current total performance
-    obj.perf(end, nowIdx) = sum(obj.perf(1:(end - 1), nowIdx));
+    [i,j] = ndgrid(1:m, 1:n);
+    obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
 end

@miSim/plot.m

@@ -1,43 +1,41 @@
-function obj = plot(obj)
+function [obj, f] = plot(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Plot domain
-    [obj.domain, obj.f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
+    [obj.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
 
     % Plot obstacles
     for ii = 1:size(obj.obstacles, 1)
-        [obj.obstacles{ii}, obj.f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, obj.f);
+        [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f);
     end
 
     % Plot objective gradient
-    obj.f = obj.domain.objective.plot(obj.objectivePlotIndices, obj.f);
+    f = obj.domain.objective.plot(obj.objectivePlotIndices, f);
 
     % Plot agents and their collision geometries
     for ii = 1:size(obj.agents, 1)
-        [obj.agents{ii}, obj.f] = obj.agents{ii}.plot(obj.spatialPlotIndices, obj.f);
+        [obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f);
     end
 
     % Plot communication links
-    obj = obj.plotConnections();
+    [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
 
     % Plot abstract network graph
-    obj = obj.plotGraph();
+    [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
 
     % Plot domain partitioning
-    obj = obj.plotPartitions();
+    [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
 
     % Enforce plot limits
     for ii = 1:size(obj.spatialPlotIndices, 2)
-        xlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
-        ylim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
-        zlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
+        xlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
+        ylim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
+        zlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
     end
-
-    % Plot performance
-    obj = obj.plotPerformance();
 end

@miSim/plotConnections.m

@@ -1,9 +1,12 @@
-function obj = plotConnections(obj)
+function [obj, f] = plotConnections(obj, ind, f)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Iterate over lower triangle off-diagonal region of the 
@@ -21,20 +24,20 @@ function obj = plotConnections(obj)
     X = X'; Y = Y'; Z = Z';
 
     % Plot the connections
-    if isnan(obj.spatialPlotIndices)
-        hold(obj.f.CurrentAxes, "on");
-        o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
-        hold(obj.f.CurrentAxes, "off");
+    if isnan(ind)
+        hold(f.CurrentAxes, "on");
+        o = plot3(f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        hold(f.CurrentAxes, "off");
     else
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
-        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        hold(f.Children(1).Children(ind(1)), "off");
     end
 
     % Copy to other plots
-    if size(obj.spatialPlotIndices, 2) > 1
-        for ii = 2:size(obj.spatialPlotIndices, 2)
-            o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
+    if size(ind, 2) > 1
+        for ii = 2:size(ind, 2)
+            o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
         end 
     end
 

@miSim/plotGraph.m

@@ -1,26 +1,29 @@
-function obj = plotGraph(obj)
+function [obj, f] = plotGraph(obj, ind, f)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Form graph from adjacency matrix
     G = graph(obj.adjacency, 'omitselfloops');
 
     % Plot graph object
-    if isnan(obj.networkGraphIndex)
-        hold(obj.f.CurrentAxes, 'on');
-        o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-        hold(obj.f.CurrentAxes, 'off');
+    if isnan(ind)
+        hold(f.CurrentAxes, 'on');
+        o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(f.CurrentAxes, 'off');
     else
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
-        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
-        if size(obj.networkGraphIndex, 2) > 1
+        hold(f.Children(1).Children(ind(1)), 'on');
+        o = plot(f.Children(1).Children(ind(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(f.Children(1).Children(ind(1)), 'off');
+        if size(ind, 2) > 1
             for ii = 2:size(ind, 2)
-                o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
+                o = [o; copyobj(o(1), f.Children(1).Children(ind(ii)))];
             end
         end
     end

@miSim/plotPartitions.m

@@ -1,22 +1,25 @@
-function obj = plotPartitions(obj)
+function [obj, f] = plotPartitions(obj, ind, f)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
-    if isnan(obj.partitionGraphIndex)
-        hold(obj.f.CurrentAxes, 'on');
-        o = imagesc(obj.f.CurrentAxes, obj.partitioning);
-        hold(obj.f.CurrentAxes, 'off');
+    if isnan(ind)
+        hold(f.CurrentAxes, 'on');
+        o = imagesc(f.CurrentAxes, obj.partitioning);
+        hold(f.CurrentAxes, 'off');
     else
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
-        o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
-        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
-        if size(obj.partitionGraphIndex, 2) > 1
+        hold(f.Children(1).Children(ind(1)), 'on');
+        o = imagesc(f.Children(1).Children(ind(1)), obj.partitioning);
+        hold(f.Children(1).Children(ind(1)), 'on');
+        if size(ind, 2) > 1
             for ii = 2:size(ind, 2)
-                o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];
+                o = [o, copyobj(o(1), f.Children(1).Children(ind(ii)))];
             end
         end
     end

@miSim/plotPerformance.m

@@ -1,36 +0,0 @@
-function obj = plotPerformance(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    axes(obj.fPerf);
-    title(obj.fPerf.Children(1), "Sensor Performance");
-    xlabel(obj.fPerf.Children(1), 'Time (s)');
-    ylabel(obj.fPerf.Children(1), 'Sensor Performance');
-    grid(obj.fPerf.Children(1), 'on');
-
-    % Plot current cumulative performance
-    hold(obj.fPerf.Children(1), 'on');
-    o = plot(obj.fPerf.Children(1), obj.perf(end, :));
-    hold(obj.fPerf.Children(1), 'off');
-
-    % Plot current agent performance
-    for ii = 1:(size(obj.perf, 1) - 1)
-        hold(obj.fPerf.Children(1), 'on');
-        o = [o; plot(obj.fPerf.Children(1), obj.perf(ii, :))];
-        hold(obj.fPerf.Children(1), 'off');
-    end
-
-    % Add legend
-    agentStrings = repmat("Agent %d", size(obj.perf, 1) - 1, 1);
-    for ii = 1:size(agentStrings, 1)
-        agentStrings(ii) = sprintf(agentStrings(ii), ii);
-    end
-    agentStrings = ["Total"; agentStrings];
-    legend(obj.fPerf.Children(1), agentStrings, 'Location', 'northwest');
-
-    obj.performancePlot = o;
-end

@miSim/run.m

@@ -1,37 +1,32 @@
-function [obj] = run(obj)
+function [obj, f] = run(obj, f)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+
+    % Set up times to iterate over
+    times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
+    partitioningTimes = times(obj.partitioningFreq:obj.partitioningFreq:size(times, 1));
+
     % Start video writer
     v = obj.setupVideoWriter();
     v.open();
 
-    steady = 0;
-    for ii = 1:size(obj.times, 1)
+    for ii = 1:size(times, 1)
         % Display current sim time
-        obj.t = obj.times(ii);
-        fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
+        t = times(ii);
+        fprintf("Sim Time: %4.2f (%d/%d)\n", t, ii, obj.maxIter)
 
         % Check if it's time for new partitions
         updatePartitions = false;
-        if ismember(obj.t, obj.partitioningTimes)
-            % Check if it's time to end the sim (performance has settled)
-            if obj.t >= obj.partitioningTimes(5)
-                idx = find(obj.t == obj.partitioningTimes);
-                newMeanTotalPerf = mean(obj.perf(end, ((idx - 5 + 1):idx)));
-                if (obj.oldMeanTotalPerf * 0.95 <= newMeanTotalPerf) && (newMeanTotalPerf <= max(1e-6, obj.oldMeanTotalPerf * 1.05))
-                    steady = steady + 1;
-                    if steady >= 3
-                        fprintf("Performance is stable, terminating early at %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
-                        break; % performance is not improving further, exit main sim loop
-                    end
-                end
-                obj.oldMeanTotalPerf = newMeanTotalPerf;
-            end
+        if ismember(t, partitioningTimes)
             updatePartitions = true;
             obj = obj.partition();
         end
@@ -42,13 +37,13 @@ function [obj] = run(obj)
         end
 
         % Update adjacency matrix
-        obj = obj.updateAdjacency();
+        obj = obj.updateAdjacency;
 
         % Update plots
-        obj = obj.updatePlots(updatePartitions);
+        [obj, f] = obj.updatePlots(f, updatePartitions);
 
         % Write frame in to video
-        I = getframe(obj.f);
+        I = getframe(f);
         v.writeVideo(I);
     end
 

@miSim/setupVideoWriter.m

@@ -9,7 +9,7 @@ function v = setupVideoWriter(obj)
     if ispc || ismac
         v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'MPEG-4');
     elseif isunix
-        v = VideoWriter(fullfile('.', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
+        v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
     end
     
     v.FrameRate = 1 / obj.timestep;

@miSim/updatePlots.m

@@ -1,10 +1,12 @@
-function [obj] = updatePlots(obj, updatePartitions)
+function [obj, f] = updatePlots(obj, f, updatePartitions)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
         updatePartitions (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
     end
 
     % Update agent positions, collision geometries
@@ -18,37 +20,24 @@ function [obj] = updatePlots(obj, updatePartitions)
 
     % Update agent connections plot
     delete(obj.connectionsPlot);
-    obj = obj.plotConnections();
+    [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
 
     % Update network graph plot
     delete(obj.graphPlot);
-    obj = obj.plotGraph();
+    [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
 
     % Update partitioning plot
     if updatePartitions
         delete(obj.partitionPlot);
-        obj = obj.plotPartitions();
+        [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
     end
 
     % reset plot limits to fit domain
     for ii = 1:size(obj.spatialPlotIndices, 2)
-        xlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
-        ylim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
-        zlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
+        xlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
+        ylim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
+        zlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
     end
+
     drawnow;
-    
-    % Update performance plot
-    if updatePartitions
-        % find index corresponding to the current time
-        nowIdx = [0; obj.partitioningTimes] == obj.t;
-        nowIdx = find(nowIdx);
-
-        % Re-normalize performance plot
-        normalizingFactor = 1/max(obj.perf(end, 1:nowIdx));
-        obj.performancePlot(1).YData(1:nowIdx) = obj.perf(end, 1:nowIdx) * normalizingFactor;
-        for ii = 2:size(obj.performancePlot, 1)
-            obj.performancePlot(ii).YData(1:nowIdx) = obj.perf(ii - 1, 1:nowIdx) * normalizingFactor;
-        end
-    end
 end

@sensingObjective/initialize.m

@@ -1,17 +1,15 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep)
     arguments (Input)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
-        protectedRange (1, 1) double = 1;
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
     end
 
     obj.groundAlt = domain.minCorner(3);
-    obj.protectedRange = protectedRange;
 
     % Extract footprint limits
     xMin = min(domain.footprint(:, 1));
@@ -29,12 +27,7 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     obj.objectiveFunction = objectiveFunction;
     obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
 
-    % Normalize
-    obj.values = obj.values ./ max(obj.values, [], "all");
-
     % store ground position
-    idx = obj.values == 1;
+    idx = obj.values == max(obj.values, [], "all");
     obj.groundPos = [obj.X(idx), obj.Y(idx)];
-
-    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -1,9 +1,9 @@
-function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
+function obj = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sensingObjective')};
         domain (1, 1) {mustBeGeometry};
-        discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
+        discretizationStep (1, 1) double = 1;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'sensingObjective')};
@@ -23,5 +23,5 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
     objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu, sig);
 
     % Regular initialization
-    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);
+    obj = obj.initialize(objectiveFunction, domain, discretizationStep);
 end

@sensingObjective/sensingObjective.m

@@ -9,12 +9,11 @@ classdef sensingObjective
         X = [];
         Y = [];
         values = [];
-        protectedRange = 1; % keep obstacles from crowding objective
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange);
-        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
+        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep);
+        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep);
         [f  ] = plot(obj, ind, f);
     end
 end

geometries/@rectangularPrism/initializeRandom.m

@@ -1,43 +1,17 @@
-function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain)
+function [obj] = initializeRandom(obj, minDimension, tag, label)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        minDimension (1, 1) double = 10;
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
-        minDimension (1, 1) double = 10;
-        maxDimension (1, 1) double= 20;
-        domain (1, 1) {mustBeGeometry} = rectangularPrism;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
     end
     
-    % Produce random bounds based on region type
-    if tag == REGION_TYPE.DOMAIN
-        % Domain
-        L = ceil(minDimension + rand * (maxDimension - minDimension));
+    % Produce random bounds
+    L = ceil(minDimension + rand * minDimension);
     bounds = [zeros(1, 3); L * ones(1, 3)];
-    else
-        % Obstacle
-
-        % Produce a corners that are contained in the domain
-        ii = 0;
-        candidateMaxCorner = domain.maxCorner + ones(1, 3);
-        candidateMinCorner = domain.minCorner - ones(1, 3);
-        % Continue until the domain contains the obstacle without crowding the objective
-        while ~domain.contains(candidateMaxCorner) || all(domain.objective.groundPos + domain.objective.protectedRange >= candidateMinCorner(1:2), 2) && all(domain.objective.groundPos - domain.objective.protectedRange <= candidateMaxCorner(1:2), 2)
-            if ii == 0 || ii > 10
-                candidateMinCorner = domain.random();
-                candidateMinCorner(3) = 0; % bind to floor
-                ii = 1;
-            end
-
-            candidateMaxCorner = candidateMinCorner + minDimension + rand(1, 3) * (maxDimension - minDimension);
-
-            ii = ii + 1;
-        end
-        
-        bounds = [candidateMinCorner; candidateMaxCorner;];
-    end
 
     % Regular initialization
     obj = obj.initialize(bounds, tag, label);

geometries/@rectangularPrism/rectangularPrism.m

@@ -28,7 +28,7 @@ classdef rectangularPrism
 
     methods (Access = public)
         [obj   ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep);
-        [obj   ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
+        [obj   ] = initializeRandom(obj, tag, label);
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);
         [d     ] = distance(obj, pos);

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Ref="sensingModels" Type="Relative"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/4-Vh5fQn4oEziz1w72O3jpEw0yMp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="420d04e4-3880-4a45-8609-11cb30d87302" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/ohKqyO-2hbPx9f-0yqpBSUUQdsgd.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info Ref="sensorModels" Type="Relative"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/ohKqyO-2hbPx9f-0yqpBSUUQdsgp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="d143c27d-6824-4569-9093-8150b60976cb" type="Reference"/>

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/AmJAKvt15XQGz6JdpTsYHCBwsj0d.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info>
-    <Category UUID="FileClassCategory">
-        <Label UUID="test"/>
-    </Category>
-</Info>

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/AmJAKvt15XQGz6JdpTsYHCBwsj0p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="test_sigmoidSensor.m" type="File"/>

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/vQEWllkHYzJ8MP7NOsUYZ2vAZCAp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plotPerformance.m" type="File"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/9ucwbDs2BkA9ZqIHQI_XVIGSH5Ap.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="sensorModels" type="File"/>

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="sensingModels" type="File"/>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/Cnje8-nVwXLSBkpqGRNU6YqMjQUp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="tiltMembership.m" type="File"/>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/JlTRUONvUsWw4xzUu6ME_Dv4PB4p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="plotParameters.m" type="File"/>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/idDvq0mRu533mATDdzl_FG-b-w4d.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info>
-    <Category UUID="FileClassCategory">
-        <Label UUID="design"/>
-    </Category>
-</Info>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/idDvq0mRu533mATDdzl_FG-b-w4p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="distanceMembership.m" type="File"/>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/jU0rMGjmBFOW1Qf0MUFrG_zqjO0d.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info>
-    <Category UUID="FileClassCategory">
-        <Label UUID="design"/>
-    </Category>
-</Info>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/lA9hUETTjsTN6BFBFM_Djjvh2CId.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info>
-    <Category UUID="FileClassCategory">
-        <Label UUID="design"/>
-    </Category>
-</Info>

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/zd__hNBRxfd17pUXZGjSSPNgl_kd.xml

@@ -1,6 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info>
-    <Category UUID="FileClassCategory">
-        <Label UUID="design"/>
-    </Category>
-</Info>

sensingModels/@sigmoidSensor/sensorPerformance.m

@@ -10,16 +10,14 @@ function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos
         value (:, 1) double;
     end
 
-    % compute direct distance and distance projected onto the ground
     d = vecnorm(agentPos - targetPos, 2, 2); % distance from sensor to target
     x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
-
-    % compute tilt angle
-    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))) - agentTilt; % degrees
+    tiltAngle = atan2(targetPos(:, 3) - agentPos(3), x) - agentTilt;
 
     % Membership functions
-    mu_d = obj.distanceMembership(d);
-    mu_t = obj.tiltMembership(tiltAngle);
+    mu_d = 1 - (1 ./ (1 + exp(-obj.betaDist .* (d - obj.alphaDist)))); % distance
+    mu_p = 1; % pan
+    mu_t = (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle - obj.alphaPan)))); % tilt
 
-    value = mu_d .* mu_t; % assume pan membership is always 1
+    value = mu_d .* mu_p .* mu_t * 1e12;
 end

sensingModels/@sigmoidSensor/sigmoidSensor.m

@@ -5,7 +5,7 @@ classdef sigmoidSensor
         betaDist = NaN;
         alphaPan = NaN;
         betaPan = NaN;
-        alphaTilt = NaN; % degrees
+        alphaTilt = NaN;
         betaTilt = NaN;
     end
 
@@ -13,10 +13,5 @@ classdef sigmoidSensor
         [obj]               = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt);
         [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning);
         [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
-        [f] = plotParameters(obj);
-    end
-    methods (Access = private)
-        x = distanceMembership(obj, d);
-        x = tiltMembership(obj, t);
     end
 end

sensorModels/@sigmoidSensor/distanceMembership.m

@@ -1,10 +0,0 @@
-function x = distanceMembership(obj, d)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-        d (:, 1) double;
-    end
-    arguments (Output)
-        x (:, 1) double;
-    end
-    x = 1 - (1 ./ (1 + exp(-obj.betaDist .* (abs(d) - obj.alphaDist))));
-end

sensorModels/@sigmoidSensor/plotParameters.m

@@ -1,42 +0,0 @@
-function f = plotParameters(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-    end
-    arguments (Output)
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-    end
-
-    % Distance and tilt sample points
-    d = 0:(obj.alphaDist / 100):(2*obj.alphaDist);
-    t = -90:1:90;
-
-    % Sample membership functions
-    d_x = obj.distanceMembership(d);
-    t_x = obj.tiltMembership(t);
-
-    % Plot resultant sigmoid curves
-    f = figure;
-    tiledlayout(f, 2, 1, "TileSpacing", "tight", "Padding", "compact");
-
-    % Distance
-    nexttile(1, [1,  1]);
-    grid("on");
-    title("Distance Membership Sigmoid");
-    xlabel("Distance (m)");
-    ylabel("Membership");
-    hold('on');
-    plot(d, d_x, 'LineWidth', 2);
-    hold('off');
-    ylim([0, 1]);
-
-    % Tilt
-    nexttile(2, [1,  1]);
-    grid("on");
-    title("Tilt Membership Sigmoid");
-    xlabel("Tilt (deg)");
-    ylabel("Membership");
-    hold('on');
-    plot(t, t_x, 'LineWidth', 2);
-    hold('off');
-    ylim([0, 1]);
-end

sensorModels/@sigmoidSensor/tiltMembership.m

@@ -1,10 +0,0 @@
-function x = tiltMembership(obj, t)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-        t (:, 1) double; % degrees
-    end
-    arguments (Output)
-        x (:, 1) double;
-    end
-    x = (1 ./ (1 + exp(-obj.betaTilt .* (t + obj.alphaTilt)))) - (1 ./ (1 + exp(-obj.betaTilt .* (t - obj.alphaTilt))));
-end

test/test_miSim.m

@@ -1,6 +1,5 @@
 classdef test_miSim < matlab.unittest.TestCase
     properties (Access = private)
-        % System under test
         testClass = miSim;
 
         % Sim
@@ -25,8 +24,8 @@ classdef test_miSim < matlab.unittest.TestCase
         objective = sensingObjective;
 
         % Agents
-        minAgents = 2; % Minimum number of agents to be randomly generated
-        maxAgents = 4; % Maximum number of agents to be randomly generated
+        minAgents = 3; % Minimum number of agents to be randomly generated
+        maxAgents = 9; % Maximum number of agents to be randomly generated
         sensingLength = 0.05; % length parameter used by sensing function
         agents = cell(0, 1);
         
@@ -35,18 +34,8 @@ classdef test_miSim < matlab.unittest.TestCase
         maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
         collisionRanges = NaN;
 
-        % Sensing
-        betaDistMin = 3;
-        betaDistMax = 15;
-        betaTiltMin = 3;
-        betaTiltMax = 15;
-        alphaDistMin = 2.5;
-        alphaDistMax = 3;
-        alphaTiltMin = 15; % degrees
-        alphaTiltMax = 30; % degrees
-
         % Communications
-        comRange = 8; % Maximum range between agents that forms a communications link
+        comRange = 5; % Maximum range between agents that forms a communications link
     end
 
     % Setup for each test
@@ -54,13 +43,15 @@ classdef test_miSim < matlab.unittest.TestCase
         % Generate a random domain
         function tc = setDomain(tc)
             % random integer-dimensioned cubic domain
-            tc.domain = tc.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension);
+            tc.domain = tc.domain.initializeRandom(tc.minDimension, REGION_TYPE.DOMAIN, "Domain");
             % Random bivariate normal PDF objective
-            tc.domain.objective = tc.domain.objective.initializeRandomMvnpdf(tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initializeRandomMvnpdf(tc.domain, tc.protectedRange, tc.discretizationStep);
         end
         % Instantiate agents
         function tc = setAgents(tc)
-            % Agents will be initialized under different parameters in individual test cases
+            % Agents will be initialized under different parameters in
+            % individual test cases
+
             % Instantiate a random number of agents according to parameters
             for ii = 1:randi([tc.minAgents, tc.maxAgents])
                 tc.agents{ii, 1} = agent;
@@ -82,15 +73,53 @@ classdef test_miSim < matlab.unittest.TestCase
             for ii = 1:size(tc.obstacles, 1)
                 badCandidate = true;
                 while badCandidate
-                    % Instantiate a rectangular prism obstacle inside the domain
+                    % Instantiate a rectangular prism obstacle
                     tc.obstacles{ii} = rectangularPrism;
-                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain);
     
-                    % Check if the obstacle collides with an existing obstacle
-                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
-                        badCandidate = false;
+                    % Randomly generate min corner for the obstacle
+                    candidateMinCorner = tc.domain.random();
+                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
+    
+                    % Randomly select a corresponding maximum corner that
+                    % satisfies min/max obstacle size specifications
+                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
+                
+                    % Initialize obstacle
+                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
+
+                    % Check if the obstacle intersects with any existing
+                    % obstacles
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
+                            violation = true;
+                            break;
                         end
                     end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure that the obstacles are fully contained by 
+                    % the domain
+                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles don't cover the sensing
+                    % objective
+                    if obstacleCoversObjective(tc.domain.objective, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles aren't too close to the
+                    % sensing objective
+                    if obstacleCrowdsObjective(tc.domain.objective, tc.obstacles{ii}, tc.protectedRange)
+                        continue;
+                    end
+                    
+                    badCandidate = false;
+                end
             end
 
             % Add agents individually, ensuring that each addition does not
@@ -104,11 +133,9 @@ classdef test_miSim < matlab.unittest.TestCase
                     if ii == 1
                         while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                             candidatePos = tc.domain.random();
-                            candidatePos(3) = 1  + rand * 3; % place agents at decent altitudes for sensing
                         end
                     else
                         candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
-                        candidatePos(3) = 1  + rand * 3; % place agents at decent altitudes for sensing
                     end
 
                     % Make sure that the candidate position is within the
@@ -152,7 +179,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
                     % Initialize candidate agent sensor model
                     sensor = sigmoidSensor;
-                    sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
+                    sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
 
                     % Initialize candidate agent
                     newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii)); 
@@ -203,7 +230,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -214,15 +241,53 @@ classdef test_miSim < matlab.unittest.TestCase
             for ii = 1:size(tc.obstacles, 1)
                 badCandidate = true;
                 while badCandidate
-                    % Instantiate a rectangular prism obstacle inside the domain
+                    % Instantiate a rectangular prism obstacle
                     tc.obstacles{ii} = rectangularPrism;
-                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain);
     
-                    % Check if the obstacle collides with an existing obstacle
-                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
-                        badCandidate = false;
+                    % Randomly generate min corner for the obstacle
+                    candidateMinCorner = tc.domain.random();
+                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
+    
+                    % Randomly select a corresponding maximum corner that
+                    % satisfies min/max obstacle size specifications
+                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
+                
+                    % Initialize obstacle
+                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
+
+                    % Check if the obstacle intersects with any existing
+                    % obstacles
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
+                            violation = true;
+                            break;
                         end
                     end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure that the obstacles are fully contained by 
+                    % the domain
+                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles don't cover the sensing
+                    % objective
+                    if obstacleCoversObjective(tc.domain.objective, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles aren't too close to the
+                    % sensing objective
+                    if obstacleCrowdsObjective(tc.domain.objective, tc.obstacles{ii}, tc.protectedRange)
+                        continue;
+                    end
+                    
+                    badCandidate = false;
+                end
             end
 
             % Add agents individually, ensuring that each addition does not
@@ -236,11 +301,9 @@ classdef test_miSim < matlab.unittest.TestCase
                     if ii == 1
                         while agentsCrowdObjective(tc.domain.objective, candidatePos, mean(tc.domain.dimensions) / 2)
                             candidatePos = tc.domain.random();
-                            candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, 0.5 + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                         end
                     else
                         candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
-                        candidatePos(3) = min([tc.domain.maxCorner(3) * 0.95, 0.5 + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
                     end
 
                     % Make sure that the candidate position is within the
@@ -284,7 +347,7 @@ classdef test_miSim < matlab.unittest.TestCase
                     
                     % Initialize candidate agent sensor model
                     sensor = sigmoidSensor;
-                    sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
+                    sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
 
                     % Initialize candidate agent
                     newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii));
@@ -335,10 +398,10 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
 
             % Run simulation loop
-            tc.testClass = tc.testClass.run();
+            [tc.testClass, f] = tc.testClass.run(f);
         end
         function test_basic_partitioning(tc)
             % place agents a fixed distance +/- X from the domain's center
@@ -348,81 +411,25 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2), eye(2)), tc.domain.footprint, tc.domain.minCorner(3), tc.discretizationStep);
         
             % Initialize agent collision geometry
-            dh = [0,0,-1]; % bias agent altitude from domain center
             geometry1 = rectangularPrism;
             geometry2 = geometry1;
-            geometry1 = geometry1.initialize([tc.domain.center + dh + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
-            geometry2 = geometry2.initialize([tc.domain.center + dh - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 2));
+            geometry1 = geometry1.initialize([tc.domain.center + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            geometry2 = geometry2.initialize([tc.domain.center - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 2));
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
-            % Homogeneous sensor model parameters
-            sensor = sensor.initialize(2.75, 9, NaN, NaN, 22.5, 9);
-            % Heterogeneous sensor model parameters
-            % sensor = sensor.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
-
-            % Plot sensor parameters (optional)
-            % f = sensor.plotParameters();
+            sensor = sensor.initialize(2.5, 3, NaN, NaN, deg2rad(15), 3);
     
             % Initialize agents
             tc.agents = {agent; agent};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + dh + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1));
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + dh - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2));
-
-            % Optional third agent along the +Y axis
-            geometry3 = rectangularPrism;
-            geometry3 = geometry3.initialize([tc.domain.center + dh - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + dh - [0, d, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 3));
-            tc.agents{3} = agent;
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, @gradientAscent, 3*d, 3, sprintf("Agent %d", 3));
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1));
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2));
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
-            close(tc.testClass.fPerf);
-        end
-        function test_single_partition(tc)
-            % make basic domain
-            l = 10; % domain size
-            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
-
-            % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2) + rand(1, 2) * 6 - 3), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            geometry1 = rectangularPrism;
-            geometry1 = geometry1.initialize([[tc.domain.center(1:2), 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2), 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
-            
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            % Homogeneous sensor model parameters
-            % sensor = sensor.initialize(2.5666, 5.0807, NaN, NaN, 20.8614, 13); % 13
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 20, 3);
-
-            % Plot sensor parameters (optional)
-            f = sensor.plotParameters();
-
-            % Initialize agents
-            tc.agents = {agent};
-            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1));
-
-            % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
-            close(tc.testClass.fPerf);
-        end
-    end
-
-    methods
-        function c = obstacleCollisionCheck(~, obstacles, obstacle)
-            % Check if the obstacle intersects with any other obstacles
-            c = false;
-            for ii = 1:size(obstacles, 1)
-                if geometryIntersects(obstacles{ii}, obstacle)
-                    c = true;
-                end
-            end
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
         end
     end
 end

test/test_sigmoidSensor.m

@@ -1,62 +0,0 @@
-classdef test_sigmoidSensor < matlab.unittest.TestCase
-    properties (Access = private)
-        % System under test
-        testClass = sigmoidSensor;
-
-        % Domain
-        domain = rectangularPrism;
-
-        % Sensor parameter ranges
-        betaDistMin = 3;
-        betaDistMax = 15;
-        betaTiltMin = 3;
-        betaTiltMax = 15;
-        alphaDistMin = 2.5;
-        alphaDistMax = 3;
-        alphaTiltMin = 15; % degrees
-        alphaTiltMax = 30; % degrees
-    end
-
-    methods (TestMethodSetup)
-        function tc = setup(tc)
-            % Reinitialize sensor with random parameters
-            tc.testClass = sigmoidSensor;
-            tc.testClass = tc.testClass.initialize(tc.alphaDistMin + rand * (tc.alphaDistMax - tc.alphaDistMin), tc.betaDistMin + rand * (tc.betaDistMax - tc.betaDistMin), NaN, NaN, tc.alphaTiltMin + rand * (tc.alphaTiltMax - tc.alphaTiltMin), tc.betaTiltMin + rand * (tc.betaTiltMax - tc.betaTiltMin));
-        end
-    end
-
-    methods (Test)
-        % Test methods
-        function test_sensorPerformance(tc)
-            tc.testClass = sigmoidSensor;
-            alphaDist = 2.5;
-            betaDist = 3;
-            alphaTilt = 15; % degrees
-            betaTilt = 3;
-            h = 1e-6;
-            tc.testClass = tc.testClass.initialize(alphaDist, betaDist, NaN, NaN, alphaTilt, betaTilt);
-            
-            % Plot (optional)
-            % tc.testClass.plotParameters();
-
-            % Anticipate perfect performance for a point directly below and
-            % extremely close
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [0, 0, 0]), 1, 'RelTol', 1e-3); 
-            % It looks like mu_t can max out at really low values like 0.37
-            % when alphaTilt and betaTilt are small, which seems wrong
-
-            % Performance at nadir point, distance alphaDist should be 1/2 exactly
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, alphaDist], NaN, 0, [0, 0, 0]), 1/2);
-
-            % Performance at (almost) 0 distance, alphaTilt should be 1/2
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [tand(alphaTilt)*h, 0, 0]), 1/2, 'RelTol', 1e-3);
-
-            % Performance at great distance should be 0
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, 10], NaN, 0, [0, 0, 0]), 0, 'AbsTol', 1e-9);
-
-            % Performance at great tilt should be 0
-            tc.verifyEqual(tc.testClass.sensorPerformance([0, 0, h], NaN, 0, [5, 5, 0]), 0, 'AbsTol', 1e-9);
-        end
-    end
-
-end