added early exit from main loop for semistable final states

@agent/agent.m

@@ -1,12 +1,16 @@
 classdef agent
-    properties (SetAccess = public, GetAccess = public)
+    properties (SetAccess = private, GetAccess = public)
         % Identifiers
+        index = NaN;
         label = "";
 
         % Sensor
         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
@@ -16,29 +20,20 @@ classdef agent
 
         % Collision
         collisionGeometry;
-        barrierFunction;
-        dBarrierFunction;
 
         % FOV cone
         fovGeometry;
 
         % Communication
         comRange = NaN;
-        commsGeometry = spherical;
-        lesserNeighbors = [];
-
-        performance = 0;
 
         % Plotting
         scatterPoints;
-        debug = false;
-        debugFig;
-        plotCommsGeometry = true;
     end
 
     methods (Access = public)
         [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, domain, partitioning, t, index);
+        [obj] = run(obj, sensingObjective, domain, partitioning);
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, comRange, label, debug, plotCommsGeometry)
+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;
@@ -6,11 +6,11 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
         pan (1, 1) double;
         tilt (1, 1) double;
         collisionGeometry (1, 1) {mustBeGeometry};
-        sensorModel (1, 1) {mustBeSensor};
-        comRange (1, 1) double;
+        sensorModel (1, 1) {mustBeSensor}
+        guidanceModel (1, 1) {mustBeA(guidanceModel, 'function_handle')};
+        comRange (1, 1) double = NaN;
+        index (1, 1) double = NaN;
         label (1, 1) string = "";
-        debug (1, 1) logical = false;
-        plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
@@ -22,57 +22,10 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
     obj.tilt = tilt;
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
+    obj.guidanceModel = guidanceModel;
+    obj.comRange = comRange;
+    obj.index = index;
     obj.label = label;
-    obj.debug = debug;
-    obj.plotCommsGeometry = plotCommsGeometry;
-
-    % Add spherical geometry based on com range
-    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
-
-    if obj.debug
-        obj.debugFig = figure;
-        tiledlayout(obj.debugFig, "TileSpacing", "tight", "Padding", "compact");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Sensor Performance");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Sensor Performance x Gradient Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Sensor Performance x Objective");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Agent Performance (C)");
-        nexttile;
-        axes(obj.debugFig.Children(1).Children(1));
-        axis(obj.debugFig.Children(1).Children(1), "image");
-        xlabel(obj.debugFig.Children(1).Children(1), "X"); ylabel(obj.debugFig.Children(1).Children(1), "Y");
-        title(obj.debugFig.Children(1).Children(1), "Gradient Agent Performance (del C)");
-    end
 
     % Initialize FOV cone
     obj.fovGeometry = cone;

@agent/plot.m

@@ -30,11 +30,6 @@ function [obj, f] = plot(obj, ind, f)
     % Plot collision geometry
     [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f);
 
