fixed lesser neighbor algorithm

@agent/agent.m

@@ -28,6 +28,8 @@ classdef agent
         % Communication
         comRange = NaN;
         commsGeometry = spherical;
+        lesserNeighbors = [];
+        
 
         performance = 0;
 

@agent/initialize.m

@@ -22,13 +22,14 @@ function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorMod
     obj.tilt = tilt;
     obj.collisionGeometry = collisionGeometry;
     obj.sensorModel = sensorModel;
+    obj.comRange = comRange;
     obj.guidanceModel = @gradientAscent;
     obj.label = label;
     obj.debug = debug;
     obj.plotCommsGeometry = plotCommsGeometry;
 
     % Add spherical geometry based on com range
-    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
+    obj.commsGeometry = obj.commsGeometry.initialize(obj.pos, obj.comRange, REGION_TYPE.COMMS, sprintf("%s Comms Geometry", obj.label));
 
     if obj.debug
         obj.debugFig = figure;

@miSim/lesserNeighbor.m

@@ -12,20 +12,69 @@ function obj = lesserNeighbor(obj)
     % constraint adjacency matrix
     % Place that choice in the constraint adjacency matrix
 
-    % Begin with all possible connections and trim down
+    constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
-    constraintAdjacencyMatrix = obj.adjacency;
 
-    % Iterate over each agent (by increasing index)
     for ii = 1:size(obj.agents, 1)
-        % Iterate over each agent of lesser index and see if a higher
+        % Find lesser neighbors of each agent
-        % indexed agent provides connectivity already
+        % Lesser neighbors of ii are jj < ii in range of ii
+        lesserNeighbors = [];
         for jj = 1:(ii - 1)
-            for kk = 1:(jj - 1)
+            if obj.adjacency(ii, jj)
-                constraintAdjacencyMatrix(ii, kk) = false;
+                lesserNeighbors = [lesserNeighbors, jj];
-                constraintAdjacencyMatrix(kk, ii) = false;
             end
         end
+        obj.agents{ii}.lesserNeighbors = lesserNeighbors;
+        
+        % Early exit for isolated agents
+        if isempty(obj.agents{ii}.lesserNeighbors)
+            continue
+        end
+
+        % Focus on subgraph defined by lesser neighbors
+        subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
+
+        % Find connected components in each agent's subgraph
+        visited = false(size(subgraphAdjacency, 1), 1);
+        components = {};
+        for jj = 1:size(subgraphAdjacency, 1)
+            if ~visited(jj)
+                reachable = bfs(subgraphAdjacency, jj);
+                visited(reachable) = true;
+                components{end+1} = obj.agents{ii}.lesserNeighbors(reachable);
+            end
         end
 
-    obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix;
+        % Connect to the greatest index in each connected component in the
+        % lesser neighborhood of this agent
+        for jj = 1:size(components, 2)
+            constraintAdjacencyMatrix(ii, max(components{jj})) = true;
+            constraintAdjacencyMatrix(max(components{jj}), ii) = true;
+        end
+    end
+    obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix | constraintAdjacencyMatrix';
+end
+
+function cComp = bfs(subgraphAdjacency, startIdx)
+    n = size(subgraphAdjacency, 1);
+    visited = false(1, n);
+    queue = startIdx;
+    cComp = startIdx;
+    visited(startIdx) = true;
+    
+    while ~isempty(queue)
+        current = queue(1);
+        queue(1) = [];
+        
+        % Find all neighbors of current node in the subgraph
+        neighbors = find(subgraphAdjacency(current, :));
+        
+        for neighbor = neighbors
+            if ~visited(neighbor)
+                visited(neighbor) = true;
+                cComp = [cComp, neighbor];
+                queue = [queue, neighbor];
+            end
+        end
+    end
+    cComp = sort(cComp);
 end

test/test_miSim.m

@@ -571,7 +571,8 @@ classdef test_miSim < matlab.unittest.TestCase
             % No communications link should be established
             tc.assertEqual(tc.testClass.adjacency, logical(eye(2)));
         end
-        function test_LNA_example_case(tc)
+        function test_LNA_case_1(tc)
+            % based on example in meeting 
             % No obstacles
             % Fixed 5 agents initial conditions
             % unitary communicaitons radius
@@ -586,16 +587,12 @@ classdef test_miSim < matlab.unittest.TestCase
             % Initialize agent collision geometry
             radius = .01;
             d = 1;
