included features from SPAWC 2026 branch

.gitignore

@@ -48,6 +48,7 @@ sandbox/*
 
 # Figures
 *.fig
+*.png
 
 # Python Virtual Environment
 aerpaw/venv/

@agent/initialize.m

@@ -15,6 +15,7 @@ function obj = initialize(obj, pos, collisionGeometry, sensorModel, comRange, ma
     end
 
     obj.pos = pos;
+    obj.lastPos = pos;
     obj.vel = zeros(1, 3);
     obj.lastVel = zeros(1, 3);
     obj.collisionGeometry = collisionGeometry;

@agent/run.m

@@ -14,6 +14,13 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
         obj (1, 1) {mustBeA(obj, "agent")};
     end
 
+    % Always update lastPos/lastVel so constrainMotion evaluates barriers at
+    % the correct (most recent) position, even when this agent has no partition.
+    obj.lastPos = obj.pos;
+    if useDoubleIntegrator
+        obj.lastVel = obj.vel;
+    end
+
     % Collect objective function values across partition
     partitionMask = partitioning == index;
     if ~any(partitionMask(:))
@@ -79,10 +86,8 @@ function obj = run(obj, domain, partitioning, timestepIndex, index, agents, useD
     gradNorm = norm(gradC);
 
     % Compute unconstrained next state
-    obj.lastPos = obj.pos;
     if useDoubleIntegrator
         % Double-integrator: gradient produces desired acceleration with damping
-        obj.lastVel = obj.vel;
         if gradNorm < 1e-100
             a_gradient = zeros(1, 3);
         else

@miSim/constrainMotion.m

@@ -39,10 +39,10 @@ function [obj] = constrainMotion(obj)
     h(logical(eye(nAgents))) = 0; % self value is 0
     for ii = 1:(nAgents - 1)
         for jj = (ii + 1):nAgents
-            h(ii, jj) = norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
+            h(ii, jj) = norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2 - (obj.agents{ii}.collisionGeometry.radius + obj.agents{jj}.collisionGeometry.radius)^2;
             h(jj, ii) = h(ii, jj);
 
-            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+            A(kk, (3 * ii - 2):(3 * ii)) =  -2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
             A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
             % Slack derived from existing params: recovery velocity = max gradient approach velocity.
             % Correction splits between 2 agents, so |A| = 2*r_sum
@@ -69,11 +69,11 @@ function [obj] = constrainMotion(obj)
     for ii = 1:nAgents
         for jj = 1:size(obj.obstacles, 1)
             % find closest position to agent on/in obstacle
-            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.pos);
+            cPos = obj.obstacles{jj}.closestToPoint(obj.agents{ii}.lastPos);
 
-            hObs(ii, jj) = dot(obj.agents{ii}.pos - cPos, obj.agents{ii}.pos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
+            hObs(ii, jj) = dot(obj.agents{ii}.lastPos - cPos, obj.agents{ii}.lastPos - cPos) - obj.agents{ii}.collisionGeometry.radius^2;
 
-            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.pos - cPos);
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (obj.agents{ii}.lastPos - cPos);
             % Floor for single-agent constraint: full correction on one agent, |A| = 2*r_i
             r_i = obj.agents{ii}.collisionGeometry.radius;
             v_max_i = obj.agents{ii}.initialStepSize / obj.timestep;
@@ -93,37 +93,37 @@ function [obj] = constrainMotion(obj)
     h_xMin = 0.0; h_xMax = 0.0; h_yMin = 0.0; h_yMax = 0.0; h_zMin = 0.0; h_zMax = 0.0;
     for ii = 1:nAgents
         % X minimum
-        h_xMin = (obj.agents{ii}.pos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMin = (obj.agents{ii}.lastPos(1) - obj.domain.minCorner(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [-1, 0, 0];
         b(kk) = obj.barrierGain * max(0, h_xMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % X maximum
-        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.pos(1)) - obj.agents{ii}.collisionGeometry.radius;
+        h_xMax = (obj.domain.maxCorner(1) - obj.agents{ii}.lastPos(1)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [1, 0, 0];
         b(kk) = obj.barrierGain * max(0, h_xMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y minimum
-        h_yMin = (obj.agents{ii}.pos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMin = (obj.agents{ii}.lastPos(2) - obj.domain.minCorner(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, -1, 0];
         b(kk) = obj.barrierGain * max(0, h_yMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Y maximum
-        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.pos(2)) - obj.agents{ii}.collisionGeometry.radius;
+        h_yMax = (obj.domain.maxCorner(2) - obj.agents{ii}.lastPos(2)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 1, 0];
         b(kk) = obj.barrierGain * max(0, h_yMax)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z minimum — enforce z >= minAlt + radius (not just z >= domain floor + radius)
-        h_zMin = (obj.agents{ii}.pos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMin = (obj.agents{ii}.lastPos(3) - obj.minAlt) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, -1];
         b(kk) = obj.barrierGain * max(0, h_zMin)^obj.barrierExponent;
         kk = kk + 1;
 
         % Z maximum
-        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.pos(3)) - obj.agents{ii}.collisionGeometry.radius;
+        h_zMax = (obj.domain.maxCorner(3) - obj.agents{ii}.lastPos(3)) - obj.agents{ii}.collisionGeometry.radius;
         A(kk, (3 * ii - 2):(3 * ii)) = [0, 0, 1];
         b(kk) = obj.barrierGain * max(0, h_zMax)^obj.barrierExponent;
         kk = kk + 1;
@@ -145,9 +145,9 @@ function [obj] = constrainMotion(obj)
             if obj.constraintAdjacencyMatrix(ii, jj)
                 paddingFactor = 0.9; % Barrier at 90% of actual range; real comms still work beyond this
                 r_comms = paddingFactor * min([obj.agents{ii}.commsGeometry.radius, obj.agents{jj}.commsGeometry.radius]);
-                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.pos - obj.agents{jj}.pos)^2;
+                hComms(ii, jj) = r_comms^2 - norm(obj.agents{ii}.lastPos - obj.agents{jj}.lastPos)^2;
 
-                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.pos - obj.agents{jj}.pos);
+                A(kk, (3 * ii - 2):(3 * ii)) =  2 * (obj.agents{ii}.lastPos - obj.agents{jj}.lastPos);
                 A(kk, (3 * jj - 2):(3 * jj)) = -A(kk, (3 * ii - 2):(3 * ii));
 
                 % One-step forward invariance: b = h/dt ensures h cannot

@miSim/initialize.m

@@ -138,6 +138,12 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Initialize variable that will store barrier function values per timestep for analysis purposes
         obj.barriers = NaN(obj.numBarriers, size(obj.times, 1));
 
+        % Initialize constraint adjacency history (nAgents x nAgents x nTimesteps)
+        nAgents = size(obj.agents, 1);
+        obj.constraintAdjacencyHist = false(nAgents, nAgents, size(obj.times, 1));
+        obj.constraintAdjacencyHist(:, :, 1) = obj.constraintAdjacencyMatrix;
+
+
         % Set up plots showing initialized state
         obj = obj.plot();
 

@miSim/miSim.m

@@ -7,7 +7,6 @@ classdef miSim
         timestepIndex = NaN; % index of the current timestep (useful for time-indexed arrays)
         maxIter = NaN; % maximum number of simulation iterations
         domain;
-        objective;
         obstacles; % geometries that define obstacles within the domain
         agents; % agents that move within the domain
         adjacency = false(0, 0); % Adjacency matrix representing communications network graph
@@ -28,6 +27,7 @@ classdef miSim
         spatialPlotIndices = [6, 4, 3, 2];
         numBarriers = 0; % Number of barrier functions needed
         barriers = []; % log barrier function values at each timestep for analysis
+        constraintAdjacencyHist = []; % log constraint adjacency matrix at each timestep
     end
 
     properties (Access = private)
@@ -67,7 +67,6 @@ classdef miSim
                 obj (1, 1) miSim
             end
             obj.domain = rectangularPrism;
-            obj.objective = sensingObjective;
             obj.obstacles = {rectangularPrism};
             obj.agents = {agent};
         end

@miSim/run.m

@@ -34,6 +34,11 @@ function [obj] = run(obj)
             obj = obj.lesserNeighbor();
         end
 
+        % Log constraint adjacency for this timestep
+        if coder.target('MATLAB')
+            obj.constraintAdjacencyHist(:, :, ii) = obj.constraintAdjacencyMatrix;
+        end
+
         % Moving
         % Iterate over agents to simulate their unconstrained motion
         for jj = 1:size(obj.agents, 1)

@miSim/teardown.m

@@ -20,6 +20,7 @@ function obj = teardown(obj)
     out.dampingCoeff = obj.dampingCoeff;
     out.useDoubleIntegrator = obj.useDoubleIntegrator;
     out.useFixedTopology = obj.useFixedTopology;
+    out.constraintAdjacency = obj.constraintAdjacencyHist(:, :, 1:(end - 1));
     for ii = 1:size(obj.agents, 1)
         out.agent(ii).pos = squeeze(obj.posHist(ii, 1:(end - 1), 1:3));
         out.agent(ii).vel = squeeze(obj.velHist(ii, 1:(end - 1), 1:3));
@@ -39,11 +40,11 @@ function obj = teardown(obj)
     obj.timestepIndex = NaN;
     obj.maxIter = NaN;
     obj.domain = rectangularPrism;
-    obj.objective = sensingObjective;
     obj.obstacles = cell(0, 1);
     obj.agents = cell(0, 1);
     obj.adjacency = NaN;
     obj.constraintAdjacencyMatrix = NaN;
+    obj.constraintAdjacencyHist = [];
     obj.partitioning = NaN;
     obj.performance = 0;
     obj.barrierGain = NaN;

@miSim/validate.m

@@ -7,11 +7,11 @@ function validate(obj)
 
     %% Communications Network Validators
     if max(conncomp(graph(obj.adjacency))) ~= 1
-        warning("Network is not connected");
+        error("Network is not connected");
     end
 
     if any(obj.adjacency - obj.constraintAdjacencyMatrix < 0, "all")
-        warning("Eliminated network connections that were necessary");
+        error("Eliminated network connections that were necessary");
     end
 
     %% Obstacle Validators
@@ -21,9 +21,8 @@ function validate(obj)
             P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
             d = obj.agents{kk}.pos - P;
             if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2
-                warning("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
+                error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, dot(d, d) - obj.agents{kk}.collisionGeometry.radius^2); % this will cause quadprog to fail
             end
         end
     end
-
 end

@miSim/writeInits.m

@@ -14,6 +14,8 @@ function writeInits(obj)
     comRanges = cellfun(@(x) x.commsGeometry.radius, obj.agents);
     initialStepSize = cellfun(@(x) x.initialStepSize, obj.agents);
     pos = cell2mat(cellfun(@(x) x.pos, obj.agents, 'UniformOutput', false));
+    obsMinCorners = cell2mat(cellfun(@(x) x.minCorner, obj.obstacles, 'UniformOutput', false));
+    obsMaxCorners = cell2mat(cellfun(@(x) x.maxCorner, obj.obstacles, 'UniformOutput', false));
 
     % Combine with simulation parameters
     inits = struct("timestep", obj.timestep, "maxIter", obj.maxIter, "minAlt", obj.obstacles{end}.maxCorner(3), ...
@@ -24,7 +26,9 @@ function writeInits(obj)
                     "useDoubleIntegrator", obj.useDoubleIntegrator, "dampingCoeff", obj.dampingCoeff, ...
                     "alphaDist", alphaDist, "betaDist", betaDist, "alphaTilt", alphaTilt, "betaTilt", betaTilt, ...
                     ... % ^^^ PARAMETERS ^^^ | vvv STATES vvv
-                    "pos", pos); % still needs obstacle states and objective state
+                    "pos", pos, "objectivePos", obj.domain.objective.groundPos, "objectiveSigma", obj.domain.objective.objectiveSigma, ...
+                    "obsMinCorners", obsMinCorners, "obsMaxCorners", obsMaxCorners, ...
+                    "objectiveIntegral", sum(obj.domain.objective.values(:)));
 
     % Save all parameters to output file
     initsFile = strcat(obj.artifactName, "_miSimInits");

@sensingObjective/initialize.m

@@ -1,4 +1,4 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange, sensorPerformanceMinimum, objectiveMu, objectiveSigma)
     arguments (Input)
         obj (1,1) {mustBeA(obj, "sensingObjective")};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, "function_handle")};
@@ -6,6 +6,8 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
         discretizationStep (1, 1) double = 1;
         protectedRange (1, 1) double = 1;
         sensorPerformanceMinimum (1, 1) double = 1e-6;
+        objectiveMu (:, 2) double = NaN(1, 2);
+        objectiveSigma (:, 2, 2) double = NaN(1, 2, 2);
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, "sensingObjective")};
@@ -37,8 +39,13 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep, pr
 
     % store ground position
     idx = obj.values == 1;    
+    if any(isnan(objectiveMu))
         obj.groundPos = [obj.X(idx), obj.Y(idx)];
         obj.groundPos = obj.groundPos(1, 1:2); % for safety, in case 2 points are maximal (somehow)
-
-    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
+    else
+        obj.groundPos = objectiveMu;
+    end
+    obj.objectiveSigma = objectiveSigma;
+
+    assert(domain.distance([obj.groundPos, ones(size(obj.groundPos, 1), 1) .* domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective");
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -11,7 +11,7 @@ function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protected
 
     % Set random objective position
     mu = domain.minCorner;
-    while domain.distance(mu) < protectedRange
+    while domain.distance(mu) < protectedRange * 1.01
         mu = domain.random();
     end
 

@sensingObjective/sensingObjective.m

@@ -2,7 +2,8 @@ classdef sensingObjective
     % Sensing objective definition parent class
     properties (SetAccess = private, GetAccess = public)
         label = "";
-        groundPos = [NaN, NaN];
+        groundPos = NaN(1, 2);
+        objectiveSigma = NaN(1, 2, 2);
         discretizationStep = NaN;
         X = [];
         Y = [];

aerpaw/config/scenario.csv

@@ -1,2 +1,2 @@
 timestep, maxIter, minAlt, discretizationStep, protectedRange, initialStepSize, barrierGain, barrierExponent, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt, domainMin, domainMax, objectivePos, objectiveVar, sensorPerformanceMinimum, initialPositions, numObstacles, obstacleMin, obstacleMax, useDoubleIntegrator, dampingCoeff, useFixedTopology
-5, 100, 30.0, 0.1, 2.0, 2.0, 100, 3, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1
+1, 150, 30.0, 0.1, 2.0, 1, 1, 1, "5.0, 5.0", "25.0, 25.0", "80.0, 80.0", "0.25, 0.25", "5.0, 5.0", "0.1, 0.1", "0.0, 0.0, 0.0", "80.0, 80.0, 80.0", "55.0, 55.0", "40, 25, 25, 40", 0.15, "15.0, 10.0, 40.0, 5.0, 10.0, 45.0", 1, "1.0, 25.0, 0.0", "30.0, 30.0, 50.0", 1, 2.0, 1

util/objectiveFunctionWrapper.m

@@ -4,12 +4,12 @@ function f = objectiveFunctionWrapper(center, sigma)
     % composite objectives in particular
     arguments (Input)
         center (:, 2) double;
-        sigma (2, 2) double = eye(2);
+        sigma (:, 2, 2) double = eye(2);
     end
     arguments (Output)
         f (1, 1) {mustBeA(f, "function_handle")};
     end
     
-    f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), sigma), 1:size(center,1), "UniformOutput", false)), 2);
-
+    assert(size(center, 1) == size(sigma, 1));
+    f = @(x,y) sum(cell2mat(arrayfun(@(i) mvnpdf([x(:), y(:)], center(i,:), squeeze(sigma(i, :, :))), 1:size(center,1), "UniformOutput", false)), 2);
 end