-    % Plot communications geometry
-    if obj.plotCommsGeometry
-        [obj.commsGeometry, f] = obj.commsGeometry.plotWireframe(ind, f);
-    end
-
     % Plot FOV geometry
     [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
 end

@agent/run.m

@@ -1,155 +1,28 @@
-function obj = run(obj, domain, partitioning, t, index)
+function obj = run(obj, sensingObjective, domain, partitioning)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
+        sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
-        t (1, 1) double;
-        index (1, 1) double;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
     end
 
-    % Collect objective function values across partition
-    partitionMask = partitioning == index;
-    objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
+    % Do sensing
+    [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
 
-    % Compute sensor performance across partition
-    maskedX = domain.objective.X(partitionMask);
-    maskedY = domain.objective.Y(partitionMask);
-    zFactor = 1;
-    sensorValues = obj.sensorModel.sensorPerformance(obj.pos, obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
-    sensorValuesLower = obj.sensorModel.sensorPerformance(obj.pos - [0, 0, zFactor * domain.objective.discretizationStep], obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n - [0, 0, z]) on W_n
-    sensorValuesHigher = obj.sensorModel.sensorPerformance(obj.pos + [0, 0, zFactor * domain.objective.discretizationStep], obj.pan, obj.tilt, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n - [0, 0, z]) on W_n
-
-    % Put the values back into the form of the partition to enable basic operations on this data
-    F = NaN(size(partitionMask));
-    F(partitionMask) = objectiveValues;
-    S = NaN(size(partitionMask));
-    Slower = S;
-    Shigher = S;
-    S(partitionMask) = sensorValues;
-    Slower(partitionMask) = sensorValuesLower;
-    Shigher(partitionMask) = sensorValuesHigher;
-
-    % Find agent's performance
-    C = S .* F;
-    obj.performance = [obj.performance, sum(C(~isnan(C)))]; % at current Z only
-    C = cat(3, Shigher, S, Slower) .* F;
-
-    % Compute gradient on agent's performance
-    [gradCX, gradCY, gradCZ] = gradient(C, domain.objective.discretizationStep); % grad C
-    gradC = cat(4, gradCX, gradCY, gradCZ);
-    nGradC = vecnorm(gradC, 2, 4);
-
-    if obj.debug
-        % Compute additional component-level values for diagnosing issues
-        [gradSensorPerformanceX, gradSensorPerformanceY] = gradient(S, domain.objective.discretizationStep); % grad S_n
-        [gradObjectiveX, gradObjectiveY] = gradient(F, domain.objective.discretizationStep); % grad f
-        gradS = cat(3, gradSensorPerformanceX, gradSensorPerformanceY, zeros(size(gradSensorPerformanceX))); % grad S_n
-        gradF = cat(3, gradObjectiveX, gradObjectiveY, zeros(size(gradObjectiveX))); % grad f
-
-        ii = 8;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), F./max(F, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), S./max(S, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), S .* vecnorm(gradF, 2, 3)./max(vecnorm(gradF, 2, 3), [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), F .* vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all')./(max(F .* vecnorm(gradS, 2, 3)./max(vecnorm(gradS, 2, 3), [], 'all'))));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), C./max(C, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        ii = ii - 1;
-        hold(obj.debugFig.Children(1).Children(ii), "on");
-        cla(obj.debugFig.Children(1).Children(ii));
-        imagesc(obj.debugFig.Children(1).Children(ii), nGradC./max(nGradC, [], 'all'));
-        hold(obj.debugFig.Children(1).Children(ii), "off");
-        [x, y] = find(nGradC == max(nGradC, [], "all"));
-
-        % just pick one
-        r = randi([1, size(x, 1)]);
-        x = x(r); y = y(r);
-
-        % switch them
-        temp = x;
-        x = y;
-        y = temp;
-        
-        % find objective location in discrete domain
-        [~, xIdx] = find(domain.objective.groundPos(1) == domain.objective.X);
-        xIdx = unique(xIdx);
-        [yIdx, ~] = find(domain.objective.groundPos(2) == domain.objective.Y);
-        yIdx = unique(yIdx);
-        for ii = 8:-1:1
-            hold(obj.debugFig.Children(1).Children(ii), "on");
-            % plot GA selection
-            scatter(obj.debugFig.Children(1).Children(ii), x, y, 'go');
-            scatter(obj.debugFig.Children(1).Children(ii), x, y, 'g+');
-            % plot objective center
-            scatter(obj.debugFig.Children(1).Children(ii), xIdx, yIdx, 'ro');
-            scatter(obj.debugFig.Children(1).Children(ii), xIdx, yIdx, 'r+');
-            hold(obj.debugFig.Children(1).Children(ii), "off");
-        end
-    end
-
-    % return now if there is no data to work with, and do not move
-    if all(isnan(nGradC), 'all')
-        return;
-    end
-
-    % Use largest grad(C) value to find the direction of the next position
-    [xNextIdx, yNextIdx, zNextIdx] = ind2sub(size(nGradC), find(nGradC == max(nGradC, [], 'all')));
-    % switch them
-    temp = xNextIdx;
-    xNextIdx = yNextIdx;
-    yNextIdx = temp;
-
-    roundingScale = 10^-log10(domain.objective.discretizationStep);
-    zKey = zFactor * [1; 0; -1];
-    pNext = [floor(roundingScale .* mean(unique(domain.objective.X(:, xNextIdx))))./roundingScale, floor(roundingScale .* mean(unique(domain.objective.Y(yNextIdx, :))))./roundingScale, obj.pos(3) + zKey(zNextIdx)]; % have to do some unfortunate rounding here sometimes
-
-    % Determine next position
-    vDir = (pNext - obj.pos)./norm(pNext - obj.pos, 2);
-    rate = 0.1 - 0.0004 * t; % slow down as you get closer, coming to a stop by the end
-    nextPos = obj.pos + vDir * rate;
+    % Determine next planned position
+    nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
 
     % Move to next position
+    % (dynamics not modeled at this time)
     obj.lastPos = obj.pos;
     obj.pos = nextPos;
 
-    % Reinitialize collision geometry in the new position
+    % Calculate movement
     d = obj.pos - obj.collisionGeometry.center;
-    if isa(obj.collisionGeometry, 'rectangularPrism')
-        obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
-    elseif isa(obj.collisionGeometry, 'spherical')
-        obj.collisionGeometry = obj.collisionGeometry.initialize(obj.collisionGeometry.center + d, obj.collisionGeometry.radius, obj.collisionGeometry.tag, obj.collisionGeometry.label);
-    else
-        error("?");
-    end
+
+    % Reinitialize collision geometry in the new position
+    obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
 end

@agent/updatePlots.m

@@ -25,17 +25,6 @@ function updatePlots(obj)
         end
     end
 
-    % Communications geometry edges
-    if obj.plotCommsGeometry
-        for jj = 1:size(obj.commsGeometry.lines, 2)
-            for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
-                obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
-                obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
-                obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
-            end
-        end
-    end
-
     % Update FOV geometry surfaces
     for jj = 1:size(obj.fovGeometry.surface, 2)
         % Update each plot

@miSim/constrainMotion.m

@@ -1,153 +0,0 @@
-function [obj] = constrainMotion(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    if size(obj.agents, 1) < 2
-        nAAPairs = 0;
-    else
-        nAAPairs = nchoosek(size(obj.agents, 1), 2); % unique agent/agent pairs
-    end
-
-    agents = [obj.agents{:}];
-    v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
-
-    % Initialize QP based on number of agents and obstacles
-    nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1); % unique agent/obstacle pairs
-    nADPairs = size(obj.agents, 1) * 5; % agents x (4 walls + 1 ceiling)
-    nLNAPairs = sum(obj.constraintAdjacencyMatrix, 'all') - size(obj.agents, 1);
-    total = nAAPairs + nAOPairs + nADPairs + nLNAPairs;
-    kk = 1;
-    A = zeros(total, 3 * size(obj.agents, 1));
-    b = zeros(total, 1);
-
-    % Set up collision avoidance constraints
-    h = NaN(size(obj.agents, 1));
-    h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
-    for ii = 1:(size(obj.agents, 1) - 1)
-        for jj = (ii + 1):size(obj.agents, 1)
-            h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
-            h(jj, ii) = h(ii, jj);
-            
-            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (agents(ii).pos - agents(jj).pos);
-            A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-            b(kk) = obj.barrierGain * h(ii, jj)^3;
-            kk = kk + 1;
-        end
-    end
-
-    hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
-    % Set up obstacle avoidance constraints
-    for ii = 1:size(obj.agents, 1)
-        for jj = 1:size(obj.obstacles, 1)
-            % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
-
-            hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
-
-            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
-            b(kk) = obj.barrierGain * hObs(ii, jj)^3;
-           
-            kk = kk + 1;
-        end
-    end
-    
-    % Set up domain constraints (walls and ceiling only)
-    % Floor constraint is implicit with an obstacle corresponding to the
-    % minimum allowed altitude, but I included it anyways
-    for ii = 1:size(obj.agents, 1)
-        % X minimum
-        h_xMin = (agents(ii).pos(1) - obj.domain.minCorner(1)) - agents(ii).collisionGeometry.radius;
-        A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
-        b(kk) = obj.barrierGain * h_xMin^3;
-        kk = kk + 1;
-    
-        % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - agents(ii).pos(1)) - agents(ii).collisionGeometry.radius;
-        A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
-        b(kk) = obj.barrierGain * h_xMax^3;
-        kk = kk + 1;
-    
-        % Y minimum
-        h_yMin = (agents(ii).pos(2) - obj.domain.minCorner(2)) - agents(ii).collisionGeometry.radius;
-        A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
-        b(kk) = obj.barrierGain * h_yMin^3;
-        kk = kk + 1;
-    
-        % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
-        A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
-        b(kk) = obj.barrierGain * h_yMax^3;
-        kk = kk + 1;
-    
-        % Z minimum
-        h_zMin = (agents(ii).pos(3) - obj.domain.minCorner(3)) - agents(ii).collisionGeometry.radius;
-        A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
-        b(kk) = obj.barrierGain * h_zMin^3;
-        kk = kk + 1;
-    
-        % Z maximum
-        h_zMax = (obj.domain.maxCorner(2) - agents(ii).pos(2)) - agents(ii).collisionGeometry.radius;
-        A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
-        b(kk) = obj.barrierGain * h_zMax^3;
-        kk = kk + 1;
-    end
-
-    % Save off h function values (ignoring network constraints which may evolve in time)
-    obj.h(:, obj.timestepIndex) = [h(triu(true(size(obj.agents, 1)), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;]; 
-
-    % Add communication network constraints
-    hComms = NaN(size(obj.agents, 1));
-    hComms(logical(eye(size(obj.agents, 1)))) = 0;
-    for ii = 1:(size(obj.agents, 1) - 1)
-        for jj = (ii + 1):size(obj.agents, 1)
-            if obj.constraintAdjacencyMatrix(ii, jj)
-                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2;
-
-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (agents(ii).pos - agents(jj).pos);
-                A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-                b(kk) = obj.barrierGain * hComms(ii, jj);
-
-                % dVNominal = v(ii, 1:3) - v(jj, 1:3); % nominal velocities
-                % h_dot_nom = -2 * (agents(ii).pos - agents(jj).pos) * dVNominal';
-                % b(kk) = -h_dot_nom + obj.barrierGain * hComms(ii, jj)^3;
-
-                kk = kk + 1; 
-            end
-        end
-    end
-
-    % Solve QP program generated earlier
-    vhat = reshape(v', 3 * size(obj.agents, 1), 1);
-    H = 2 * eye(3 * size(obj.agents, 1));
-    f = -2 * vhat;
-    
-    % Update solution based on constraints
-    assert(size(A,2) == size(H,1))
-    assert(size(A,1) == size(b,1))
-    assert(size(H,1) == length(f))
-    opt = optimoptions('quadprog', 'Display', 'off');
-    [vNew, ~, exitflag, m] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
-    assert(exitflag == 1, sprintf('quadprog failure... %s%s', newline, m.message));
-    vNew = reshape(vNew, 3, size(obj.agents, 1))';
-
-    if exitflag <= 0
-        warning("QP failed, continuing with unconstrained solution...")
-        vNew = v;
-    end
-
-    % Update the "next position" that was previously set by unconstrained
-    % GA using the constrained solution produced here
-    for ii = 1:size(vNew, 1)
-        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
-    end
-
-    % Here we run this at the simulation level, but in reality there is no
-    % parent level, so this would be run independently on each agent.
-    % Running at the simulation level is just meant to simplify the
-    % simulation
-
-end

@miSim/initialize.m

@@ -1,34 +1,20 @@
-function obj = initialize(obj, domain, objective, agents, minAlt, timestep, partitoningFreq, maxIter, obstacles, makePlots, makeVideo)
+function obj = 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;
-        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;
-        makeVideo (1, 1) logical = true;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    % enable/disable plotting and video writer
-    obj.makePlots = makePlots;
-    if ~obj.makePlots
-        if makeVideo
-            warning("makeVideo set to true, but makePlots set to false. Setting makeVideo to false.");
-            makeVideo = false;
-        end
-    end
-    obj.makeVideo = makeVideo;
-
     % Define simulation time parameters
     obj.timestep = timestep;
-    obj.timestepIndex = 0;
     obj.maxIter = maxIter - 1;
 
     % Define domain
@@ -38,56 +24,26 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
     % Add geometries representing obstacles within the domain
     obj.obstacles = obstacles;
 
-    % Add an additional obstacle spanning the domain's footprint to 
-    % represent the minimum allowable altitude
-    obj.minAlt = minAlt;
-    if obj.minAlt > 0
-        obj.obstacles{end + 1, 1} = rectangularPrism;
-        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
-    end
-
     % Define objective
     obj.objective = objective;
 
     % Define agents
     obj.agents = agents;
-    obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
-    % Set labels for agents and collision geometries in cases where they
-    % were not provieded at the time of their initialization
-    for ii = 1:size(obj.agents, 1)
-        % Agent
-        if isempty(char(obj.agents{ii}.label))
-            obj.agents{ii}.label = sprintf("Agent %d", ii);
-        end
-
-        % Collision geometry
-        if isempty(char(obj.agents{ii}.collisionGeometry.label))
-            obj.agents{ii}.collisionGeometry.label = sprintf("Agent %d Collision Geometry", ii);
-        end
-    end
-
-    % Compute adjacency matrix and lesser neighbors
+    % Compute adjacency matrix
     obj = obj.updateAdjacency();
-    obj = obj.lesserNeighbor();
 
     % 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)];
 
-    % Prepare h function data store
-    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();
 
-    % Initialize variable that will store agent positions for trail plots
-    obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
-    obj.posHist(1:size(obj.agents, 1), 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
-
     % Set up plots showing initialized state
     obj = obj.plot();
 end

@miSim/lesserNeighbor.m

@@ -1,76 +0,0 @@
-function obj = lesserNeighbor(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    % initialize solution with self-connections only
-    constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
-
-    for ii = 1:size(obj.agents, 1)
-        % Find lesser neighbors of each agent
-        % Lesser neighbors of ii are jj < ii in range of ii
-        lesserNeighbors = [];
-        for jj = 1:(ii - 1)
-            if obj.adjacency(ii, jj)
-                lesserNeighbors = [lesserNeighbors, jj];
-            end
-        end
-        obj.agents{ii}.lesserNeighbors = lesserNeighbors;
-        
-        % Early exit for isolated agents
-        if isempty(obj.agents{ii}.lesserNeighbors)
-            continue
-        end
-
-        % Focus on subgraph defined by lesser neighbors
-        subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
-
-        % Find connected components in each agent's subgraph
-        % TODO: rewrite this using matlab "conncomp" function?
-        visited = false(size(subgraphAdjacency, 1), 1);
-        components = {};
-        for jj = 1:size(subgraphAdjacency, 1)
-            if ~visited(jj)
-                reachable = bfs(subgraphAdjacency, jj);
-                visited(reachable) = true;
-                components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
-            end
-        end
-
-        % Connect to the greatest index in each connected component in the
-        % lesser neighborhood of this agent
-        for jj = 1:size(components, 2)
-            constraintAdjacencyMatrix(ii, max(components{jj})) = true;
-            constraintAdjacencyMatrix(max(components{jj}), ii) = true;
-        end
-    end
-    obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
-end
-
-function cComp = bfs(subgraphAdjacency, startIdx)
-    n = size(subgraphAdjacency, 1);
-    visited = false(1, n);
-    queue = startIdx;
-    cComp = startIdx;
-    visited(startIdx) = true;
-    
-    while ~isempty(queue)
-        current = queue(1);
-        queue(1) = [];
-        
-        % Find all neighbors of current node in the subgraph
-        neighbors = find(subgraphAdjacency(current, :));
-        
-        for neighbor = neighbors
-            if ~visited(neighbor)
-                visited(neighbor) = true;
-                cComp = [cComp, neighbor];
-                queue = [queue, neighbor];
-            end
-        end
-    end
-    cComp = sort(cComp);
-end

@miSim/miSim.m

@@ -4,7 +4,6 @@ classdef miSim
     % Simulation parameters
     properties (SetAccess = private, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
-        timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
         maxIter = NaN; % maximum number of simulation iterations
         domain = rectangularPrism;
@@ -12,13 +11,10 @@ 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
-        constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
         sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
-        perf; % sensor performance timeseries array
-        performance = 0; % simulation performance timeseries vector
-        barrierGain = 100; % collision avoidance parameter
-        minAlt = 1; % minimum allowed altitude constraint
+        performance = NaN; % current cumulative sensor performance
+        oldMeanTotalPerf = 0;
 
         fPerf; % performance plot figure
     end
@@ -26,52 +22,37 @@ classdef miSim
     properties (Access = private)
         % Sim
         t = NaN; % current sim time
+        perf; % sensor performance timeseries array
         times;
         partitioningTimes;
 
         % Plot objects
-        makePlots = true; % enable/disable simulation plotting (performance implications)
-        makeVideo = true; % enable/disable VideoWriter (performance implications)
-        f; % main plotting tiled layout figure
+        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
 
-        posHist; % data for trail plot
-        trailPlot; % objects for agent trail 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];
         networkGraphIndex = 5;
         partitionGraphIndex = 1;
-
-        % CBF plotting
-        h; % h function values
-        hf; % h function plotting figure
-        caPlot; % objects for collision avoidance h function plot
-        obsPlot; % objects for obstacle h function plot
-        domPlot; % objects for domain h function plot
     end
 
     methods (Access = public)
         [obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
         [obj] = run(obj);
-        [obj] = lesserNeighbor(obj);
-        [obj] = constrainMotion(obj);
         [obj] = partition(obj);
         [obj] = updateAdjacency(obj);
         [obj] = plot(obj);
         [obj] = plotConnections(obj);
         [obj] = plotPartitions(obj);
         [obj] = plotGraph(obj);
-        [obj] = plotTrails(obj);
-        [obj] = plotH(obj);
         [obj] = updatePlots(obj, updatePartitions);
-        validate(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);
     end
-end
+end

@miSim/partition.m

@@ -16,13 +16,7 @@ function obj = partition(obj)
     [~, 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 = 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{:});
 
@@ -30,4 +24,18 @@ function obj = partition(obj)
     [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));
 end

@miSim/plot.m

@@ -5,11 +5,6 @@ function obj = plot(obj)
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
-    
-    % fast exit when plotting is disabled
-    if ~obj.makePlots
-        return;
-    end
 
     % Plot domain
     [obj.domain, obj.f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
@@ -22,7 +17,7 @@ function obj = plot(obj)
     % Plot objective gradient
     obj.f = obj.domain.objective.plot(obj.objectivePlotIndices, obj.f);
 
-    % Plot agents and their collision/communications geometries
+    % 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);
     end
@@ -36,9 +31,6 @@ function obj = plot(obj)
     % Plot domain partitioning
     obj = obj.plotPartitions();
 
-    % Plot agent trails
-    obj = obj.plotTrails();
-
     % 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)]);
@@ -48,7 +40,4 @@ function obj = plot(obj)
 
     % Plot performance
     obj = obj.plotPerformance();
-
-    % Plot h functions
-    obj = obj.plotH();
 end

@miSim/plotConnections.m

@@ -9,9 +9,9 @@ function obj = plotConnections(obj)
     % Iterate over lower triangle off-diagonal region of the 
     % adjacency matrix to plot communications links between agents
     X = []; Y = []; Z = [];
-    for ii = 2:size(obj.constraintAdjacencyMatrix, 1)
+    for ii = 2:size(obj.adjacency, 1)
         for jj = 1:(ii - 1)
-            if obj.constraintAdjacencyMatrix(ii, jj)
+            if obj.adjacency(ii, jj)
                 X = [X; obj.agents{ii}.pos(1), obj.agents{jj}.pos(1)];
                 Y = [Y; obj.agents{ii}.pos(2), obj.agents{jj}.pos(2)];
                 Z = [Z; obj.agents{ii}.pos(3), obj.agents{jj}.pos(3)];

@miSim/plotGraph.m

@@ -7,7 +7,7 @@ function obj = plotGraph(obj)
     end
 
     % Form graph from adjacency matrix
-    G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
+    G = graph(obj.adjacency, 'omitselfloops');
 
     % Plot graph object
     if isnan(obj.networkGraphIndex)

@miSim/plotH.m

@@ -1,61 +0,0 @@
-function obj = plotH(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    obj.hf = figure;
-    tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
-
-    nexttile(obj.hf.Children(1));
-    axes(obj.hf.Children(1).Children(1));
-    grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
-    title(obj.hf.Children(1).Children(1), "Collision Avoidance");
-    hold(obj.hf.Children(1).Children(1), "on");
-    obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
-    legendStrings = [];
-    for ii = 2:size(obj.agents, 1)
-        for jj = 1:(ii - 1)
-            legendStrings = [legendStrings; sprintf("A%d A%d", jj, ii)];
-        end
-    end
-    legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
-    hold(obj.hf.Children(1).Children(2), "off");
-
-    nexttile(obj.hf.Children(1));
-    axes(obj.hf.Children(1).Children(1));
-    grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(2), "");
-    title(obj.hf.Children(1).Children(1), "Obstacles");
-    hold(obj.hf.Children(1).Children(1), "on");
-    obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
-    legendStrings = [];
-    for ii = 1:size(obj.obstacles, 1)
-        for jj = 1:size(obj.agents, 1)
-            legendStrings = [legendStrings; sprintf("A%d O%d", jj, ii)];
-        end
-    end
-    legend(obj.hf.Children(1).Children(1), legendStrings, 'Location', 'bestoutside');
-    hold(obj.hf.Children(1).Children(2), "off");
-
-    nexttile(obj.hf.Children(1));
-    axes(obj.hf.Children(1).Children(1));
-    grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
-    title(obj.hf.Children(1).Children(1), "Domain");
-    hold(obj.hf.Children(1).Children(1), "on");
-    obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
-    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], 'Location', 'bestoutside');
-    hold(obj.hf.Children(1).Children(2), "off");
-
-    nexttile(obj.hf.Children(1));
-    axes(obj.hf.Children(1).Children(1));
-    grid(obj.hf.Children(1).Children(1), "on");
-    xlabel(obj.hf.Children(1).Children(1), "Time (s)"); % ylabel(obj.hf.Children(1).Children(1), "");
-    title(obj.hf.Children(1).Children(1), "Communications");
-    % skipped this for now because it is very complicated
-
-end

