fixed lesser neighbor algorithm

@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 = obj.adjacency;
+    constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
-    % 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
-        % indexed agent provides connectivity already
+        % Find lesser neighbors of each agent
+        % Lesser neighbors of ii are jj < ii in range of ii
+        lesserNeighbors = [];
         for jj = 1:(ii - 1)
-            for kk = 1:(jj - 1)
-                constraintAdjacencyMatrix(ii, kk) = false;
-                constraintAdjacencyMatrix(kk, ii) = false;
+            if obj.adjacency(ii, jj)
+                lesserNeighbors = [lesserNeighbors, jj];
+            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
+
+        % 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
-
-    obj.constraintAdjacencyMatrix = constraintAdjacencyMatrix;
+    cComp = sort(cComp);
 end