added cone geometry, implemented fov visualization

agent.m

@@ -5,18 +5,25 @@ classdef agent
         label = "";
 
         % Sensor
-        sensingFunction = @(r) 0.5; % probability of detection as a function of range
+        sensorModel;
         sensingLength = 0.05; % length parameter used by sensing function
 
+        % Guidance
+        guidanceModel;
+
         % State
         lastPos = NaN(1, 3); % position from previous timestep
         pos = NaN(1, 3); % current position
         vel = NaN(1, 3); % current velocity
-        cBfromC = NaN(3); % current DCM body from sim cartesian (assume fixed for now)
+        pan = NaN; % pan angle
+        tilt = NaN; % tilt angle
 
         % Collision
         collisionGeometry;
 
+        % FOV cone
+        fovGeometry;
+
         % Communication
         comRange = NaN;
 
@@ -25,15 +32,16 @@ classdef agent
     end
 
     methods (Access = public)
-        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, sensingLength, comRange, index, label)
+        function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'agent')};
                 pos (1, 3) double;
                 vel (1, 3) double;
-                cBfromC (3, 3) double {mustBeDcm};
+                pan (1, 1) double;
+                tilt (1, 1) double;
                 collisionGeometry (1, 1) {mustBeGeometry};
-                sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')} = @(r) 0.5;
+                sensorModel (1, 1) {mustBeSensor}
-                sensingLength (1, 1) double = NaN;
+                guidanceModel (1, 1) {mustBeA(guidanceModel, 'function_handle')};
                 comRange (1, 1) double = NaN;
                 index (1, 1) double = NaN;
                 label (1, 1) string = "";
@@ -44,26 +52,35 @@ classdef agent
 
             obj.pos = pos;
             obj.vel = vel;
-            obj.cBfromC = cBfromC;
+            obj.pan = pan;
+            obj.tilt = tilt;
             obj.collisionGeometry = collisionGeometry;
-            obj.sensingFunction = sensingFunction;
+            obj.sensorModel = sensorModel;
-            obj.sensingLength = sensingLength;
+            obj.guidanceModel = guidanceModel;
             obj.comRange = comRange;
             obj.index = index;
             obj.label = label;
+
+            % Initialize FOV cone
+            obj.fovGeometry = cone;
+            obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], obj.sensorModel.r, obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
         end
-        function obj = run(obj, objectiveFunction, domain)
+        function obj = run(obj, sensingObjective, domain, partitioning)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'agent')};
-                objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+                sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
                 domain (1, 1) {mustBeGeometry};
+                partitioning (:, :) double;
             end
             arguments (Output)
                 obj (1, 1) {mustBeA(obj, 'agent')};
             end
 
-            % Do sensing to determine target position
+            % Do sensing
-            nextPos = obj.sensingFunction(objectiveFunction, domain, obj.pos, obj.sensingLength);
+            [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
+
+            % Determine next planned position
+            nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
 
             % Move to next position
             % (dynamics not modeled at this time)
@@ -103,11 +120,18 @@ classdef agent
                 end
             end
 
-            % Network connections
+            % Update FOV geometry surfaces
+            for jj = 1:size(obj.fovGeometry.surface, 2)
+                % Update each plot
+                obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
+                obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
+                obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
+            end
         end
-        function [obj, f] = plot(obj, f)
+        function [obj, f] = plot(obj, ind, f)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'agent')};
+                ind (1, :) double = NaN;
                 f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
             end
             arguments (Output)
@@ -119,22 +143,25 @@ classdef agent
             f = firstPlotSetup(f);
 
             % Plot points representing the agent position
-            hold(f.CurrentAxes, "on");
+            hold(f.Children(1).Children(end), "on");
-            o = scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
+            o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
-            hold(f.CurrentAxes, "off");
+            hold(f.Children(1).Children(end), "off");
 
             % Check if this is a tiled layout figure
             if strcmp(f.Children(1).Type, 'tiledlayout')
                 % Add to other perspectives
                 o = [o; copyobj(o(1), f.Children(1).Children(2))];
                 o = [o; copyobj(o(1), f.Children(1).Children(3))];
-                o = [o; copyobj(o(1), f.Children(1).Children(5))];
+                o = [o; copyobj(o(1), f.Children(1).Children(4))];
             end
 
             obj.scatterPoints = o;
 
             % Plot collision geometry
