now doing partitioning on every timestep, looks super smooth

@miSim/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, domain, objective, agents, minAlt, timestep, partitoningFreq, maxIter, obstacles, makePlots, makeVideo)
+function obj = initialize(obj, domain, objective, agents, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
         domain (1, 1) {mustBeGeometry};
@@ -6,7 +6,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
         agents (:, 1) cell;
         minAlt (1, 1) double = 1;
         timestep (:, 1) double = 0.05;
-        partitoningFreq (:, 1) double = 0.25
         maxIter (:, 1) double = 1000;
         obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
         makePlots(1, 1) logical = true;
@@ -33,7 +32,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
 
     % Define domain
     obj.domain = domain;
-    obj.partitioningFreq = partitoningFreq;
 
     % Add geometries representing obstacles within the domain
     obj.obstacles = obstacles;
@@ -73,7 +71,6 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
 
     % 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.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
@@ -82,7 +79,7 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
     obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1) - 1);
 
     % Create initial partitioning
-    obj = obj.partition();
+    obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
 
     % Initialize variable that will store agent positions for trail plots
     obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);

@miSim/miSim.m

@@ -67,7 +67,7 @@ classdef miSim
         [obj] = plotGraph(obj);
         [obj] = plotTrails(obj);
         [obj] = plotH(obj);
-        [obj] = updatePlots(obj, updatePartitions);
+        [obj] = updatePlots(obj);
         validate(obj);
     end
     methods (Access = private)

@miSim/partition.m

@@ -1,33 +0,0 @@
-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{end + 1} = obj.domain.objective.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
-    indices = 1:size(obj.agents, 1);
-    agentInds = squeeze(tensorprod(indices, ones(size(obj.objective.X))));
-    if size(agentInds, 1) ~= size(obj.agents, 1)
-        agentInds = reshape(agentInds, [size(obj.agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
-    end
-    agentInds = num2cell(agentInds, 2:3);
-    agentInds = cellfun(@(x) squeeze(x), agentInds, '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
-    [m, n, ~] = size(agentInds);
-    [jj, kk] = ndgrid(1:m, 1:n);
-    obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
-end

@miSim/run.m

@@ -18,19 +18,18 @@ function [obj] = run(obj)
         obj.timestepIndex = ii;
         fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
+        % Before moving
         % Validate current simulation configuration
         obj.validate();
 
-        % Check if it's time for new partitions
+        % Update partitioning before moving (this one is strictly for
-        updatePartitions = false;
+        % plotting purposes, the real partitioning is done by the agents)
-        if ismember(obj.t, obj.partitioningTimes)
+        obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
-            updatePartitions = true;
-            obj = obj.partition();
-        end
 
         % Determine desired communications links
         obj = obj.lesserNeighbor();
 
+        % Moving
         % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)
             obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
@@ -40,8 +39,7 @@ function [obj] = run(obj)
         % CBF constraints solved by QP
         obj = constrainMotion(obj);
 
-        % Finished simulation for this timestep, do accounting
+        % After moving
-
         % Update agent position history array
         obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
 
@@ -52,7 +50,7 @@ function [obj] = run(obj)
         obj = obj.updateAdjacency();
 
         % Update plots
-        obj = obj.updatePlots(updatePartitions);
+        obj = obj.updatePlots();
 
         % Write frame in to video
         if obj.makeVideo

@miSim/updatePlots.m

@@ -1,7 +1,6 @@
-function [obj] = updatePlots(obj, updatePartitions)
+function [obj] = updatePlots(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        updatePartitions (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
@@ -30,10 +29,8 @@ function [obj] = updatePlots(obj, updatePartitions)
     obj = obj.plotGraph();
 
     % Update partitioning plot
-    if updatePartitions
+    delete(obj.partitionPlot);
-        delete(obj.partitionPlot);
+    obj = obj.plotPartitions();
-        obj = obj.plotPartitions();
-    end
 
     % reset plot limits to fit domain
     for ii = 1:size(obj.spatialPlotIndices, 2)

test/test_miSim.m

@@ -210,7 +210,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -344,7 +344,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo);
 
             % Run simulation loop
             tc.testClass = tc.testClass.run();
@@ -385,10 +385,10 @@ classdef test_miSim < matlab.unittest.TestCase
             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);
             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, 3*d);
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + dh - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, 3*d, tc.maxIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), false, false);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false);
         
             tc.verifyEqual(tc.testClass.partitioning(500, 500:502), [2, 3, 1]); % all three near center
             tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all')); % more non-assignments than partition 1 assignments
@@ -423,7 +423,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2), 3], zeros(1,3), 0, 0, geometry1, sensor, 3, tc.maxIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1), false, false);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, cell(0, 1), false, false);
             close(tc.testClass.fPerf);
 
             tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
@@ -435,7 +435,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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(:)], [5, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [7, 6]), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
             geometry1 = rectangularPrism;
@@ -452,11 +452,12 @@ classdef test_miSim < matlab.unittest.TestCase
             % f = sensor.plotParameters();
 
             % Initialize agents
+            nIter = 100;
             tc.agents = {agent};
-            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, tc.maxIter);
+            tc.agents{1} = tc.agents{1}.initialize([tc.domain.center(1:2)-tc.domain.dimensions(1)/4, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, nIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, cell(0, 1));
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, nIter, cell(0, 1));
             
             % Run the simulation
             tc.testClass = tc.testClass.run();
@@ -496,7 +497,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry2, sensor, 5, nIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, nIter, cell(0, 1), tc.makeVideo, tc.makePlots);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, nIter, cell(0, 1), tc.makeVideo, tc.makePlots);
             
             % Run the simulation
             tc.testClass.run();
@@ -545,7 +546,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius  *1.1 + yOffset, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius, tc.maxIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.maxIter, tc.obstacles, tc.makeVideo);
             
             % Run the simulation
             tc.testClass.run();
@@ -586,7 +587,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{2} = tc.agents{2}.initialize(dom.center - d, zeros(1,3), 0, 0, geometry2, sensor, commsRadius, nIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, true, false);
+            tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, nIter, tc.obstacles, true, false);
             
             % Run the simulation
             tc.testClass = tc.testClass.run();
@@ -628,7 +629,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, false, false);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false);
 
             % No communications link should be established
             tc.assertEqual(tc.testClass.adjacency, logical(true(2)));
@@ -672,7 +673,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius, nIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, false, false);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
@@ -725,7 +726,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], zeros(1,3), 0, 0, geometry7, sensor, commsRadius, nIter);
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, nIter, tc.obstacles, false, false);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, nIter, tc.obstacles, false, false);
 
             % Constraint adjacency matrix defined by LNA should be as follows
             tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...