protected objective from domain edges

agent.m

@@ -23,7 +23,7 @@ classdef agent
                 pos (1, 3) double;
                 vel (1, 3) double;
                 cBfromC (3, 3) double {mustBeDcm};
-                collisionGeometry (1, 1) {mustBeConstraintGeometries};
+                collisionGeometry (1, 1) {mustBeGeometry};
                 index (1, 1) double = NaN;
                 label (1, 1) string = "";
             end

geometries/rectangularPrism.m

@@ -1,5 +1,5 @@
-classdef rectangularPrismConstraint
+classdef rectangularPrism
-    % Rectangular prism constraint geometry
+    % Rectangular prism geometry
     properties (SetAccess = private, GetAccess = public)
         tag = REGION_TYPE.INVALID;
         label = "";
@@ -19,13 +19,13 @@ classdef rectangularPrismConstraint
     methods (Access = public)
         function obj = initialize(obj, bounds, tag, label)
             arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
                 bounds (2, 3) double;
                 tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
                 label (1, 1) string = "";
             end
             arguments (Output)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
             end
 
             obj.tag = tag;
@@ -59,26 +59,62 @@ classdef rectangularPrismConstraint
         end
         function r = random(obj)
             arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
             end
             arguments (Output)
                 r (1, 3) double
             end
             r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
         end
+        function d = distance(obj, pos)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+                pos (:, 3) double;
+            end
+            arguments (Output)
+                d (:, 1) 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
+            d = norm(cPos - pos);
+        end
+        function d = interiorDistance(obj, pos)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+                pos (:, 3) double;
+            end
+            arguments (Output)
+                d (:, 1) double
+            end
+            % find minimum distance to any face
+            d = min([pos(1) - obj.minCorner(1), ...
+                     pos(2) - obj.minCorner(2), ...
+                     pos(3) - obj.minCorner(3), ...
+                     obj.maxCorner(1) - pos(1), ...
+                     obj.maxCorner(2) - pos(2), ...
+                     obj.maxCorner(3) - pos(3)]);
+        end
         function c = contains(obj, pos)
             arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
                 pos (:, 3) double;
             end
             arguments (Output)
                 c (:, 1) logical
             end
-            c = all(pos >= repmat(obj.minCorner, size(pos, 2), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 2), 1), 2);
+            c = all(pos >= repmat(obj.minCorner, size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 1), 1), 2);
         end
         function f = plotWireframe(obj, f)
             arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
                 f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
             end
             arguments (Output)
@@ -97,7 +133,7 @@ classdef rectangularPrismConstraint
             Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]';
             Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]';
 
-            % Plot the boundaries of the constraint geometry
+            % Plot the boundaries of the geometry
             hold(f.CurrentAxes, "on");
             plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
             hold(f.CurrentAxes, "off");

miSim.m

@@ -3,20 +3,20 @@ classdef miSim
 
     % Simulation parameters
     properties (SetAccess = private, GetAccess = public)
-        domain = rectangularPrismConstraint;
+        domain = rectangularPrism;
         objective = sensingObjective;
-        constraintGeometries = cell(0, 1); % geometries that define constraints within the domain
+        obstacles = cell(0, 1); % geometries that define obstacles within the domain
         agents = cell(0, 1); % agents that move within the domain
     end
 
     methods (Access = public)
-        function obj = initialize(obj, domain, objective, agents, constraintGeometries)
+        function obj = initialize(obj, domain, objective, agents, obstacles)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
-                domain (1, 1) {mustBeConstraintGeometries};
+                domain (1, 1) {mustBeGeometry};
                 objective (1, 1) {mustBeA(objective, 'sensingObjective')};
                 agents (:, 1) cell {mustBeAgents};
-                constraintGeometries (:, 1) cell {mustBeConstraintGeometries} = cell(0, 1);
+                obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
             end
             arguments (Output)
                 obj (1, 1) {mustBeA(obj, 'miSim')};
@@ -25,8 +25,8 @@ classdef miSim
             %% Define domain
             obj.domain = domain;
 
