cleaned up randomly generated obstacle collision code

@miSim/miSim.m

@@ -11,6 +11,7 @@ classdef miSim
         obstacles = cell(0, 1); % geometries that define obstacles within the domain
         agents = cell(0, 1); % agents that move within the domain
         adjacency = NaN; % Adjacency matrix representing communications network graph
+        sensorPerformanceMinimum = 1e-6; % minimum sensor performance to allow assignment of a point in the domain to a partition
         partitioning = NaN;
     end
 

@miSim/partition.m

@@ -9,6 +9,7 @@ function obj = partition(obj)
     % Assess sensing performance of each agent at each sample point
     % in the domain
     agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
+    agentPerformances{end + 1} = obj.sensorPerformanceMinimum * ones(size(agentPerformances{end})); % add additional layer to represent the threshold that has to be cleared for assignment to any partiton
     agentPerformances = cat(3, agentPerformances{:});
     
     % Get highest performance value at each point
@@ -16,6 +17,7 @@ function obj = partition(obj)
 
     % Collect agent indices in the same way
     agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
+    agentInds{end + 1} = zeros(size(agentInds{end})); % index for no assignment
     agentInds = cat(3, agentInds{:});
 
     % Get highest performing agent's index

@sensingObjective/initialize.m

@@ -1,15 +1,17 @@
-function obj = initialize(obj, objectiveFunction, domain, discretizationStep)
+function obj = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange)
     arguments (Input)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
         objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
         domain (1, 1) {mustBeGeometry};
         discretizationStep (1, 1) double = 1;
+        protectedRange (1, 1) double = 1;
     end
     arguments (Output)
         obj (1,1) {mustBeA(obj, 'sensingObjective')};
     end
 
     obj.groundAlt = domain.minCorner(3);
+    obj.protectedRange = protectedRange;
 
     % Extract footprint limits
     xMin = min(domain.footprint(:, 1));
@@ -30,4 +32,6 @@ function obj = initialize(obj, objectiveFunction, domain, discretizationStep)
     % store ground position
     idx = obj.values == max(obj.values, [], "all");
     obj.groundPos = [obj.X(idx), obj.Y(idx)];
+
+    assert(domain.distance([obj.groundPos, domain.center(3)]) > protectedRange, "Domain is crowding the sensing objective")
 end

@sensingObjective/initializeRandomMvnpdf.m

@@ -1,9 +1,9 @@
-function obj = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep)
+function obj = initializeRandomMvnpdf(obj, domain, discretizationStep, protectedRange)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'sensingObjective')};
         domain (1, 1) {mustBeGeometry};
-        protectedRange (1, 1) double = 1;
         discretizationStep (1, 1) double = 1;
+        protectedRange (1, 1) double = 1;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'sensingObjective')};
@@ -23,5 +23,5 @@ function obj = initializeRandomMvnpdf(obj, domain, protectedRange, discretizatio
     objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu, sig);
 
     % Regular initialization
-    obj = obj.initialize(objectiveFunction, domain, discretizationStep);
+    obj = obj.initialize(objectiveFunction, domain, discretizationStep, protectedRange);
 end

@sensingObjective/sensingObjective.m

@@ -9,11 +9,12 @@ classdef sensingObjective
         X = [];
         Y = [];
         values = [];
+        protectedRange = 1; % keep obstacles from crowding objective
     end
 
     methods (Access = public)
-        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep);
+        [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange);
-        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep);
+        [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange);
         [f  ] = plot(obj, ind, f);
     end
 end

geometries/@rectangularPrism/initializeRandom.m

@@ -1,17 +1,43 @@
-function [obj] = initializeRandom(obj, minDimension, tag, label)
+function [obj] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain)
     arguments (Input)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
-        minDimension (1, 1) double = 10;
         tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
         label (1, 1) string = "";
+        minDimension (1, 1) double = 10;
+        maxDimension (1, 1) double= 20;
+        domain (1, 1) {mustBeGeometry} = rectangularPrism;
     end
     arguments (Output)
         obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
     end
     
