fixed bug allowing obstructed coms connections

agent.m

@@ -20,13 +20,14 @@ classdef agent
     end
 
     methods (Access = public)
-        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, comRange, index, label)
+        function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, 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')}
                 comRange (1, 1) double = NaN;
                 index (1, 1) double = NaN;
                 label (1, 1) string = "";

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
@@ -11,33 +13,58 @@ classdef miSim
     end
 
     methods (Access = public)
-        function obj = initialize(obj, domain, objective, agents, obstacles)
+        function obj = 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')};
             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();
 
+        end
+        function obj = run(obj)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+            end
+            arguments (Output)
+                obj (1, 1) {mustBeA(obj, 'miSim')};
+            end
+            keyboard
+            % Iterate over agents to simulate their motion
+            for ii = 1:size(obj.agents, 1)
+                obj.agents{ii}
+            end
+
+            % Update adjacency matrix
+            obj = obj.updateAdjacency;
+
+            % Update plots
+
         end
         function obj = updateAdjacency(obj)
             arguments (Input)
@@ -54,10 +81,16 @@ 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])
+                        % 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
@@ -86,7 +119,7 @@ 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
@@ -111,11 +144,10 @@ classdef miSim
 
             % Check if this is a tiled layout figure
             if strcmp(f.Children(1).Type, 'tiledlayout')
-                o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k');
+                o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
             else
-                o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k');
+                o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
             end
-
         end
     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
@@ -74,7 +76,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 +186,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.comRange, ii, sprintf("Agent %d", ii));
                     
                     % Make sure candidate agent doesn't collide with
                     % domain
@@ -232,7 +234,7 @@ 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 = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
 
             % Plot domain
             f = tc.testClass.domain.plotWireframe;
@@ -262,5 +264,198 @@ classdef test_miSim < matlab.unittest.TestCase
             % Plot abstract network graph
             f = tc.testClass.plotGraph(f);
         end
+        function misim_run(tc)
+            % randomly create obstacles
+            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
+            tc.obstacles = cell(nGeom, 1);
+
+            % Iterate over obstacles to initialize
+            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));
+
+                    % 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
+
+            % Add agents individually, ensuring that each addition does not
+            % 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
+
+                    % Make sure that the candidate position is within the
+                    % domain
+                    if ~tc.domain.contains(candidatePos)
+                        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)), @(r) 0.5, 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 = 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();
+            
+        end
     end
 end