fixed guidance only pulling things towards the middle and added CA QP CBF code

@miSim/constrainMotion.m

@@ -0,0 +1,61 @@
+function [obj] = constrainMotion(obj)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+    arguments (Output)
+        obj (1, 1) {mustBeA(obj, 'miSim')};
+    end
+
+    if size(obj.agents, 1) < 2
+        return;
+        % this doesn't work right now with only one agent
+    end
+
+    agents = [obj.agents{:}];
+    v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
+
+    h = NaN(size(obj.agents, 1));
+    h(logical(eye(size(obj.agents, 1)))) = 0; % self value is 0
+    nCon = nchoosek(size(obj.agents, 1), 2);
+    kk = 1;
+    A = zeros(nCon, 3 * size(obj.agents, 1));
+    b = zeros(nCon, 1);
+    for ii = 1:(size(obj.agents, 1) - 1)
+        for jj = (ii + 1):size(obj.agents, 1)
+            h(ii, jj) = norm(agents(ii).pos - agents(jj).pos)^2 - (agents(ii).collisionGeometry.radius + agents(jj).collisionGeometry.radius)^2;
+            h(jj, ii) = h(ii, jj);
+            
+            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 * h(ii, jj)^3;
+            kk = kk + 1;
+        end
+    end
+
+    % Solve QP program generated earlier
+    vhat = reshape(v', 3 * size(obj.agents, 1), 1);
+    H = 2 * eye(3 * size(obj.agents, 1));
+    f = -2 * vhat;
+    
+    % Update solution based on constraints
+    opt = optimoptions('quadprog', 'Display', 'off');
+    [vNew, ~, exitflag] = quadprog(sparse(H), double(f), A, b, [],[], [], [], [], opt);
+    vNew = reshape(vNew, 3, size(obj.agents, 1))';
+
+    if exitflag <= 0
+        warning("QP failed, continuing with unconstrained solution...")
+        vNew = v;
+    end
+
+    % Update the "next position" that was previously set by unconstrained
+    % GA using the constrained solution produced here
+    for ii = 1:size(vNew, 1)
+        obj.agents{ii}.pos = obj.agents{ii}.lastPos + vNew(ii, :) * obj.timestep;
+    end
+
+    % Here we run this at the simulation level, but in reality there is no
+    % parent level, so this would be run independently on each agent.
+    % Running at the simulation level is just meant to simplify the
+    % simulation
+
+end

@miSim/miSim.m

@@ -14,6 +14,7 @@ classdef miSim
         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
+        barrierGain = 100; % collision avoidance parameter
 
         fPerf; % performance plot figure
     end
@@ -43,6 +44,7 @@ classdef miSim
     methods (Access = public)
         [obj] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles);
         [obj] = run(obj);
+        [obj] = constrainMotion(obj);
         [obj] = partition(obj);
         [obj] = updateAdjacency(obj);
         [obj] = plot(obj);

@miSim/run.m

@@ -27,6 +27,10 @@ function [obj] = run(obj)
             obj.agents{jj} = obj.agents{jj}.run(obj.domain, obj.partitioning, obj.t);
         end
 
+        % Adjust motion determined by unconstrained gradient ascent using
+        % CBF constraints solved by QP
+        obj = constrainMotion(obj);
+
         % Update total performance
         obj.performance = [obj.performance, sum(cellfun(@(x) x.performance(end), obj.agents))];