fixed and verified communications constraint

@miSim/constrainMotion.m

@@ -102,21 +102,17 @@ function [obj] = constrainMotion(obj)
     for ii = 1:(size(obj.agents, 1) - 1)
         for jj = (ii + 1):size(obj.agents, 1)
             if obj.constraintAdjacencyMatrix(ii, jj)
-                % d = agents(ii).pos - agents(jj).pos;
-                % h = agents(ii).commsGeometry.radius^2 - dot(d,d);
-                % 
-                % A(kk, (3*ii-2):(3*ii)) =  2*d;
-                % A(kk, (3*jj-2):(3*jj)) = -2*d;
-                % b(kk) = obj.barrierGain * h^3;
-                % 
-                % kk = kk + 1;
+                hComms(ii, jj) = min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])^2 - norm(agents(ii).pos - agents(jj).pos)^2;
 
-                hComms(ii, jj) = agents(ii).commsGeometry.radius^2 - norm(agents(ii).pos - agents(jj).pos)^2;
-
-                A(kk, (3 * ii - 2):(3 * ii)) = 2 * (agents(ii).pos - agents(jj).pos);
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (agents(ii).pos - agents(jj).pos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
-                b(kk) = obj.barrierGain * hComms(ii, jj)^3;
-                kk = kk + 1;
+                b(kk) = obj.barrierGain * hComms(ii, jj);
+
+                % dVNominal = v(ii, 1:3) - v(jj, 1:3); % nominal velocities
+                % h_dot_nom = -2 * (agents(ii).pos - agents(jj).pos) * dVNominal';
+                % b(kk) = -h_dot_nom + obj.barrierGain * hComms(ii, jj)^3;
+
+                kk = kk + 1; 
             end
         end
     end

@miSim/lesserNeighbor.m

@@ -6,12 +6,7 @@ function obj = lesserNeighbor(obj)
         obj (1, 1) {mustBeA(obj, 'miSim')};
     end
 
-    % Check possible connections from adjacency matrix
-    % Choose connections which fully connect network by selecting maximum
-    % indices according to the previous columns (or rows) of the new
-    % constraint adjacency matrix
-    % Place that choice in the constraint adjacency matrix
-
+    % initialize solution with self-connections only
     constraintAdjacencyMatrix = logical(eye(size(obj.agents, 1)));
 
     for ii = 1:size(obj.agents, 1)
@@ -34,6 +29,7 @@ function obj = lesserNeighbor(obj)
         subgraphAdjacency = obj.adjacency(obj.agents{ii}.lesserNeighbors, obj.agents{ii}.lesserNeighbors);
 
         % Find connected components in each agent's subgraph
+        % TODO: rewrite this using matlab "conncomp" function?
         visited = false(size(subgraphAdjacency, 1), 1);
         components = {};
         for jj = 1:size(subgraphAdjacency, 1)

@miSim/miSim.m

@@ -15,7 +15,8 @@ classdef miSim
         constraintAdjacencyMatrix = NaN; % Adjacency matrix representing desired lesser neighbor connections
         sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
-        performance = 0; % cumulative sensor performance
+        perf; % sensor performance timeseries array
+        performance = 0; % simulation performance timeseries vector
         barrierGain = 100; % collision avoidance parameter
         minAlt = 1; % minimum allowed altitude constraint
 
@@ -25,7 +26,6 @@ classdef miSim
     properties (Access = private)
         % Sim
         t = NaN; % current sim time
-        perf; % sensor performance timeseries array
         times;
         partitioningTimes;
 
@@ -61,6 +61,7 @@ classdef miSim
         [obj] = plotGraph(obj);
         [obj] = plotTrails(obj);
         [obj] = updatePlots(obj, updatePartitions);
+        validate(obj);
     end
     methods (Access = private)
         [v] = setupVideoWriter(obj);

@miSim/run.m

@@ -18,6 +18,9 @@ function [obj] = run(obj)
         obj.timestepIndex = ii;
         fprintf("Sim Time: %4.2f (%d/%d)\n", obj.t, ii, obj.maxIter + 1);
 
+        % Validate current simulation configuration
+        obj.validate();
+
         % Check if it's time for new partitions
         updatePartitions = false;
         if ismember(obj.t, obj.partitioningTimes)

@miSim/updateAdjacency.m

@@ -13,7 +13,7 @@ function obj = updateAdjacency(obj)
     for ii = 2:size(A, 1)
         for jj = 1:(ii - 1)
             % Check that agents are not out of range
-            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.comRange, obj.agents{jj}.comRange]);
+            if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius])
                 A(ii, jj) = false; % comm range violation
                 continue;
             end
@@ -31,6 +31,5 @@ function obj = updateAdjacency(obj)
 
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, 'all')
         warning("Eliminated network connections that were necessary");
-        keyboard
     end
 end

@miSim/validate.m

@@ -0,0 +1,12 @@
+function validate(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+    end
+
+    if max(conncomp(graph(obj.adjacency))) ~= 1
+        warning("Network is not connected");
+    end
+
+end