-            geometry1 = spherical;
+            geometry5 = spherical;
-            geometry2 = geometry1;
+            geometry1 = geometry5.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION);
-            geometry3 = geometry2;
+            geometry2 = geometry5.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
-            geometry4 = geometry3;
+            geometry3 = geometry5.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry5 = geometry4;
+            geometry4 = geometry5.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry1 = geometry1.initialize(tc.domain.center + [d, 0, 0], radius, REGION_TYPE.COLLISION);
+            geometry5 = geometry5.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry2 = geometry2.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
-            geometry3 = geometry2.initialize(tc.domain.center + [-d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry4 = geometry2.initialize(tc.domain.center + [-2*d, d, 0], radius, REGION_TYPE.COLLISION);
-            geometry5 = geometry2.initialize(tc.domain.center + [0, d, 0], radius, REGION_TYPE.COLLISION);
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
@@ -619,8 +616,62 @@ classdef test_miSim < matlab.unittest.TestCase
                 [ 1, 1, 0, 0, 0; ...
                   1, 1, 0, 0, 1; ...
                   0, 0, 1, 1, 1;
-                  0, 0, 1, 0, 0;
+                  0, 0, 1, 1, 0;
-                  0, 1, 1, 0, 0; ]));
+                  0, 1, 1, 0, 1; ]));
+        end
+        function test_LNA_case_2(tc)
+            % based on example in paper Asynchronous Local Construction of Bounded-Degree Network Topologies Using Only Neighborhood Information
+            % No obstacles
+            % Fixed 7 agents initial conditions
+            % unitary communicaitons radius
+            % negligible collision 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 = .01;
+            d = 1;
+            geometry7 = spherical;
+            geometry1 = geometry7.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION);
+            geometry2 = geometry7.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], radius, REGION_TYPE.COLLISION);
+            geometry3 = geometry7.initialize(tc.domain.center + [0.9 * d, 0, 0], radius, REGION_TYPE.COLLISION);
+            geometry4 = geometry7.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], radius, REGION_TYPE.COLLISION);
+            geometry5 = geometry7.initialize(tc.domain.center + [0, 0.9 * d, 0], radius, REGION_TYPE.COLLISION);
+            geometry6 = geometry7.initialize(tc.domain.center, radius, REGION_TYPE.COLLISION);
+            geometry7 = geometry7.initialize(tc.domain.center + [d/2, d/2, 0], radius, REGION_TYPE.COLLISION);
+            
+            % 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 = d;
+            tc.agents = {agent; agent; agent; agent; agent; agent; agent;};
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [-0.9 * d/sqrt(2), 0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry1, sensor, commsRadius);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center + [-0.5 * d, 0.25 * d, 0], zeros(1,3), 0, 0, geometry2, sensor, commsRadius);
+            tc.agents{3} = tc.agents{3}.initialize(tc.domain.center + [0.9 * d, 0, 0], zeros(1,3), 0, 0, geometry3, sensor, commsRadius);
+            tc.agents{4} = tc.agents{4}.initialize(tc.domain.center + [0.9 * d/sqrt(2), -0.9 * d/sqrt(2), 0], zeros(1,3), 0, 0, geometry4, sensor, commsRadius);
+            tc.agents{5} = tc.agents{5}.initialize(tc.domain.center + [0, 0.9 * d, 0], zeros(1,3), 0, 0, geometry5, sensor, commsRadius);
+            tc.agents{6} = tc.agents{6}.initialize(tc.domain.center, zeros(1,3), 0, 0, geometry6, sensor, commsRadius);
+            tc.agents{7} = tc.agents{7}.initialize(tc.domain.center + [d/2, d/2, 0], zeros(1,3), 0, 0, geometry7, sensor, commsRadius);
+
+            % Initialize the simulation
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, false, false);
+
+            % Constraint adjacency matrix defined by LNA should be as follows
+            tc.assertEqual(tc.testClass.constraintAdjacencyMatrix, logical( ...
+                [ 1, 1, 0, 0, 0, 0, 0; ...
+                  1, 1, 0, 0, 1, 0, 0; ...
+                  0, 0, 1, 1, 0, 0, 0;
+                  0, 0, 1, 1, 0, 1, 0;
+                  0, 1, 0, 0, 1, 1, 0;
+                  0, 0, 0, 1, 1, 1, 1;
+                  0, 0, 0, 0, 0, 1, 1; ]));
         end
     end