diff --git a/geometries/@rectangularPrism/containsLine.m b/geometries/@rectangularPrism/containsLine.m index 4cae634..5c21807 100644 --- a/geometries/@rectangularPrism/containsLine.m +++ b/geometries/@rectangularPrism/containsLine.m @@ -21,7 +21,7 @@ function c = containsLine(obj, pos1, pos2) tMax = 1; for ii = 1:3 % line is parallel to geometry - if abs(d(ii)) < 1e-12 + if abs(d(ii)) < 1e-9 if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii) c = false; return; diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index 7e92f1b..94c4dc4 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -3,10 +3,9 @@ classdef parametricTestSuite < matlab.unittest.TestCase % System under test testClass = miSim; domain = rectangularPrism; - obstacles = cell(1, 0); % RNG control - seed = 1234567890; + seed = 1; %% Diagnostic Parameters % No effect on simulation dynamics @@ -41,7 +40,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase % Read file csv = readtable(csvPath, "TextType", "String", "NumHeaderLines", 0, "VariableNamingRule", "Preserve"); - csv.Properties.VariableNames = ["timestep", "maxIter", "minAlt", "discretizationStep", "sensorPerformanceMinimum", "initialStepSize", "barrierGain", "barrierExponent", "numObstacles", "numAgents", "collisionRadius", "comRange", "alphaDist", "betaDist", "alphaTilt", "betaTilt"]; + csv.Properties.VariableNames = ["timestep", "maxIter", "minAlt", "discretizationStep", "protectedRange", "sensorPerformanceMinimum", "initialStepSize", "barrierGain", "barrierExponent", "numObstacles", "numAgents", "collisionRadius", "comRange", "alphaDist", "betaDist", "alphaTilt", "betaTilt"]; for ii = 1:size(csv.Properties.VariableNames, 2) csv.(csv.Properties.VariableNames{ii}) = cell2mat(cellfun(@(x) str2num(x), csv.(csv.Properties.VariableNames{ii}), "UniformOutput", false)); @@ -49,7 +48,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase % Put params into standard structure params = struct("timestep", csv.timestep, "maxIter", csv.maxIter, "minAlt", csv.minAlt, "discretizationStep", csv.discretizationStep, ... - "sensorPerformanceMinimum", csv.sensorPerformanceMinimum, "initialStepSize", csv.initialStepSize, ... + "protectedRange", csv.protectedRange, "sensorPerformanceMinimum", csv.sensorPerformanceMinimum, "initialStepSize", csv.initialStepSize, ... "barrierGain", csv.barrierGain, "barrierExponent", csv.barrierExponent, "numObstacles", csv.numObstacles,... "numAgents", csv.numAgents, "collisionRadius", csv.collisionRadius, "comRange", csv.comRange, "alphaDist", csv.alphaDist, "betaDist", csv.betaDist, "alphaTilt", csv.alphaTilt, "betaTilt", csv.betaTilt); end @@ -70,7 +69,7 @@ classdef parametricTestSuite < matlab.unittest.TestCase for ii = 1:size(params.timestep, 1) % Set up square domain tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); - tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([.75 * l, 0.75 * l]), tc.domain, params.discretizationStep(ii), tc.protectedRange, params.sensorPerformanceMinimum(ii)); + tc.domain.objective = tc.domain.objective.initialize(objectiveFunctionWrapper([.75 * l, 0.75 * l]), tc.domain, params.discretizationStep(ii), params.protectedRange(ii), params.sensorPerformanceMinimum(ii)); % Initialize agents agents = cell(params.numAgents(ii), 1); @@ -105,29 +104,27 @@ classdef parametricTestSuite < matlab.unittest.TestCase retry = true; while retry agentPos = agents{baseAgentIdx}.commsGeometry.random(); + retry = false; % Check that the agent's greatest sensor % performance clears the threshold for partitioning if sensorModel.sensorPerformance(agentPos, [agentPos(1:2), 0]) < params.sensorPerformanceMinimum(ii) retry = true; - continue; end % Check that candidate position is well inside the domain bounds = [params.collisionRadius(ii, jj) * ones(1, 2), max([params.collisionRadius(ii, jj), params.minAlt(ii)]); l / 2 * ones(1, 2), l - params.collisionRadius(ii, jj)]; - if ~isequal(agentPos < bounds, [false, false, false; true, true, true]) + if ~retry && ~isequal(agentPos < bounds, [false, false, false; true, true, true]) retry = true; - continue; end % Check that candidate position does not collide with existing agents for kk = 1:(jj - 1) - if norm(agents{kk}.pos - agentPos, 2) < agents{kk}.collisionGeometry.radius + params.collisionRadius(ii, jj) + if ~retry && norm(agents{kk}.pos - agentPos, 2) < agents{kk}.collisionGeometry.radius + params.collisionRadius(ii, jj) retry = true; - continue; + break; end end - retry = false; end % Initialize agent @@ -135,9 +132,66 @@ classdef parametricTestSuite < matlab.unittest.TestCase agents{jj} = agents{jj}.initialize(agentPos, collisionGeometry, sensorModel, params.comRange(ii, jj), params.maxIter(ii), params.initialStepSize(ii), sprintf("Agent %d", jj), tc.plotCommsGeometry); end + % randomly shuffle agents to make the network more interesting (probably) + agents = agents(randperm(numel(agents))); + + % Set up obstacles + obstacles = cell(params.numObstacles(ii), 1); + [obstacles{:}] = deal(rectangularPrism); + + % Define ranges to permit obstacles (relies on certain + % assumptions about agent and objective placement) + bounds = [max(cell2mat(cellfun(@(x) x.pos(1:2), agents, "UniformOutput", false))) + max(cellfun(@(x) x.collisionGeometry.radius, agents)); ... + tc.domain.objective.groundPos - tc.domain.objective.protectedRange]; + + for jj = 1:size(obstacles, 1) + % randomly place obstacles in at least the X or Y or X + % and Y range defined by the objective region and the + % agents initial region + retry = true; + while retry + retry = false; + + % candidate corners for obstacle + corners = [sort(tc.domain.maxCorner(1, 1:2) .* rand(2), 1, "ascend"), [params.minAlt(ii); params.minAlt(ii) + rand * (tc.domain.maxCorner(3) - params.minAlt(ii))]]; + + % Check X falls into bucket in at least one vertex + if ~retry && ~(any(corners(:, 1) > bounds(1, 1) & corners(:, 1) < bounds(2, 1)) || any(corners(:, 2) > bounds(1, 2) & corners(:, 2) < bounds(2, 2))) + retry = true; + end + + % Initialize obstacle using proposed coordinates + if ~retry + obstacles{jj} = obstacles{jj}.initialize(corners, REGION_TYPE.OBSTACLE, sprintf("Obstacle %d", jj)); + end + + + % Make sure the obstacle doesn't crowd the objective + if ~retry && obstacles{jj}.distance([tc.domain.objective.groundPos, params.minAlt(ii)]) <= tc.domain.objective.protectedRange + retry = true; + end + + % Check if the obstacle collides with an existing obstacle + if ~retry && jj > 1 && tc.obstacleCollisionCheck(obstacles(1:(jj - 1)), obstacles{jj}) + retry = true; + end + + % Check if the obstacle collides with an agent + if ~retry + AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obstacles), agents, "UniformOutput", false); + if any(vertcat(AO_collisions{:})) + retry = true; + end + end + + if retry + continue; + end + end + end + % Set up simulation - agents = agents(randperm(numel(agents))); % randomly shuffle agents to make the network more interesting (probably) - tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain(ii), params.barrierExponent(ii), params.minAlt(ii), params.timestep(ii), params.maxIter(ii), tc.obstacles, tc.makePlots, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, agents, params.barrierGain(ii), params.barrierExponent(ii), params.minAlt(ii), params.timestep(ii), params.maxIter(ii), obstacles, tc.makePlots, tc.makeVideo); % Save simulation parameters to output file tc.testClass.writeParams(); @@ -148,7 +202,19 @@ classdef parametricTestSuite < matlab.unittest.TestCase % Cleanup tc.testClass = tc.testClass.teardown(); end - + 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; + return; + end + end end end end \ No newline at end of file diff --git a/test/testIterations.csv b/test/testIterations.csv index 1b93c91..b20b088 100644 --- a/test/testIterations.csv +++ b/test/testIterations.csv @@ -1,3 +1,3 @@ -timestep, maxIter, minAlt, discretizationStep, sensorPerformanceMinimum, initialStepSize, barrierGain, barrierExponent, numObstacles, numAgents, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt -1, 10, 1, 0.01, 1e-6, 0.2, 100, 3, 4, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "2.5, 2.5, 2.5, 2.5, 2.5", "3, 3, 3, 3, 3", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3", "3, 3, 3, 3, 3" -1, 10, 1, 0.01, 1e-6, 0.2, 100, 3, 4, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "3.5, 3.5, 3.5, 3.5, 3.5", "15, 15, 15, 15, 15", "30, 30, 30, 30, 30", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3" +timestep, maxIter, minAlt, discretizationStep, protectedRange, sensorPerformanceMinimum, initialStepSize, barrierGain, barrierExponent, numObstacles, numAgents, collisionRadius, comRange, alphaDist, betaDist, alphaTilt, betaTilt +1, 10, 1, 0.01, 1, 1e-6, 0.2, 100, 3, 4, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "2.5, 2.5, 2.5, 2.5, 2.5", "3, 3, 3, 3, 3", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3", "3, 3, 3, 3, 3" +1, 10, 1, 0.01, 1, 1e-6, 0.2, 100, 3, 4, 5, "0.1, 0.1, 0.1, 0.1, 0.1", "3.5, 3.5, 3.5, 3.5, 3.5", "15, 15, 15, 15, 15", "30, 30, 30, 30, 30", "15, 15, 15, 15, 15", "3, 3, 3, 3, 3"