fixed comms LOS obstruction by obstacles

@miSim/initialize.m

@@ -32,8 +32,10 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
     % Add an additional obstacle spanning the domain's footprint to 
     % represent the minimum allowable altitude
     obj.minAlt = minAlt;
-    obj.obstacles{end + 1, 1} = rectangularPrism;
+    if obj.minAlt > 0
-    obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+        obj.obstacles{end + 1, 1} = rectangularPrism;
+        obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+    end
 
     % Define objective
     obj.objective = objective;

@miSim/updateAdjacency.m

@@ -7,26 +7,41 @@ function obj = updateAdjacency(obj)
     end
 
     % Initialize assuming only self-connections
-    A = logical(eye(size(obj.agents, 1)));
+    A = true(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])
+            % Check that agents are not out of range
-                % Make sure that obstacles don't obstruct the line
+            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.comRange, obj.agents{jj}.comRange]);
-                % of sight, breaking the connection
+                A(ii, jj) = false; % comm range violation
-                for kk = 1:size(obj.obstacles, 1)
+                continue;
-                    if ~obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos)
+            end
-                        A(ii, jj) = true;
+
-                    end
+            % Check that agents do not have their line of sight obstructed
-                end
+            for kk = 1:size(obj.obstacles, 1)
-                % need extra handling for cases with no obstacles
+                if obj.obstacles{kk}.containsLine(obj.agents{jj}.pos, obj.agents{ii}.pos)
-                if isempty(obj.obstacles)
+                    A(ii, jj) = false;
-                    A(ii, jj) = true;
                 end
             end
+
+
+
+            % 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 A(ii, jj) && obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos)
+            %             A(ii, jj) = false;
+            %         end
+            %     end
+            %     % need extra handling for cases with no obstacles
+            %     if isempty(obj.obstacles)
+            %         A(ii, jj) = true;
+            %     end
+            % end
         end
     end
 
-    obj.adjacency = A | A';
+    obj.adjacency = A & A';
 end

geometries/@rectangularPrism/containsLine.m

@@ -9,33 +9,66 @@ function c = containsLine(obj, pos1, pos2)
     end
     
     d = pos2 - pos1;
-    
+
-    % edge case where the line is parallel to the geometry
+    % endpoint contained (trivial case)
-    if abs(d) < 1e-12
+    if obj.contains(pos1) || obj.contains(pos2)
-        % check if it happens to start or end inside or outside of
+        c = true;
-        % the geometry
-        if obj.contains(pos1) || obj.contains(pos2)
-            c = true;
-        else
-            c = false;
-        end
         return;
     end
-    
+
-    tmin = -inf;        
+    % parameterize the line segment to check for an intersection
-    tmax = inf;
+    tMin = 0;
-    
+    tMax = 1;
-    % Standard case
     for ii = 1:3
-        t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
+        % line is parallel to geometry
-        t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii);
+        if abs(d(ii)) < 1e-12
-        tmin = max(tmin, min(t1, t2));
+            if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii)
-        tmax = min(tmax, max(t1, t2));
+                c = false;
-        if tmin > tmax
+                return;
-            c = false;
+            end
-            return;
+        else
+            t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
+            t2 = (obj.maxCorner(ii) - pos1(ii)) / d(ii);
+
+            tLow  = min(t1, t2);
+            tHigh = max(t1, t2);
+
+            tMin = max(tMin, tLow);
+            tMax = min(tMax, tHigh);
+
+            if tMin > tMax
+                c = false;
+                return;
+            end
         end
     end
+    c = true;
     
-    c = (tmax >= 0) && (tmin <= 1);
+    % if abs(d) < 1e-12
-    end
+    %     % 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

test/test_miSim.m

@@ -490,7 +490,7 @@ classdef test_miSim < matlab.unittest.TestCase
         end
         function test_obstacle_avoidance(tc)
             % Fixed single obstacle
-            % Fixed single agent initial conditions
+            % Fixed two agents initial conditions
             % Exaggerated large collision geometries
             % make basic domain
             l = 10; % domain size
@@ -514,8 +514,8 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Initialize agents
             tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*radius, 1, sprintf("Agent %d", 1), false);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 5*radius, 1, sprintf("Agent %d", 1), false);
-            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0] - [0, 1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*radius, 2, sprintf("Agent %d", 2), false);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0] - [0, 1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 5*radius, 2, sprintf("Agent %d", 2), false);
             
             % Initialize obstacles
             obstacleLength = 1;
@@ -523,11 +523,53 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, tc.makeVideo);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 100, tc.obstacles, tc.makeVideo);
             
             % Run the simulation
             tc.testClass.run();
         end
+
+        function test_obstacle_blocks_comms_LOS(tc)
+            % Fixed single obstacle
+            % Fixed two agents initial conditions
+            % Exaggerated large communications radius
+            % make basic domain
+            l = 10; % domain size
+            tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
+
+            % make basic sensing objective
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange);
+        
+            % Initialize agent collision geometry
+            radius = .25;
+            d = 2;
+            geometry1 = spherical;
+            geometry2 = geometry1;
+            geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            
+            % Initialize agent sensor model
+            sensor = sigmoidSensor;
+            alphaDist = l/2; % half of domain length/width
+            sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
+
+            % Initialize agents
+            commsRadius = 5;
+            tc.agents = {agent; agent;};
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, commsRadius, 1, sprintf("Agent %d", 1), false);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, commsRadius, 2, sprintf("Agent %d", 2), false);
+            
+            % Initialize obstacles
+            obstacleLength = 1.5;
+            tc.obstacles{1} = rectangularPrism;
+            tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1");
+
+            % Initialize the simulation
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, tc.makeVideo);
+        
+            % No communications link should be established
+            tc.assertEqual(tc.testClass.adjacency, logical(eye(2)));
+        end
     end
 
     methods