added video writing feature

.gitignore

@@ -38,3 +38,6 @@ codegen/
 # SimBiology backup files
 *.sbproj.backup
 *.sbproj.bak
+
+# Sandbox contents
+sandbox/*

agent.m

@@ -6,27 +6,34 @@ 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, comRange, 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) {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 = "";
@@ -39,16 +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
 
@@ -63,10 +126,15 @@ classdef agent
             % Check if this is a tiled layout figure
             if strcmp(f.Children(1).Type, 'tiledlayout')
                 % Add to other perspectives
-                copyobj(o, f.Children(1).Children(2));
+                o = [o; copyobj(o(1), f.Children(1).Children(2))];
-                copyobj(o, f.Children(1).Children(3));
+                o = [o; copyobj(o(1), f.Children(1).Children(3))];
-                copyobj(o, f.Children(1).Children(5));
+                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

@@ -9,7 +9,7 @@ function f = firstPlotSetup(f)
         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("Top-down Perspective");
+        title(f.Children(1).Children(1), "Top-down Perspective");
 
         % Communications graph
         nexttile(3, [1, 1]);
@@ -17,33 +17,33 @@ function f = firstPlotSetup(f)
         axis(f.Children(1).Children(1), "image");
         grid(f.Children(1).Children(1), "off");
         view(f.Children(1).Children(1), 0, 0);
-        title("Network Graph");
+        title(f.Children(1).Children(1), "Network Graph");
 
         % 3D view
-        title("3D Perspective");
         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
-        title("Side-on Perspective");
         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
-        title("Front-on Perspective");
         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

@@ -1,23 +1,25 @@
 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
 
-        footprint = NaN(4, 2);
+        % Plotting
+        lines;
     end
 
     methods (Access = public)
@@ -161,12 +163,13 @@ classdef rectangularPrism
 
             c = (tmax >= 0) && (tmin <= 1);
         end
-        function f = plotWireframe(obj, f)
+        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
 
@@ -186,10 +189,12 @@ classdef rectangularPrism
             % Check if this is a tiled layout figure
             if strcmp(f.Children(1).Type, 'tiledlayout')
                 % Add to other perspectives
-                copyobj(o, f.Children(1).Children(2));
+                o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
-                copyobj(o, f.Children(1).Children(3));
+                o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
-                copyobj(o, f.Children(1).Children(5));
+                o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
             end
+
+            obj.lines = o;
         end
     end
 end

miSim.m

@@ -3,6 +3,8 @@ classdef miSim
 
     % Simulation parameters
     properties (SetAccess = private, GetAccess = public)
+        timestep = NaN; % delta time interval for simulation iterations
+        maxIter = NaN; % maximum number of simulation iterations
         domain = rectangularPrism;
         objective = sensingObjective;
         obstacles = cell(0, 1); % geometries that define obstacles within the domain
@@ -10,34 +12,149 @@ classdef miSim
         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.mp4')));
+        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, obstacles)
+        function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
                 domain (1, 1) {mustBeGeometry};
                 objective (1, 1) {mustBeA(objective, 'sensingObjective')};
                 agents (:, 1) cell {mustBeAgents};
+                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 domain
+            % Define simulation time parameters
+            obj.timestep = timestep;
+            obj.maxIter = maxIter;
+
+            % Define domain
             obj.domain = domain;
 
-            %% Add geometries representing obstacles within the domain
+            % Add geometries representing obstacles within the domain
             obj.obstacles = obstacles;
 
-            %% Define objective
+            % Define objective
             obj.objective = objective;
 
-            %% Define agents
+            % Define agents
             obj.agents = agents;
 
-            %% Compute adjacency matrix
+            % 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)
@@ -54,19 +171,26 @@ classdef miSim
             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])
-                        A(ii, jj) = true;
+                        % 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
 
             obj.adjacency = A | A';
         end
-        function f = plotNetwork(obj, f)
+        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
 
@@ -86,36 +210,34 @@ classdef miSim
 
             % Plot the connections
             hold(f.CurrentAxes, "on");
-            o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 1, 'LineStyle', '--');
+            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
-                copyobj(o, f.Children(1).Children(2));
+                o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
-                copyobj(o, f.Children(1).Children(3));
+                o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
-                copyobj(o, f.Children(1).Children(5));
+                o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
             end
+
+            obj.connectionsPlot = o;
         end
-        function f = plotGraph(obj, f)
+        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');
 
-            % Check if this is a tiled layout figure
+            % Plot graph object
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
-                o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k');
-            else
-                o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k');
-            end
-
         end
     end
 

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/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/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"/>

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

test_miSim.m

@@ -4,6 +4,8 @@ classdef test_miSim < matlab.unittest.TestCase
 
         % Domain
         domain = rectangularPrism; % domain geometry
+        maxIter = 1000;
+        timestep = 0.05
 
         % Obstacles
         minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
@@ -20,6 +22,7 @@ classdef test_miSim < matlab.unittest.TestCase
         % Agents
         minAgents = 3; % Minimum number of agents to be randomly generated
         maxAgents = 9; % Maximum number of agents to be randomly generated
+        sensingLength = 0.05; % length parameter used by sensing function
         agents = cell(0, 1);
         
         % Collision
@@ -74,7 +77,7 @@ classdef test_miSim < matlab.unittest.TestCase
     methods (Test)
         % Test methods
         function misim_initialization(tc)
-            % randomly create 2-3 obstacles
+            % randomly create obstacles
             nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
             tc.obstacles = cell(nGeom, 1);
 
@@ -184,7 +187,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
                     % 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)), tc.comRange, ii, sprintf("Agent %d", ii));
+                    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));
                     
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -232,35 +235,171 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.obstacles);
+            [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;
+    
+                    % Randomly generate min corner for the obstacle
+                    candidateMinCorner = tc.domain.random();
+                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
+    
+                    % Randomly select a corresponding maximum corner that
+                    % satisfies min/max obstacle size specifications
+                    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));
 
-            % Set plotting limits to focus on the domain
+                    % Check if the obstacle intersects with any existing
-            xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]);
+                    % obstacles
-            ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]);
+                    violation = false;
-            zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]);
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
+                            violation = true;
+                            break;
+                        end
+                    end
+                    if violation
+                        continue;
+                    end
 
-            % Plot obstacles
+                    % Make sure that the obstacles are fully contained by 
-            for ii = 1:size(tc.testClass.obstacles, 1)
+                    % the domain
-                tc.testClass.obstacles{ii}.plotWireframe(f);
+                    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}.plot(f);
+                    if ~tc.domain.contains(candidatePos)
-                f = tc.testClass.agents{ii}.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
 
-            % Plot communication links
+            % Initialize the simulation
-            f = tc.testClass.plotNetwork(f);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
 
-            % Plot abstract network graph
+            % Run simulation loop
-            f = tc.testClass.plotGraph(f);
+            [tc.testClass, f] = tc.testClass.run(f);
         end
     end
 end