-    % Produce random bounds
+    % Produce random bounds based on region type
-    L = ceil(minDimension + rand * minDimension);
+    if tag == REGION_TYPE.DOMAIN
-    bounds = [zeros(1, 3); L * ones(1, 3)];
+        % Domain
+        L = ceil(minDimension + rand * (maxDimension - minDimension));
+        bounds = [zeros(1, 3); L * ones(1, 3)];
+    else
+        % Obstacle
+
+        % Produce a corners that are contained in the domain
+        ii = 0;
+        candidateMaxCorner = domain.maxCorner + ones(1, 3);
+        candidateMinCorner = domain.minCorner - ones(1, 3);
+        % Continue until the domain contains the obstacle without crowding the objective
+        while ~domain.contains(candidateMaxCorner) || all(domain.objective.groundPos + domain.objective.protectedRange >= candidateMinCorner(1:2), 2) && all(domain.objective.groundPos - domain.objective.protectedRange <= candidateMaxCorner(1:2), 2)
+            if ii == 0 || ii > 10
+                candidateMinCorner = domain.random();
+                candidateMinCorner(3) = 0; % bind to floor
+                ii = 1;
+            end
+
+            candidateMaxCorner = candidateMinCorner + minDimension + rand(1, 3) * (maxDimension - minDimension);
+
+            ii = ii + 1;
+        end
+        
+        bounds = [candidateMinCorner; candidateMaxCorner;];
+    end
 
     % Regular initialization
     obj = obj.initialize(bounds, tag, label);

geometries/@rectangularPrism/rectangularPrism.m

@@ -28,7 +28,7 @@ classdef rectangularPrism
 
     methods (Access = public)
         [obj   ] = initialize(obj, bounds, tag, label, objectiveFunction, discretizationStep);
-        [obj   ] = initializeRandom(obj, tag, label);
+        [obj   ] = initializeRandom(obj, tag, label, minDimension, maxDimension, domain);
         [r     ] = random(obj);
         [c     ] = contains(obj, pos);
         [d     ] = distance(obj, pos);

sensingModels/@sigmoidSensor/sensorPerformance.m

@@ -17,7 +17,7 @@ function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos
     % Membership functions
     mu_d = 1 - (1 ./ (1 + exp(-obj.betaDist .* (d - obj.alphaDist)))); % distance
     mu_p = 1; % pan
-    mu_t = (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle - obj.alphaPan)))); % tilt
+    mu_t = (1 ./ (1 + exp(-obj.betaTilt .* (tiltAngle + obj.alphaTilt)))) - (1 ./ (1 + exp(-obj.betaTilt .* (tiltAngle - obj.alphaTilt)))); % tilt
 
-    value = mu_d .* mu_p .* mu_t * 1e12;
+    value = mu_d .* mu_p .* mu_t;
 end

test/test_miSim.m

@@ -25,7 +25,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
         % Agents
         minAgents = 3; % Minimum number of agents to be randomly generated
-        maxAgents = 9; % Maximum number of agents to be randomly generated
+        maxAgents = 6; % Maximum number of agents to be randomly generated
         sensingLength = 0.05; % length parameter used by sensing function
         agents = cell(0, 1);
         
@@ -43,15 +43,13 @@ classdef test_miSim < matlab.unittest.TestCase
         % Generate a random domain
         function tc = setDomain(tc)
             % random integer-dimensioned cubic domain
-            tc.domain = tc.domain.initializeRandom(tc.minDimension, REGION_TYPE.DOMAIN, "Domain");
+            tc.domain = tc.domain.initializeRandom(REGION_TYPE.DOMAIN, "Domain", tc.minDimension);
             % Random bivariate normal PDF objective
-            tc.domain.objective = tc.domain.objective.initializeRandomMvnpdf(tc.domain, tc.protectedRange, tc.discretizationStep);
+            tc.domain.objective = tc.domain.objective.initializeRandomMvnpdf(tc.domain, tc.discretizationStep, tc.protectedRange);
         end
         % Instantiate agents
         function tc = setAgents(tc)
-            % Agents will be initialized under different parameters in
+            % Agents will be initialized under different parameters in individual test cases
-            % individual test cases
-
             % Instantiate a random number of agents according to parameters
             for ii = 1:randi([tc.minAgents, tc.maxAgents])
                 tc.agents{ii, 1} = agent;
@@ -73,52 +71,14 @@ classdef test_miSim < matlab.unittest.TestCase
             for ii = 1:size(tc.obstacles, 1)
                 badCandidate = true;
                 while badCandidate
-                    % Instantiate a rectangular prism obstacle
+                    % Instantiate a rectangular prism obstacle inside the domain
                     tc.obstacles{ii} = rectangularPrism;
+                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain);
     
