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,39 +1,39 @@
-classdef rectangularPrismConstraint
+classdef rectangularPrism
-    % Rectangular prism constraint geometry
+    % Rectangular prism geometry
     properties (SetAccess = private, GetAccess = public)
         tag = REGION_TYPE.INVALID;
         label = "";
 
-        minCorner = NaN(3, 1);
+        minCorner = NaN(1, 3);
-        maxCorner = NaN(3, 1);
+        maxCorner = NaN(1, 3);
 
-        dimensions = NaN(3, 1);
+        dimensions = NaN(1, 3);
 
         center = NaN;
 
         vertices = NaN(8, 3);
 
-        footprint = NaN(2, 4);
+        footprint = NaN(4, 2);
     end
 
     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 (3, 2) double;
+                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;
             obj.label = label;
 
             %% Define geometry bounds by LL corner and UR corner
-            obj.minCorner = bounds(:, 1);
+            obj.minCorner = bounds(1, 1:3);
-            obj.maxCorner = bounds(:, 2);
+            obj.maxCorner = bounds(2, 1:3);
 
             % Compute L, W, H
             obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
@@ -42,43 +42,79 @@ classdef rectangularPrismConstraint
             obj.center = obj.minCorner + obj.dimensions ./ 2;
 
             % Compute vertices
-            obj.vertices = [obj.minCorner';
+            obj.vertices = [obj.minCorner;
-                            obj.maxCorner(1), obj.minCorner(2:3)';
+                            obj.maxCorner(1), obj.minCorner(2:3);
-                            obj.maxCorner(1:2)', obj.minCorner(3);
+                            obj.maxCorner(1:2), obj.minCorner(3);
                             obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
-                            obj.minCorner(1:2)', obj.maxCorner(3);
+                            obj.minCorner(1:2), obj.maxCorner(3);
                             obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
-                            obj.minCorner(1), obj.maxCorner(2:3)'
+                            obj.minCorner(1), obj.maxCorner(2:3)
-                            obj.maxCorner';];
+                            obj.maxCorner;];
 
             % Compute footprint
-            obj.footprint = [obj.minCorner(1:2, 1), ...
+            obj.footprint = [obj.minCorner(1:2); ...
-                             [obj.minCorner(1, 1); obj.maxCorner(2, 1)], ...
+                             [obj.minCorner(1), obj.maxCorner(2)]; ...
-                             [obj.maxCorner(1, 1); obj.minCorner(2, 1)], ...
+                             [obj.maxCorner(1), obj.minCorner(2)]; ...
-                             obj.maxCorner(1:2, 1)];
+                             obj.maxCorner(1:2)];
         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 c = contains(obj, pos)
+        function d = distance(obj, pos)
             arguments (Input)
-                obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
                 pos (:, 3) double;
             end
             arguments (Output)
-                c (1, 1) logical
+                d (:, 1) double
             end
-            c = all(pos >= repmat(obj.minCorner', size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner', size(pos, 1), 1), 2);
+            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, 'rectangularPrism')};
+                pos (:, 3) double;
+            end
+            arguments (Output)
+                c (:, 1) logical
+            end
+            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"/>

sensingObjective.m

@@ -16,7 +16,7 @@ classdef sensingObjective
             arguments (Input)
                 obj (1,1) {mustBeA(obj, 'sensingObjective')};
                 objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
-                footprint (2, :) double;
+                footprint (:, 2) double;
                 groundAlt (1, 1) double = 0;
                 discretizationStep (1, 1) double = 1;
             end
@@ -27,10 +27,10 @@ classdef sensingObjective
             obj.groundAlt = groundAlt;
 
             % Extract footprint limits
-            xMin = min(footprint(1, :));
+            xMin = min(footprint(:, 1));
-            xMax = max(footprint(1, :));
+            xMax = max(footprint(:, 1));
-            yMin = min(footprint(2, :));
+            yMin = min(footprint(:, 2));
-            yMax = max(footprint(2, :));
+            yMax = max(footprint(:, 2));
 
             xGrid = unique([xMin:discretizationStep:xMax, xMax]);
             yGrid = unique([yMin:discretizationStep:yMax, yMax]);

test_miSim.m

@@ -2,15 +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;
@@ -30,16 +34,21 @@ 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(([0, L; 0, L; 0, L]), REGION_TYPE.DOMAIN, "Domain");
+            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();
-            tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3, 1), tc.objectiveDiscretizationStep);
+            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
         % parameters in individual test cases
@@ -54,39 +63,49 @@ 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(3, 1);
+                candidateMinCorner = [-Inf(1, 2), 0];
-                candidateMaxCorner = Inf(3, 1);
+                candidateMaxCorner = Inf(1, 3);
 
