diff --git a/@sensingObjective/initialize.m b/@sensingObjective/initialize.m index 69c00eb..20bc42a 100644 --- a/@sensingObjective/initialize.m +++ b/@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 \ No newline at end of file diff --git a/@sensingObjective/initializeRandomMvnpdf.m b/@sensingObjective/initializeRandomMvnpdf.m index b421acb..4f9528d 100644 --- a/@sensingObjective/initializeRandomMvnpdf.m +++ b/@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 \ No newline at end of file diff --git a/@sensingObjective/sensingObjective.m b/@sensingObjective/sensingObjective.m index f24eefa..53c843f 100644 --- a/@sensingObjective/sensingObjective.m +++ b/@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] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep); + [obj] = initialize(obj, objectiveFunction, domain, discretizationStep, protectedRange); + [obj] = initializeRandomMvnpdf(obj, domain, protectedRange, discretizationStep, protectedRange); [f ] = plot(obj, ind, f); end end \ No newline at end of file diff --git a/geometries/@rectangularPrism/initializeRandom.m b/geometries/@rectangularPrism/initializeRandom.m index b57330f..aece5a7 100644 --- a/geometries/@rectangularPrism/initializeRandom.m +++ b/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 - L = ceil(minDimension + rand * minDimension); - bounds = [zeros(1, 3); L * ones(1, 3)]; + % Produce random bounds based on region type + if tag == REGION_TYPE.DOMAIN + % 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); diff --git a/geometries/@rectangularPrism/rectangularPrism.m b/geometries/@rectangularPrism/rectangularPrism.m index 59cc217..7c124bd 100644 --- a/geometries/@rectangularPrism/rectangularPrism.m +++ b/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); diff --git a/test/test_miSim.m b/test/test_miSim.m index b553c19..8bad8fe 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -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 - % individual test cases - + % Agents will be initialized under different parameters in 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,20 +71,10 @@ 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 - candidateMinCorner = tc.domain.random(); - candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain - - % 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; @@ -99,24 +87,6 @@ classdef test_miSim < matlab.unittest.TestCase 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 @@ -241,20 +211,10 @@ 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 - candidateMinCorner = tc.domain.random(); - candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain - - % 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; @@ -267,24 +227,6 @@ classdef test_miSim < matlab.unittest.TestCase 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 @@ -411,7 +353,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)), tc.domain, 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;