@miSim/plotPerformance.m

@@ -6,13 +6,6 @@ function obj = plotPerformance(obj)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    % fast exit when plotting is disabled
-    if ~obj.makePlots
-        return;
-    end
-    
-    obj.fPerf = figure;
-
     axes(obj.fPerf);
     title(obj.fPerf.Children(1), "Sensor Performance");
     xlabel(obj.fPerf.Children(1), 'Time (s)');
@@ -22,22 +15,20 @@ function obj = plotPerformance(obj)
     % Plot current cumulative performance
     hold(obj.fPerf.Children(1), 'on');
     o = plot(obj.fPerf.Children(1), obj.perf(end, :));
-    warning('off', 'MATLAB:gui:array:InvalidArrayShape'); % suppress this warning to avoid polluting output
-    o.XData = NaN(1, obj.maxIter); % correct time will be set at runtime
-    o.YData = [0, NaN(1, obj.maxIter - 1)];
     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, :))];
-        o(end).XData = NaN(1, obj.maxIter); % correct time will be set at runtime
-        o(end).YData = [0, NaN(1, obj.maxIter - 1)];
         hold(obj.fPerf.Children(1), 'off');
     end
 
     % Add legend
-    agentStrings = string(cellfun(@(x) x.label, obj.agents, 'UniformOutput', false));
+    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');
 

