test case cleanup

.gitignore

@@ -40,4 +40,11 @@ codegen/
 *.sbproj.bak
 
 # Sandbox contents
-sandbox/*
+sandbox/*
+
+# Videos
+*.mp4
+*.avi
+
+# Figures
+*.fig

@agent/agent.m

@@ -0,0 +1,43 @@
+classdef agent
+    properties (SetAccess = public, GetAccess = public)
+        % Identifiers
+        label = "";
+
+        % State
+        lastPos = NaN(1, 3); % position from previous timestep
+        pos = NaN(1, 3); % current position
+
+        % Sensor
+        sensorModel;
+
+        % Collision
+        collisionGeometry;
+
+        % FOV cone
+        fovGeometry;
+
+        % Communication
+        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, 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
+end

@agent/initialize.m

@@ -0,0 +1,34 @@
+function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, maxIter, initialStepSize, label, plotCommsGeometry)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        pos (1, 3) double;
+        collisionGeometry (1, 1) {mustBeGeometry};
+        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.collisionGeometry = collisionGeometry;
+    obj.sensorModel = sensorModel;
+    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: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

@@ -0,0 +1,41 @@
+function [obj, f] = plot(obj, ind, f)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+
+    % Plot points representing the agent position
+    hold(f.Children(1).Children(end), "on");
+    o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
+    hold(f.Children(1).Children(end), "off");
+
+    % Check if this is a tiled layout figure
+    if strcmp(f.Children(1).Type, 'tiledlayout')
+        % Add to other perspectives
+        o = [o; copyobj(o(1), f.Children(1).Children(2))];
+        o = [o; copyobj(o(1), f.Children(1).Children(3))];
+        o = [o; copyobj(o(1), f.Children(1).Children(4))];
+    end
+
+    obj.scatterPoints = o;
+
+    % 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
+    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

@@ -0,0 +1,92 @@
+function obj = run(obj, domain, partitioning, timestepIndex, index, agents)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+        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
+
+    % 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
+
+    % 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
+    obj.lastPos = obj.pos;
+    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

@@ -0,0 +1,51 @@
+function updatePlots(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'agent')};
+    end
+    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);
+        obj.scatterPoints(ii).YData = obj.pos(2);
+        obj.scatterPoints(ii).ZData = obj.pos(3);
+    end
+
+    % Collision geometry edges
+    for jj = 1:size(obj.collisionGeometry.lines, 2)
+        % Update plotting
+        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
+
+    % 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);
+    end
+end

@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

@@ -0,0 +1,97 @@
+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};
+        agents (:, 1) cell;
+        barrierGain (1, 1) double = 100;
+        barrierExponent (1, 1) double = 3;
+        minAlt (1, 1) double = 1;
+        timestep (:, 1) double = 0.05;
+        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;
+
+    % 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
+    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)));
+
+    % 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)';
+
+    % Prepare performance data store (at t = 0, all have 0 performance)
+    obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
+
+    % 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.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

@@ -0,0 +1,76 @@
+classdef miSim
+    % multiagent interconnection simulation
+
+    % 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)
+        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
+        constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
+        partitioning = NaN;
+        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
+        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
+        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
+
+        % 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, 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] = plotTrails(obj);
+        [obj] = plotH(obj);
+        [obj] = updatePlots(obj);
+        validate(obj);
+        teardown(obj);
+    end
+    methods (Access = private)
+        [v] = setupVideoWriter(obj);
+    end
+end

@miSim/plot.m

@@ -0,0 +1,57 @@
+function obj = plot(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 domain
+    [obj.domain, obj.f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
+
+    % Plot obstacles
+    for ii = 1:size(obj.obstacles, 1)
+        [obj.obstacles{ii}, obj.f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, obj.f);
+    end
+
+    % Plot objective gradient
+    obj.f = obj.domain.objective.plot(obj.objectivePlotIndices, obj.f);
+
+    % 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
+
+    % Plot communication links
+    obj = obj.plotConnections();
+
+    % Plot abstract network graph
+    obj = obj.plotGraph();
+
+    % 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)]);
+        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
+
+    % Plot performance
+    obj = obj.plotPerformance();
+
+    % Plot h functions
+    obj = obj.plotH();
+
+    % Switch back to primary figure
+    figure(obj.f);
+end

@miSim/plotConnections.m

@@ -0,0 +1,40 @@
+function obj = plotConnections(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+
+    % 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 jj = 1:(ii - 1)
+            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)];
+            end
+        end
+    end
+    X = X'; Y = Y'; Z = Z';
+
+    % Plot the connections
+    if isnan(obj.spatialPlotIndices)
+        hold(obj.f.CurrentAxes, "on");
+        o = plot3(obj.f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        hold(obj.f.CurrentAxes, "off");
+    else
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "on");
+        o = plot3(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+        hold(obj.f.Children(1).Children(obj.spatialPlotIndices(1)), "off");
+    end
+
+    % Copy to other plots
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
+    end
+
+    obj.connectionsPlot = o;
+end

@miSim/plotGraph.m

@@ -0,0 +1,28 @@
+function obj = plotGraph(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+
+    % Form graph from adjacency matrix
+    G = graph(obj.constraintAdjacencyMatrix, 'omitselfloops');
+
+    % Plot graph object
+    if isnan(obj.networkGraphIndex)
+        hold(obj.f.CurrentAxes, 'on');
+        o = plot(obj.f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(obj.f.CurrentAxes, 'off');
+    else
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'on');
+        o = plot(obj.f.Children(1).Children(obj.networkGraphIndex(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+        hold(obj.f.Children(1).Children(obj.networkGraphIndex(1)), 'off');
+        if size(obj.networkGraphIndex, 2) > 1
+            for ii = 2:size(ind, 2)
+                o = [o; copyobj(o(1), obj.f.Children(1).Children(obj.networkGraphIndex(ii)))];
+            end
+        end
+    end
+    obj.graphPlot = o;
+end

@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/plotPartitions.m

@@ -0,0 +1,24 @@
+function obj = plotPartitions(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+
+    if isnan(obj.partitionGraphIndex)
+        hold(obj.f.CurrentAxes, 'on');
+        o = imagesc(obj.f.CurrentAxes, obj.partitioning);
+        hold(obj.f.CurrentAxes, 'off');
+    else
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        o = imagesc(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), obj.partitioning);
+        hold(obj.f.Children(1).Children(obj.partitionGraphIndex(1)), 'on');
+        if size(obj.partitionGraphIndex, 2) > 1
+            for ii = 2:size(ind, 2)
+                o = [o, copyobj(o(1), obj.f.Children(1).Children(obj.partitionGraphIndex(ii)))];
+            end
+        end
+    end
+    obj.partitionPlot = o;
+end

@miSim/plotPerformance.m

@@ -0,0 +1,45 @@
+function obj = plotPerformance(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
+    
+    obj.fPerf = figure;
+
+    axes(obj.fPerf);
+    title(obj.fPerf.Children(1), "Sensor Performance");
+    xlabel(obj.fPerf.Children(1), 'Time (s)');
+    ylabel(obj.fPerf.Children(1), 'Sensor Performance');
+    grid(obj.fPerf.Children(1), 'on');
+
+    % 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,30 @@
+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 to other plots
+    for ii = 2:size(obj.spatialPlotIndices, 2)
+        o = [o, copyobj(o(:, 1), obj.f.Children(1).Children(obj.spatialPlotIndices(ii)))];
+    end 
+
+    % Add legend?
+
+    obj.trailPlot = o;
+end

@miSim/run.m

@@ -0,0 +1,66 @@
+function [obj] = run(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    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);
+
+        % 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
+
+        % 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();
+
+        % 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

@@ -0,0 +1,17 @@
+function v = setupVideoWriter(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        v (1, 1) {mustBeA(v, 'VideoWriter')};
+    end
+
+    if ispc || ismac
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'MPEG-4');
+    elseif isunix
+        v = VideoWriter(fullfile(matlab.project.rootProject().RootFolder, 'sandbox', strcat(obj.artifactName, "_miSimHist")), 'Motion JPEG AVI');
+    end
+    
+    v.FrameRate = 1 / obj.timestep;
+    v.Quality = 90;
+end

@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

@@ -0,0 +1,24 @@
+function obj = updateAdjacency(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+
+    % Initialize assuming only self-connections
+    A = true(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;
+            end
+        end
+    end
+
+    obj.adjacency = A & A';
+end

@miSim/updatePlots.m

@@ -0,0 +1,73 @@
+function [obj] = updatePlots(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
+
+    % Update agent positions, collision/communication 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 
+    % re-plotting, which is much better for performance boost
+
+    % Update agent connections plot
+    delete(obj.connectionsPlot);
+    obj = obj.plotConnections();
+
+    % Update network graph plot
+    delete(obj.graphPlot);
+    obj = obj.plotGraph();
+
+    % Update partitioning plot
+    delete(obj.partitionPlot);
+    obj = obj.plotPartitions();
+
+    % reset plot limits to fit domain
+    for ii = 1:size(obj.spatialPlotIndices, 2)
+        xlim(obj.f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
+        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 jj = 1:size(obj.spatialPlotIndices, 2)
+        for ii = 1:size(obj.agents, 1)
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).XData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 1);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).YData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 2);
+            obj.trailPlot((jj - 1) * size(obj.agents, 1) + ii).ZData(obj.timestepIndex) = obj.posHist(ii, obj.timestepIndex, 3);
+        end
+    end
+
+    drawnow;
+    
+    % Update performance plot
+    % 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

@@ -0,0 +1,46 @@
+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;
+
+    % Extract footprint limits
+    xMin = min(domain.footprint(:, 1));
+    xMax = max(domain.footprint(:, 1));
+    yMin = min(domain.footprint(:, 2));
+    yMax = max(domain.footprint(:, 2));
+
+    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);
+
+    % Evaluate function over grid points
+    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 == 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/initializeRandomMvnpdf.m

@@ -0,0 +1,26 @@
+function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
+        domain (1, 1) {mustBeGeometry};
+        discretizationStep (1, 1) double = 1;
+        protectedRange (1, 1) double = 1;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'sensingObjective')};
+    end
+
+    % Set random objective position
+    mu = domain.minCorner;
+    while domain.distance(mu) < protectedRange
+        mu = domain.random();
+    end
+
+    % Set random distribution parameters
+    sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
+
+    % Set up random bivariate normal distribution function
+    objectiveFunction = objectiveFunctionWrapper(mu(1:2), sig);
+
+    % Regular initialization
+    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);
+end

@sensingObjective/plot.m

@@ -0,0 +1,35 @@
+function f = plot(obj, ind, f)
+    arguments (Input)
+        obj (1,1) {mustBeA(obj, 'sensingObjective')};
+        ind (1, :) double = NaN;
+        f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+    end
+    arguments (Output)
+        f (1,1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+
+    % Plot gradient on the "floor" of the domain
+    if isnan(ind)
+        hold(f.CurrentAxes, "on");
+        o = surf(f.CurrentAxes, obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+        o.HitTest = 'off';
+        o.PickableParts = 'none';
+        hold(f.CurrentAxes, "off");
+    else
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = surf(f.Children(1).Children(ind(1)), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+        o.HitTest = 'off';
+        o.PickableParts = 'none';
+        hold(f.Children(1).Children(ind(1)), "off");
+    end
+
+    % Add to other perspectives
+    if size(ind, 2) > 1
+        for ii = 2:size(ind, 2)
+            copyobj(o, f.Children(1).Children(ind(ii)));
+        end
+    end
+end

@sensingObjective/sensingObjective.m

@@ -0,0 +1,21 @@
+classdef sensingObjective
+    % Sensing objective definition parent class
+    properties (SetAccess = private, GetAccess = public)
+        label = "";
+        groundAlt = NaN;
+        groundPos = [NaN, NaN];
+        discretizationStep = NaN;
+        objectiveFunction = @(x, y) NaN; % define objective functions over a grid in this manner
+        X = [];
+        Y = [];
+        values = [];
+        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, sensorPerformanceMinimum);
+        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
+        [f  ] = plot(obj, ind, f);
+    end
+end

@sigmoidSensor/distanceMembership.m

@@ -0,0 +1,10 @@
+function x = distanceMembership(obj, d)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        d (:, 1) double;
+    end
+    arguments (Output)
+        x (:, 1) double;
+    end
+    x = 1 - (1 ./ (1 + exp(-obj.betaDist .* (abs(d) - obj.alphaDist))));
+end

@sigmoidSensor/initialize.m

@@ -0,0 +1,17 @@
+function obj = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+        alphaDist (1, 1) double;
+        betaDist (1, 1) double;
+        alphaTilt (1, 1) double;
+        betaTilt (1, 1) double;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}
+    end
+    
+    obj.alphaDist = alphaDist;
+    obj.betaDist = betaDist;
+    obj.alphaTilt = alphaTilt;
+    obj.betaTilt = betaTilt;
+end

@sigmoidSensor/plotParameters.m

@@ -0,0 +1,42 @@
+function f = plotParameters(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+    end
+    arguments (Output)
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+
+    % Distance and tilt sample points
+    d = 0:(obj.alphaDist / 100):(2*obj.alphaDist);
+    t = -90:1:90;
+
+    % Sample membership functions
+    d_x = obj.distanceMembership(d);
+    t_x = obj.tiltMembership(t);
+
+    % Plot resultant sigmoid curves
+    f = figure;
+    tiledlayout(f, 2, 1, "TileSpacing", "tight", "Padding", "compact");
+
+    % Distance
+    nexttile(1, [1,  1]);
+    grid("on");
+    title("Distance Membership Sigmoid");
+    xlabel("Distance (m)");
+    ylabel("Membership");
+    hold('on');
+    plot(d, d_x, 'LineWidth', 2);
+    hold('off');
+    ylim([0, 1]);
+
+    % Tilt
+    nexttile(2, [1,  1]);
+    grid("on");
+    title("Tilt Membership Sigmoid");
+    xlabel("Tilt (deg)");
+    ylabel("Membership");
+    hold('on');
+    plot(t, t_x, 'LineWidth', 2);
+    hold('off');
+    ylim([0, 1]);
+end

@sigmoidSensor/sensorPerformance.m

@@ -0,0 +1,23 @@
+function value = sensorPerformance(obj, agentPos, targetPos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        agentPos (1, 3) 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)
+
+    % compute tilt angle
+    tiltAngle = (180 - atan2d(x, targetPos(:, 3) - agentPos(3))); % degrees
+
+    % Membership functions
+    mu_d = obj.distanceMembership(d);
+    mu_t = obj.tiltMembership(tiltAngle);
+
+    value = mu_d .* mu_t; % assume pan membership is always 1
+end

@sigmoidSensor/sigmoidSensor.m

@@ -0,0 +1,19 @@
+classdef sigmoidSensor
+    properties (SetAccess = private, GetAccess = public)
+        % Sensor parameters
+        alphaDist = NaN;
+        betaDist = NaN;
+        alphaTilt = NaN; % degrees
+        betaTilt = NaN;
+    end
+
+    methods (Access = public)
+        [obj]               = initialize(obj, alphaDist, betaDist, alphaTilt, betaTilt);
+        [value]             = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos);
+        [f] = plotParameters(obj);
+    end
+    methods (Access = private)
+        x = distanceMembership(obj, d);
+        x = tiltMembership(obj, t);
+    end
+end

@sigmoidSensor/tiltMembership.m

@@ -0,0 +1,10 @@
+function x = tiltMembership(obj, t)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'sigmoidSensor')};
+        t (:, 1) double; % degrees
+    end
+    arguments (Output)
+        x (:, 1) double;
+    end
+    x = (1 ./ (1 + exp(-obj.betaTilt .* (t + obj.alphaTilt)))) - (1 ./ (1 + exp(-obj.betaTilt .* (t - obj.alphaTilt))));
+end

agent.m

@@ -1,167 +0,0 @@
-classdef agent
-    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
-        vel = NaN(1, 3); % current velocity
-        pan = NaN; % pan angle
-        tilt = NaN; % tilt angle
-
-        % Collision
-        collisionGeometry;
-
-        % FOV cone
-        fovGeometry;
-
-        % Communication
-        comRange = NaN;
-
-        % Plotting
-        scatterPoints;
-    end
-
-    methods (Access = public)
-        function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, index, label)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'agent')};
-                pos (1, 3) double;
-                vel (1, 3) double;
-                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;
-                label (1, 1) string = "";
-            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;
-
-            % Initialize FOV cone
-            obj.fovGeometry = cone;
-            obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], obj.sensorModel.r, obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
-        end
-        function obj = run(obj, sensingObjective, domain, partitioning)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'agent')};
-                sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
-                domain (1, 1) {mustBeGeometry};
-                partitioning (:, :) double;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'agent')};
-            end
-
-            % Do sensing
-            [sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
-
-            % Determine next planned position
-            nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
-
-            % Move to next position
-            % (dynamics not modeled at this time)
-            obj.lastPos = obj.pos;
-            obj.pos = nextPos;
-
-            % Calculate movement
-            d = obj.pos - obj.collisionGeometry.center;
-
-            % 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
-        function updatePlots(obj)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'agent')};
-            end
-            arguments (Output)
-            end
-
-            % Scatterplot point positions
-            for ii = 1:size(obj.scatterPoints, 1)
-                obj.scatterPoints(ii).XData = obj.pos(1);
-                obj.scatterPoints(ii).YData = obj.pos(2);
-                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
-                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
-
-            % Update FOV geometry surfaces
-            for jj = 1:size(obj.fovGeometry.surface, 2)
-                % Update each plot
-                obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
-                obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
-                obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
-            end
-        end
-        function [obj, f] = plot(obj, ind, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'agent')};
-                ind (1, :) double = NaN;
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'agent')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % Create axes if they don't already exist
-            f = firstPlotSetup(f);
-
-            % Plot points representing the agent position
-            hold(f.Children(1).Children(end), "on");
-            o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
-            hold(f.Children(1).Children(end), "off");
-
-            % Check if this is a tiled layout figure
-            if strcmp(f.Children(1).Type, 'tiledlayout')
-                % Add to other perspectives
-                o = [o; copyobj(o(1), f.Children(1).Children(2))];
-                o = [o; copyobj(o(1), f.Children(1).Children(3))];
-                o = [o; copyobj(o(1), f.Children(1).Children(4))];
-            end
-
-            obj.scatterPoints = o;
-
-            % Plot collision geometry
-            [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f);
-
-            % Plot FOV geometry
-            [obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
-        end
-    end
-end

geometries/@cone/cone.m

@@ -0,0 +1,22 @@
+classdef cone
+    % Conical geometry
+    properties (SetAccess = private, GetAccess = public)
+        % Meta
+        tag = REGION_TYPE.INVALID;
+        label = "";
+
+        % Spatial
+        center = NaN;
+        radius = NaN;
+        height = NaN;
+
+        % Plotting
+        surface;
+        n = 32;
+    end
+
+    methods (Access = public)
+        [obj   ] = initialize(obj, center, radius, height, tag, label);
+        [obj, f] = plot(obj, ind, f, maxAlt);
+    end
+end

geometries/@cone/initialize.m

@@ -0,0 +1,19 @@
+function obj = initialize(obj, center, radius, height, tag, label)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'cone')};
+        center (1, 3) double;
+        radius (1, 1) double;
+        height (1, 1) double;
+        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+        label (1, 1) string = "";
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'cone')};
+    end
+
+    obj.center = center;
+    obj.radius = radius;
+    obj.height = height;
+    obj.tag = tag;
+    obj.label = label;
+end

geometries/@cone/plot.m

@@ -0,0 +1,46 @@
+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')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+    
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+    
+    scalingFactor = (maxAlt / obj.height);
+
+    % Plot cone
+    [X, Y, Z] = cylinder([scalingFactor * obj.radius, 0], obj.n);
+    
+    % Scale to match height
+    Z = Z * maxAlt;
+    
+    % Move to center location
+    X = X + obj.center(1);
+    Y = Y + obj.center(2);
+    Z = Z + obj.center(3) - maxAlt;
+    
+    % Plot
+    if isnan(ind)
+        o = surf(f.CurrentAxes, X, Y, Z);
+    else
+        hold(f.Children(1).Children(ind(1)), "on");
+        o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
+        hold(f.Children(1).Children(ind(1)), "off");
+    end
+    
+    % Copy to other requested tiles
+    if numel(ind) > 1
+        for ii = 2:size(ind, 2)
+            o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
+        end
+    end
+    
+    obj.surface = o;
+end

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/contains.m

@@ -0,0 +1,10 @@
+function c = contains(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        c (:, 1) logical
+    end
+    c = all(pos >= repmat(obj.minCorner, size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 1), 1), 2);
+end

geometries/@rectangularPrism/containsLine.m

@@ -0,0 +1,46 @@
+function c = containsLine(obj, pos1, pos2)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        pos1 (1, 3) double;
+        pos2 (1, 3) double;
+    end
+    arguments (Output)
+        c (1, 1) logical
+    end
+    
+    d = pos2 - pos1;
+
+    % endpoint contained (trivial case)
+    if obj.contains(pos1) || obj.contains(pos2)
+        c = true;
+        return;
+    end
+
+    % 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) - 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
+    end
+    c = true;
+end

geometries/@rectangularPrism/distance.m

@@ -0,0 +1,32 @@
+function d = distance(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        d (:, 1) double;
+    end
+    if obj.contains(pos)
+        % Queried point is inside geometry
+        % find minimum distance to any face
+        d = min([pos(1) - obj.minCorner(1), ...
+                 pos(2) - obj.minCorner(2), ...
+                 pos(3) - obj.minCorner(3), ...
+                 obj.maxCorner(1) - pos(1), ...
+                 obj.maxCorner(2) - pos(2), ...
+                 obj.maxCorner(3) - pos(3)]);
+    else
+        % Queried point is outside geometry
+        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
+        d = norm(cPos - pos);
+    end
+end

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

@@ -0,0 +1,51 @@
+function obj = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        bounds (2, 3) double;
+        tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
+        label (1, 1) string = "";
+        objectiveFunction (1, 1) function_handle = @(x, y) 1;
+        discretizationStep (1, 1) double = 1;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+    end
+
+    obj.tag = tag;
+    obj.label = label;
+
+    % Define geometry bounds by LL corner and UR corner
+    obj.minCorner = bounds(1, 1:3);
+    obj.maxCorner = bounds(2, 1:3);
+
+    % Compute L, W, H
+    obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
+
+    % 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);
+                    obj.maxCorner(1:2), obj.minCorner(3);
+                    obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
+                    obj.minCorner(1:2), obj.maxCorner(3);
+                    obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
+                    obj.minCorner(1), obj.maxCorner(2:3)
+                    obj.maxCorner;];
+
+    % Compute footprint
+    obj.footprint = [obj.minCorner(1:2); ...
+                     [obj.minCorner(1), obj.maxCorner(2)]; ...
+                     [obj.maxCorner(1), obj.minCorner(2)]; ...
+                     obj.maxCorner(1:2)];
+
+    % Instantiate sensingObjective only for DOMAIN-type regions
+    if tag == REGION_TYPE.DOMAIN
+        obj.objective = sensingObjective;
+    end
+end

geometries/@rectangularPrism/initializeRandom.m

@@ -0,0 +1,45 @@
+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;
+        domain (1, 1) {mustBeGeometry} = rectangularPrism;
+        minAlt (1, 1) double = 1;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+    end
+    
+    % Produce random bounds based on region type
+    if tag == REGION_TYPE.DOMAIN
+        % Domain
+        L = ceil(minDimension + rand * (maxDimension - minDimension));
+        bounds = [zeros(1, 3); L * ones(1, 3)];
+    else
+        % Obstacle
+
+        % Produce a corners that are contained in the domain
+        ii = 0;
+        candidateMaxCorner = domain.maxCorner + ones(1, 3);
+        candidateMinCorner = domain.minCorner - ones(1, 3);
+        % Continue until the domain contains the obstacle without crowding the objective
+        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)
+                ii = 1;
+            end
+
+            candidateMaxCorner = candidateMinCorner + minDimension + rand(1, 3) * (maxDimension - minDimension);
+
+            ii = ii + 1;
+        end
+        
+        bounds = [candidateMinCorner; candidateMaxCorner;];
+    end
+
+    % Regular initialization
+    obj = obj.initialize(bounds, tag, label);
+end

geometries/@rectangularPrism/plotWireframe.m

@@ -0,0 +1,37 @@
+function [obj, f] = plotWireframe(obj, ind, f)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        ind (1, :) double = NaN;
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+    end
+
+    % Create axes if they don't already exist
+    f = firstPlotSetup(f);
+
+    % Create plotting inputs from vertices and edges
+    X = [obj.vertices(obj.edges(:,1),1), obj.vertices(obj.edges(:,2),1)]';
+    Y = [obj.vertices(obj.edges(:,1),2), obj.vertices(obj.edges(:,2),2)]';
+    Z = [obj.vertices(obj.edges(:,1),3), obj.vertices(obj.edges(:,2),3)]';
+
+    % Plot the boundaries of the geometry 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/@rectangularPrism/random.m

@@ -0,0 +1,9 @@
+function r = random(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+    end
+    arguments (Output)
+        r (1, 3) double
+    end
+    r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
+end

geometries/@rectangularPrism/rectangularPrism.m

@@ -0,0 +1,41 @@
+classdef rectangularPrism
+    % Rectangular prism geometry
+    properties (SetAccess = private, GetAccess = public)
+        % Meta
+        tag = REGION_TYPE.INVALID;
+
+        % Spatial
+        minCorner = NaN(1, 3);
+        maxCorner = NaN(1, 3);
+        dimensions = NaN(1, 3);
+        center = NaN;
+        footprint = NaN(4, 2);
+        radius = NaN; % fake radius
+
+        % Graph
+        vertices = NaN(8, 3);
+        edges = [1 2; 2 3; 3 4; 4 1;  % bottom square
+            5 6; 6 8; 8 7; 7 5;  % top square
+            1 5; 2 6; 3 8; 4 7]; % vertical edges
+
+        % Plotting
+        lines;
+    end
+    properties (SetAccess = public, GetAccess = public)
+        label = "";
+        % Sensing objective (for DOMAIN region type only)
+        objective;
+    end
+
+    methods (Access = public)
+        [obj   ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep);
+        [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
+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', 1);
+        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)

geometries/cone.m

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

geometries/rectangularPrism.m

@@ -1,204 +0,0 @@
-classdef rectangularPrism
-    % Rectangular prism geometry
-    properties (SetAccess = private, GetAccess = public)
-        % Meta
-        tag = REGION_TYPE.INVALID;
-        label = "";
-
-        % Spatial
-        minCorner = NaN(1, 3);
-        maxCorner = NaN(1, 3);
-        dimensions = NaN(1, 3);
-        center = NaN;
-        footprint = NaN(4, 2);
-
-        % Graph
-        vertices = NaN(8, 3);
-        edges = [1 2; 2 3; 3 4; 4 1;  % bottom square
-            5 6; 6 8; 8 7; 7 5;  % top square
-            1 5; 2 6; 3 8; 4 7]; % vertical edges
-
-        % Plotting
-        lines;
-    end
-
-    methods (Access = public)
-        function obj = initialize(obj, bounds, tag, label)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                bounds (2, 3) double;
-                tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
-                label (1, 1) string = "";
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-            end
-
-            obj.tag = tag;
-            obj.label = label;
-
-            %% Define geometry bounds by LL corner and UR corner
-            obj.minCorner = bounds(1, 1:3);
-            obj.maxCorner = bounds(2, 1:3);
-
-            % Compute L, W, H
-            obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
-
-            % Compute center
-            obj.center = obj.minCorner + obj.dimensions ./ 2;
-
-            % Compute vertices
-            obj.vertices = [obj.minCorner;
-                            obj.maxCorner(1), obj.minCorner(2:3);
-                            obj.maxCorner(1:2), obj.minCorner(3);
-                            obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
-                            obj.minCorner(1:2), obj.maxCorner(3);
-                            obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
-                            obj.minCorner(1), obj.maxCorner(2:3)
-                            obj.maxCorner;];
-
-            % Compute footprint
-            obj.footprint = [obj.minCorner(1:2); ...
-                             [obj.minCorner(1), obj.maxCorner(2)]; ...
-                             [obj.maxCorner(1), obj.minCorner(2)]; ...
-                             obj.maxCorner(1:2)];
-        end
-        function r = random(obj)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-            end
-            arguments (Output)
-                r (1, 3) double
-            end
-            r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
-        end
-        function d = distance(obj, pos)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                pos (:, 3) double;
-            end
-            arguments (Output)
-                d (:, 1) double
-            end
-            assert(~obj.contains(pos), "Cannot determine distance for a point inside of the geometry");
-            
-            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
-            d = norm(cPos - pos);
-        end
-        function d = interiorDistance(obj, pos)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                pos (:, 3) double;
-            end
-            arguments (Output)
-                d (:, 1) double
-            end
-            assert(obj.contains(pos), "Cannot determine interior distance for a point outside of the geometry");
-
-            % find minimum distance to any face
-            d = min([pos(1) - obj.minCorner(1), ...
-                     pos(2) - obj.minCorner(2), ...
-                     pos(3) - obj.minCorner(3), ...
-                     obj.maxCorner(1) - pos(1), ...
-                     obj.maxCorner(2) - pos(2), ...
-                     obj.maxCorner(3) - pos(3)]);
-        end
-        function c = contains(obj, pos)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                pos (:, 3) double;
-            end
-            arguments (Output)
-                c (:, 1) logical
-            end
-            c = all(pos >= repmat(obj.minCorner, size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 1), 1), 2);
-        end
-        function c = containsLine(obj, pos1, pos2)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                pos1 (1, 3) double;
-                pos2 (1, 3) double;
-            end
-            arguments (Output)
-                c (1, 1) logical
-            end
-            
-            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
-                if obj.contains(pos1) || obj.contains(pos2)
-                    c = true;
-                else
-                    c = false;
-                end
-                return;
-            end
-
-            tmin = -inf;        
-            tmax = inf;
-
-            % Standard case
-            for ii = 1:3
-                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 = (tmax >= 0) && (tmin <= 1);
-        end
-        function [obj, f] = plotWireframe(obj, ind, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                ind (1, :) double = NaN;
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % Create axes if they don't already exist
-            f = firstPlotSetup(f);
-
-            % Create plotting inputs from vertices and edges
-            X = [obj.vertices(obj.edges(:,1),1), obj.vertices(obj.edges(:,2),1)]';
-            Y = [obj.vertices(obj.edges(:,1),2), obj.vertices(obj.edges(:,2),2)]';
-            Z = [obj.vertices(obj.edges(:,1),3), obj.vertices(obj.edges(:,2),3)]';
-
-            % Plot the boundaries of the geometry 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
-    end
-end

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

miSim.m

@@ -1,379 +0,0 @@
-classdef miSim
-    % multiagent interconnection simulation
-
-    % Simulation parameters
-    properties (SetAccess = private, GetAccess = public)
-        timestep = NaN; % delta time interval for simulation iterations
-        partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
-        maxIter = NaN; % maximum number of simulation iterations
-        domain = rectangularPrism;
-        objective = sensingObjective;
-        obstacles = cell(0, 1); % geometries that define obstacles within the domain
-        agents = cell(0, 1); % agents that move within the domain
-        adjacency = NaN; % Adjacency matrix representing communications network graph
-        partitioning = NaN;
-    end
-
-    properties (Access = private)
-        % Plot objects
-        connectionsPlot; % objects for lines connecting agents in spatial plots
-        graphPlot; % objects for abstract network graph plot
-        partitionPlot; % objects for partition plot
-
-        % Indicies for various plot types in the main tiled layout figure
-        spatialPlotIndices = [6, 4, 3, 2];
-        objectivePlotIndices = [6, 4];
-        networkGraphIndex = 5;
-        partitionGraphIndex = 1;
-    end
-
-    methods (Access = public)
-        function [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                domain (1, 1) {mustBeGeometry};
-                objective (1, 1) {mustBeA(objective, 'sensingObjective')};
-                agents (:, 1) cell;
-                timestep (:, 1) double = 0.05;
-                partitoningFreq (:, 1) double = 0.25
-                maxIter (:, 1) double = 1000;
-                obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % Define simulation time parameters
-            obj.timestep = timestep;
-            obj.maxIter = maxIter;
-
-            % Define domain
-            obj.domain = domain;
-            obj.partitioningFreq = partitoningFreq;
-
-            % Add geometries representing obstacles within the domain
-            obj.obstacles = obstacles;
-
-            % Define objective
-            obj.objective = objective;
-
-            % Define agents
-            obj.agents = agents;
-
-            % Compute adjacency matrix
-            obj = obj.updateAdjacency();
-
-            % Create initial partitioning
-            obj = obj.partition();
-
-            % Set up initial plot
-            % Set up axes arrangement
-            % Plot domain
-            [obj.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
-
-            % Plot obstacles
-            for ii = 1:size(obj.obstacles, 1)
-                [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f);
-            end
-
-            % Plot objective gradient
-            f = obj.objective.plot(obj.objectivePlotIndices, f);
-
-            % Plot agents and their collision geometries
-            for ii = 1:size(obj.agents, 1)
-                [obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f);
-            end
-
-            % Plot communication links
-            [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
-
-            % Plot abstract network graph
-            [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
-
-            % Plot domain partitioning
-            [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
-        end
-        function [obj, f] = run(obj, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % Create axes if they don't already exist
-            f = firstPlotSetup(f);
-
-            % Set up times to iterate over
-            times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
-            partitioningTimes = times(obj.partitioningFreq:obj.partitioningFreq:size(times, 1));
-
-            % Start video writer
-            v = setupVideoWriter(obj.timestep);
-            v.open();
-
-            for ii = 1:size(times, 1)
-                % Display current sim time
-                t = times(ii);
-                fprintf("Sim Time: %4.2f (%d/%d)\n", t, ii, obj.maxIter)
-
-                % Check if it's time for new partitions
-                updatePartitions = false;
-                if ismember(t, partitioningTimes)
-                    updatePartitions = true;
-                    obj = obj.partition();
-                end
-
-                % Iterate over agents to simulate their motion
-                for jj = 1:size(obj.agents, 1)
-                    obj.agents{jj} = obj.agents{jj}.run(obj.objective, obj.domain, obj.partitioning);
-                end
-    
-                % Update adjacency matrix
-                obj = obj.updateAdjacency;
-    
-                % Update plots
-                [obj, f] = obj.updatePlots(f, updatePartitions);
-
-                % Write frame in to video
-                I = getframe(f);
-                v.writeVideo(I);
-            end
-
-            % Close video file
-            v.close();
-        end
-        function obj = partition(obj)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-            end
-
-            % Assess sensing performance of each agent at each sample point
-            % in the domain
-            agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-            agentPerformances = cat(3, agentPerformances{:});
-            
-            % Get highest performance value at each point
-            [~, idx] = max(agentPerformances, [], 3);
-
-            % Collect agent indices in the same way
-            agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
-            agentInds = cat(3, agentInds{:});
-
-            % Get highest performing agent's index
-            [m,n,~] = size(agentInds);
-            [i,j] = ndgrid(1:m, 1:n);
-            obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
-        end
-        function [obj, f] = updatePlots(obj, f, updatePartitions)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-                updatePartitions (1, 1) logical = false;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % Update agent positions, collision 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 
-            % re-plotting, which is much better for performance boost
-
-            % Update agent connections plot
-            delete(obj.connectionsPlot);
-            [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
-
-            % Update network graph plot
-            delete(obj.graphPlot);
-            [obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
-
-            % Update partitioning plot
-            if updatePartitions
-                delete(obj.partitionPlot);
-                [obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
-            end
-
-            % reset plot limits to fit domain
-            for ii = 1:size(obj.spatialPlotIndices, 2)
-                xlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
-                ylim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
-                zlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
-            end
-
-            drawnow;
-        end
-        function obj = updateAdjacency(obj)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-            end
-
-            % Initialize assuming only self-connections
-            A = logical(eye(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
-                    end
-                end
-            end
-
-            obj.adjacency = A | A';
-        end
-        function [obj, f] = plotConnections(obj, ind, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                ind (1, :) double = NaN;
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % 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 jj = 1:(ii - 1)
-                    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)];
-                    end
-                end
-            end
-            X = X'; Y = Y'; Z = Z';
-
-            % Plot the connections
-            if isnan(ind)
-                hold(f.CurrentAxes, "on");
-                o = plot3(f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
-                hold(f.CurrentAxes, "off");
-            else
-                hold(f.Children(1).Children(ind(1)), "on");
-                o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
-                hold(f.Children(1).Children(ind(1)), "off");
-            end
-
-            % Copy to other plots
-            if size(ind, 2) > 1
-                for ii = 2:size(ind, 2)
-                    o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
-                end 
-            end
-
-            obj.connectionsPlot = o;
-        end
-        function [obj, f] = plotPartitions(obj, ind, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                ind (1, :) double = NaN;
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            if isnan(ind)
-                hold(f.CurrentAxes, 'on');
-                o = imagesc(f.CurrentAxes, obj.partitioning);
-                hold(f.CurrentAxes, 'off');
-            else
-                hold(f.Children(1).Children(ind(1)), 'on');
-                o = imagesc(f.Children(1).Children(ind(1)), obj.partitioning);
-                hold(f.Children(1).Children(ind(1)), 'on');
-                if size(ind, 2) > 1
-                    for ii = 2:size(ind, 2)
-                        o = [o, copyobj(o(1), f.Children(1).Children(ind(ii)))];
-                    end
-                end
-            end
-            obj.partitionPlot = o;
-
-        end
-        function [obj, f] = plotGraph(obj, ind, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                ind (1, :) double = NaN;
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'miSim')};
-                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
-            end
-
-            % Form graph from adjacency matrix
-            G = graph(obj.adjacency, 'omitselfloops');
-
-            % Plot graph object
-            if isnan(ind)
-                hold(f.CurrentAxes, 'on');
-                o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-                hold(f.CurrentAxes, 'off');
-            else
-                hold(f.Children(1).Children(ind(1)), 'on');
-                o = plot(f.Children(1).Children(ind(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-                hold(f.Children(1).Children(ind(1)), 'off');
-                if size(ind, 2) > 1
-                    for ii = 2:size(ind, 2)
-                        o = [o; copyobj(o(1), f.Children(1).Children(ind(ii)))];
-                    end
-                end
-            end
-            obj.graphPlot = o;
-        end
-    end
-
-    methods (Access = private)
-        function validateInitialization(obj)
-            % Assert obstacles do not intersect with the domain
-
-            % Assert obstacles do not intersect with each other
-
-            % Assert the objective has only one maxima within the domain
-
-            % Assert the objective's sole maximum is not inaccessible due
-            % to the placement of an obstacle
-
-        end
-        function validateLoop(obj)
-            % Assert that agents are safely inside the domain
-
-            % Assert that agents are not in proximity to obstacles
-
-            % Assert that agents are not in proximity to each other
-
-            % Assert that agents form a connected graph
-
-
-        end
-    end
-end

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/CCiVFlEZGOWF8RpyKjlFUC_OrW4d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/CCiVFlEZGOWF8RpyKjlFUC_OrW4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1bb12f2e-385b-41a4-83e8-f9a9326d95ee" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/QAWdSKdcuP9OGm_S2NybPkCBTOYd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/QAWdSKdcuP9OGm_S2NybPkCBTOYp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="89c37511-fb1f-420e-919a-c5b38c02b501" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/QCjAM-pfjkxuIc0-mouM97I9zHEd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/QCjAM-pfjkxuIc0-mouM97I9zHEp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="30f974f9-50a8-405c-9a83-e7fd84333f0e" 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/EEtUlUb-dLAdf0KpMVivaUlztwA/bC9JguT7xdg6hTVY9GqmttWOU6Ud.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/bC9JguT7xdg6hTVY9GqmttWOU6Up.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="5f96e524-3aac-4fa1-95df-67fd6ce02ff3" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="b7c7eec5-a318-4c17-adb2-b13a21bf0609" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/s5udZfVFP9-PGBZPOy6PEk_nJDEd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/s5udZfVFP9-PGBZPOy6PEk_nJDEp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="2e3f60de-3b82-4ad5-af81-57781353dcbf" type="Reference"/>

resources/project/FI0gxbH-PhwjE_riDQGHPyYMHks/AmJAKvt15XQGz6JdpTsYHCBwsj0d.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/AmJAKvt15XQGz6JdpTsYHCBwsj0p.xml

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

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

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