-            %% Add constraint geometries against the domain
+            %% Add geometries representing obstacles within the domain
-            obj.constraintGeometries = constraintGeometries;
+            obj.obstacles = obstacles;
 
             %% Define objective
             obj.objective = objective;

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/dcQXSYjif37BKJ8bynJny6jq5-sp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="mustBeConstraintGeometries.m" type="File"/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="mustBeGeometry.m" type="File"/>

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/NmnqsjHeF9rAhG9A30BKV_vI7tIp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="rectangularPrism.m" type="File"/>

resources/project/NRbCR7m2f1_2pHz6LyHrTz7eFpc/ia5FvKngd_-0tJFYAO64kIgybSkp.xml

@@ -1,2 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<Info location="rectangularPrismConstraint.m" type="File"/>

test_miSim.m

@@ -2,16 +2,19 @@ classdef test_miSim < matlab.unittest.TestCase
     properties (Access = private)
         testClass = miSim;
         % Domain
-        domain = rectangularPrismConstraint;
+        domain = rectangularPrism;
 
         % Obstacles
-        constraintGeometries = cell(1, 0);
+        minNumObstacles = 1;
+        maxNumObstacles = 3;
+        obstacles = cell(1, 0);
         minObstacleDimension = 1;
         
         % Objective
         objective = sensingObjective;
         objectiveFunction = @(x, y) 0;
         objectiveDiscretizationStep = 0.01;
+        protectedRange = 1;
 
         % Agents
         minAgents = 3;
@@ -31,15 +34,20 @@ classdef test_miSim < matlab.unittest.TestCase
     methods (TestMethodSetup)
         % Generate a random domain
         function tc = setDomain(tc)
-            % random integer-sized domain within [-10, 10] in all dimensions
+            % random integer-sized domain ranging from [0, 5] to [0, 25] in all dimensions
             L = ceil(5 + rand * 10 + rand * 10);
             tc.domain = tc.domain.initialize([zeros(1, 3); L * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
         end
         % Generate a random sensing objective within that domain
         function tc = setSensingObjective(tc)
-            mu = tc.domain.random();
+            mu = tc.domain.minCorner;
-            sig = [3, 1; 1, 4];
+            while tc.domain.interiorDistance(mu) < tc.protectedRange
-            tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1, 1:2), sig);
+                mu = tc.domain.random();
+            end
+            mu(3) = 0;
+            assert(tc.domain.contains(mu));
+            sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
+            tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig);
             tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
         end
         % Instantiate agents, they will be initialized under different
@@ -55,15 +63,14 @@ classdef test_miSim < matlab.unittest.TestCase
     methods (Test)
         % Test methods
         function misim_initialization(tc)
-            % randomly create 2-3 constraint geometries
+            % randomly create 2-3 obstacles
-            nGeom = 1 + randi(2);
+            nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
-            tc.constraintGeometries = cell(nGeom, 1);
+            tc.obstacles = cell(nGeom, 1);
-            for ii = 1:size(tc.constraintGeometries, 1)
+            for ii = 1:size(tc.obstacles, 1)
-                % Instantiate a rectangular prism constraint that spans the
+                % Instantiate a rectangular prism obstacle
-                % domain's height
+                tc.obstacles{ii, 1} = rectangularPrism;
-                tc.constraintGeometries{ii, 1} = rectangularPrismConstraint;
 
-                % Randomly come up with constraint geometries until they
+                % Randomly come up with dimensions until they
                 % fit within the domain
                 candidateMinCorner = [-Inf(1, 2), 0];
                 candidateMaxCorner = Inf(1, 3);
@@ -71,7 +78,8 @@ classdef test_miSim < matlab.unittest.TestCase
                 % make sure obstacles are not too small in any dimension
                 tooSmall = true;
                 while tooSmall
-                    % make sure the obstacles don't contain the sensing objective
+                    % make sure the obstacles don't contain the sensing
+                    % objective or encroach on it too much
                     obstructs = true;
                     while obstructs
     
@@ -105,12 +113,13 @@ classdef test_miSim < matlab.unittest.TestCase
                 candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
                 candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
 