-            [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(f);
+            [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f);
+
+            % Plot FOV geometry
+            [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
         end
     end
 end

firstPlotSetup.m

@@ -1,49 +1,78 @@
 function f = firstPlotSetup(f)
+    arguments (Input)
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
     if isempty(f.CurrentAxes)
-        tiledlayout(f, 4, 3, "TileSpacing", "tight", "Padding", "compact");
+        tiledlayout(f, 5, 5, "TileSpacing", "tight", "Padding", "compact");
-
-        % Top-down view
-        nexttile(1, [1, 2]);
-        axes(f.Children(1).Children(1));
-        axis(f.Children(1).Children(1), "image");
-        grid(f.Children(1).Children(1), "on");
-        view(f.Children(1).Children(1), 0, 90);
-        xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
-        title(f.Children(1).Children(1), "Top-down Perspective");
-
-        % Communications graph
-        nexttile(3, [1, 1]);
-        axes(f.Children(1).Children(1));
-        axis(f.Children(1).Children(1), "image");
-        grid(f.Children(1).Children(1), "off");
-        view(f.Children(1).Children(1), 0, 0);
-        title(f.Children(1).Children(1), "Network Graph");
 
         % 3D view
-        nexttile(4, [2, 2]);
+        nexttile(1, [4, 5]);
         axes(f.Children(1).Children(1));
         axis(f.Children(1).Children(1), "image");
         grid(f.Children(1).Children(1), "on");
         view(f.Children(1).Children(1), 3);
         xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
-        title(f.Children(1).Children(1), "3D Perspective");
+        title(f.Children(1).Children(1), "3D View");
+
+        % Communications graph
+        nexttile(21, [1, 1]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "off");
+        view(f.Children(1).Children(1), 0, 90);
+        title(f.Children(1).Children(1), "Network Graph");
+        set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
+        set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
+        set(f.Children(1).Children(1), 'XTickLabel', {});
+        set(f.Children(1).Children(1), 'YTickLabel', {});
+        set(f.Children(1).Children(1), 'XTick', []);
+        set(f.Children(1).Children(1), 'YTick', []);
+        set(f.Children(1).Children(1), 'XColor', 'none');
+        set(f.Children(1).Children(1), 'YColor', 'none');
+        
+        % Top-down view
+        nexttile(22, [1, 1]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "on");
+        view(f.Children(1).Children(1), 0, 90);
+        xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
+        title(f.Children(1).Children(1), "Top-down View");
 
         % Side-on view
-        nexttile(6, [2, 1]);
+        nexttile(23, [1, 1]);
         axes(f.Children(1).Children(1));
         axis(f.Children(1).Children(1), "image");
         grid(f.Children(1).Children(1), "on");
         view(f.Children(1).Children(1), 90, 0);
         ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
-        title(f.Children(1).Children(1), "Side-on Perspective");
+        title(f.Children(1).Children(1), "Side-on View");
 
         % Front-on view
-        nexttile(10, [1, 2]);
+        nexttile(24, [1, 1]);
         axes(f.Children(1).Children(1));
         axis(f.Children(1).Children(1), "image");
         grid(f.Children(1).Children(1), "on");
         view(f.Children(1).Children(1), 0, 0);
         xlabel(f.Children(1).Children(1), "X"); zlabel(f.Children(1).Children(1), "Z");
-        title(f.Children(1).Children(1), "Front-on Perspective");
+        title(f.Children(1).Children(1), "Front-on View");
+
+        % Partitioning
+        nexttile(25, [1, 1]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "on");
+        view(f.Children(1).Children(1), 0, 90);
+        xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
+        title(f.Children(1).Children(1), "Domain Partitioning");
+        set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
+        set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
+        set(f.Children(1).Children(1), 'XTickLabel', {});
+        set(f.Children(1).Children(1), 'YTickLabel', {});
+        set(f.Children(1).Children(1), 'XTick', []);
+        set(f.Children(1).Children(1), 'YTick', []);
     end
 end

geometries/REGION_TYPE.m

@@ -8,6 +8,7 @@ classdef REGION_TYPE
         DOMAIN (1, [0, 0, 0]); % domain region
         OBSTACLE (2, [255, 127, 127]); % obstacle region
         COLLISION (3, [255, 255, 128]); % collision avoidance region
+        FOV (4, [255, 165, 0]); % field of view region
     end
     methods
         function obj = REGION_TYPE(id, color)

geometries/cone.m

@@ -0,0 +1,82 @@
+classdef cone
+    % Conical geometry
+    properties (SetAccess = private, GetAccess = public)
+        % Meta
+        tag = REGION_TYPE.INVALID;
+        label = "";
+
+        % Spatial
+        center = NaN;
+        radius = NaN;
+        height = NaN;
+
+        % Plotting
+        surface;
+        n = 32;
+    end
+
+    methods
+        function obj = initialize(obj, center, radius, height, tag, label)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'cone')};
+                center (1, 3) double;
+                radius (1, 1) double;
+                height (1, 1) double;
+                tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+                label (1, 1) string = "";
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'cone')};
+            end
+
+            obj.center = center;
+            obj.radius = radius;
+            obj.height = height;
+            obj.tag = tag;
+            obj.label = label;
+        end
+        function [obj, f] = plot(obj, ind, f)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'cone')};
+                ind (1, :) double = NaN;
+                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'cone')};
+                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+            end
+
+            % Create axes if they don't already exist
+            f = firstPlotSetup(f);
+
+            % Plot cone
+            [X, Y, Z] = cylinder([obj.radius, 0], obj.n);
+            
+            % Scale to match height
+            Z = Z * obj.height;
+
+            % Move to center location
+            X = X + obj.center(1);
+            Y = Y + obj.center(2);
+            Z = Z + obj.center(3);
+
+            % Plot
+            if isnan(ind)
+                o = surf(f.CurrentAxes, X, Y, Z);
+            else
+                hold(f.Children(1).Children(ind(1)), "on");
+                o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
+                hold(f.Children(1).Children(ind(1)), "off");
+            end
+
+            % Copy to other requested tiles
+            if numel(ind) > 1
+                for ii = 2:size(ind, 2)
+                    o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
+                end
+            end
+
+            obj.surface = o;
+        end
+    end
+end

