partitioning introduced to main loop

agent.m

@@ -58,18 +58,19 @@ classdef agent
             obj.index = index;
             obj.label = 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
-            [sensedValues, sensedPositions] = obj.sensorModel.sense(objectiveFunction, domain, obj.pos);
+            [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
 
             % Determine next planned position
             nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);

guidanceModels/gradientAscent.m

@@ -1,14 +1,26 @@
-function nextPos = gradientAscent(sensedValues, sensedPositions, pos)
+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,6 +4,7 @@ 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;
@@ -27,13 +28,14 @@ classdef miSim
     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;
                 timestep (:, 1) double = 0.05;
+                partitoningFreq (:, 1) double = 0.25
                 maxIter (:, 1) double = 1000;
                 obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
             end
@@ -48,6 +50,7 @@ classdef miSim
 
             % Define domain
             obj.domain = domain;
+            obj.partitioningFreq = partitoningFreq;
 
             % Add geometries representing obstacles within the domain
             obj.obstacles = obstacles;
@@ -106,6 +109,7 @@ 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
             v = setupVideoWriter(obj.timestep);
@@ -116,16 +120,23 @@ classdef miSim
                 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);
@@ -160,10 +171,11 @@ classdef miSim
             [i,j] = ndgrid(1:m, 1:n);
             obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
         end
-        function [obj, f] = updatePlots(obj, f)
+        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')};
@@ -181,11 +193,17 @@ 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
 
             drawnow;
         end

sensingModels/fixedCardinalSensor.m

@@ -15,12 +15,13 @@ classdef fixedCardinalSensor
             end
             obj.r = r;
         end
-        function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos)
+        function [neighborValues, neighborPos] = sense(obj, agent, sensingObjective, domain, partitioning)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')};
-                objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+                agent (1, 1) {mustBeA(agent, 'agent')};
+                sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
                 domain (1, 1) {mustBeGeometry};
-                pos (1, 3) double;
+                partitioning (:, :) double = NaN;
             end
             arguments (Output)
                 neighborValues (4, 1) double;
@@ -28,7 +29,7 @@ classdef fixedCardinalSensor
             end
         
             % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
-            currentPos = pos(1:2);
+            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)
@@ -44,13 +45,13 @@ classdef fixedCardinalSensor
             end
         
             % Replace out of bounds positions with inoffensive in-bounds positions
-            neighborPos(outOfBounds, 1:3) = repmat(pos, sum(outOfBounds), 1);
+            neighborPos(outOfBounds, 1:3) = repmat(agent.pos, sum(outOfBounds), 1);
         
             % Sense values at selected positions
-            neighborValues = [objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
+            neighborValues = [sensingObjective.objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
-                              objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
+                              sensingObjective.objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
-                              objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
+                              sensingObjective.objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
-                              objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
+                              sensingObjective.objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
                              ];
         
             % Prevent out of bounds locations from ever possibly being selected

sensingModels/sigmoidSensor.m

@@ -1,12 +1,12 @@
 classdef sigmoidSensor
     properties (SetAccess = private, GetAccess = public)
         % Sensor parameters
-        alphaDist;
+        alphaDist = NaN;
-        betaDist;
+        betaDist = NaN;
-        alphaPan;
+        alphaPan = NaN;
-        betaPan;
+        betaPan = NaN;
-        alphaTilt;
+        alphaTilt = NaN;
-        betaTilt;
+        betaTilt = NaN;
     end
 
     methods (Access = public)
@@ -31,19 +31,26 @@ classdef sigmoidSensor
             obj.alphaTilt = alphaTilt;
             obj.betaTilt = betaTilt;
         end
-        function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos)
+        function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-                objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+                agent (1, 1) {mustBeA(agent, 'agent')};
+                sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
                 domain (1, 1) {mustBeGeometry};
-                pos (1, 3) double;
+                partitioning (:, :) double;
             end
             arguments (Output)
-                neighborValues (4, 1) double;
+                values (:, 1) double;
-                neighborPos (4, 3) 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)

test_miSim.m

@@ -6,6 +6,7 @@ classdef test_miSim < matlab.unittest.TestCase
         domain = rectangularPrism; % domain geometry
         maxIter = 250;
         timestep = 0.05
+        partitoningFreq = 5;
 
         % Obstacles
         minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
@@ -242,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
@@ -410,7 +411,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);
 
             % Run simulation loop
             [tc.testClass, f] = tc.testClass.run(f);
@@ -418,7 +419,6 @@ classdef test_miSim < matlab.unittest.TestCase
         function test_basic_partitioning(tc)
             % place agents a fixed distance +/- X from the domain's center
             d = 1;
-            c = 0.1;
 
             % make basic domain
             tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
@@ -442,7 +442,7 @@ classdef test_miSim < matlab.unittest.TestCase
             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.maxIter);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
         end
     end
 end