@miSim/plotTrails.m

@@ -1,26 +0,0 @@
-function obj = plotTrails(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')}
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')}
-    end
-    
-    % fast exit when plotting is disabled
-    if ~obj.makePlots
-        return;
-    end
-
-    % Plot full range of position history on each spatial plot axes
-    o = [];
-    for ii = 1:(size(obj.posHist, 1))
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'on');
-        o = [o; plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), obj.posHist(ii, 1:obj.maxIter, 1), obj.posHist(ii, 1:obj.maxIter, 2), obj.posHist(ii, 1:obj.maxIter, 3), 'Color', 'k', 'LineWidth', 1)];
-        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), 'off');
-    end
-
-    % Copy trails to other figures?
-    obj.trailPlot = o;
-
-    % Add legend?
-end

@miSim/run.m

@@ -7,47 +7,40 @@ function [obj] = run(obj)
     end
 
     % Start video writer
-    if obj.makeVideo
-        v = obj.setupVideoWriter();
-        v.open();
-    end
-    
+    v = obj.setupVideoWriter();
+    v.open();
+
+    steady = 0;
     for ii = 1:size(obj.times, 1)
         % Display current sim time
         obj.t = obj.times(ii);
-        obj.timestepIndex = ii;
         fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
-        % Validate current simulation configuration
-        obj.validate();
-
         % 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
             updatePartitions = true;
             obj = obj.partition();
         end
 
-        % Determine desired communications links
-        obj = obj.lesserNeighbor();
-
-        % Iterate over agents to simulate their unconstrained motion
+        % Iterate over agents to simulate their motion
         for jj = 1:size(obj.agents, 1)
-            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t, jj);
+            obj.agents{jj} = obj.agents{jj}.run(obj.objective, obj.domain, obj.partitioning);
         end
 
-        % Adjust motion determined by unconstrained gradient ascent using
-        % CBF constraints solved by QP
-        obj = constrainMotion(obj);
-
-        % Finished simulation for this timestep, do accounting
-
-        % 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);
-
-        % Update total performance
-        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];
-
         % Update adjacency matrix
         obj = obj.updateAdjacency();
 
@@ -55,14 +48,10 @@ function [obj] = run(obj)
         obj = obj.updatePlots(updatePartitions);
 
         % Write frame in to video
-        if obj.makeVideo
-            I = getframe(obj.f);
-            v.writeVideo(I);
-        end
+        I = getframe(obj.f);
+        v.writeVideo(I);
     end
 
-    if obj.makeVideo
-        % Close video file
-        v.close();
-    end
-end
+    % Close video file
+    v.close();
+end

@miSim/updateAdjacency.m

@@ -7,29 +7,26 @@ function obj = updateAdjacency(obj)
     end
 
     % Initialize assuming only self-connections