-                    % Randomly generate min corner for the obstacle
+                    % Check if the obstacle collides with an existing obstacle
-                    candidateMinCorner = tc.domain.random();
+                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
-                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
+                        badCandidate = false;
-    
-                    % Randomly select a corresponding maximum corner that
-                    % satisfies min/max obstacle size specifications
-                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
-                
-                    % Initialize obstacle
-                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
-
-                    % Check if the obstacle intersects with any existing
-                    % obstacles
-                    violation = false;
-                    for kk = 1:(ii - 1)
-                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
-                            violation = true;
-                            break;
-                        end
                     end
-                    if violation
-                        continue;
-                    end
-
-                    % Make sure that the obstacles are fully contained by 
-                    % the domain
-                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
-                        continue;
-                    end
-
-                    % Make sure that the obstacles don't cover the sensing
-                    % objective
-                    if obstacleCoversObjective(tc.domain.objective, tc.obstacles{ii})
-                        continue;
-                    end
-
-                    % Make sure that the obstacles aren't too close to the
-                    % sensing objective
-                    if obstacleCrowdsObjective(tc.domain.objective, tc.obstacles{ii}, tc.protectedRange)
-                        continue;
-                    end
-                    
-                    badCandidate = false;
                 end
             end
 
@@ -241,52 +201,14 @@ classdef test_miSim < matlab.unittest.TestCase
             for ii = 1:size(tc.obstacles, 1)
                 badCandidate = true;
                 while badCandidate
-                    % Instantiate a rectangular prism obstacle
+                    % Instantiate a rectangular prism obstacle inside the domain
                     tc.obstacles{ii} = rectangularPrism;
+                    tc.obstacles{ii} = tc.obstacles{ii}.initializeRandom(REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", ii), tc.minObstacleSize, tc.maxObstacleSize, tc.domain);
     
-                    % Randomly generate min corner for the obstacle
+                    % Check if the obstacle collides with an existing obstacle
-                    candidateMinCorner = tc.domain.random();
+                    if ~tc.obstacleCollisionCheck(tc.obstacles(1:(ii - 1)), tc.obstacles{ii})
-                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
+                        badCandidate = false;
-    
-                    % Randomly select a corresponding maximum corner that
-                    % satisfies min/max obstacle size specifications
-                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
-                
-                    % Initialize obstacle
-                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
-
-                    % Check if the obstacle intersects with any existing
-                    % obstacles
-                    violation = false;
-                    for kk = 1:(ii - 1)
-                        if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii})
-                            violation = true;
-                            break;
-                        end
                     end
-                    if violation
-                        continue;
-                    end
-
-                    % Make sure that the obstacles are fully contained by 
-                    % the domain
-                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
-                        continue;
-                    end
-
-                    % Make sure that the obstacles don't cover the sensing
-                    % objective
-                    if obstacleCoversObjective(tc.domain.objective, tc.obstacles{ii})
-                        continue;
-                    end
-
-                    % Make sure that the obstacles aren't too close to the
-                    % sensing objective
-                    if obstacleCrowdsObjective(tc.domain.objective, tc.obstacles{ii}, tc.protectedRange)
-                        continue;
-                    end
-                    
-                    badCandidate = false;
                 end
             end
 
@@ -411,7 +333,7 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
 
             % make basic sensing objective
-            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2), eye(2)), tc.domain.footprint, tc.domain.minCorner(3), tc.discretizationStep);
+            tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2)), tc.domain, tc.discretizationStep, tc.protectedRange);
         
             % Initialize agent collision geometry
             geometry1 = rectangularPrism;
@@ -428,8 +350,26 @@ classdef test_miSim < matlab.unittest.TestCase
             tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1));
             tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2));
 
+            % Optional third agent along the +Y axis
+            % geometry3 = rectangularPrism;
+            % geometry3 = geometry3.initialize([tc.domain.center - [0, d, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [0, d, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 3));
+            % tc.agents{3} = agent;
+            % tc.agents{3} = tc.agents{3}.initialize(tc.domain.center - [0, d, 0], zeros(1, 3), 0, 0, geometry3, sensor, @gradientAscent, 3*d, 3, sprintf("Agent %d", 3));
+
             % Initialize the simulation
             [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.timestep, tc.partitoningFreq, tc.maxIter);
         end
     end
+
+    methods
+        function c = obstacleCollisionCheck(~, obstacles, obstacle)
+            % Check if the obstacle intersects with any other obstacles
+            c = false;
+            for ii = 1:size(obstacles, 1)
+                if geometryIntersects(obstacles{ii}, obstacle)
+                    c = true;
+                end
+            end
+        end
+    end
 end