geometries/rectangularPrism.m

@@ -163,9 +163,10 @@ classdef rectangularPrism
 
             c = (tmax >= 0) && (tmin <= 1);
         end
-        function [obj, f] = plotWireframe(obj, f)
+        function [obj, f] = plotWireframe(obj, ind, f)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+                ind (1, :) double = NaN;
                 f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
             end
             arguments (Output)
@@ -181,17 +182,20 @@ classdef rectangularPrism
             Y = [obj.vertices(obj.edges(:,1),2), obj.vertices(obj.edges(:,2),2)]';
             Z = [obj.vertices(obj.edges(:,1),3), obj.vertices(obj.edges(:,2),3)]';
 
-            % Plot the boundaries of the geometry
+            % Plot the boundaries of the geometry into 3D view
-            hold(f.CurrentAxes, "on");
+            if isnan(ind)
-            o = plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+                o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
-            hold(f.CurrentAxes, "off");
+            else
+                hold(f.Children(1).Children(ind(1)), "on");
+                o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+                hold(f.Children(1).Children(ind(1)), "off");
+            end
 
-            % Check if this is a tiled layout figure
+            % Copy to other requested tiles
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            if numel(ind) > 1
-                % Add to other perspectives
+                for ii = 2:size(ind, 2)
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
+                    o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
+                end
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
             end
 
             obj.lines = o;

guidanceModels/gradientAscent.m

@@ -0,0 +1,26 @@
+function nextPos = gradientAscent(sensedValues, sensedPositions, pos, rate)
+    arguments (Input)
+        sensedValues (:, 1) double;
+        sensedPositions (:, 3) double;
+        pos (1, 3) double;
+        rate (1, 1) double = 0.1;
+    end
+    arguments (Output)
+        nextPos(1, 3) double;
+    end
+
+    % As a default, maintain current position
+    if size(sensedValues, 1) == 0 && size(sensedPositions, 1) == 0
+        nextPos = pos;
+        return;
+    end
+    
+    % Select next position by maximum sensed value
+    nextPos = sensedPositions(sensedValues == max(sensedValues), :);
+    nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
+
+    % rate-limit motion
+    v = nextPos - pos;
+    nextPos = pos + (v / norm(v, 2)) * rate;
+
+end

miSim.m