-    A = true(size(obj.agents, 1));
+    A = logical(eye(size(obj.agents, 1)));
 
     % Check lower triangle off-diagonal connections
     for ii = 2:size(A, 1)
         for jj = 1:(ii - 1)
-            % Check that agents are not out of range
-            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])
-                A(ii, jj) = false; % comm range violation
-                continue;
+            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) <= min([obj.agents{ii}.comRange, obj.agents{jj}.comRange])
+                % Make sure that obstacles don't obstruct the line
+                % of sight, breaking the connection
+                for kk = 1:size(obj.obstacles, 1)
+                    if ~obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos)
+                        A(ii, jj) = true;
+                    end
+                end
+                % need extra handling for cases with no obstacles
+                if isempty(obj.obstacles)
+                    A(ii, jj) = true;
+                end
             end
-
-            % % Check that agents do not have their line of sight obstructed
-            % for kk = 1:size(obj.obstacles, 1)
-            %     if obj.obstacles{kk}.containsLine(obj.agents{jj}.pos, obj.agents{ii}.pos)
-            %         A(ii, jj) = false;
-            %     end
-            % end
         end
     end
 
-    obj.adjacency = A & A';
-
-    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
-        warning("Eliminated network connections that were necessary");
-    end
+    obj.adjacency = A | A';
 end

@miSim/updatePlots.m

@@ -7,18 +7,13 @@ function [obj] = updatePlots(obj, updatePartitions)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    % Fast exit when plotting is disabled
-    if ~obj.makePlots
-        return;
-    end
-
-    % Update agent positions, collision/communication geometries
+    % Update agent positions, collision geometries
     for ii = 1:size(obj.agents, 1)
         obj.agents{ii}.updatePlots();
     end
 
-    % The remaining updates might should all be possible to do in a clever 
-    % way that moves existing lines instead of clearing and 
+    % The remaining updates might be possible to do in a clever way
+    % that moves existing lines instead of clearing and 
     % re-plotting, which is much better for performance boost
 
     % Update agent connections plot
@@ -41,34 +36,19 @@ function [obj] = updatePlots(obj, updatePartitions)
         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)]);
     end
-
-    % Update agent trails
-    for ii = 1:size(obj.agents, 1)
-        obj.trailPlot(ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
-        obj.trailPlot(ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
-        obj.trailPlot(ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
-    end
-
     drawnow;
     
     % Update performance plot
-    % Re-normalize performance plot
-    normalizingFactor = 1/max(obj.performance(end));
-    obj.performancePlot(1).YData(1:length(obj.performance)) = obj.performance * normalizingFactor;
-    obj.performancePlot(1).XData(obj.timestepIndex) = obj.t;
-    for ii = 2:(size(obj.agents, 1) + 1)
-        obj.performancePlot(ii).YData(1:length(obj.performance)) = obj.agents{ii - 1}.performance * normalizingFactor;
-        obj.performancePlot(ii).XData(obj.timestepIndex) = obj.t;
-    end
+    if updatePartitions
+        % find index corresponding to the current time
+        nowIdx = [0; obj.partitioningTimes] == obj.t;
+        nowIdx = find(nowIdx);
 
-    % Update h function plots
-    for ii = 1:size(obj.caPlot, 1)
-        obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
-    end
-    for ii = 1:size(obj.obsPlot, 1)
-        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), obj.timestepIndex);
-    end
-    for ii = 1:size(obj.domPlot, 1)
-        obj.domPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1) + size(obj.obsPlot, 1), obj.timestepIndex);
+        % 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

@miSim/validate.m

@@ -1,12 +0,0 @@
-function validate(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-    end
-
-    if max(conncomp(graph(obj.adjacency))) ~= 1
-        warning("Network is not connected");
-    end
-
-end

@sensingObjective/initialize.m

@@ -10,8 +10,6 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
     end
 
-    obj.discretizationStep = discretizationStep;
-
     obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 
@@ -21,8 +19,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     yMin = min(domain.footprint(:, 2));
     yMax = max(domain.footprint(:, 2));
 
-    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
-    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
+    xGrid = unique([xMin:discretizationStep:xMax, xMax]);
+    yGrid = unique([yMin:discretizationStep:yMax, yMax]);
     
     % Store grid points for plotting later
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
@@ -37,7 +35,6 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     % store ground position
     idx = obj.values == 1;
     obj.groundPos = [obj.X(idx), obj.Y(idx)];
-    obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
 
     assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

geometries/@rectangularPrism/closestToPoint.m

@@ -1,19 +0,0 @@
-function cPos = closestToPoint(obj, pos)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-        pos (:, 3) double;
-    end
-    arguments (Output)
-        cPos (:, 3) double;
-    end
-    cPos = NaN(1, 3);
-    for ii = 1:3
-        if pos(ii) < obj.minCorner(ii)
-            cPos(ii) = obj.minCorner(ii);
-        elseif pos(ii) > obj.maxCorner(ii)
-            cPos(ii) = obj.maxCorner(ii);
-        else
-            cPos(ii) = pos(ii);
-        end
-    end
-end

geometries/@rectangularPrism/containsLine.m

@@ -9,38 +9,33 @@ function c = containsLine(obj, pos1, pos2)
     end
     
     d = pos2 - pos1;
-
-    % endpoint contained (trivial case)
-    if obj.contains(pos1) || obj.contains(pos2)
-        c = true;
+    
+    % edge case where the line is parallel to the geometry
+    if abs(d) < 1e-12
+        % check if it happens to start or end inside or outside of
+        % the geometry
+        if obj.contains(pos1) || obj.contains(pos2)
+            c = true;
+        else
+            c = false;
+        end
         return;
     end
-
-    % parameterize the line segment to check for an intersection
-    tMin = 0;
-    tMax = 1;
+    
+    tmin = -inf;        
+    tmax = inf;
+    
+    % Standard case
     for ii = 1:3
-        % line is parallel to geometry
-        if abs(d(ii)) < 1e-12
-            if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii)
-                c = false;
-                return;
-            end
-        else
-            t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
-            t2 = (obj.maxCorner(ii) - pos1(ii)) / d(ii);
-
-            tLow  = min(t1, t2);
-            tHigh = max(t1, t2);
-
-            tMin = max(tMin, tLow);
-            tMax = min(tMax, tHigh);
-
-            if tMin > tMax
-                c = false;
-                return;
-            end
+        t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
+        t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii);
+        tmin = max(tmin, min(t1, t2));
+        tmax = min(tmax, max(t1, t2));
+        if tmin > tmax
+            c = false;
+            return;
         end
     end
-    c = true;
-end
+    
+    c = (tmax >= 0) && (tmin <= 1);
+    end

geometries/@rectangularPrism/distance.m

@@ -4,7 +4,7 @@ function d = distance(obj, pos)
         pos (:, 3) double;
     end
     arguments (Output)
-        d (:, 1) double;
+        d (:, 1) double
     end
     if obj.contains(pos)
         % Queried point is inside geometry

geometries/@rectangularPrism/distanceGradient.m

@@ -1,42 +0,0 @@
-function g = distanceGradient(obj, pos)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-        pos (:, 3) double;
-    end
-    arguments (Output)
-        g (:, 3) double
-    end
-    
-    % find nearest point on surface to query position
-    q = min(max(pos, obj.minCorner), obj.maxCorner);
-
-    % Find distance and direction between pos and q
-    v = pos - q;
-    vNorm = norm(v);
-
-    % position is outside geometry
-    if vNorm > 0
-        % gradient is normalized vector from q to p
-        g = v / vNorm;
-        return;
-    end
-
-    % position is on or in geometry
-    % find distances to each face in each dimension
-    distances = [pos(1) - obj.minCorner(1), obj.maxCorner(1) - pos(1), pos(2) - obj.minCorner(2), obj.maxCorner(2) - pos(2), pos(3) - obj.minCorner(3), obj.maxCorner(3) - pos(3)];
-    [~, idx] = min(distances);
-
-    % I think there needs to be additional handling here for the
-    % edge/corner cases, where there are ways to balance or resolve ties
-    % when two faces are equidistant to the query position
-    assert(sum(idx) == idx, "Implement edge case handling");
-
-    % select gradient that brings us quickest to the nearest face
-    g = [ 1,  0,  0; ...
-         -1,  0,  0; ...
-          0,  1,  0; ...
-          0, -1,  0; ...
-          0,  0,  1; ...
-          0,  0, -1;];
-    g = g(idx, :);
-end

