obstacle avoidance

@miSim/constrainMotion.m

@@ -14,12 +14,16 @@ function [obj] = constrainMotion(obj)
     agents = [obj.agents{:}];
     v = reshape(([agents.pos] - [agents.lastPos])./obj.timestep, 3, size(obj.agents, 1))';
 
+    % Initialize QP based on number of agents and obstacles
     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);
+    nAAPairs = nchoosek(size(obj.agents, 1), 2);
+    nAOPairs = size(obj.agents, 1) * size(obj.obstacles, 1);
     kk = 1;
-    A = zeros(nCon, 3 * size(obj.agents, 1));
+    A = zeros(nAAPairs + nAOPairs, 3 * size(obj.agents, 1));
-    b = zeros(nCon, 1);
+    b = zeros(nAAPairs + nAOPairs, 1);
+
+    % Set up collision avoidance constraints
     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;
@@ -32,6 +36,23 @@ function [obj] = constrainMotion(obj)
         end
     end
 
+    hObs = NaN(size(obj.agents, 1), size(obj.obstacles, 1));
+    % Set up obstacle avoidance constraints
+    for ii = 1:size(obj.agents, 1)
+        for jj = 1:size(obj.obstacles, 1)
+            % find closest position to agent on/in obstacle
+            cPos = obj.obstacles{jj}.closestToPoint(agents(ii).pos);
+
+            hObs(ii, jj) = dot(agents(ii).pos - cPos, agents(ii).pos - cPos) - agents(ii).collisionGeometry.radius^2;
+
+            A(kk, (3 * ii - 2):(3 * ii)) = -2 * (agents(ii).pos - cPos);
+            b(kk) = obj.barrierGain * hObs(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));

@miSim/initialize.m

@@ -32,8 +32,8 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part
     % Add an additional obstacle spanning the domain's footprint to 
     % represent the minimum allowable altitude
     obj.minAlt = minAlt;
-    obj.obstacles{end + 1} = rectangularPrism;
+    obj.obstacles{end + 1, 1} = rectangularPrism;
-    obj.obstacles{end} = obj.obstacles{end}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
+    obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint");
 
     % Define objective
     obj.objective = objective;

geometries/@rectangularPrism/closestToPoint.m

@@ -0,0 +1,19 @@
+function cPos = closestToPoint(obj, pos)
+    arguments (Input)
+        obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+        pos (:, 3) double;
+    end
+    arguments (Output)
+        cPos (:, 3) double;
+    end
+    cPos = NaN(1, 3);
+    for ii = 1:3
+        if pos(ii) < obj.minCorner(ii)
+            cPos(ii) = obj.minCorner(ii);
+        elseif pos(ii) > obj.maxCorner(ii)
+            cPos(ii) = obj.maxCorner(ii);
+        else
+            cPos(ii) = pos(ii);
+        end
+    end
+end

geometries/@rectangularPrism/distance.m

@@ -4,7 +4,7 @@ function d = distance(obj, pos)
         pos (:, 3) double;
     end
     arguments (Output)
-        d (:, 1) double
+        d (:, 1) double;
     end
     if obj.contains(pos)
         % Queried point is inside geometry

geometries/@rectangularPrism/rectangularPrism.m

@@ -36,6 +36,7 @@ classdef rectangularPrism
         [obj   ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);
+        [cPos  ] = closestToPoint(obj, pos);
         [d     ] = distance(obj, pos);
         [g     ] = distanceGradient(obj, pos);
         [c     ] = containsLine(obj, pos1, pos2);

test/test_miSim.m

@@ -503,7 +503,9 @@ classdef test_miSim < matlab.unittest.TestCase
             radius = 1.5;
             d = [3, 0, 0];
             geometry1 = spherical;
-            geometry1 = geometry1.initialize(tc.domain.center - d, radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            geometry2 = geometry1;
+            geometry1 = geometry1.initialize(tc.domain.center - d + [0, radius * 1.1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
+            geometry2 = geometry2.initialize(tc.domain.center - d - [0, radius * 1.1, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1));
             
             % Initialize agent sensor model
             sensor = sigmoidSensor;
@@ -511,8 +513,9 @@ classdef test_miSim < matlab.unittest.TestCase
             sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3);
 
             % Initialize agents
-            tc.agents = {agent};
+            tc.agents = {agent; agent;};
-            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d, zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), false);
+            tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.1, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3, 1, sprintf("Agent %d", 1), false);
+            tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3, 2, sprintf("Agent %d", 2), false);
             
             % Initialize obstacles
             obstacleLength = 1;