@@ -4,28 +4,38 @@ classdef miSim
     % Simulation parameters
     properties (SetAccess = private, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
+        partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
         maxIter = NaN; % maximum number of simulation iterations
         domain = rectangularPrism;
         objective = sensingObjective;
         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
+        partitioning = NaN;
     end
 
     properties (Access = private)
-        v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')));
+        % Plot objects
         connectionsPlot; % objects for lines connecting agents in spatial plots
         graphPlot; % objects for abstract network graph plot
+        partitionPlot; % objects for partition plot
+
+        % Indicies for various plot types in the main tiled layout figure
+        spatialPlotIndices = [6, 4, 3, 2];
+        objectivePlotIndices = [6, 4];
+        networkGraphIndex = 5;
+        partitionGraphIndex = 1;
     end
 
     methods (Access = public)
-        function [obj, f] = initialize(obj, domain, objective, agents, timestep, 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};
                 objective (1, 1) {mustBeA(objective, 'sensingObjective')};
-                agents (:, 1) cell {mustBeAgents};
+                agents (:, 1) cell;
                 timestep (:, 1) double = 0.05;
+                partitoningFreq (:, 1) double = 0.25
                 maxIter (:, 1) double = 1000;
                 obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
             end
@@ -40,6 +50,7 @@ classdef miSim
 
             % Define domain
             obj.domain = domain;
+            obj.partitioningFreq = partitoningFreq;
 
             % Add geometries representing obstacles within the domain
             obj.obstacles = obstacles;
@@ -53,34 +64,35 @@ classdef miSim
             % Compute adjacency matrix
             obj = obj.updateAdjacency();
 
+            % Create initial partitioning
+            obj = obj.partition();
+
             % Set up initial plot
             % Set up axes arrangement
             % Plot domain
-            [obj.domain, f] = obj.domain.plotWireframe();
+            [obj.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
-
-            % Set plotting limits to focus on the domain
-            xlim([obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
-            ylim([obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
-            zlim([obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
 
             % Plot obstacles
             for ii = 1:size(obj.obstacles, 1)
-                [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(f);
+                [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f);
             end
 
             % Plot objective gradient
-            f = obj.objective.plot(f);
+            f = obj.objective.plot(obj.objectivePlotIndices, f);
 
             % Plot agents and their collision geometries
             for ii = 1:size(obj.agents, 1)
-                [obj.agents{ii}, f] = obj.agents{ii}.plot(f);
+                [obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f);
             end
 
             % Plot communication links
-            [obj, f] = obj.plotConnections(f);
+            [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
 
             % Plot abstract network graph
-            [obj, f] = obj.plotGraph(f);
+            [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
+
+            % Plot domain partitioning
+            [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
         end
         function [obj, f] = run(obj, f)
             arguments (Input)
@@ -97,40 +109,73 @@ classdef miSim
 
             % 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
-            obj.v.FrameRate = 1/obj.timestep;
+            v = setupVideoWriter(obj.timestep);
-            obj.v.Quality = 90;
+            v.open();
-            obj.v.open();
 
             for ii = 1:size(times, 1)
                 % Display current sim time
                 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(t, partitioningTimes)
+                    updatePartitions = true;
+                    obj = obj.partition();
+                end
+
                 % Iterate over agents to simulate their motion
                 for jj = 1:size(obj.agents, 1)
-                    obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain);
+                    obj.agents{jj} = obj.agents{jj}.run(obj.objective, obj.domain, obj.partitioning);
                 end
     
                 % Update adjacency matrix
                 obj = obj.updateAdjacency;
     
                 % Update plots
-                [obj, f] = obj.updatePlots(f);
+                [obj, f] = obj.updatePlots(f, updatePartitions);
 
                 % Write frame in to video
                 I = getframe(f);
-                obj.v.writeVideo(I);
+                v.writeVideo(I);
             end
 
             % Close video file
-            obj.v.close();
+            v.close();
         end
-        function [obj, f] = updatePlots(obj, f)
+        function obj = partition(obj)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+            end
+
+            % 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 = cat(3, agentPerformances{:});
+            
+            % Get highest performance value at each point
+            [~, idx] = max(agentPerformances, [], 3);
+
+            % Collect agent indices in the same way
+            agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
+            agentInds = cat(3, agentInds{:});
+
+            % Get highest performing agent's index
+            [m,n,~] = size(agentInds);
+            [i,j] = ndgrid(1:m, 1:n);
+            obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
+        end
+        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')};
@@ -148,11 +193,24 @@ classdef miSim
 
             % Update agent connections plot
             delete(obj.connectionsPlot);
-            [obj, f] = obj.plotConnections(f);
+            [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
 
             % Update network graph plot
             delete(obj.graphPlot);
-            [obj, f] = obj.plotGraph(f);
+            [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
+
+            % Update partitioning plot
+            if updatePartitions
+                delete(obj.partitionPlot);
+                [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
+            end
+
+            % reset plot limits to fit domain
+            for ii = 1:size(obj.spatialPlotIndices, 2)
+                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;
         end
@@ -178,15 +236,20 @@ classdef miSim
                                 A(ii, jj) = true;
                             end
                         end
+                        % need extra handling for cases with no obstacles
+                        if isempty(obj.obstacles)
+                            A(ii, jj) = true;
+                        end
                     end
                 end
             end
 
             obj.adjacency = A | A';
         end
-        function [obj, f] = plotConnections(obj, f)
+        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)
@@ -209,23 +272,57 @@ classdef miSim
             X = X'; Y = Y'; Z = Z';
 
             % Plot the connections
-            hold(f.CurrentAxes, "on");
+            if isnan(ind)
-            o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+                hold(f.CurrentAxes, "on");
-            hold(f.CurrentAxes, "off");
+                o = plot3(f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+                hold(f.CurrentAxes, "off");
+            else
+                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
 
-            % Check if this is a tiled layout figure
+            % Copy to other plots
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            if size(ind, 2) > 1
-                % Add to other plots
+                for ii = 2:size(ind, 2)
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
+                    o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
+                end 
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
             end
 
             obj.connectionsPlot = o;
         end
-        function [obj, f] = plotGraph(obj, f)
+        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(ind)
+                hold(f.CurrentAxes, 'on');
+                o = imagesc(f.CurrentAxes, obj.partitioning);
+                hold(f.CurrentAxes, 'off');
+            else
+                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), f.Children(1).Children(ind(ii)))];
+                    end
+                end
+            end
+            obj.partitionPlot = o;
+
+        end
+        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)
@@ -237,7 +334,21 @@ classdef miSim
             G = graph(obj.adjacency, 'omitselfloops');
 
             % Plot graph object
-            obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+            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(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), f.Children(1).Children(ind(ii)))];
+                    end
+                end
+            end
+            obj.graphPlot = o;
         end
     end
 

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/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="9c9ce3cb-5989-41e8-a20d-358a95c08b20" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1d8d2b42-2863-4985-9cf2-980917971eba" type="Reference"/>

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUd.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/yxu_W2T0waL9FartpZB5JfpFMUUp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGId.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/pWmr-S3Aa4bo9IjolP0M5UKtHGIp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/G4ZDRUCYKeWyePBmClgd0b2gv90d.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/G4ZDRUCYKeWyePBmClgd0b2gv90p.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8d.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/uhUDZwLuHiPGihzjzILZj0glbh8p.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Md.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4d.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/H1DNOJ2xfieNyGozuRxm3sAKlR4p.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4d.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/gMC6J87GUhmyVw-U2Udmy4HIhA4p.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUd.xml

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

resources/project/uhUDZwLuHiPGihzjzILZj0glbh8/oYuyHxrJmpZT32QCx5mVYR2TRLUp.xml

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

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8d.xml

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

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml

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

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8d.xml

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

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/dS2dFNGSOpJCcekFKRAWAvowsw8p.xml

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

sensingFunctions/basicGradientAscent.m

@@ -1,44 +0,0 @@
-function nextPos = basicGradientAscent(objectiveFunction, domain, pos, r)
-    arguments (Input)
-        objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
-        domain (1, 1) {mustBeGeometry};
-        pos (1, 3) double;
-        r (1, 1) double;
-    end
-    arguments (Output)
-        nextPos(1, 3) double;
-    end
-
-    % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
-    currentPos = pos(1:2);
-    neighborPos = [currentPos(1) + r, currentPos(2); ... % (+x)
-                   currentPos(1), currentPos(2) + r; ... % (+y)
-                   currentPos(1) - r, currentPos(2); ... % (-x)
-                   currentPos(1), currentPos(2) - r; ... % (-y)
-                  ];
-
-    % Check for neighbor positions that fall outside of the domain
-    outOfBounds = false(size(neighborPos, 1), 1);
-    for ii = 1:size(neighborPos, 1)
-        if ~domain.contains([neighborPos(ii, :), 0])
-            outOfBounds(ii) = true;
-        end
-    end
-
-    % Replace out of bounds positions with inoffensive in-bounds positions
-    neighborPos(outOfBounds, 1:3) = repmat(pos, sum(outOfBounds), 1);
-
-    % Sense values at selected positions
-    neighborValues = [objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
-                      objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
-                      objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
-                      objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
-                     ];
-
-    % Prevent out of bounds locations from ever possibly being selected
-    neighborValues(outOfBounds) = 0;
-    
-    % Select next position by maximum sensed value
-    nextPos = neighborPos(neighborValues == max(neighborValues), :);
-    nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
-end

sensingModels/fixedCardinalSensor.m

@@ -0,0 +1,76 @@
+classdef fixedCardinalSensor
+    % Senses in the +/-x, +/- y directions at some specified fixed length
+    properties
+        r = 0.1; % fixed sensing length
+    end
+
+    methods (Access = public)
+        function obj = initialize(obj, r)
+            arguments(Input)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+                r (1, 1) double;
+            end
+            arguments(Output)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+            end
+            obj.r = r;
+        end
+        function [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+                agent (1, 1) {mustBeA(agent, 'agent')};
+                sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
+                domain (1, 1) {mustBeGeometry};
+                partitioning (:, :) double = NaN;
+            end
+            arguments (Output)
+                neighborValues (4, 1) double;
+                neighborPos (4, 3) double;
+            end
+        
+            % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
+            currentPos = agent.pos(1:2);
+            neighborPos = [currentPos(1) + obj.r, currentPos(2); ... % (+x)
+                           currentPos(1), currentPos(2) + obj.r; ... % (+y)
+                           currentPos(1) - obj.r, currentPos(2); ... % (-x)
+                           currentPos(1), currentPos(2) - obj.r; ... % (-y)
+                          ];
+        
+            % Check for neighbor positions that fall outside of the domain
+            outOfBounds = false(size(neighborPos, 1), 1);
+            for ii = 1:size(neighborPos, 1)
+                if ~domain.contains([neighborPos(ii, :), 0])
+                    outOfBounds(ii) = true;
+                end
+            end
+        
+            % Replace out of bounds positions with inoffensive in-bounds positions
+            neighborPos(outOfBounds, 1:3) = repmat(agent.pos, sum(outOfBounds), 1);
+        
+            % Sense values at selected positions
+            neighborValues = [sensingObjective.objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
+                              sensingObjective.objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
+                              sensingObjective.objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
+                              sensingObjective.objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
+                             ];
+        
+            % Prevent out of bounds locations from ever possibly being selected
+            neighborValues(outOfBounds) = 0;
+        end
+        function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
+                agentPos (1, 3) double;
+                agentPan (1, 1) double;
+                agentTilt (1, 1) double;
+                targetPos (:, 3) double;
+            end
+            arguments (Output)
+                value (:, 1) double;
+            end
+
+            value = 0.5 * ones(size(targetPos, 1), 1);
+            
+        end
+    end
+end

sensingModels/sigmoidSensor.m

@@ -0,0 +1,83 @@
+classdef sigmoidSensor
+    properties (SetAccess = private, GetAccess = public)
+        % Sensor parameters
+        alphaDist = NaN;
+        betaDist = NaN;
+        alphaPan = NaN;
+        betaPan = NaN;
+        alphaTilt = NaN;
+        betaTilt = NaN;
+
+        r = NaN;
+    end
+
+    methods (Access = public)
+        function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+                alphaDist (1, 1) double;
+                betaDist (1, 1) double;
+                alphaPan (1, 1) double;
+                betaPan (1, 1) double;
+                alphaTilt (1, 1) double;
+                betaTilt (1, 1) double;
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+            end
+
+            obj.alphaDist = alphaDist;
+            obj.betaDist = betaDist;
+            obj.alphaPan = alphaPan;
+            obj.betaPan = betaPan;
+            obj.alphaTilt = alphaTilt;
+            obj.betaTilt = betaTilt;
+
+            obj.r = obj.alphaDist;
+        end
+        function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+                agent (1, 1) {mustBeA(agent, 'agent')};
+                sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
+                domain (1, 1) {mustBeGeometry};
+                partitioning (:, :) double;
+            end
+            arguments (Output)
+                values (:, 1) double;
+                positions (:, 3) double;
+            end
+
+            % Find positions for this agent's assigned partition in the domain
+            idx = partitioning == agent.index;
+            positions = [sensingObjective.X(idx), sensingObjective.Y(idx), zeros(size(sensingObjective.X(idx)))];
+
+            % Evaluate objective function at every point in this agent's
+            % assigned partiton
+            values = sensingObjective.values(idx);
+        end
+        function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+                agentPos (1, 3) double;
+                agentPan (1, 1) double;
+                agentTilt (1, 1) double;
+                targetPos (:, 3) double;
+            end
+            arguments (Output)
+                value (:, 1) double;
+            end
+
+            d = vecnorm(agentPos - targetPos, 2, 2);
+            panAngle = atan2(targetPos(:, 2) - agentPos(2), targetPos(:, 1) - agentPos(1)) - agentPan;
+            tiltAngle = atan2(targetPos(:, 3) - agentPos(3), d) - agentTilt;
+
+            % Membership functions
+            mu_d = 1 - (1 ./ (1 + exp(-obj.betaDist .* (d - obj.alphaDist)))); % distance
+            mu_p = (1 ./ (1 + exp(-obj.betaPan .* (panAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (panAngle - obj.alphaPan)))); % pan
+            mu_t = (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle - obj.alphaPan)))); % tilt
+
+            value = mu_d .* mu_p .* mu_t;
+        end
+    end
+end

sensingObjective.m

@@ -46,9 +46,10 @@ classdef sensingObjective
             idx = obj.values == max(obj.values, [], "all");
             obj.groundPos = [obj.X(idx), obj.Y(idx)];
         end
-        function f = plot(obj, f)
+        function f = plot(obj, ind, f)
             arguments (Input)
                 obj (1,1) {mustBeA(obj, 'sensingObjective')};
+                ind (1, :) double = NaN;
                 f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
             end
             arguments (Output)
@@ -58,24 +59,27 @@ classdef sensingObjective
             % Create axes if they don't already exist
             f = firstPlotSetup(f);
 
-            % Check if this is a tiled layout figure
+            % Plot gradient on the "floor" of the domain
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            if isnan(ind)
-                % Plot gradient on the "floor" of the domain
-                hold(f.Children(1).Children(3), "on");
-                o = surf(f.Children(1).Children(3), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
-                o.HitTest = 'off';
-                o.PickableParts = 'none';
-                hold(f.Children(1).Children(3), "off");
-
-                % Add to other perspectives
-                copyobj(o, f.Children(1).Children(5));
-            else
-                % Plot gradient on the "floor" of the domain
                 hold(f.CurrentAxes, "on");
-                o = surf(obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+                o = surf(f.CurrentAxes, obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
                 o.HitTest = 'off';
                 o.PickableParts = 'none';
                 hold(f.CurrentAxes, "off");
+
+            else
+                hold(f.Children(1).Children(ind(1)), "on");
+                o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+                o.HitTest = 'off';
+                o.PickableParts = 'none';
+                hold(f.Children(1).Children(ind(1)), "off");
+            end
+
+            % Add to other perspectives
+            if size(ind, 2) > 1
+                for ii = 2:size(ind, 2)
+                    copyobj(o, f.Children(1).Children(ind(ii)));
+                end
             end
         end
     end

setupVideoWriter.m

@@ -0,0 +1,17 @@
+function v = setupVideoWriter(timestep)
+    arguments (Input)
+        timestep (1, 1) double;
+    end
+    arguments (Output)
+        v (1, 1) {mustBeA(v, 'VideoWriter')};
+    end
+
+    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('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')), 'Motion JPEG AVI');
+    end
+    
+    v.FrameRate = 1/timestep;
+    v.Quality = 90;
+end

test_miSim.m

@@ -4,8 +4,9 @@ classdef test_miSim < matlab.unittest.TestCase
 
         % Domain
         domain = rectangularPrism; % domain geometry
-        maxIter = 1000;
+        maxIter = 250;
         timestep = 0.05
+        partitoningFreq = 5;
 
         % Obstacles
         minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
@@ -185,10 +186,17 @@ classdef test_miSim < matlab.unittest.TestCase
                         continue;
                     end
 
-                    % Initialize candidate agent
+                    % Initialize candidate agent collision geometry
                     candidateGeometry = rectangularPrism;
-                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), @(r) 0.5, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
+                    candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
-                    
+
+                    % Initialize candidate agent sensor model
+                    sensor = fixedCardinalSensor;
+                    sensor = sensor.initialize(tc.sensingLength);
+
+                    % Initialize candidate agent
+                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii)); 
+
                     % Make sure candidate agent doesn't collide with
                     % domain
                     violation = false;
@@ -235,7 +243,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -346,9 +354,16 @@ classdef test_miSim < matlab.unittest.TestCase
                         continue;
                     end
 
-                    % Initialize candidate agent
+                    % Initialize candidate agent collision geometry
                     candidateGeometry = rectangularPrism;
-                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), @basicGradientAscent, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
+                    candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
+                    
+                    % Initialize candidate agent sensor model
+                    sensor = sigmoidSensor;
+                    sensor = sensor.initialize(1, 1, 1, 1, 1, 1);
+
+                    % Initialize candidate agent
+                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii));
                     
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -396,10 +411,38 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
 
             % Run simulation loop
             [tc.testClass, f] = tc.testClass.run(f);
         end
+        function test_basic_partitioning(tc)
+            % place agents a fixed distance +/- X from the domain's center
+            d = 1;
+
+            % make basic domain
+            tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2), eye(2)), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
+        
+            % Initialize agent collision geometry
+            geometry1 = rectangularPrism;
+            geometry2 = geometry1;
+            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;
+            sensor = sensor.initialize(1, 1, 1, 1, 1, 1);
+    
+            % Initialize agents
+            tc.agents = {agent; agent};
+            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, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
+        end
     end
 end