-                % make sure the obstacles don't contain the sensing objective
+                % make sure obstacles are not too small in any dimension
-                obstructs = true;
+                tooSmall = true;
-                while obstructs
+                while tooSmall
-
+                    % make sure the obstacles don't contain the sensing
-                    % Make sure the obstacle is in the domain
+                    % objective or encroach on it too much
-                    while any(candidateMinCorner(1:2, 1) < tc.domain.minCorner(1:2, 1)) 
+                    obstructs = true;
-                        candidateMinCorner = tc.domain.minCorner(1:3, 1) + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1); -Inf]; % random spots on the ground
+                    while obstructs
+    
+                        % Make sure the obstacle is in the domain
+                        while any(candidateMinCorner < tc.domain.minCorner) 
+                            candidateMinCorner = tc.domain.minCorner(1:3) + [(tc.domain.maxCorner(1:2) - tc.domain.minCorner(1:2)) .* rand(1, 2), 0]; % random spots on the ground
+                        end
+                        while any(candidateMaxCorner > tc.domain.maxCorner)
+                            candidateMaxCorner = [candidateMinCorner(1:2), 0] + ((tc.domain.maxCorner(1:3) - tc.domain.minCorner(1:3)) .* rand(1, 3) ./ 2); % halved to keep from being excessively large
+                        end
+    
+                        % once a domain-valid obstacle has been found, make
+                        % sure it doesn't obstruct the sensing target
+                        if all(candidateMinCorner(1:2) <= tc.objective.groundPos) && all(candidateMaxCorner(1:2) >= tc.objective.groundPos)
+                            % reset to try again
+                            candidateMinCorner = [-Inf(1, 2), 0];
+                            candidateMaxCorner = Inf(1, 3);
+                        else
+                            obstructs = false;
+                        end
                     end
-                    while any(candidateMaxCorner(1:3, 1) > tc.domain.maxCorner(1:3, 1))
+                    if min(candidateMaxCorner - candidateMinCorner) >= tc.minObstacleDimension
-                        candidateMaxCorner = [candidateMinCorner(1:2, 1); 0] + ((tc.domain.maxCorner(1:3, 1) - tc.domain.minCorner(1:3, 1)) .* rand(3, 1) ./ 2); % halved to keep from being excessively large
+                        tooSmall = false;
-                    end
-
-                    % once a domain-valid obstacle has been found, make
-                    % sure it doesn't obstruct the sensing target
-                    if all(candidateMinCorner(1:2, 1)' <= tc.objective.groundPos) && all(candidateMaxCorner(1:2, 1)' >= tc.objective.groundPos)
-                        % reset to try again
-                        candidateMinCorner = -Inf(3, 1);
-                        candidateMaxCorner = Inf(3, 1);
                     else
-                        obstructs = false;
+                        candidateMinCorner = [-Inf(1, 2), 0];
+                        candidateMaxCorner = Inf(1, 3);
                     end
                 end
 
@@ -94,27 +113,36 @@ 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, 1} = tc.constraintGeometries{ii, 1}.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
                 for ii = 1:size(tc.agents, 1)
                     posInvalid = true;
                     while posInvalid
-                        % Initialize the agent into a random spot in the domain
+                        % Initialize the agent into a random spot in the
-                        candidatePos = tc.domain.random();
+                        % domain (that is not too close to the sensing
-                        candidateGeometry = rectangularPrismConstraint;
+                        % objective)
-                        tc.agents{ii, 1} = tc.agents{ii, 1}.initialize(candidatePos, zeros(1, 3), eye(3), candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii, 1) * ones(1, 3); candidatePos + tc.collisionRanges(ii, 1) * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii)), ii, sprintf("Agent %d", ii));
+                        boringInit = true;
+                        while boringInit
+                            candidatePos = tc.domain.random();
+                            if norm(candidatePos(1:2) - tc.objective.groundPos) >= norm(tc.domain.footprint(4, :) - tc.domain.footprint(1, :))/2
+                                boringInit = false;
+                            end
+                        end
+                        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;
@@ -127,8 +155,8 @@ classdef test_miSim < matlab.unittest.TestCase
                         end
     
                         % Create a collision geometry for this agent
-                        candidateGeometry = rectangularPrismConstraint;
+                        candidateGeometry = rectangularPrism;
-                        candidateGeometry = candidateGeometry.initialize([tc.agents{ii, 1}.pos - 0.1 * ones(1, 3); tc.agents{ii, 1}.pos + 0.1 * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
+                        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
                         for jj = 1:(ii - 1)
@@ -171,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;
@@ -184,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