refactored constraint plotting to remove superfluous property

@miSim/constrainMotion.m

@@ -132,11 +132,6 @@ function [obj] = constrainMotion(obj)
         idx = idx + 6;
     end
 
-    if coder.target('MATLAB')
-        % Save off h function values (logging only — not needed in compiled mode)
-        obj.h(:, obj.timestepIndex) = [h(triu(true(nAgents), 1)); reshape(hObs, [], 1); h_xMin; h_xMax; h_yMin; h_yMax; h_zMin; h_zMax;];
-    end
-
     % Add communication network constraints
     hComms = NaN(nAgents, nAgents);
     hComms(logical(eye(nAgents))) = 0;

@miSim/initialize.m

@@ -109,8 +109,6 @@ function [obj] = initialize(obj, domain, agents, barrierGain, barrierExponent, m
         % Prepare performance data store (at t = 0, all have 0 performance)
         obj.perf = [zeros(size(obj.agents, 1) + 1, 1), NaN(size(obj.agents, 1) + 1, size(obj.partitioningTimes, 1) - 1)];
 
-        % Prepare h function data store
-        obj.h = NaN(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2 + size(obj.agents, 1) * size(obj.obstacles, 1) + 6, size(obj.times, 1));
     end
 
     % Create initial partitioning

@miSim/miSim.m

@@ -54,7 +54,6 @@ classdef miSim
         partitionGraphIndex = 1;
 
         % CBF plotting
-        h; % h function values
         hf; % h function plotting figure
         caPlot; % objects for collision avoidance h function plot
         obsPlot; % objects for obstacle h function plot

@miSim/plotH.m

@@ -6,6 +6,10 @@ function obj = plotH(obj)
         obj (1, 1) {mustBeA(obj, "miSim")};
     end
 
+    nCA  = size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2;
+    nObs = size(obj.agents, 1) * size(obj.obstacles, 1);
+    nDom = size(obj.agents, 1) * 6;
+
     obj.hf = figure;
     tiledlayout(obj.hf, 4, 1, "TileSpacing", "tight", "Padding", "compact");
 
@@ -15,7 +19,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Collision Avoidance");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.caPlot = plot(obj.h(1:(size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2), :)');
+    obj.caPlot = plot(obj.barriers(1:nCA, :)');
     legendStrings = [];
     for ii = 2:size(obj.agents, 1)
         for jj = 1:(ii - 1)
@@ -31,7 +35,7 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Obstacles");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.obsPlot = plot(obj.h((1 + (size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)):(((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1)), :)');
+    obj.obsPlot = plot(obj.barriers((nCA + 1):(nCA + nObs), :)');
     legendStrings = [];
     for ii = 1:size(obj.obstacles, 1)
         for jj = 1:size(obj.agents, 1)
@@ -47,8 +51,13 @@ function obj = plotH(obj)
     xlabel(obj.hf.Children(1).Children(1), "Time (s)");
     title(obj.hf.Children(1).Children(1), "Domain");
     hold(obj.hf.Children(1).Children(1), "on");
-    obj.domPlot = plot(obj.h((1 + (((size(obj.agents, 1) * (size(obj.agents, 1) - 1) / 2)) + size(obj.agents, 1) * size(obj.obstacles, 1))):size(obj.h, 1), 1:end)');
+    obj.domPlot = plot(obj.barriers((nCA + nObs + 1):(nCA + nObs + nDom), :)');
-    legend(obj.hf.Children(1).Children(1), ["X Min"; "X Max"; "Y Min"; "Y Max"; "Z Min"; "Z Max";], "Location", "bestoutside");
+    domLabels = ["X Min", "X Max", "Y Min", "Y Max", "Z Min", "Z Max"];
+    legendStrings = strings(nDom, 1);
+    for ii = 1:size(obj.agents, 1)
+        legendStrings((ii - 1) * 6 + (1:6)) = sprintf("A%d ", ii) + domLabels;
+    end
+    legend(obj.hf.Children(1).Children(1), legendStrings, "Location", "bestoutside");
     hold(obj.hf.Children(1).Children(2), "off");
 
     nexttile(obj.hf.Children(1));

@miSim/updatePlots.m

@@ -61,13 +61,15 @@ function [obj] = updatePlots(obj)
     end
 
     % Update h function plots
-    for ii = 1:size(obj.caPlot, 1)
+    nCA  = size(obj.caPlot, 1);
-        obj.caPlot(ii).YData(obj.timestepIndex) = obj.h(ii, obj.timestepIndex);
+    nObs = size(obj.obsPlot, 1);
+    for ii = 1:nCA
+        obj.caPlot(ii).YData(obj.timestepIndex) = obj.barriers(ii, obj.timestepIndex);
     end
-    for ii = 1:size(obj.obsPlot, 1)
+    for ii = 1:nObs
-        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1), obj.timestepIndex);
+        obj.obsPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + ii, obj.timestepIndex);
     end
     for ii = 1:size(obj.domPlot, 1)
-        obj.domPlot(ii).YData(obj.timestepIndex) = obj.h(ii + size(obj.caPlot, 1) + size(obj.obsPlot, 1), obj.timestepIndex);
+        obj.domPlot(ii).YData(obj.timestepIndex) = obj.barriers(nCA + nObs + ii, obj.timestepIndex);
     end
 end

@miSim/validate.m

@@ -20,8 +20,8 @@ function validate(obj)
         for kk = 1:size(obj.agents, 1)
             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
+            if dot(d, d) < obj.agents{kk}.collisionGeometry.radius^2 - 1e-3
-                error("%s colliding with %s by %d", obj.agents{kk}.label, obj.obstacles{jj}.label, obj.agents{kk}.collisionGeometry.radius^2 - dot(d, d)); % 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