geometries/@rectangularPrism/initialize.m

@@ -24,10 +24,6 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
     % Compute center
     obj.center = obj.minCorner + obj.dimensions ./ 2;
 
-    % Compute a (fake) radius
-    % fully contains the rectangular prism from the center
-    obj.radius = (1/2) * sqrt(sum(obj.dimensions.^2));
-
     % Compute vertices
     obj.vertices = [obj.minCorner;
                     obj.maxCorner(1), obj.minCorner(2:3);
@@ -48,13 +44,4 @@ function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretiza
     if tag == REGION_TYPE.DOMAIN
         obj.objective = sensingObjective;
     end
-
-    % Initialize CBF
-    % first part evaluates to +/-1 if the point is outside/inside the collision geometry
-    % Second part determines the distance from the point to the boundary of the collision geometry
-    obj.barrierFunction = @(x) (1 - 2 * obj.collisionGeometry.contains(x)) * obj.collisionGeometry.distance(x); % x is 1x3
-    % gradient of barrier function 
-    obj.dBarrierFunction = @(x) obj.collisionGeometry.distanceGradient(x); % x is 1x3
-    % as long as the collisionGeometry object is updated during runtime,
-    % these functions never have to be updated again
 end

geometries/@rectangularPrism/initializeRandom.m

@@ -1,12 +1,11 @@
-function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain, minAlt)
+function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
         minDimension (1, 1) double = 10;
-        maxDimension (1, 1) double = 20;
+        maxDimension (1, 1) double= 20;
         domain (1, 1) {mustBeGeometry} = rectangularPrism;
-        minAlt (1, 1) double = 0;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
@@ -28,7 +27,7 @@ function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, d
         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) = minAlt; % bind to floor (plus minimum altitude constraint)
+                candidateMinCorner(3) = 0; % bind to floor
                 ii = 1;
             end
 

geometries/@rectangularPrism/rectangularPrism.m

@@ -3,6 +3,7 @@ classdef rectangularPrism
     properties (SetAccess = private, GetAccess = public)
         % Meta
         tag = REGION_TYPE.INVALID;
+        label = "";
 
         % Spatial
         minCorner = NaN(1, 3);
@@ -10,7 +11,6 @@ classdef rectangularPrism
         dimensions = NaN(1, 3);
         center = NaN;
         footprint = NaN(4, 2);
-        radius = NaN; % fake radius
 
         % Graph
         vertices = NaN(8, 3);
@@ -20,13 +20,8 @@ classdef rectangularPrism
 
         % Plotting
         lines;
-
-        % collision
-        barrierFunction;
-        dBarrierFunction;
     end
     properties (SetAccess = public, GetAccess = public)
-        label = "";
         % Sensing objective (for DOMAIN region type only)
         objective;
     end
@@ -36,9 +31,7 @@ classdef rectangularPrism
         [obj   ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);
-        [cPos  ] = closestToPoint(obj, pos);
         [d     ] = distance(obj, pos);
-        [g     ] = distanceGradient(obj, pos);
         [c     ] = containsLine(obj, pos1, pos2);
         [obj, f] = plotWireframe(obj, ind, f);
     end

geometries/@spherical/contains.m

@@ -1,10 +0,0 @@
-function c = contains(obj, pos)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-        pos (:, 3) double;
-    end
-    arguments (Output)
-        c (:, 1) logical
-    end
-    c = norm(obj.center - pos) <= obj.radius;
-end

geometries/@spherical/containsLine.m

@@ -1,28 +0,0 @@
-function c = containsLine(obj, pos1, pos2)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-        pos1 (1, 3) double;
-        pos2 (1, 3) double;
-    end
-    arguments (Output)
-        c (1, 1) logical
-    end
-    
-    d = pos2 - pos1;
-    f = pos1 - obj.center;
-
-    a = dot(d, d);
-    b = 2 * dot(f, d);
-    c = dot(f, f) - obj.radius^2;
-
-    disc = b^2 - 4*a*c;
-
-    if disc < 0
-        c = false;
-        return;
-    end
-
-    t = [(-b - sqrt(disc)) / (2 * a), (-b + sqrt(disc)) / (2 * a)];
-
-    c = (t(1) >= 0 && t(1) <= 1) || (t(2) >= 0 && t(2) <= 1);
-end

geometries/@spherical/initialize.m

@@ -1,42 +0,0 @@
-function obj = initialize(obj, center, radius, tag, label)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-        center (1, 3) double;
-        radius  (1, 1) double;
-        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
-        label (1, 1) string = "";
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-    end
-
-    obj.tag = tag;
-    obj.label = label;
-
-    % Define geometry
-    obj.center = center;
-    obj.radius = radius;
-    obj.diameter = 2 * obj.radius;
-
-    % Initialize CBF
-    obj.barrierFunction = @(x) NaN;
-    % gradient of barrier function 
-    obj.dBarrierFunction = @(x) NaN;
-
-    % fake vertices in a cross pattern
-    obj.vertices = [obj.center + [obj.radius, 0, 0]; ...
-                    obj.center - [obj.radius, 0, 0]; ...
-                    obj.center + [0, obj.radius, 0]; ...
-                    obj.center - [0, obj.radius, 0]; ...
-                    obj.center + [0, 0, obj.radius]; ...
-                    obj.center - [0, 0, obj.radius]];
-    % fake edges in two perpendicular rings
-    obj.edges = [1, 3; ...
-                 3, 2; ...
-                 2, 4; ...
-                 4, 1; ...
-                 1, 5; ...
-                 5, 2; ...
-                 2, 6; ...
-                 6, 1];
-end

geometries/@spherical/plotWireframe.m

@@ -1,43 +0,0 @@
-function [obj, f] = plotWireframe(obj, ind, f)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-        ind (1, :) double = NaN;
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-    end
-
-    % Create axes if they don't already exist
-    f = firstPlotSetup(f);
-
-    % Create plotting inputs
-    [X, Y, Z] = sphere(8);
-    % Scale
-    X = X * obj.radius;
-    Y = Y * obj.radius;
-    Z = Z * obj.radius;
-    % Shift
-    X = X + obj.center(1);
-    Y = Y + obj.center(2);
-    Z = Z + obj.center(3);
-
-    % Plot the boundaries of the geometry into 3D view
-    if isnan(ind)
-        o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
-    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
-
-    % 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.lines = o;
-end

geometries/@spherical/random.m

@@ -1,15 +0,0 @@
-function r = random(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'spherical')};
-    end
-    arguments (Output)
-        r (1, 3) double
-    end
-    y = (rand - 0.5) * 2; % uniform draw on [-1, 1]
-    R = sqrt(1 - y^2);
-    lon = (rand - 0.5) * 2 * pi; % uniform draw on [-pi, pi]
-    s = [R * sin(lon), y, R * cos(lon)]; % random point on surface
-    r = s * rand^(1/3); % scaled to random normalized radius [0, 1]
-
-    r = obj.center + obj.radius * r;
-end

geometries/@spherical/spherical.m