-                % Initialize constraint geometry
+                % Initialize obstacle geometry
-                tc.constraintGeometries{ii} = tc.constraintGeometries{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
+                tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
             end
             
             % Repeat this until a connected set of agent initial conditions
             % is found by random chance
+            nIter = 0;
             connected = false;
             while ~connected
                 % Randomly place agents in the domain
@@ -127,13 +136,13 @@ classdef test_miSim < matlab.unittest.TestCase
                                 boringInit = false;
                             end
                         end
-                        candidateGeometry = rectangularPrismConstraint;
+                        candidateGeometry = rectangularPrism;
                         tc.agents{ii} = tc.agents{ii}.initialize(candidatePos, zeros(1, 3), eye(3), candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii) * ones(1, 3); candidatePos + tc.collisionRanges(ii) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), ii, sprintf("Agent %d", ii));
     
                         % Check obstacles to confirm that none are violated
-                        for jj = 1:size(tc.constraintGeometries, 1)
+                        for jj = 1:size(tc.obstacles, 1)
                             inside = false;
-                            if tc.constraintGeometries{jj, 1}.contains(tc.agents{ii, 1}.pos)
+                            if tc.obstacles{jj, 1}.contains(tc.agents{ii, 1}.pos)
                                 % Found a violation, stop checking
                                 inside = true;
                                 break;
@@ -146,7 +155,7 @@ classdef test_miSim < matlab.unittest.TestCase
                         end
     
                         % Create a collision geometry for this agent
-                        candidateGeometry = rectangularPrismConstraint;
+                        candidateGeometry = rectangularPrism;
                         candidateGeometry = candidateGeometry.initialize([tc.agents{ii}.pos - 0.1 * ones(1, 3); tc.agents{ii}.pos + 0.1 * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
     
                         % Check previously placed agents for collisions
@@ -190,10 +199,11 @@ classdef test_miSim < matlab.unittest.TestCase
                 % Check connectivity
                 G = graph(adjacency);
                 connected = all(conncomp(G) == 1);
+                nIter = nIter + 1;
             end
 
             % Initialize the simulation
-            tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.constraintGeometries);
+            tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.obstacles);
 
             % Plot domain
             f = tc.testClass.domain.plotWireframe;
@@ -203,9 +213,9 @@ classdef test_miSim < matlab.unittest.TestCase
             ylim([tc.testClass.domain.minCorner(2) - 0.5, tc.testClass.domain.maxCorner(2) + 0.5]);
             zlim([tc.testClass.domain.minCorner(3) - 0.5, tc.testClass.domain.maxCorner(3) + 0.5]);
 
-            % Plot constraint geometries
+            % Plot obstacles
-            for ii = 1:size(tc.testClass.constraintGeometries, 1)
+            for ii = 1:size(tc.testClass.obstacles, 1)
-                tc.testClass.constraintGeometries{ii, 1}.plotWireframe(f);
+                tc.testClass.obstacles{ii, 1}.plotWireframe(f);
             end
 
             % Plot objective gradient

validators/mustBeConstraintGeometries.m

@@ -1,10 +0,0 @@
-function mustBeConstraintGeometries(constraintGeometry)
-    validGeometries = ["rectangularPrismConstraint";];
-    if isa(constraintGeometry, 'cell')
-        for ii = 1:size(constraintGeometry, 1)
-            assert(isa(constraintGeometry{ii}, validGeometries), "Constraint geometry in index %d is not a valid constraint geometry class", ii);
-        end
-    else
-        assert(isa(constraintGeometry, validGeometries), "Constraint geometry is not a valid constraint geometry class");
-    end
-end

validators/mustBeGeometry.m

@@ -0,0 +1,10 @@
+function mustBeGeometry(geometry)
+    validGeometries = ["rectangularPrism";];
+    if isa(geometry, 'cell')
+        for ii = 1:size(geometry, 1)
+            assert(isa(geometry{ii}, validGeometries), "Geometry in index %d is not a valid geometry class", ii);
+        end
+    else
+        assert(isa(geometry, validGeometries), "Geometry is not a valid geometry class");
+    end
+end