fixed filename

.gitattributes

@@ -0,0 +1,33 @@
+* text=auto
+
+*.fig binary
+*.mat binary
+*.mdl binary diff merge=mlAutoMerge
+*.mdlp binary
+*.mex* binary
+*.mlapp binary
+*.mldatx binary merge=mlAutoMerge
+*.mlproj binary
+*.mlx binary
+*.p binary
+*.plprj binary
+*.sbproj binary
+*.sfx binary
+*.sldd binary
+*.slreqx binary merge=mlAutoMerge
+*.slmx binary merge=mlAutoMerge
+*.sltx binary
+*.slxc binary
+*.slx binary merge=mlAutoMerge
+*.slxp binary
+
+## MATLAB Project metadata files use LF line endings
+/resources/project/**/*.xml text eol=lf
+
+## Other common binary file types
+*.docx binary
+*.exe binary
+*.jpg binary
+*.pdf binary
+*.png binary
+*.xlsx binary

.gitignore

@@ -0,0 +1,43 @@
+# Autosave files
+*.asv
+*.m~
+*.autosave
+*.slx.r*
+*.mdl.r*
+
+# Derived content-obscured files
+*.p
+
+# Compiled MEX files
+*.mex*
+
+# Packaged app and toolbox files
+*.mlappinstall
+*.mltbx
+
+# Deployable archives
+*.ctf
+
+# Generated helpsearch folders
+helpsearch*/
+
+# Code generation folders
+slprj/
+sccprj/
+codegen/
+
+# Cache files
+*.slxc
+
+# Cloud based storage dotfile
+.MATLABDriveTag
+
+# buildtool cache folder
+.buildtool/
+
+# SimBiology backup files
+*.sbproj.backup
+*.sbproj.bak
+
+# Sandbox contents
+sandbox/*

agent.m

@@ -6,24 +6,35 @@ classdef agent
 
         % Sensor
         sensingFunction = @(r) 0.5; % probability of detection as a function of range
+        sensingLength = 0.05; % length parameter used by sensing function
 
         % State
-        pos = NaN(1, 3);
+        lastPos = NaN(1, 3); % position from previous timestep
-        vel = NaN(1, 3);
+        pos = NaN(1, 3); % current position
-        cBfromC = NaN(3); % DCM body from sim cartesian (assume fixed for now)
+        vel = NaN(1, 3); % current velocity
+        cBfromC = NaN(3); % current DCM body from sim cartesian (assume fixed for now)
 
         % Collision
         collisionGeometry;
+
+        % Communication
+        comRange = NaN;
+
+        % Plotting
+        scatterPoints;
     end
 
     methods (Access = public)
-        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, index, label)
+        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, sensingLength, comRange, index, label)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'agent')};
                 pos (1, 3) double;
                 vel (1, 3) double;
                 cBfromC (3, 3) double {mustBeDcm};
-                collisionGeometry (1, 1) {mustBeConstraintGeometries};
+                collisionGeometry (1, 1) {mustBeGeometry};
+                sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')} = @(r) 0.5;
+                sensingLength (1, 1) double = NaN;
+                comRange (1, 1) double = NaN;
                 index (1, 1) double = NaN;
                 label (1, 1) string = "";
             end
@@ -35,15 +46,72 @@ classdef agent
             obj.vel = vel;
             obj.cBfromC = cBfromC;
             obj.collisionGeometry = collisionGeometry;
+            obj.sensingFunction = sensingFunction;
+            obj.sensingLength = sensingLength;
+            obj.comRange = comRange;
             obj.index = index;
             obj.label = label;
         end
-        function f = plot(obj, f)
+        function obj = run(obj, objectiveFunction, domain)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'agent')};
+                objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+                domain (1, 1) {mustBeGeometry};
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'agent')};
+            end
+
+            % Do sensing to determine target position
+            nextPos = obj.sensingFunction(objectiveFunction, domain, obj.pos, obj.sensingLength);
+
+            % 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
+
+            % Network connections
+        end
+        function [obj, f] = plot(obj, f)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'agent')};
                 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
 
@@ -52,8 +120,21 @@ classdef agent
 
             % Plot points representing the agent position
             hold(f.CurrentAxes, "on");
-            scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 50);
+            o = scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
             hold(f.CurrentAxes, "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(5))];
+            end
+
+            obj.scatterPoints = o;
+
+            % Plot collision geometry
+            [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(f);
         end
     end
 end

firstPlotSetup.m

@@ -1,8 +1,49 @@
 function f = firstPlotSetup(f)
     if isempty(f.CurrentAxes)
-        axes(f);
+        tiledlayout(f, 4, 3, "TileSpacing", "tight", "Padding", "compact");
-        axis(f.CurrentAxes, "equal");
+
-        grid(f.CurrentAxes, "on");
+        % Top-down view
-        view(f.CurrentAxes, 3);
+        nexttile(1, [1, 2]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "on");
+        view(f.Children(1).Children(1), 0, 90);
+        xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
+        title(f.Children(1).Children(1), "Top-down Perspective");
+
+        % Communications graph
+        nexttile(3, [1, 1]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "off");
+        view(f.Children(1).Children(1), 0, 0);
+        title(f.Children(1).Children(1), "Network Graph");
+
+        % 3D view
+        nexttile(4, [2, 2]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "on");
+        view(f.Children(1).Children(1), 3);
+        xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
+        title(f.Children(1).Children(1), "3D Perspective");
+
+        % Side-on view
+        nexttile(6, [2, 1]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "on");
+        view(f.Children(1).Children(1), 90, 0);
+        ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
+        title(f.Children(1).Children(1), "Side-on Perspective");
+
+        % Front-on view
+        nexttile(10, [1, 2]);
+        axes(f.Children(1).Children(1));
+        axis(f.Children(1).Children(1), "image");
+        grid(f.Children(1).Children(1), "on");
+        view(f.Children(1).Children(1), 0, 0);
+        xlabel(f.Children(1).Children(1), "X"); zlabel(f.Children(1).Children(1), "Z");
+        title(f.Children(1).Children(1), "Front-on Perspective");
     end
 end

geometries/rectangularPrism.m

@@ -0,0 +1,200 @@
+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, f)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+                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
+            hold(f.CurrentAxes, "on");
+            o = plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+            hold(f.CurrentAxes, "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(5))];
+            end
+
+            obj.lines = o;
+        end
+    end
+end

miSim.m

@@ -3,37 +3,241 @@ classdef miSim
 
     % Simulation parameters
     properties (SetAccess = private, GetAccess = public)
-        domain = rectangularPrismConstraint;
+        timestep = NaN; % delta time interval for simulation iterations
+        maxIter = NaN; % maximum number of simulation iterations
+        domain = rectangularPrism;
         objective = sensingObjective;
-        constraintGeometries = cell(0, 1); % geometries that define constraints within the domain
+        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
+    end
+
+    properties (Access = private)
+        v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')));
+        connectionsPlot; % objects for lines connecting agents in spatial plots
+        graphPlot; % objects for abstract network graph plot
     end
 
     methods (Access = public)
-        function obj = initialize(obj, domain, objective, agents, constraintGeometries)
+        function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
-                domain (1, 1) {mustBeConstraintGeometries};
+                domain (1, 1) {mustBeGeometry};
                 objective (1, 1) {mustBeA(objective, 'sensingObjective')};
                 agents (:, 1) cell {mustBeAgents};
-                constraintGeometries (:, 1) cell {mustBeConstraintGeometries} = cell(0, 1);
+                timestep (:, 1) double = 0.05;
+                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;
+
+            % 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();
+
+            % Set up initial plot
+            % Set up axes arrangement
+            % Plot domain
+            [obj.domain, f] = obj.domain.plotWireframe();
+
+            % Set plotting limits to focus on the domain
+            xlim([obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
+            ylim([obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
+            zlim([obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
+
+            % Plot obstacles
+            for ii = 1:size(obj.obstacles, 1)
+                [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(f);
+            end
+
+            % Plot objective gradient
+            f = obj.objective.plot(f);
+
+            % Plot agents and their collision geometries
+            for ii = 1:size(obj.agents, 1)
+                [obj.agents{ii}, f] = obj.agents{ii}.plot(f);
+            end
+
+            % Plot communication links
+            [obj, f] = obj.plotConnections(f);
+
+            % Plot abstract network graph
+            [obj, f] = obj.plotGraph(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)';
+
+            % Start video writer
+            obj.v.FrameRate = 1/obj.timestep;
+            obj.v.Quality = 90;
+            obj.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)
+
+                % Iterate over agents to simulate their motion
+                for jj = 1:size(obj.agents, 1)
+                    obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain);
+                end
+    
+                % Update adjacency matrix
+                obj = obj.updateAdjacency;
+    
+                % Update plots
+                [obj, f] = obj.updatePlots(f);
+
+                % Write frame in to video
+                I = getframe(f);
+                obj.v.writeVideo(I);
+            end
+
+            % Close video file
+            obj.v.close();
+        end
+        function [obj, f] = updatePlots(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
+
+            % 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(f);
+
+            % Update network graph plot
+            delete(obj.graphPlot);
+            [obj, f] = obj.plotGraph(f);
+
+            drawnow;
+        end
+        function obj = updateAdjacency(obj)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
             end
             arguments (Output)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
             end
 
-            %% Define domain
+            % Initialize assuming only self-connections
-            obj.domain = domain;
+            A = logical(eye(size(obj.agents, 1)));
 
-            %% Add constraint geometries against the domain
+            % Check lower triangle off-diagonal connections
-            obj.constraintGeometries = constraintGeometries;
+            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
+                    end
+                end
+            end
 
-            %% Define objective
+            obj.adjacency = A | A';
-            obj.objective = objective;
+        end
+        function [obj, f] = plotConnections(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
 
-            %% Define agents
+            % Iterate over lower triangle off-diagonal region of the 
-            obj.agents = agents;
+            % 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
+            hold(f.CurrentAxes, "on");
+            o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+            hold(f.CurrentAxes, "off");
+
+            % Check if this is a tiled layout figure
+            if strcmp(f.Children(1).Type, 'tiledlayout')
+                % Add to other plots
+                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(5))];
+            end
+
+            obj.connectionsPlot = o;
+        end
+        function [obj, f] = plotGraph(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
+
+            % Form graph from adjacency matrix
+            G = graph(obj.adjacency, 'omitselfloops');
+
+            % Plot graph object
+            obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
         end
     end
 

miSim.prj

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<MATLABProject xmlns="http://www.mathworks.com/MATLABProjectFile"/>

rectangularPrismConstraint.m

@@ -1,106 +0,0 @@
-classdef rectangularPrismConstraint
-    % Rectangular prism constraint geometry
-    properties (SetAccess = private, GetAccess = public)
-        tag = REGION_TYPE.INVALID;
-        label = "";
-
-        minCorner = NaN(3, 1);
-        maxCorner = NaN(3, 1);
-
-        dimensions = NaN(3, 1);
-
-        center = NaN;
-
-        vertices = NaN(8, 3);
-
-        footprint = NaN(2, 4);
-    end
-
-    methods (Access = public)
-        function obj = initialize(obj, bounds, tag, label)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
-                bounds (3, 2) double;
-                tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
-                label (1, 1) string = "";
-            end
-            arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
-            end
-
-            obj.tag = tag;
-            obj.label = label;
-
-            %% Define geometry bounds by LL corner and UR corner
-            obj.minCorner = bounds(:, 1);
-            obj.maxCorner = bounds(:, 2);
-
-            % 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, 1), ...
-                             [obj.minCorner(1, 1); obj.maxCorner(2, 1)], ...
-                             [obj.maxCorner(1, 1); obj.minCorner(2, 1)], ...
-                             obj.maxCorner(1:2, 1)];
-        end
-        function r = random(obj)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
-            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 c = contains(obj, pos)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
-                pos (:, 3) double;
-            end
-            arguments (Output)
-                c (1, 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 f = plotWireframe(obj, f)
-            arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
-                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);
-
-            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
-
-            % Create plotting inputs from vertices and edges
-            X = [obj.vertices(edges(:,1),1), obj.vertices(edges(:,2),1)]';
-            Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]';
-            Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]';
-
-            % Plot the boundaries of the constraint geometry
-            hold(f.CurrentAxes, "on");
-            plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
-            hold(f.CurrentAxes, "off");
-        end
-    end
-end

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/HYICfq4P1rcDM3M3ck-Bdx9axZ8d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/HYICfq4P1rcDM3M3ck-Bdx9axZ8p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="5e8de0f6-dbb2-499b-a851-32a867fd50c2" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/LBBP3VzTDNqtmQGoBzn8TyGr_JYd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/LBBP3VzTDNqtmQGoBzn8TyGr_JYp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="7d58d2d3-2500-426a-b99d-912a79467335" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4d.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Lzv9PNrakrESxUF7UZmIf8m-ri4p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="9c9ce3cb-5989-41e8-a20d-358a95c08b20" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Zft-eetWhSiBfHeQkgT2iI9nSuId.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/Zft-eetWhSiBfHeQkgT2iI9nSuIp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="ff13e617-a2ad-49b1-a9b5-668ac2cffc4a" type="Reference"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/bC9JguT7xdg6hTVY9GqmttWOU6Ud.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/bC9JguT7xdg6hTVY9GqmttWOU6Up.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMd.xml

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

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMp.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/3f4CTErydQ2TqIMJrHErKW1HTm8d.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/3f4CTErydQ2TqIMJrHErKW1HTm8p.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQd.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQp.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/eU5R_sxHD3gVw76yiaPpbMUKFIkd.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/eU5R_sxHD3gVw76yiaPpbMUKFIkp.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/xLI7SQj40yDbM71LQ8pVZ0dWmrgd.xml

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

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/xLI7SQj40yDbM71LQ8pVZ0dWmrgp.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/2C0ooPmXaSISA_J0x7PMu2uZ660d.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/2C0ooPmXaSISA_J0x7PMu2uZ660p.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yd.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yp.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEd.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEp.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kd.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kp.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcd.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcp.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUd.xml

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

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tId.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/aUjk9RtIbQ9S9PboEHDBe8PRzPId.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/aUjk9RtIbQ9S9PboEHDBe8PRzPIp.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/p3X7RLZlL9TzzpK7fJIwY38rB_Yd.xml

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

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/p3X7RLZlL9TzzpK7fJIwY38rB_Yp.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/2kj09UetkV_lru3gvSPXnY6-nM4d.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="Test" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/2kj09UetkV_lru3gvSPXnY6-nM4p.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/KKyDJtbdIBOlaeHmIZd5VX6vqx8d.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="Other" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/KKyDJtbdIBOlaeHmIZd5VX6vqx8p.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/QWNDYJD5mGW1bWYvPx9DtKnxzw4d.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="Convenience" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/QWNDYJD5mGW1bWYvPx9DtKnxzw4p.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/R1RggVhA72agIvELiuhWPRS8F0Id.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="None" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/R1RggVhA72agIvELiuhWPRS8F0Ip.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/aEHSZBIY-yve10yGis12Zr5DLZod.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="Derived" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/aEHSZBIY-yve10yGis12Zr5DLZop.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/j4xwF_j8iFTVayUMfxLgMnTbencd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="Design" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/j4xwF_j8iFTVayUMfxLgMnTbencp.xml

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

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/r8LR4nLmg9ai3oHrW1r_-KocQzkd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="Artifact" ReadOnly="READ_ONLY"/>

resources/project/NjSPEMsIuLUyIpr2u1Js5bVPsOs/r8LR4nLmg9ai3oHrW1r_-KocQzkp.xml

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

resources/project/Project.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info MetadataType="fixedPathV2"/>

resources/project/fjRQtWiSIy7hIlj-Kmk87M7s21k/NjSPEMsIuLUyIpr2u1Js5bVPsOsd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info DataType="None" Name="Classification" ReadOnly="1" SingleValued="1"/>

resources/project/fjRQtWiSIy7hIlj-Kmk87M7s21k/NjSPEMsIuLUyIpr2u1Js5bVPsOsp.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/ulE9PYd_TlaqYfhxJlcxk9uZKPwd.xml

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

resources/project/q138eJA8Ym4eSfM3RFMVvg63QtU/ulE9PYd_TlaqYfhxJlcxk9uZKPwp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/DnSrh-_-SnAVM_QmOB_8JskdVJUd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/DnSrh-_-SnAVM_QmOB_8JskdVJUp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/IkPyncn3ZGRxjBV47QmicBzm2kUd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/IkPyncn3ZGRxjBV47QmicBzm2kUp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/M3axfhPiVFfHfWnq8PF7Q8OtKAUd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/M3axfhPiVFfHfWnq8PF7Q8OtKAUp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NRbCR7m2f1_2pHz6LyHrTz7eFpcd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NRbCR7m2f1_2pHz6LyHrTz7eFpcp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfId.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/NSdjf1q7sJuLYHZ864kCIhygPfIp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/QMJD9OLFzxcTTbPOoh-ahQ4zTRUd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/QMJD9OLFzxcTTbPOoh-ahQ4zTRUp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/TMK4UzWHdRLhy_w-CHt9y11Q8XAd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/TMK4UzWHdRLhy_w-CHt9y11Q8XAp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/q138eJA8Ym4eSfM3RFMVvg63QtUd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/q138eJA8Ym4eSfM3RFMVvg63QtUp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/qD-kr16wmwlzR-nIg1IG_vvRrWkd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/qD-kr16wmwlzR-nIg1IG_vvRrWkp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/raP7OV2lamoMNbKeDBUa13s5Imwd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/raP7OV2lamoMNbKeDBUa13s5Imwp.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/u_8f8riMl9bapFIdVg6GQo796t8d.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/u_8f8riMl9bapFIdVg6GQo796t8p.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/wdgofbR4UWdlJ0HwKeczjlphIxQd.xml

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

resources/project/qaw0eS1zuuY1ar9TdPn1GMfrjbQ/wdgofbR4UWdlJ0HwKeczjlphIxQp.xml

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

resources/project/root/EEtUlUb-dLAdf0KpMVivaUlztwAp.xml

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

resources/project/root/GiiBklLgTxteCEmomM8RCvWT0nQd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Name="miSim"/>

resources/project/root/GiiBklLgTxteCEmomM8RCvWT0nQp.xml

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

resources/project/root/fjRQtWiSIy7hIlj-Kmk87M7s21kp.xml

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

resources/project/root/qaw0eS1zuuY1ar9TdPn1GMfrjbQp.xml

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

resources/project/rootp.xml

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

sensingFunctions/basicGradientAscent.m

@@ -0,0 +1,44 @@
+function nextPos = basicGradientAscent(objectiveFunction, domain, pos, r)
+    arguments (Input)
+        objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
+        domain (1, 1) {mustBeGeometry};
+        pos (1, 3) double;
+        r (1, 1) double;
+    end
+    arguments (Output)
+        nextPos(1, 3) double;
+    end
+
+    % Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
+    currentPos = pos(1:2);
+    neighborPos = [currentPos(1) + r, currentPos(2); ... % (+x)
+                   currentPos(1), currentPos(2) + r; ... % (+y)
+                   currentPos(1) - r, currentPos(2); ... % (-x)
+                   currentPos(1), currentPos(2) - r; ... % (-y)
+                  ];
+
+    % Check for neighbor positions that fall outside of the domain
+    outOfBounds = false(size(neighborPos, 1), 1);
+    for ii = 1:size(neighborPos, 1)
+        if ~domain.contains([neighborPos(ii, :), 0])
+            outOfBounds(ii) = true;
+        end
+    end
+
+    % Replace out of bounds positions with inoffensive in-bounds positions
+    neighborPos(outOfBounds, 1:3) = repmat(pos, sum(outOfBounds), 1);
+
+    % Sense values at selected positions
+    neighborValues = [objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
+                      objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
+                      objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
+                      objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
+                     ];
+
+    % Prevent out of bounds locations from ever possibly being selected
+    neighborValues(outOfBounds) = 0;
+    
+    % Select next position by maximum sensed value
+    nextPos = neighborPos(neighborValues == max(neighborValues), :);
+    nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
+end

sensingObjective.m

@@ -16,7 +16,7 @@ classdef sensingObjective
             arguments (Input)
                 obj (1,1) {mustBeA(obj, 'sensingObjective')};
                 objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
-                footprint (2, :) double;
+                footprint (:, 2) double;
                 groundAlt (1, 1) double = 0;
                 discretizationStep (1, 1) double = 1;
             end
@@ -27,10 +27,10 @@ classdef sensingObjective
             obj.groundAlt = groundAlt;
 
             % Extract footprint limits
-            xMin = min(footprint(1, :));
+            xMin = min(footprint(:, 1));
-            xMax = max(footprint(1, :));
+            xMax = max(footprint(:, 1));
-            yMin = min(footprint(2, :));
+            yMin = min(footprint(:, 2));
-            yMax = max(footprint(2, :));
+            yMax = max(footprint(:, 2));
 
             xGrid = unique([xMin:discretizationStep:xMax, xMax]);
             yGrid = unique([yMin:discretizationStep:yMax, yMax]);
@@ -58,12 +58,25 @@ classdef sensingObjective
             % Create axes if they don't already exist
             f = firstPlotSetup(f);
 
-            % Plot gradient on the "floor" of the domain
+            % Check if this is a tiled layout figure
-            hold(f.CurrentAxes, "on");
+            if strcmp(f.Children(1).Type, 'tiledlayout')
-            s = surf(obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
+                % Plot gradient on the "floor" of the domain
-            s.HitTest = 'off';
+                hold(f.Children(1).Children(3), "on");
-            s.PickableParts = 'none';
+                o = surf(f.Children(1).Children(3), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
-            hold(f.CurrentAxes, "off");
+                o.HitTest = 'off';
+                o.PickableParts = 'none';
+                hold(f.Children(1).Children(3), "off");
+
+                % Add to other perspectives
+                copyobj(o, f.Children(1).Children(5));
+            else
+                % Plot gradient on the "floor" of the domain
+                hold(f.CurrentAxes, "on");
+                o = surf(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");
+            end
         end
     end
 end

startup.m

@@ -1 +1 @@
-addpath("validators");
+open("miSim.prj");

test_miSim.m

@@ -1,51 +1,75 @@
 classdef test_miSim < matlab.unittest.TestCase
     properties (Access = private)
         testClass = miSim;
+
         % Domain
-        domain = rectangularPrismConstraint;
+        domain = rectangularPrism; % domain geometry
+        maxIter = 1000;
+        timestep = 0.05
 
         % Obstacles
-        constraintGeometries = cell(1, 0);
+        minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
+        maxNumObstacles = 3; % Maximum number of obstacles to be randomly generated
+        minObstacleSize = 1; % Minimum size of a randomly generated obstacle
+        maxObstacleSize = 6; % Maximum size of a randomly generated obstacle
+        obstacles = cell(1, 0);
         
         % Objective
+        objectiveDiscretizationStep = 0.01; % Step at which the objective function is solved in X and Y space
+        protectedRange = 1; % Minimum distance between the sensing objective and the edge of the domain
         objective = sensingObjective;
-        objectiveFunction = @(x, y) 0;
-        objectiveDiscretizationStep = 0.01;
 
         % Agents
-        minAgents = 3;
+        minAgents = 3; % Minimum number of agents to be randomly generated
-        maxAgents = 9;
+        maxAgents = 9; % Maximum number of agents to be randomly generated
-        agents = cell(1, 0);
+        sensingLength = 0.05; % length parameter used by sensing function
+        agents = cell(0, 1);
         
         % Collision
-        minCollisionRange = 0.1;
+        minCollisionRange = 0.1; % Minimum randomly generated collision geometry size
-        maxCollisionRange = 0.5;
+        maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
         collisionRanges = NaN;
 
         % Communications
-        comRange = 5;
+        comRange = 5; % Maximum range between agents that forms a communications link
     end
 
     % Setup for each test
     methods (TestMethodSetup)
         % Generate a random domain
         function tc = setDomain(tc)
-            % random integer-sized domain within [-10, 10] in all dimensions
+            % random integer-sized cube domain ranging from [0, 5 -> 25]
-            tc.domain = tc.domain.initialize(ceil([rand * -10, rand * 10; rand * -10, rand * 10; rand * -10, rand * 10]), REGION_TYPE.DOMAIN, "Domain");
+            % in all dimensions
+            L = ceil(5 + rand * 10 + rand * 10);
+            tc.domain = tc.domain.initialize([zeros(1, 3); L * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
         end
         % Generate a random sensing objective within that domain
         function tc = setSensingObjective(tc)
-            mu = tc.domain.random();
+            % Using a bivariate normal distribution
-            sig = [3, 1; 1, 4];
+            % Set peak position (mean)
-            tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1, 1:2), sig);
+            mu = tc.domain.minCorner;
-            tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3, 1), tc.objectiveDiscretizationStep);
+            while tc.domain.interiorDistance(mu) < tc.protectedRange
+                mu = tc.domain.random();
+            end
+            mu(3) = 0;
+
+            % Set standard deviations of bivariate distribution
+            sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
+
+            % Define objective
+            tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
         end
-        % Instantiate agents, they will be initialized under different
+        % Instantiate agents
-        % parameters in individual test cases
         function tc = setAgents(tc)
+            % Agents will be initialized under different parameters in
+            % individual test cases
+
+            % Instantiate a random number of agents according to parameters
             for ii = 1:randi([tc.minAgents, tc.maxAgents])
                 tc.agents{ii, 1} = agent;
             end
+
+            % Define random collision ranges for each agent
             tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
         end
     end
@@ -53,149 +77,329 @@ classdef test_miSim < matlab.unittest.TestCase
     methods (Test)
         % Test methods
         function misim_initialization(tc)
-            % randomly create 2-3 constraint geometries
+            % randomly create obstacles
-            nGeom = 1 + randi(2);
+            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
-            tc.constraintGeometries = cell(nGeom, 1);
+            tc.obstacles = cell(nGeom, 1);
-            for ii = 1:size(tc.constraintGeometries, 1)
-                % Instantiate a rectangular prism constraint that spans the
-                % domain's height
-                tc.constraintGeometries{ii, 1} = rectangularPrismConstraint;
 
-                % Randomly come up with constraint geometries until they
+            % Iterate over obstacles to initialize
-                % fit within the domain
+            for ii = 1:size(tc.obstacles, 1)
-                candidateMinCorner = -Inf(3, 1);
+                badCandidate = true;
-                candidateMaxCorner = Inf(3, 1);
+                while badCandidate
+                    % Instantiate a rectangular prism obstacle
+                    tc.obstacles{ii} = rectangularPrism;
     
-                % make sure the obstacles don't contain the sensing objective
+                    % Randomly generate min corner for the obstacle
-                obstructs = true;
+                    candidateMinCorner = tc.domain.random();
-                while obstructs
+                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
     
-                    % Make sure the obstacle is in the domain
+                    % Randomly select a corresponding maximum corner that
-                    while any(candidateMinCorner(1:2, 1) < tc.domain.minCorner(1:2, 1)) 
+                    % satisfies min/max obstacle size specifications
-                        candidateMinCorner = tc.domain.minCorner(1:3, 1) + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1); -Inf]; % random spots on the ground
+                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
+                
+                    % Initialize obstacle
+                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
+
+                    % Check if the obstacle intersects with any existing
+                    % obstacles
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
+                            violation = true;
+                            break;
+                        end
                     end
-                    while any(candidateMaxCorner(1:2, 1) > tc.domain.maxCorner(1:2, 1))
+                    if violation
-                        candidateMaxCorner = [candidateMinCorner(1:2, 1); 0] + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1) ./ 2; Inf]; % halved to keep from being excessively large
+                        continue;
                     end
 
-                    % once a domain-valid obstacle has been found, make
+                    % Make sure that the obstacles are fully contained by 
-                    % sure it doesn't obstruct the sensing target
+                    % the domain
-                    if all(candidateMinCorner(1:2, 1)' <= tc.objective.groundPos) && all(candidateMaxCorner(1:2, 1)' >= tc.objective.groundPos)
+                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
-                        % reset to try again
+                        continue;
-                        candidateMinCorner = -Inf(3, 1);
-                        candidateMaxCorner = Inf(3, 1);
-                    else
-                        obstructs = false;
                     end
+
+                    % Make sure that the obstacles don't cover the sensing
+                    % objective
+                    if obstacleCoversObjective(tc.objective, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles aren't too close to the
+                    % sensing objective
+                    if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange)
+                        continue;
+                    end
+                    
+                    badCandidate = false;
                 end
-
-                % Reduce infinite dimensions to the domain
-                candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
-                candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
-
-                % Initialize constraint geometry
-                tc.constraintGeometries{ii, 1} = tc.constraintGeometries{ii, 1}.initialize([candidateMinCorner, candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
             end
 
-            % Repeat this until a connected set of agent initial conditions
+            % Add agents individually, ensuring that each addition does not
-            % is found by random chance
+            % invalidate the initialization setup
-            connected = false;
+            for ii = 1:size(tc.agents, 1)
-            while ~connected
+                initInvalid = true;
-                % Randomly place agents in the domain
+                while initInvalid
-                for ii = 1:size(tc.agents, 1)
+                    candidatePos = [tc.objective.groundPos, 0];
-                    posInvalid = true;
+                    % Generate a random position for the agent based on
-                    while posInvalid
+                    % existing agent positions
-                        % Initialize the agent into a random spot in the domain
+                    if ii == 1
-                        candidatePos = tc.domain.random();
+                        while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
-                        candidateGeometry = rectangularPrismConstraint;
+                            candidatePos = tc.domain.random();
-                        tc.agents{ii, 1} = tc.agents{ii, 1}.initialize(candidatePos, zeros(1, 3), eye(3), candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii, 1) * ones(1, 3); candidatePos + tc.collisionRanges(ii, 1) * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), ii, sprintf("Agent %d", ii));
+                        end
+                    else
+                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
+                    end
 
-                        % Check obstacles to confirm that none are violated
+                    % Make sure that the candidate position is within the
-                        for jj = 1:size(tc.constraintGeometries, 1)
+                    % domain
-                            inside = false;
+                    if ~tc.domain.contains(candidatePos)
-                            if tc.constraintGeometries{jj, 1}.contains(tc.agents{ii, 1}.pos)
+                        continue;
-                                % Found a violation, stop checking
+                    end
-                                inside = true;
+
-                                break;
+                    % Make sure that the candidate position does not crowd
+                    % the sensing objective and create boring scenarios
+                    if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                        continue;
+                    end
+
+                    % Make sure that there exist unobstructed lines of sight at
+                    % appropriate ranges to form a connected communications 
+                    % graph between the agents
+                    connections = false(1, ii - 1);
+                    for jj = 1:(ii - 1)
+                        if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
+                            % Check new agent position against all existing
+                            % agent positions for communications range
+                            connections(jj) = true;
+                            for kk = 1:size(tc.obstacles, 1)
+                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
+                                    connections(jj) = false;
+                                end
                             end
                         end
-    
-                        % Agent is inside obstacle, try again
-                        if inside
-                            continue;
-                        end
-    
-                        % Create a collision geometry for this agent
-                        candidateGeometry = rectangularPrismConstraint;
-                        candidateGeometry = candidateGeometry.initialize([tc.agents{ii, 1}.pos - 0.1 * ones(1, 3); tc.agents{ii, 1}.pos + 0.1 * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
-    
-                        % Check previously placed agents for collisions
-                        for jj = 1:(ii - 1)
-                            % Check if previously defined agents collide with
-                            % this one
-                            colliding = false;
-                            if candidateGeometry.contains(tc.agents{jj, 1}.pos)
-                                % Found a violation, stop checking
-                                colliding = true;
-                                break;
-                            end
-                        end
-    
-                        % Agent is colliding with another, try again
-                        if ii ~= 1 && colliding
-                            continue;
-                        end
-    
-                        % Allow to proceed since no obstacle/collision
-                        % violations were found
-                        posInvalid = false;
                     end
-                end
 
-                % Collect all agent positions
+                    % New agent must be connected to an existing agent to
-                posArray = arrayfun(@(x) x{1}.pos, tc.agents, 'UniformOutput', false);
+                    % be valid
-                posArray = reshape([posArray{:}], size(tc.agents, 1), 3);  
+                    if ii ~= 1 && ~any(connections)
+                        continue;
+                    end
 
-                % Communications checks
+                    % Initialize candidate agent
-                adjacency = false(size(tc.agents, 1), size(tc.agents, 1));
+                    candidateGeometry = rectangularPrism;
-                for ii = 1:size(tc.agents, 1)
+                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), @(r) 0.5, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
-                    % Compute distance from each to all agents
+                    
-                    for jj = 1:(size(tc.agents, 1))
+                    % Make sure candidate agent doesn't collide with
-                        if norm(posArray(ii, 1:3) - posArray(jj, 1:3)) <= tc.comRange
+                    % domain
-                            adjacency(ii, jj) = true;
+                    violation = false;
+                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
+                        % Check if collision geometry exits domain
+                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                            violation = true;
+                            break;
                         end
                     end
-                end
+                    if violation
+                        continue;
+                    end
 
-                % Check connectivity
+                    % Make sure candidate doesn't collide with obstacles
-                G = graph(adjacency);
+                    violation = false;
-                connected = all(conncomp(G) == 1);
+                    for kk = 1:size(tc.obstacles, 1)
+                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure candidate doesn't collide with existing
+                    % agents
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Candidate agent is valid, store to pass in to sim
+                    initInvalid = false;
+                    tc.agents{ii} = newAgent;
+                end
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.constraintGeometries);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
+        end
+        function misim_run(tc)
+            % randomly create obstacles
+            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
+            tc.obstacles = cell(nGeom, 1);
 
-            % Plot domain
+            % Iterate over obstacles to initialize
-            f = tc.testClass.domain.plotWireframe;
+            for ii = 1:size(tc.obstacles, 1)
+                badCandidate = true;
+                while badCandidate
+                    % Instantiate a rectangular prism obstacle
+                    tc.obstacles{ii} = rectangularPrism;
     
-            % Set plotting limits to focus on the domain
+                    % Randomly generate min corner for the obstacle
-            xlim([tc.testClass.domain.minCorner(1) - 0.5, tc.testClass.domain.maxCorner(1) + 0.5]);
+                    candidateMinCorner = tc.domain.random();
-            ylim([tc.testClass.domain.minCorner(2) - 0.5, tc.testClass.domain.maxCorner(2) + 0.5]);
+                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
-            zlim([tc.testClass.domain.minCorner(3) - 0.5, tc.testClass.domain.maxCorner(3) + 0.5]);
     
-            % Plot constraint geometries
+                    % Randomly select a corresponding maximum corner that
-            for ii = 1:size(tc.testClass.constraintGeometries, 1)
+                    % satisfies min/max obstacle size specifications
-                tc.testClass.constraintGeometries{ii, 1}.plotWireframe(f);
+                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
+                
+                    % Initialize obstacle
+                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
+
+                    % Check if the obstacle intersects with any existing
+                    % obstacles
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure that the obstacles are fully contained by 
+                    % the domain
+                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles don't cover the sensing
+                    % objective
+                    if obstacleCoversObjective(tc.objective, tc.obstacles{ii})
+                        continue;
+                    end
+
+                    % Make sure that the obstacles aren't too close to the
+                    % sensing objective
+                    if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange)
+                        continue;
+                    end
+                    
+                    badCandidate = false;
+                end
             end
 
-            % Plot objective gradient
+            % Add agents individually, ensuring that each addition does not
-            f = tc.testClass.objective.plot(f);
+            % invalidate the initialization setup
+            for ii = 1:size(tc.agents, 1)
+                initInvalid = true;
+                while initInvalid
+                    candidatePos = [tc.objective.groundPos, 0];
+                    % Generate a random position for the agent based on
+                    % existing agent positions
+                    if ii == 1
+                        while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                            candidatePos = tc.domain.random();
+                        end
+                    else
+                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
+                    end
 
-            % Plot agents and their collision geometries
+                    % Make sure that the candidate position is within the
-            for ii = 1:size(tc.testClass.agents, 1)
+                    % domain
-                f = tc.testClass.agents{ii, 1}.plot(f);
+                    if ~tc.domain.contains(candidatePos)
-                f = tc.testClass.agents{ii, 1}.collisionGeometry.plotWireframe(f);
+                        continue;
+                    end
+
+                    % Make sure that the candidate position does not crowd
+                    % the sensing objective and create boring scenarios
+                    if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                        continue;
+                    end
+
+                    % Make sure that there exist unobstructed lines of sight at
+                    % appropriate ranges to form a connected communications 
+                    % graph between the agents
+                    connections = false(1, ii - 1);
+                    for jj = 1:(ii - 1)
+                        if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
+                            % Check new agent position against all existing
+                            % agent positions for communications range
+                            connections(jj) = true;
+                            for kk = 1:size(tc.obstacles, 1)
+                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
+                                    connections(jj) = false;
+                                end
+                            end
+                        end
+                    end
+
+                    % New agent must be connected to an existing agent to
+                    % be valid
+                    if ii ~= 1 && ~any(connections)
+                        continue;
+                    end
+
+                    % Initialize candidate agent
+                    candidateGeometry = rectangularPrism;
+                    newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), @basicGradientAscent, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
+                    
+                    % Make sure candidate agent doesn't collide with
+                    % domain
+                    violation = false;
+                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
+                        % Check if collision geometry exits domain
+                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure candidate doesn't collide with obstacles
+                    violation = false;
+                    for kk = 1:size(tc.obstacles, 1)
+                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Make sure candidate doesn't collide with existing
+                    % agents
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
+
+                    % Candidate agent is valid, store to pass in to sim
+                    initInvalid = false;
+                    tc.agents{ii} = newAgent;
+                end
             end
+
+            % Initialize the simulation
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
+
+            % Run simulation loop
+            [tc.testClass, f] = tc.testClass.run(f);
         end
     end
 end