@@ -1,37 +0,0 @@
-classdef spherical
-    % Rectangular prism geometry
-    properties (SetAccess = private, GetAccess = public)
-        % Spatial
-        center = NaN;
-        radius = NaN;
-        diameter = NaN;
-
-        vertices; % fake vertices
-        edges; % fake edges
-
-        % Plotting
-        lines;
-
-        % collision
-        barrierFunction;
-        dBarrierFunction;
-    end
-    properties (SetAccess = public, GetAccess = public)
-        % Meta
-        tag = REGION_TYPE.INVALID;
-        label = "";
-        
-        % Sensing objective (for DOMAIN region type only)
-        objective;
-    end
-
-    methods (Access = public)
-        [obj   ] = initialize(obj, center, radius, tag, label);
-        [r     ] = random(obj);
-        [c     ] = contains(obj, pos);
-        [d     ] = distance(obj, pos);
-        [g     ] = distanceGradient(obj, pos);
-        [c     ] = containsLine(obj, pos1, pos2);
-        [obj, f] = plotWireframe(obj, ind, f);
-    end
-end

geometries/REGION_TYPE.m

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

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

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/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBId.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rrrOIkYUdFsPpnaydWK8XER8pkwp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakd.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/OYUaWUmC6jZNQaYWobzy5I-iESkd.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/OYUaWUmC6jZNQaYWobzy5I-iESkp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/_tVnHInN54-Fys1puCBYQ1j4NwUd.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/_tVnHInN54-Fys1puCBYQ1j4NwUp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/hyvGevruV7eLIdxFdEJJT6QLP1Yd.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/hyvGevruV7eLIdxFdEJJT6QLP1Yp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/zfqtoTzQztkirqSAjA2vYKiZn2Mp.xml

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

resources/project/svhNcplUM7n23Qjwql1uZ5L6c6s/zd__hNBRxfd17pUXZGjSSPNgl_kp.xml

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

resources/project/tBBDcBPWdBTCTMSBG9IGJ_TsICk/4TB7e-h1cL8M9VKf-O2iM-tNAksd.xml

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

resources/project/tBBDcBPWdBTCTMSBG9IGJ_TsICk/4TB7e-h1cL8M9VKf-O2iM-tNAksp.xml

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

resources/project/tBBDcBPWdBTCTMSBG9IGJ_TsICk/789El5-CdC-5REJUNa0jxTyTfvod.xml

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

resources/project/tBBDcBPWdBTCTMSBG9IGJ_TsICk/789El5-CdC-5REJUNa0jxTyTfvop.xml

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

resources/project/zfqtoTzQztkirqSAjA2vYKiZn2M/H7wWoItGZZXFLdjsjGa3nymk8U8p.xml

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

sensorModels/@sigmoidSensor/sense.m

@@ -0,0 +1,21 @@
+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

test/test_miSim.m

@@ -3,11 +3,6 @@ classdef test_miSim < matlab.unittest.TestCase
         % System under test
         testClass = miSim;
 
-        % Debug
-        makeVideo = true; % disable video writing for big performance increase
-        makePlots = true; % disable plotting for big performance increase (also disables video)
-        plotCommsGeometry = false; % disable plotting communications geometries
-
         % Sim
         maxIter = 250;
         timestep = 0.05
@@ -16,7 +11,6 @@ classdef test_miSim < matlab.unittest.TestCase
         % Domain
         domain = rectangularPrism; % domain geometry
         minDimension = 10;
-        minAlt = 1; % minimum allowed agent altitude
 
         % Obstacles
         minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
@@ -31,8 +25,8 @@ classdef test_miSim < matlab.unittest.TestCase
         objective = sensingObjective;
 
         % Agents
-        minAgents = 4; % Minimum number of agents to be randomly generated
-        maxAgents = 6; % Maximum number of agents to be randomly generated
+        minAgents = 2; % Minimum number of agents to be randomly generated
+        maxAgents = 4; % Maximum number of agents to be randomly generated
         sensingLength = 0.05; % length parameter used by sensing function
         agents = cell(0, 1);
         
@@ -90,13 +84,12 @@ classdef test_miSim < matlab.unittest.TestCase
                 while badCandidate
                     % Instantiate a rectangular prism obstacle inside the domain
                     tc.obstacles{ii} = rectangularPrism;
-                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt);
+                    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;
                     end
-
                 end
             end
 
@@ -111,11 +104,11 @@ 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) = tc.minAlt  + rand * 3; % place agents at decent altitudes for sensing
+                            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) = tc.minAlt  + rand * 3; % place agents at decent altitudes for sensing
+                        candidatePos(3) = 1  + rand * 3; % place agents at decent altitudes for sensing
                     end
 
                     % Make sure that the candidate position is within the
@@ -155,14 +148,14 @@ classdef test_miSim < matlab.unittest.TestCase
 
                     % Initialize candidate agent collision geometry
                     candidateGeometry = rectangularPrism;
-                    candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
+                    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(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));
 
                     % Initialize candidate agent
-                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, tc.comRange); 
+                    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
@@ -210,7 +203,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.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -223,7 +216,7 @@ classdef test_miSim < matlab.unittest.TestCase
                 while badCandidate
                     % Instantiate a rectangular prism obstacle inside the domain
                     tc.obstacles{ii} = rectangularPrism;
-                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain, tc.minAlt);
+                    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})
@@ -243,11 +236,11 @@ 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, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
+                            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, tc.minAlt + rand * (tc.alphaDistMax * (1.1)  - 0.5)]); % place agents at decent altitudes for sensing
+                        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
@@ -286,17 +279,15 @@ classdef test_miSim < matlab.unittest.TestCase
                     end
 
                     % Initialize candidate agent collision geometry
-                    % candidateGeometry = rectangularPrism;
-                    % candidateGeometry = candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION);
-                    candidateGeometry = spherical;
-                    candidateGeometry = candidateGeometry.initialize(candidatePos, tc.collisionRanges(ii), REGION_TYPE.COLLISION);
-
+                    candidateGeometry = rectangularPrism;
+                    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(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));
 
                     % Initialize candidate agent
-                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, tc.comRange);
+                    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
@@ -344,7 +335,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.timestep, tc.partitoningFreq, tc.maxIter, tc.obstacles);
 
             % Run simulation loop
             tc.testClass = tc.testClass.run();
@@ -363,8 +354,8 @@ classdef test_miSim < matlab.unittest.TestCase
             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);
-            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);
+            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));
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
@@ -378,23 +369,18 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % 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, 3*d);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + dh - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, 3*d);
+            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);
+            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, 3*d);
+            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));
 
             % 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.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
-            tc.verifyLessThan(sum(tc.testClass.partitioning == 2, 'all'), sum(tc.testClass.partitioning == 1, 'all')); % more partition 1 assignments than partition 2 assignments
-            tc.verifyLessThan(sum(tc.testClass.partitioning == 3, 'all'), sum(tc.testClass.partitioning == 2, 'all')); % more partition 3 assignments than partition 2 assignments
-            tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1; 2; 3;]);
+            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
@@ -406,7 +392,7 @@ classdef test_miSim < matlab.unittest.TestCase
         
             % 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);
+            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;
@@ -416,324 +402,15 @@ classdef test_miSim < matlab.unittest.TestCase
             sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 20, 3);
 
             % Plot sensor parameters (optional)
-            % f = sensor.plotParameters();
+            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, 3);
+            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.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.timestep, tc.partitoningFreq, tc.maxIter);
             close(tc.testClass.fPerf);
