fixed unit tests

.gitignore

@@ -45,3 +45,6 @@ sandbox/*
 # Videos
 *.mp4
 *.avi
+
+# Figures
+*.fig

@agent/agent.m

@@ -1,22 +1,14 @@
 classdef agent
-    properties (SetAccess = private, GetAccess = public)
+    properties (SetAccess = public, 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
-        vel = NaN(1, 3); % current velocity
-        pan = NaN; % pan angle
-        tilt = NaN; % tilt angle
+
+        % Sensor
+        sensorModel;
 
         % Collision
         collisionGeometry;
@@ -25,15 +17,26 @@ classdef agent
         fovGeometry;
 
         % Communication
-        comRange = NaN;
+        commsGeometry = spherical;
+        lesserNeighbors = [];
+
+        % Performance
+        performance = 0;
 
         % Plotting
         scatterPoints;
+        plotCommsGeometry = true;
+    end
+
+    properties (SetAccess = private, GetAccess = public)
+        initialStepSize = NaN;
+        stepDecayRate = NaN;
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
-        [obj] = run(obj, sensingObjective, domain, partitioning);
+        [obj] = initialize(obj, pos, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label);
+        [obj] = run(obj, domain, partitioning, t, index, agents);
+        [partitioning] = partition(obj, agents, objective)
         [obj, f] = plot(obj, ind, f);
         updatePlots(obj);
     end

@agent/initialize.m

@@ -1,33 +1,34 @@
-function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label)
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
         pos (1, 3) double;
-        vel (1, 3) double;
-        pan (1, 1) double;
-        tilt (1, 1) double;
         collisionGeometry (1, 1) {mustBeGeometry};
-        sensorModel (1, 1) {mustBeSensor}
-        guidanceModel (1, 1) {mustBeA(guidanceModel, 'function_handle')};
-        comRange (1, 1) double = NaN;
-        index (1, 1) double = NaN;
+        sensorModel (1, 1) {mustBeSensor};
+        comRange (1, 1) double;
+        maxIter (1, 1) double;
+        initialStepSize (1, 1) double = 0.2;
         label (1, 1) string = "";
+        plotCommsGeometry (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
     end
 
     obj.pos = pos;
-    obj.vel = vel;
-    obj.pan = pan;
-    obj.tilt = tilt;
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
-    obj.guidanceModel = guidanceModel;
-    obj.comRange = comRange;
-    obj.index = index;
     obj.label = label;
+    obj.plotCommsGeometry = plotCommsGeometry;
+    obj.initialStepSize = initialStepSize;
+    obj.stepDecayRate = obj.initialStepSize / maxIter;
+
+    % Initialize performance vector
+    obj.performance = [0, NaN(1, maxIter), 0];
+
+    % Add spherical geometry based on com range
+    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
     % Initialize FOV cone
     obj.fovGeometry = cone;
-    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], tan(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
+    obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:3)], tand(obj.sensorModel.alphaTilt) * obj.pos(3), obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
 end

@agent/partition.m

@@ -0,0 +1,35 @@
+function [partitioning] = partition(obj, agents, objective)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        agents (:, 1) {mustBeA(agents, 'cell')};
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+    end
+    arguments (Output)
+        partitioning (:, :) double;
+    end
+    
+    % Assess sensing performance of each agent at each sample point
+    % in the domain
+    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, [objective.X(:), objective.Y(:), zeros(size(objective.X(:)))]), size(objective.X)), agents, 'UniformOutput', false);
+    agentPerformances{end + 1} = objective.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
+    agentPerformances = cat(3, agentPerformances{:});
+
+    % Get highest performance value at each point
+    [~, idx] = max(agentPerformances, [], 3);
+
+    % Collect agent indices in the same way as performance
+    indices = 1:size(agents, 1);
+    agentInds = squeeze(tensorprod(indices, ones(size(objective.X))));
+    if size(agentInds, 1) ~= size(agents, 1)
+        agentInds = reshape(agentInds, [size(agents, 1), size(agentInds)]); % needed for cases with 1 agent where prior squeeze is too agressive
+    end
+    agentInds = num2cell(agentInds, 2:3);
+    agentInds = cellfun(@(x) squeeze(x), agentInds, 'UniformOutput', false);
+    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
+    agentInds = cat(3, agentInds{:});
+
+    % Use highest performing agent's index to form partitions
+    [m, n, ~] = size(agentInds);
+    [jj, kk] = ndgrid(1:m, 1:n);
+    partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
+end

@agent/plot.m

@@ -30,6 +30,12 @@ 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);
+    maxAlt = f.Children(1).Children(end).ZLim(2); % to avoid scaling the FOV geometry as the sim runs, let's just make it really big and hide the excess under the floor of the domain. Check the domain altitude to figure out how big it needs to be to achieve this deception.
+    [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f, maxAlt);
 end

@agent/run.m

@@ -1,28 +1,92 @@
-function obj = run(obj, sensingObjective, domain, partitioning)
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'agent')};
-        sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
         domain (1, 1) {mustBeGeometry};
         partitioning (:, :) double;
+        timestepIndex (1, 1) double;
+        index (1, 1) double;
+        agents (:, 1) {mustBeA(agents, 'cell')};
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'agent')};
     end
 
-    % Do sensing
-    [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
+    % Collect objective function values across partition
+    partitionMask = partitioning == index;
+    if ~unique(partitionMask)
+        % This agent has no partition, maintain current state
+        return;
+    end
+    objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
 
-    % Determine next planned position
-    nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
+    % Compute sensor performance on partition
+    maskedX = domain.objective.X(partitionMask);
+    maskedY = domain.objective.Y(partitionMask);
+
+    % Compute agent performance at the current position and each delta position +/- X, Y, Z
+    delta = domain.objective.discretizationStep; % smallest possible step size that gets different results
+    deltaApplicator = [0, 0, 0; 1, 0, 0; -1, 0, 0; 0, 1, 0; 0, -1, 0; 0, 0, 1; 0, 0, -1]; % none, +X, -X, +Y, -Y, +Z, -Z
+    C_delta = NaN(7, 1); % agent performance at delta steps in each direction
+    for ii = 1:7
+        % Apply delta to position
+        pos = obj.pos + delta * deltaApplicator(ii, 1:3);
+        
+        % Compute performance values on partition
+        if ii < 5
+            % Compute sensing performance
+            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
+            % Objective performance does not change for 0, +/- X, Y steps.
+            % Those values are computed once before the loop and are only
+            % recomputed when +/- Z steps are applied
+        else
+            % Redo partitioning for Z stepping only
+            partitioning = obj.partition(agents, domain.objective);
+
+            % Recompute partiton-derived performance values for objective
+            partitionMask = partitioning == index;
+            objectiveValues = domain.objective.values(partitionMask); % f(omega) on W_n
+
+            % Recompute partiton-derived performance values for sensing
+            maskedX = domain.objective.X(partitionMask);
+            maskedY = domain.objective.Y(partitionMask);
+            sensorValues = obj.sensorModel.sensorPerformance(pos, [maskedX, maskedY, zeros(size(maskedX))]); % S_n(omega, P_n) on W_n
+        end
+
+        % Rearrange data into image arrays
+        F = NaN(size(partitionMask));
+        F(partitionMask) = objectiveValues;
+        S = NaN(size(partitionMask));
+        S(partitionMask) = sensorValues;
+
+        % Compute agent performance
+        C = S .* F;
+        C_delta(ii) = sum(C(~isnan(C)));
+    end
+
+    % Store agent performance at current time and place
+    obj.performance(timestepIndex + 1) = C_delta(1);
+
+    % Compute gradient by finite central differences
+    gradC = [(C_delta(2)-C_delta(3))/(2*delta), (C_delta(4)-C_delta(5))/(2*delta), (C_delta(6)-C_delta(7))/(2*delta)];
+
+    % Compute scaling factor
+    targetRate = obj.initialStepSize - obj.stepDecayRate * timestepIndex; % slow down as you get closer
+    rateFactor = targetRate / norm(gradC);
+
+    % Compute unconstrained next position
+    pNext = obj.pos + rateFactor * gradC;
 
     % Move to next position
-    % (dynamics not modeled at this time)
     obj.lastPos = obj.pos;
-    obj.pos = nextPos;
-
-    % Calculate movement
-    d = obj.pos - obj.collisionGeometry.center;
+    obj.pos = pNext;
 
     % Reinitialize collision geometry in the new position
+    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
 end

@agent/updatePlots.m

@@ -5,6 +5,13 @@ function updatePlots(obj)
     arguments (Output)
     end
 
+    % Find change in agent position since last timestep
+    deltaPos = obj.pos - obj.lastPos;
+    if all(isnan(deltaPos)) || all(deltaPos == zeros(1, 3))
+        % Agent did not move, so nothing has to move on the plots
+        return;
+    end
+
     % Scatterplot point positions
     for ii = 1:size(obj.scatterPoints, 1)
         obj.scatterPoints(ii).XData = obj.pos(1);
@@ -12,9 +19,6 @@ function updatePlots(obj)
         obj.scatterPoints(ii).ZData = obj.pos(3);
     end
 
-    % Find change in agent position since last timestep
-    deltaPos = obj.pos - obj.lastPos;
-
     % Collision geometry edges
     for jj = 1:size(obj.collisionGeometry.lines, 2)
         % Update plotting
@@ -25,9 +29,21 @@ 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
+        % obj.fovGeometry = obj.fovGeometry.plot(obj.spatialPlotIndices)
         obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
         obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
         obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);

@miSim/constrainMotion.m

@@ -0,0 +1,154 @@
+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))';
+    if all(isnan(v), 'all') || all(v == zeros(size(obj.agents, 1), 3), 'all')
+        % Agents are not attempting to move, so there is no motion to be
+        % constrained
+        return;
+    end
+
+    % 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)^obj.barrierExponent;
+            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)^obj.barrierExponent;
+           
+            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^obj.barrierExponent;
+        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^obj.barrierExponent;
+        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^obj.barrierExponent;
+        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^obj.barrierExponent;
+        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^obj.barrierExponent;
+        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^obj.barrierExponent;
+        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)^obj.barrierExponent;
+
+                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,49 +1,97 @@
-function obj = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
+function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
         domain (1, 1) {mustBeGeometry};
-        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
         agents (:, 1) cell;
+        barrierGain (1, 1) double = 100;
+        barrierExponent (1, 1) double = 3;
+        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;
+
+    % Generate artifact(s) name
+    obj.artifactName = strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'));
+
     % Define simulation time parameters
     obj.timestep = timestep;
+    obj.timestepIndex = 0;
     obj.maxIter = maxIter - 1;
 
     % Define domain
     obj.domain = domain;
-    obj.partitioningFreq = partitoningFreq;
 
     % Add geometries representing obstacles within the domain
     obj.obstacles = obstacles;
 
-    % Define objective
-    obj.objective = objective;
+    % Add an additional obstacle spanning the domain's footprint to 
+    % represent the minimum allowable altitude
+    if 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), minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+    end
 
     % Define agents
     obj.agents = agents;
+    obj.constraintAdjacencyMatrix = logical(eye(size(agents, 1)));
 
-    % Compute adjacency matrix
+    % 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
+
+    % Set CBF parameters
+    obj.barrierGain = barrierGain;
+    obj.barrierExponent = barrierExponent;
+
+    % Compute adjacency matrix and lesser neighbors
     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));
+
     % Create initial partitioning
-    obj = obj.partition();
+    obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+
+    % Initialize variable that will store agent positions for trail plots
+    obj.posHist = NaN(size(obj.agents, 1), obj.maxIter + 1, 3);
+    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();
+
+    % Run validations
+    obj.validate();
 end

@miSim/lesserNeighbor.m

@@ -0,0 +1,76 @@
+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,51 +4,71 @@ classdef miSim
     % Simulation parameters
     properties (SetAccess = private, GetAccess = public)
         timestep = NaN; % delta time interval for simulation iterations
-        partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
+        timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
         domain = rectangularPrism;
         objective = sensingObjective;
         obstacles = cell(0, 1); % geometries that define obstacles within the domain
         agents = cell(0, 1); % agents that move within the domain
         adjacency = NaN; % Adjacency matrix representing communications network graph
-        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
+        constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
         partitioning = NaN;
-        performance = NaN; % current cumulative sensor performance
+        perf; % sensor performance timeseries array
+        performance = 0; % simulation performance timeseries vector
+        barrierGain = 100; % CBF gain parameter
+        barrierExponent = 3; % CBF exponent parameter
+        artifactName = "";
+        fPerf; % performance plot figure
     end
 
     properties (Access = private)
         % Sim
         t = NaN; % current sim time
-        perf; % sensor performance timeseries array
         times;
         partitioningTimes;
 
         % Plot objects
-        f = firstPlotSetup(); % main plotting tiled layout figure
+        makePlots = true; % enable/disable simulation plotting (performance implications)
+        makeVideo = true; % enable/disable VideoWriter (performance implications)
+        f; % 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
-
-        fPerf; % performance plot figure
         performancePlot; % objects for sensor performance plot
 
+        posHist; % data for trail plot
+        trailPlot; % objects for agent trail 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] = initialize(obj, domain, agents, barrierGain, barrierExponent, minAlt, timestep, maxIter, obstacles, makePlots, makeVideo);
         [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] = updatePlots(obj, updatePartitions);
+        [obj] = plotTrails(obj);
+        [obj] = plotH(obj);
+        [obj] = updatePlots(obj);
+        validate(obj);
+        teardown(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/partition.m

@@ -1,36 +0,0 @@
-function obj = partition(obj)
-    arguments (Input)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-    arguments (Output)
-        obj (1, 1) {mustBeA(obj, 'miSim')};
-    end
-
-    % Assess sensing performance of each agent at each sample point
-    % in the domain
-    agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-    agentPerformances{end + 1} = obj.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
-    agentPerformances = cat(3, agentPerformances{:});
-    
-    % Get highest performance value at each point
-    [~, idx] = max(agentPerformances, [], 3);
-
-    % Collect agent indices in the same way as performance
-    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{:});
-
-    % Get highest performing agent's index
-    [m,n,~] = size(agentInds);
-    [jj,kk] = ndgrid(1:m, 1:n);
-    obj.partitioning = agentInds(sub2ind(size(agentInds), jj, kk, idx));
-
-    % Get individual agent sensor performance
-    nowIdx = [0; obj.partitioningTimes] == obj.t;
-    for ii = 1:size(obj.agents, 1)
-        obj.perf(ii, nowIdx) = sum(agentPerformances(sub2ind(size(agentInds), jj, kk, ii)), 'all');
-    end
-
-    % Current total performance
-    obj.perf(end, nowIdx) = sum(obj.perf(1:(end - 1), nowIdx));
-end

@miSim/plot.m

@@ -6,6 +6,11 @@ function obj = plot(obj)
         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);
 
@@ -17,7 +22,7 @@ function obj = plot(obj)
     % Plot objective gradient
     obj.f = obj.domain.objective.plot(obj.objectivePlotIndices, obj.f);
 
-    % Plot agents and their collision geometries
+    % Plot agents and their collision/communications geometries
     for ii = 1:size(obj.agents, 1)
         [obj.agents{ii}, obj.f] = obj.agents{ii}.plot(obj.spatialPlotIndices, obj.f);
     end
@@ -31,6 +36,9 @@ 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)]);
@@ -40,4 +48,10 @@ function obj = plot(obj)
 
     % Plot performance
     obj = obj.plotPerformance();
+
+    % Plot h functions
+    obj = obj.plotH();
+
+    % Switch back to primary figure
+    figure(obj.f);
 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.adjacency, 1)
+    for ii = 2:size(obj.constraintAdjacencyMatrix, 1)
         for jj = 1:(ii - 1)
-            if obj.adjacency(ii, jj)
+            if obj.constraintAdjacencyMatrix(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.adjacency, 'omitselfloops');
+    G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
 
     % Plot graph object
     if isnan(obj.networkGraphIndex)

@miSim/plotH.m

@@ -0,0 +1,61 @@
+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,6 +6,13 @@ 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)');
@@ -15,14 +22,24 @@ 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 = ["Total"; agentStrings];
+    legend(obj.fPerf.Children(1), agentStrings, 'Location', 'northwest');
+
     obj.performancePlot = o;
 end

@miSim/plotTrails.m

@@ -0,0 +1,26 @@
+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,37 +7,60 @@ function [obj] = run(obj)
     end
 
     % Start video writer
+    if obj.makeVideo
         v = obj.setupVideoWriter();
         v.open();
+    end
     
     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);
 
-        % Check if it's time for new partitions
-        updatePartitions = false;
-        if ismember(obj.t, obj.partitioningTimes)
-            updatePartitions = true;
-            obj = obj.partition();
+        % Before moving
+        % Validate current simulation configuration
+        obj.validate();
+
+        % Update partitioning before moving (this one is strictly for
+        % plotting purposes, the real partitioning is done by the agents)
+        obj.partitioning = obj.agents{1}.partition(obj.agents, obj.domain.objective);
+
+        % Determine desired communications links
+        obj = obj.lesserNeighbor();
+
+        % Moving
+        % Iterate over agents to simulate their unconstrained motion
+        for jj = 1:size(obj.agents, 1)
+            obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.timestepIndex, jj, obj.agents);
         end
 
-        % Iterate over agents to simulate their motion
-        for jj = 1:size(obj.agents, 1)
-            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);
+
+        % After moving
+        % Update agent position history array
+        obj.posHist(1:size(obj.agents, 1), obj.timestepIndex + 1, 1:3) = reshape(cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false)), size(obj.agents, 1), 1, 3);
+
+        % Update total performance
+        obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(obj.timestepIndex+1), obj.agents))];
 
         % Update adjacency matrix
         obj = obj.updateAdjacency();
 
         % Update plots
-        obj = obj.updatePlots(updatePartitions);
+        obj = obj.updatePlots();
 
         % Write frame in to video
+        if obj.makeVideo
             I = getframe(obj.f);
             v.writeVideo(I);
         end
+    end
 
+    if obj.makeVideo
         % Close video file
         v.close();
+    end
 end

@miSim/setupVideoWriter.m

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

@miSim/teardown.m

@@ -0,0 +1,13 @@
+function teardown(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    % Close plots
+    close(obj.hf);
+    close(obj.fPerf);
+    close(obj.f);
+
+end

@miSim/updateAdjacency.m

@@ -7,26 +7,18 @@ function obj = updateAdjacency(obj)
     end
 
     % Initialize assuming only self-connections
-    A = logical(eye(size(obj.agents, 1)));
+    A = true(size(obj.agents, 1));
 
     % Check lower triangle off-diagonal connections
     for ii = 2:size(A, 1)
         for jj = 1:(ii - 1)
-            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
+            % 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;
             end
         end
     end
 
-    obj.adjacency = A | A';
+    obj.adjacency = A & A';
 end

@miSim/updatePlots.m

@@ -1,19 +1,23 @@
-function [obj] = updatePlots(obj, updatePartitions)
+function [obj] = updatePlots(obj)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'miSim')};
-        updatePartitions (1, 1) logical = false;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    % Update agent positions, collision geometries
+    % Fast exit when plotting is disabled
+    if ~obj.makePlots
+        return;
+    end
+
+    % Update agent positions, collision/communication geometries
     for ii = 1:size(obj.agents, 1)
         obj.agents{ii}.updatePlots();
     end
 
-    % The remaining updates might be possible to do in a clever way
-    % that moves existing lines instead of clearing and 
+    % The remaining updates might should all 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
@@ -25,10 +29,8 @@ function [obj] = updatePlots(obj, updatePartitions)
     obj = obj.plotGraph();
 
     % Update partitioning plot
-    if updatePartitions
     delete(obj.partitionPlot);
     obj = obj.plotPartitions();
-    end
 
     % reset plot limits to fit domain
     for ii = 1:size(obj.spatialPlotIndices, 2)
@@ -36,17 +38,34 @@ 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
-    if updatePartitions
-        nowIdx = [0; obj.partitioningTimes] == obj.t;
-        % set(obj.performancePlot(1), 'YData', obj.perf(end, 1:find(nowIdx)));
-        obj.performancePlot(1).YData(nowIdx) = obj.perf(end, nowIdx);
-        for ii = 2:size(obj.performancePlot, 1)
-            obj.performancePlot(ii).YData(nowIdx) = obj.perf(ii, nowIdx);
-        end
-        drawnow;
+    % Re-normalize performance plot
+    normalizingFactor = 1/max(obj.performance);
+    obj.performancePlot(1).YData(1:(length(obj.performance) + 1)) = [obj.performance, 0] * normalizingFactor;
+    obj.performancePlot(1).XData([obj.timestepIndex, obj.timestepIndex + 1]) = [obj.t, obj.t + obj.timestep];
+    for ii = 1:(size(obj.agents, 1))
+        obj.performancePlot(ii + 1).YData(1:(length(obj.performance) + 1)) = [obj.agents{ii}.performance(1:length(obj.performance)), 0] * normalizingFactor;
+        obj.performancePlot(ii + 1).XData([obj.timestepIndex, obj.timestepIndex + 1]) = [obj.t, obj.t + obj.timestep];
     end
 
+    % 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);
+    end
 end

@miSim/validate.m

@@ -0,0 +1,27 @@
+function validate(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    %% Communications Network Validators
+    if max(conncomp(graph(obj.adjacency))) ~= 1
+        warning("Network is not connected");
+    end
+
+    if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
+        warning("Eliminated network connections that were necessary");
+    end
+
+    %% Obstacle Validators
+    AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, 'UniformOutput', false);
+    AO_collisions = vertcat(AO_collisions{:});
+    if any(AO_collisions)
+        [idx, idy] = find(AO_collisions);
+        for ii = 1:size(idx, 1)
+            error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii));
+        end
+    end
+
+end

@miSim/writeParams.m

@@ -0,0 +1,25 @@
+function writeParams(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    % Collect agent parameters
+    collisionRadii = cellfun(@(x) x.collisionGeometry.radius, obj.agents);
+    alphaDist = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    betaDist = cellfun(@(x) x.sensorModel.betaDist, obj.agents);
+    alphaTilt = cellfun(@(x) x.sensorModel.alphaTilt, obj.agents);
+    betaTilt = cellfun(@(x) x.sensorModel.alphaDist, obj.agents);
+    comRange = cellfun(@(x) x.commsGeometry.radius, obj.agents);
+
+    % Combine with simulation parameters
+    params = struct('timestep', obj.timestep, 'maxIter', obj.maxIter, 'minAlt', obj.obstacles{end}.maxCorner(3), 'discretizationStep', obj.domain.objective.discretizationStep, ...
+                    'collisionRadius', collisionRadii, 'alphaDist', alphaDist, 'betaDist', betaDist, ...
+                    'alphaTilt', alphaTilt, 'betaTilt', betaTilt, 'comRange', comRange);
+
+    % Save all parameters to output file
+    paramsFile = strcat(obj.artifactName, "_miSimParams");
+    paramsFile = fullfile(matlab.project.rootProject().RootFolder, 'sandbox', paramsFile);
+    save(paramsFile, "-struct", "params");
+end

@sensingObjective/initialize.m

@@ -1,15 +1,20 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
     arguments (Input)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
+        sensorPerformanceMinimum (1, 1) double = 1e-6;
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
     end
 
+    obj.discretizationStep = discretizationStep;
+
+    obj.sensorPerformanceMinimum = sensorPerformanceMinimum;
+
     obj.groundAlt = domain.minCorner(3);
     obj.protectedRange = protectedRange;
 
@@ -19,8 +24,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     yMin = min(domain.footprint(:, 2));
     yMax = max(domain.footprint(:, 2));
 
-    xGrid = unique([xMin:discretizationStep:xMax, xMax]);
-    yGrid = unique([yMin:discretizationStep:yMax, yMax]);
+    xGrid = unique([xMin:obj.discretizationStep:xMax, xMax]);
+    yGrid = unique([yMin:obj.discretizationStep:yMax, yMax]);
     
     % Store grid points for plotting later
     [obj.X, obj.Y] = meshgrid(xGrid, yGrid);
@@ -29,9 +34,13 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
     obj.objectiveFunction = objectiveFunction;
     obj.values = reshape(obj.objectiveFunction(obj.X, obj.Y), size(obj.X));
     
+    % Normalize
+    obj.values = obj.values ./ max(obj.values, [], "all");
+
     % store ground position
-    idx = obj.values == max(obj.values, [], "all");
+    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

@sensingObjective/sensingObjective.m

@@ -2,18 +2,19 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundAlt = 0;
-        groundPos = [0, 0];
-        discretizationStep = 1;
-        objectiveFunction = @(x, y) 0; % define objective functions over a grid in this manner
+        groundAlt = NaN;
+        groundPos = [NaN, NaN];
+        discretizationStep = NaN;
+        objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
         X = [];
         Y = [];
         values = [];
-        protectedRange = 1; % keep obstacles from crowding objective
+        protectedRange = NaN; % keep obstacles from crowding objective
+        sensorPerformanceMinimum = NaN; % minimum sensor performance to allow assignment of a point in the domain to a partition
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange);
+        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum);
         [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end

@sigmoidSensor/initialize.m

@@ -1,10 +1,8 @@
-function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt)
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
         alphaDist (1, 1) double;
         betaDist (1, 1) double;
-        alphaPan (1, 1) double;
-        betaPan (1, 1) double;
         alphaTilt (1, 1) double;
         betaTilt (1, 1) double;
     end
@@ -14,8 +12,6 @@ function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt
     
     obj.alphaDist = alphaDist;
     obj.betaDist = betaDist;
-    obj.alphaPan = alphaPan;
-    obj.betaPan = betaPan;
     obj.alphaTilt = alphaTilt;
     obj.betaTilt = betaTilt;
 end

@sigmoidSensor/plotParameters.m

@@ -12,7 +12,7 @@ function f = plotParameters(obj)
 
     % Sample membership functions
     d_x = obj.distanceMembership(d);
-    t_x = obj.tiltMembership(deg2rad(t));
+    t_x = obj.tiltMembership(t);
 
     % Plot resultant sigmoid curves
     f = figure;

@sigmoidSensor/sensorPerformance.m

@@ -1,18 +1,19 @@
-function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos)
+function value = sensorPerformance(obj, agentPos, targetPos)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
         agentPos (1, 3) double;
-        agentPan (1, 1) double;
-        agentTilt (1, 1) double;
         targetPos (:, 3) double;
     end
     arguments (Output)
         value (:, 1) double;
     end
 
+    % compute direct distance and distance projected onto the ground
     d = vecnorm(agentPos - targetPos, 2, 2); % distance from sensor to target
     x = vecnorm(agentPos(1:2) - targetPos(:, 1:2), 2, 2); % distance from sensor nadir to target nadir (i.e. distance ignoring height difference)
-    tiltAngle = atan2(targetPos(:, 3) - agentPos(3), x) - agentTilt;
+
+    % compute tilt angle
+    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))); % degrees
 
     % Membership functions
     mu_d = obj.distanceMembership(d);

@sigmoidSensor/sigmoidSensor.m

@@ -3,15 +3,12 @@ classdef sigmoidSensor
         % Sensor parameters
         alphaDist = NaN;
         betaDist = NaN;
-        alphaPan = NaN;
-        betaPan = NaN;
-        alphaTilt = NaN;
+        alphaTilt = NaN; % degrees
         betaTilt = NaN;
     end
 
     methods (Access = public)
-        [obj]               = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt);
-        [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning);
+        [obj]               = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt);
         [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
         [f] = plotParameters(obj);
     end

@sigmoidSensor/tiltMembership.m

@@ -1,7 +1,7 @@
 function x = tiltMembership(obj, t)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
-        t (:, 1) double;
+        t (:, 1) double; % degrees
     end
     arguments (Output)
         x (:, 1) double;

geometries/@cone/cone.m

@@ -17,6 +17,6 @@ classdef cone
 
     methods (Access = public)
         [obj   ] = initialize(obj, center, radius, height, tag, label);
-        [obj, f] = plot(obj, ind, f);
+        [obj, f] = plot(obj, ind, f, maxAlt);
     end
 end

geometries/@cone/plot.m

@@ -1,8 +1,9 @@
-function [obj, f] = plot(obj, ind, f)
+function [obj, f] = plot(obj, ind, f, maxAlt)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'cone')};
         ind (1, :) double = NaN;
         f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+        maxAlt (1, 1) = 10;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'cone')};
@@ -12,16 +13,18 @@ function [obj, f] = plot(obj, ind, f)
     % Create axes if they don't already exist
     f = firstPlotSetup(f);
     
+    scalingFactor = (maxAlt / obj.height);
+
     % Plot cone
-    [X, Y, Z] = cylinder([obj.radius, 0], obj.n);
+    [X, Y, Z] = cylinder([scalingFactor * obj.radius, 0], obj.n);
     
     % Scale to match height
-    Z = Z * obj.height;
+    Z = Z * maxAlt;
     
     % Move to center location
     X = X + obj.center(1);
     Y = Y + obj.center(2);
-    Z = Z + obj.center(3);
+    Z = Z + obj.center(3) - maxAlt;
     
     % Plot
     if isnan(ind)

geometries/@rectangularPrism/closestToPoint.m

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

@@ -10,32 +10,37 @@ function c = containsLine(obj, pos1, pos2)
     
     d = pos2 - pos1;
 
-    % 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
+    % endpoint contained (trivial case)
     if obj.contains(pos1) || obj.contains(pos2)
         c = true;
-        else
-            c = false;
-        end
         return;
     end
 
-    tmin = -inf;        
-    tmax = inf;
-    
-    % Standard case
+    % parameterize the line segment to check for an intersection
+    tMin = 0;
+    tMax = 1;
     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) - pos2(ii)) / d(ii);
-        tmin = max(tmin, min(t1, t2));
-        tmax = min(tmax, max(t1, t2));
-        if tmin > tmax
+            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
         end
-    
-    c = (tmax >= 0) && (tmin <= 1);
     end
+    c = true;
+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

@@ -0,0 +1,42 @@
+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,6 +24,10 @@ 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);

geometries/@rectangularPrism/initializeRandom.m

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

geometries/@rectangularPrism/rectangularPrism.m

@@ -3,7 +3,6 @@ classdef rectangularPrism
     properties (SetAccess = private, GetAccess = public)
         % Meta
         tag = REGION_TYPE.INVALID;
-        label = "";
 
         % Spatial
         minCorner = NaN(1, 3);
@@ -11,6 +10,7 @@ classdef rectangularPrism
         dimensions = NaN(1, 3);
         center = NaN;
         footprint = NaN(4, 2);
+        radius = NaN; % fake radius
 
         % Graph
         vertices = NaN(8, 3);
@@ -22,6 +22,7 @@ classdef rectangularPrism
         lines;
     end
     properties (SetAccess = public, GetAccess = public)
+        label = "";
         % Sensing objective (for DOMAIN region type only)
         objective;
     end
@@ -31,7 +32,9 @@ 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

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

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

@@ -0,0 +1,37 @@
+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;
+
+    % 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

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

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

@@ -0,0 +1,33 @@
+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;
+    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,6 +9,7 @@ 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

@@ -1,26 +0,0 @@
-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/9I1NhbvmbTiuVshItfSCHZiDxxI/F1_77bY6s29xV3hf65cyjVDQctIp.xml

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

resources/project/9I1NhbvmbTiuVshItfSCHZiDxxI/l6NIqYudBCIzq4wYJm1naLreh70p.xml

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

resources/project/E2mMq2X73DyjKhlQAouGqrsyLgg/QQkomTANNGDauE43PAdpZJlxaiQp.xml

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

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

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

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

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/XCp_6IVryeT94420M1370QjM5u4p.xml

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

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/jMxqi1-UdFyFMXqowBRZrZ8JeFEd.xml

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

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/jMxqi1-UdFyFMXqowBRZrZ8JeFEp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/OJQQt9oLZ-3u4fU3OVlfDGdcdxcp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/QXWY7O9zl20hsQdVz8SSSmnWtfcp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/Zm6-Pu5TuNDu7Nb1xPKdbRS1mBIp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/rTZ-bC86s4c3irmoSemsIqFlUBUp.xml

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

resources/project/HVl1IMKCtbARK0HKB9fPxObVDd8/scbJcgEZnU7v24KBE-JNEEeTzakp.xml

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

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/2JKMt-jJFCbC-SMT4nST0in4PQEp.xml

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

resources/project/LMc5a8ETDcRip3rYsQxn56S6obM/PPOY66CYBPYXvf5ZNLF4sLYz8psp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/HVl1IMKCtbARK0HKB9fPxObVDd8p.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/BtZvZtLyHugR7nfGjg6ukY77EG0p.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/OYUaWUmC6jZNQaYWobzy5I-iESkp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/_tVnHInN54-Fys1puCBYQ1j4NwUp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/c0mYkONNU4mIjcCgfDyvbU49RuMp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/hyvGevruV7eLIdxFdEJJT6QLP1Yp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/lJsL2R-bNQunxq-01eTS9mZYaLsp.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/uMGDwEZ0yJ74zW0K0aO7UX4-C-Yd.xml

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

resources/project/SIL3u_W39LwE7HHYsarfFmr9gVQ/uMGDwEZ0yJ74zW0K0aO7UX4-C-Yp.xml

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

resources/project/hjJloWDVwJVRCwGO0y5r9iO7IDk/PR5LYpNCHNsKl-th1ypVJR7QPBAd.xml

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

resources/project/o6vRLbVRBKxltajotIPvuRfoO58/GguIqNeeBWU5xZrynPO-wpFYSAEd.xml

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

resources/project/o6vRLbVRBKxltajotIPvuRfoO58/HXew_loNxIfLeyeZuyA5EgMo6NMd.xml

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