geometries move in plots as sim runs

agent.m

@@ -9,15 +9,19 @@ classdef agent
         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)
@@ -48,29 +52,66 @@ classdef agent
             obj.index = index;
             obj.label = label;
         end
-        function obj = run(obj, objectiveFunction)
+        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, obj.pos, obj.sensingLength);
+            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 f = plot(obj, f)
+        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
 
@@ -85,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

@@ -13,7 +13,7 @@ classdef miSim
     end
 
     methods (Access = public)
-        function obj = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
+        function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
                 domain (1, 1) {mustBeGeometry};
@@ -25,6 +25,7 @@ classdef miSim
             end
             arguments (Output)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
+                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
             end
 
             % Define simulation time parameters
@@ -46,33 +47,87 @@ classdef miSim
             % 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
+            f = obj.plotNetwork(f);
+
+            % Plot abstract network graph
+            f = obj.plotGraph(f);
         end
-        function obj = run(obj)
+        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)';
 
             for ii = 1:size(times, 1)
-                % Get current sim time
+                % 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.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain);
                 end
     
                 % Update adjacency matrix
                 obj = obj.updateAdjacency;
     
                 % Update plots
-
+                f = obj.updatePlots(f);
             end
+
+        end
+        function f = updatePlots(obj, f)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
+            end
+            arguments (Output)
+                f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
+            end
+
+            % Update agent positions, collision geometries, connections
+            for ii = 1:size(obj.agents, 1)
+                obj.agents{ii}.updatePlots();
+            end
+
+            % Update network graph plot
+
+            drawnow;
+
         end
         function obj = updateAdjacency(obj)
             arguments (Input)

sensingFunctions/basicGradientAscent.m

@@ -1,6 +1,7 @@
-function nextPos = basicGradientAscent(objectiveFunction, pos, r)
+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
@@ -39,5 +40,5 @@ function nextPos = basicGradientAscent(objectiveFunction, pos, r)
     
     % Select next position by maximum sensed value
     nextPos = neighborPos(neighborValues == max(neighborValues), :);
-    nextPos = nextPos(1, 1:3); % just in case two get selected, simply pick one
+    nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
 end

test_miSim.m

@@ -235,35 +235,7 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
-
-            % Plot domain
-            f = tc.testClass.domain.plotWireframe;
-
-            % Set plotting limits to focus on the domain
-            xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]);
-            ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]);
-            zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]);
-
-            % Plot obstacles
-            for ii = 1:size(tc.testClass.obstacles, 1)
-                tc.testClass.obstacles{ii}.plotWireframe(f);
-            end
-
-            % Plot objective gradient
-            f = tc.testClass.objective.plot(f);
-
-            % Plot agents and their collision geometries
-            for ii = 1:size(tc.testClass.agents, 1)
-                f = tc.testClass.agents{ii}.plot(f);
-                f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
-            end
-
-            % Plot communication links
-            f = tc.testClass.plotNetwork(f);
-
-            % Plot abstract network graph
-            f = tc.testClass.plotGraph(f);
         end
         function misim_run(tc)
             % randomly create obstacles
@@ -424,39 +396,10 @@ classdef test_miSim < matlab.unittest.TestCase
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
+            [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
-
-            % Plot domain
-            f = tc.testClass.domain.plotWireframe;
-
-            % Set plotting limits to focus on the domain
-            xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]);
-            ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]);
-            zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]);
-
-            % Plot obstacles
-            for ii = 1:size(tc.testClass.obstacles, 1)
-                tc.testClass.obstacles{ii}.plotWireframe(f);
-            end
-
-            % Plot objective gradient
-            f = tc.testClass.objective.plot(f);
-
-            % Plot agents and their collision geometries
-            for ii = 1:size(tc.testClass.agents, 1)
-                f = tc.testClass.agents{ii}.plot(f);
-                f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
-            end
-
-            % Plot communication links
-            f = tc.testClass.plotNetwork(f);
-
-            % Plot abstract network graph
-            f = tc.testClass.plotGraph(f);
 
             % Run simulation loop
-            tc.testClass.run();
+            [tc.testClass, f] = tc.testClass.run(f);
-            
         end
     end
 end