Added connections to plots

agent.m

@@ -14,16 +14,20 @@ classdef agent
 
         % Collision
         collisionGeometry;
+
+        % Communication
+        comRange = NaN;
     end
 
     methods (Access = public)
-        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, index, label)
+        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, 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};
+                comRange (1, 1) double = NaN;
                 index (1, 1) double = NaN;
                 label (1, 1) string = "";
             end
@@ -35,6 +39,7 @@ classdef agent
             obj.vel = vel;
             obj.cBfromC = cBfromC;
             obj.collisionGeometry = collisionGeometry;
+            obj.comRange = comRange;
             obj.index = index;
             obj.label = label;
         end

miSim.m

@@ -7,6 +7,7 @@ classdef miSim
         objective = sensingObjective;
         obstacles = cell(0, 1); % geometries that define obstacles within the domain
         agents = cell(0, 1); % agents that move within the domain
+        adjacency = NaN; % Adjacency matrix representing communications network graph
     end
 
     methods (Access = public)
@@ -34,6 +35,67 @@ classdef miSim
             %% Define agents
             obj.agents = agents;
 
+            %% Compute adjacency matrix
+            obj = obj.updateAdjacency();
+
+        end
+        function obj = updateAdjacency(obj)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+            end
+
+            % Initialize assuming only self-connections
+            A = logical(eye(size(obj.agents, 1)));
+
+            % Check lower triangle off-diagonal connections
+            for ii = 2:size(A, 1)
+                for jj = 1:(ii - 1)
+                    if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) <= min([obj.agents{ii}.comRange, obj.agents{jj}.comRange])
+                        A(ii, jj) = true;
+                    end
+                end
+            end
+
+            obj.adjacency = A;
+        end
+        function f = plotNetwork(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
+
+            % Iterate over lower triangle off-diagonal region of the 
+            % adjacency matrix to plot communications links between agents
+            X = []; Y = []; Z = [];
+            for ii = 2:size(obj.adjacency, 1)
+                for jj = 1:(ii - 1)
+                    if obj.adjacency(ii, jj)
+                        X = [X; obj.agents{ii}.pos(1), obj.agents{jj}.pos(1)];
+                        Y = [Y; obj.agents{ii}.pos(2), obj.agents{jj}.pos(2)];
+                        Z = [Z; obj.agents{ii}.pos(3), obj.agents{jj}.pos(3)];
+                    end
+                end
+            end
+            X = X'; Y = Y'; Z = Z';
+
+            % Plot the connections
+            hold(f.CurrentAxes, "on");
+            o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 1, '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));
+                copyobj(o, f.Children(1).Children(3));
+                copyobj(o, f.Children(1).Children(5));
+            end
         end
     end
 

test_miSim.m

@@ -184,7 +184,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)), 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)), tc.comRange, ii, sprintf("Agent %d", ii));
                     
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -255,6 +255,9 @@ classdef test_miSim < matlab.unittest.TestCase
                 f = tc.testClass.agents{ii}.plot(f);
                 f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
             end
+
+            % Plot communication links
+            f = tc.testClass.plotNetwork(f);
         end
     end
 end