-
-            tc.verifyEqual(unique(tc.testClass.partitioning), [0; 1]);
-            tc.verifyLessThan(sum(tc.testClass.partitioning == 1, 'all'), sum(tc.testClass.partitioning == 0, 'all'));
-        end
-        function test_single_partition_basic_GA(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(:)], [2, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            geometry1 = rectangularPrism;
-            geometry1 = geometry1.initialize([[tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3] - tc.collisionRanges(1) * ones(1, 3); [tc.domain.center(1:2)-tc.domain.dimensions(1)/3, 3] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION);
-            
-            % 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)-tc.domain.dimensions(1)/3, 3], zeros(1,3), 0, 0, geometry1, sensor, 3, "", false);
-
-            % 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), true, false);
-            
-            % Run the simulation
-            tc.testClass = tc.testClass.run();
-            if isgraphics(tc.testClass.agents{1}.debugFig)
-                close(tc.testClass.agents{1}.debugFig);
-            end
-
-            % tc.verifyGreaterThan(tc.testClass.performance(end)/max(tc.testClass.performance), 0.99); % ends up very near a relative maximum
-        end
-        function test_collision_avoidance(tc)
-            % No obstacles
-            % Fixed agent initial conditions
-            % Exaggerated large collision geometries to test CA
-            % 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(:)], [3, 7]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            radius = 1.5;
-            d = [2.5, 0, 0];
-            geometry1 = spherical;
-            geometry2 = spherical;
-            geometry1 = geometry1.initialize(tc.domain.center + d, radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - d, radius, REGION_TYPE.COLLISION);
-            
-            % 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, 15, 3);
-
-            % Initialize agents
-            tc.agents = {agent; agent};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + d, zeros(1,3), 0, 0, geometry1, sensor, 5);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry2, sensor, 5);
-
-            % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 50, cell(0, 1), tc.makeVideo, tc.makePlots);
-            
-            % Run the simulation
-            tc.testClass.run();
-        end
-        function test_obstacle_avoidance(tc)
-            % Right now this seems to prove that the communications
-            % constraints are working, but the result is dissatisfying
-
-            % Fixed single obstacle
-            % Fixed two agents initial conditions
-            % Exaggerated large collision geometries
-            % 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(:)], [8, 5.2195]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            radius = 1.1;
-            d = [3, 0, 0];
-
-            yOffset = 0;
-            % choice of 0 leads to the agents getting stuck attempting to go around the obstacle on both sides
-            % choice of 1 leads to one agent easily going around while the other gets stuck and the communications link is broken
-
-            geometry1 = spherical;
-            geometry2 = geometry1;
-            geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.1 - yOffset, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.1 + yOffset, 0], radius, REGION_TYPE.COLLISION);
-            
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
-            
-            % Initialize obstacles
-            obstacleLength = 1;
-            tc.obstacles{1} = rectangularPrism;
-            tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
-
-            % Initialize agents
-            commsRadius = (2*radius + obstacleLength) * 0.9; % defined such that they cannot go around the obstacle on both sides
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.1 - yOffset, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            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);
-
-            % 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);
-            
-            % Run the simulation
-            tc.testClass.run();
-        end
-        function test_communications_constraint(tc)
-            % No obstacles
-            % Fixed two agents initial conditions
-            % Negligible collision geometries
-            % Non-standard domain with two objectives that will try to pull the
-            % agents apart
-            l = 10; % domain size
-            dom = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
-
-            % make basic sensing objective
-            dom.objective = dom.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [2, 8]) + mvnpdf([x(:), y(:)], [8, 8]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            radius = 0.1;
-            d = [1, 0, 0];
-            geometry1 = spherical;
-            geometry2 = geometry1;
-            geometry1 = geometry1.initialize(dom.center + d, radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(dom.center - d, radius, REGION_TYPE.COLLISION);
-            
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
-            
-            % Initialize obstacles
-            tc.obstacles = {};
-            
-            % Initialize agents
-            commsRadius = 4; % defined such that they cannot reach their objective without breaking connectivity
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(dom.center + d, zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(dom.center - d, zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-
-            % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(dom, dom.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 75, tc.obstacles, true, false);
-            
-            % Run the simulation
-            tc.testClass = tc.testClass.run();
-        end
-        function test_obstacle_blocks_comms_LOS(tc)
-            % Fixed single obstacle
-            % Fixed two agents initial conditions
-            % Exaggerated large communications radius
-            % 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(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            radius = .25;
-            d = 2;
-            geometry1 = spherical;
-            geometry2 = geometry1;
-            geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], radius, REGION_TYPE.COLLISION);
-            
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
-
-            % Initialize agents
-            commsRadius = 5;
-            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-            
-            % Initialize obstacles
-            obstacleLength = 1.5;
-            tc.obstacles{1} = rectangularPrism;
-            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, 125, tc.obstacles, false, false);
-
-            % No communications link should be established
-            tc.assertEqual(tc.testClass.adjacency, logical(eye(2)));
-        end
-        function test_LNA_case_1(tc)
-            % based on example in meeting 
-            % No obstacles
-            % Fixed 5 agents initial conditions
-            % unitary communicaitons radius
-            % negligible collision radius
-            % 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(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            radius = .01;
-            d = 1;
-            geometry5 = spherical;
-            geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry5.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
-            geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION);
-            
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
-
-            % Initialize agents
-            commsRadius = d;
-            tc.agents = {agent; agent; agent; agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center, zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [-d, d, 0], zeros(1,3), 0, 0, geometry3, sensor, commsRadius);
-            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [-2*d, d, 0], zeros(1,3), 0, 0, geometry4, sensor, commsRadius);
-            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius);
-
-            % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false);
-
-            % Constraint adjacency matrix defined by LNA should be as follows
-            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
-                [ 1, 1, 0, 0, 0; ...
-                  1, 1, 0, 0, 1; ...
-                  0, 0, 1, 1, 1;
-                  0, 0, 1, 1, 0;
-                  0, 1, 1, 0, 1;]));
-        end
-        function test_LNA_case_2(tc)
-            % based on example in paper Asynchronous Local Construction of Bounded-Degree Network Topologies Using Only Neighborhood Information
-            % No obstacles
-            % Fixed 7 agents initial conditions
-            % unitary communicaitons radius
-            % negligible collision radius
-            % 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(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
-        
-            % Initialize agent collision geometry
-            radius = .01;
-            d = 1;
-            geometry7 = spherical;
-            geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], radius, REGION_TYPE.COLLISION);
-            geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION);
-            geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], radius, REGION_TYPE.COLLISION);
-            geometry6 = geometry7.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
-            geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], radius, REGION_TYPE.COLLISION);
-            
-            % Initialize agent sensor model
-            sensor = sigmoidSensor;
-            alphaDist = l/2; % half of domain length/width
-            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
-
-            % Initialize agents
-            commsRadius = d;
-            tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
-            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], zeros(1,3), 0, 0, geometry3, sensor, commsRadius);
-            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry4, sensor, commsRadius);
-            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius);
-            tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, zeros(1,3), 0, 0, geometry6, sensor, commsRadius);
-            tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], zeros(1,3), 0, 0, geometry7, sensor, commsRadius);
-
-            % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false);
-
-            % Constraint adjacency matrix defined by LNA should be as follows
-            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
-                [ 1, 1, 0, 0, 0, 0, 0; ...
-                  1, 1, 0, 0, 1, 0, 0; ...
-                  0, 0, 1, 1, 0, 0, 0;
-                  0, 0, 1, 1, 0, 1, 0;
-                  0, 1, 0, 0, 1, 1, 0;
-                  0, 0, 0, 1, 1, 1, 1;
-                  0, 0, 0, 0, 0, 1, 1; ]));
         end
     end
 
@@ -748,4 +425,4 @@ classdef test_miSim < matlab.unittest.TestCase
             end
         end
     end
-end
+end

util/validators/arguments/mustBeGeometry.m

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