validators/arguments/mustBeAgents.m

@@ -1,10 +0,0 @@
-function mustBeAgents(agents)
-    validGeometries = ["rectangularPrismConstraint";];
-    if isa(agents, 'cell')
-        for ii = 1:size(agents, 1)
-            assert(isa(agents{ii}, "agent"), "Agent in index %d is not a valid agent class", ii);
-        end
-    else
-        assert(isa(agents, validGeometries), "Agent is not a valid agent class");
-    end
-end

validators/arguments/mustBeDcm.m

@@ -1,12 +0,0 @@
-function mustBeDcm(dcm)
-    % Assert 2D
-    assert(numel(size(dcm)) == 2, "DCM is not 2D");
-    % Assert square
-    assert(size(unique(size(dcm)), 1) == 1, "DCM is not a square matrix");
-
-    epsilon = 1e-9;
-    % Assert inverse equivalent to transpose
-    assert(all(abs(inv(dcm) - dcm') < epsilon, "all"), "DCM inverse is not equivalent to transpose");
-    % Assert determinant is 1
-    assert(det(dcm) > 1 - epsilon && det(dcm) < 1 + epsilon, "DCM has determinant not equal to 1");
-end

validators/arguments/mustBeGeometry.m

@@ -2,9 +2,9 @@ function mustBeGeometry(geometry)
     validGeometries = ["rectangularPrism";];
     if isa(geometry, 'cell')
         for ii = 1:size(geometry, 1)
-            assert(isa(geometry{ii}, validGeometries), "Geometry in index %d is not a valid geometry class", ii);
+            assert(any(arrayfun(@(x) isa(geometry{ii}, x), validGeometries)), "Geometry in index %d is not a valid geometry class", ii);
         end
     else
-        assert(isa(geometry, validGeometries), "Geometry is not a valid geometry class");
+        assert(any(arrayfun(@(x) isa(geometry, x), validGeometries)), "Geometry is not a valid geometry class");
     end
 end

validators/arguments/mustBeSensor.m

@@ -0,0 +1,10 @@
+function mustBeSensor(sensorModel)
+    validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
+    if isa(sensorModel, 'cell')
+        for ii = 1:size(sensorModel, 1)
+            assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii);
+        end
+    else
+        assert(any(arrayfun(@(x) isa(sensorModel, x), validSensorModels)), "Sensor is not a valid sensor class");
+    end
+end