diff --git a/geometries/rectangularPrism.m b/geometries/rectangularPrism.m index c2b37a9..a1b6001 100644 --- a/geometries/rectangularPrism.m +++ b/geometries/rectangularPrism.m @@ -74,6 +74,8 @@ classdef rectangularPrism arguments (Output) d (:, 1) double end + assert(~obj.contains(pos), "Cannot determine distance for a point inside of the geometry"); + cPos = NaN(1, 3); for ii = 1:3 if pos(ii) < obj.minCorner(ii) @@ -94,6 +96,8 @@ classdef rectangularPrism arguments (Output) d (:, 1) double end + assert(obj.contains(pos), "Cannot determine interior distance for a point outside of the geometry"); + % find minimum distance to any face d = min([pos(1) - obj.minCorner(1), ... pos(2) - obj.minCorner(2), ... @@ -112,6 +116,47 @@ classdef rectangularPrism end c = all(pos >= repmat(obj.minCorner, size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 1), 1), 2); end + function c = containsLine(obj, pos1, pos2) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; + pos1 (1, 3) double; + pos2 (1, 3) double; + end + arguments (Output) + c (1, 1) logical + end + + d = pos2 - pos1; + + % edge case where the line is parallel to the geometry + if abs(d) < 1e-12 + % check if it happens to start or end inside or outside of + % the geometry + if obj.contains(pos1) || obj.contains(pos2) + c = true; + else + c = false; + end + return; + end + + tmin = -inf; + tmax = inf; + + % Standard case + for ii = 1:3 + t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii); + t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii); + tmin = max(tmin, min(t1, t2)); + tmax = min(tmax, max(t1, t2)); + if tmin > tmax + c = false; + return; + end + end + + c = (tmax >= 0) && (tmin <= 1); + end function f = plotWireframe(obj, f) arguments (Input) obj (1, 1) {mustBeA(obj, 'rectangularPrism')}; diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMd.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMd.xml new file mode 100644 index 0000000..2bd39f9 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMp.xml b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMp.xml new file mode 100644 index 0000000..442ec92 --- /dev/null +++ b/resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/ZHl_0ut8g6oljcuQL6lY7K4NP_Ed.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/3f4CTErydQ2TqIMJrHErKW1HTm8d.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/ZHl_0ut8g6oljcuQL6lY7K4NP_Ed.xml rename to resources/project/IkDHDbHimostG0Ka3Qk97pof68k/3f4CTErydQ2TqIMJrHErKW1HTm8d.xml diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/3f4CTErydQ2TqIMJrHErKW1HTm8p.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28p.xml rename to resources/project/IkDHDbHimostG0Ka3Qk97pof68k/3f4CTErydQ2TqIMJrHErKW1HTm8p.xml diff --git a/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQd.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQp.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQp.xml new file mode 100644 index 0000000..01cb34e --- /dev/null +++ b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/eU5R_sxHD3gVw76yiaPpbMUKFIkd.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oA3NoVC5FBKXh-LtCKBuVxHDs28d.xml rename to resources/project/IkDHDbHimostG0Ka3Qk97pof68k/eU5R_sxHD3gVw76yiaPpbMUKFIkd.xml diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/ZHl_0ut8g6oljcuQL6lY7K4NP_Ep.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/eU5R_sxHD3gVw76yiaPpbMUKFIkp.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/ZHl_0ut8g6oljcuQL6lY7K4NP_Ep.xml rename to resources/project/IkDHDbHimostG0Ka3Qk97pof68k/eU5R_sxHD3gVw76yiaPpbMUKFIkp.xml diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oUK2iWSD0GT8w9SFN0zCkhZLgUod.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/xLI7SQj40yDbM71LQ8pVZ0dWmrgd.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oUK2iWSD0GT8w9SFN0zCkhZLgUod.xml rename to resources/project/IkDHDbHimostG0Ka3Qk97pof68k/xLI7SQj40yDbM71LQ8pVZ0dWmrgd.xml diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oUK2iWSD0GT8w9SFN0zCkhZLgUop.xml b/resources/project/IkDHDbHimostG0Ka3Qk97pof68k/xLI7SQj40yDbM71LQ8pVZ0dWmrgp.xml similarity index 100% rename from resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/oUK2iWSD0GT8w9SFN0zCkhZLgUop.xml rename to resources/project/IkDHDbHimostG0Ka3Qk97pof68k/xLI7SQj40yDbM71LQ8pVZ0dWmrgp.xml diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yd.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yp.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yp.xml new file mode 100644 index 0000000..82a6428 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEd.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEp.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEp.xml new file mode 100644 index 0000000..3f2bd30 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kd.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kd.xml new file mode 100644 index 0000000..4356a6a --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kd.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kp.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kp.xml new file mode 100644 index 0000000..52177dc --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcd.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcp.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcp.xml new file mode 100644 index 0000000..7984fb9 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUd.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUd.xml new file mode 100644 index 0000000..99772b4 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUd.xml @@ -0,0 +1,6 @@ + + + + + \ No newline at end of file diff --git a/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUp.xml b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUp.xml new file mode 100644 index 0000000..8f14f17 --- /dev/null +++ b/resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUp.xml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/test_miSim.m b/test_miSim.m index 7ca6362..42ced91 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -1,61 +1,72 @@ classdef test_miSim < matlab.unittest.TestCase properties (Access = private) testClass = miSim; + % Domain - domain = rectangularPrism; + domain = rectangularPrism; % domain geometry % Obstacles - minNumObstacles = 1; - maxNumObstacles = 3; + minNumObstacles = 1; % Minimum number of obstacles to be randomly generated + maxNumObstacles = 3; % Maximum number of obstacles to be randomly generated + minObstacleSize = 1; % Minimum size of a randomly generated obstacle + maxObstacleSize = 6; % Maximum size of a randomly generated obstacle obstacles = cell(1, 0); - minObstacleDimension = 1; % Objective + objectiveDiscretizationStep = 0.01; % Step at which the objective function is solved in X and Y space + protectedRange = 1; % Minimum distance between the sensing objective and the edge of the domain objective = sensingObjective; - objectiveFunction = @(x, y) 0; - objectiveDiscretizationStep = 0.01; - protectedRange = 1; % Agents - minAgents = 3; - maxAgents = 9; - agents = cell(1, 0); + minAgents = 3; % Minimum number of agents to be randomly generated + maxAgents = 9; % Maximum number of agents to be randomly generated + agents = cell(0, 1); % Collision - minCollisionRange = 0.1; - maxCollisionRange = 0.5; + minCollisionRange = 0.1; % Minimum randomly generated collision geometry size + maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size collisionRanges = NaN; % Communications - comRange = 5; + comRange = 5; % Maximum range between agents that forms a communications link end % Setup for each test methods (TestMethodSetup) % Generate a random domain function tc = setDomain(tc) - % random integer-sized domain ranging from [0, 5] to [0, 25] in all dimensions + % random integer-sized cube domain ranging from [0, 5 -> 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) + % Using a bivariate normal distribution + % Set peak position (mean) mu = tc.domain.minCorner; while tc.domain.interiorDistance(mu) < tc.protectedRange mu = tc.domain.random(); end mu(3) = 0; - assert(tc.domain.contains(mu)); + + % Set standard deviations of bivariate distribution 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); + + % Define objective + tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep); end - % Instantiate agents, they will be initialized under different - % parameters in individual test cases + % Instantiate agents function tc = setAgents(tc) + % 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; end + + % Define random collision ranges for each agent tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange); end end @@ -66,140 +77,137 @@ classdef test_miSim < matlab.unittest.TestCase % randomly create 2-3 obstacles nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles); tc.obstacles = cell(nGeom, 1); + + % Iterate over obstacles to initialize for ii = 1:size(tc.obstacles, 1) - % Instantiate a rectangular prism obstacle - tc.obstacles{ii, 1} = rectangularPrism; - - % Randomly come up with dimensions until they - % fit within the domain - candidateMinCorner = [-Inf(1, 2), 0]; - candidateMaxCorner = Inf(1, 3); - - % make sure obstacles are not too small in any dimension - tooSmall = true; - while tooSmall - % make sure the obstacles don't contain the sensing - % objective or encroach on it too much - obstructs = true; - while obstructs + badCandidate = true; + while badCandidate + % Instantiate a rectangular prism obstacle + tc.obstacles{ii} = rectangularPrism; - % 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 + % Randomly generate min corner for the obstacle + candidateMinCorner = tc.domain.random(); + candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain - % 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 + % 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)); + + % Make sure that the obstacles are fully contained by + % the domain + if ~domainContainsObstacle(tc.domain, tc.obstacles{ii}) + continue; end - if min(candidateMaxCorner - candidateMinCorner) >= tc.minObstacleDimension - tooSmall = false; - else - candidateMinCorner = [-Inf(1, 2), 0]; - candidateMaxCorner = Inf(1, 3); + + % Make sure that the obstacles don't cover the sensing + % objective + if obstacleCoversObjective(tc.objective, tc.obstacles{ii}) + continue; end + + % Make sure that the obstacles aren't too close to the + % sensing objective + if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange) + continue; + end + + badCandidate = false; end - - % Reduce infinite dimensions to the domain - candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner)); - candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner)); - - % Initialize obstacle geometry - 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 (that is not too close to the sensing - % objective) - boringInit = true; - while boringInit + + % Add agents individually, ensuring that each addition does not + % invalidate the initialization setup + for ii = 1:size(tc.agents, 1) + initInvalid = true; + while initInvalid + candidatePos = [tc.objective.groundPos, 0]; + % Generate a random position for the agent based on + % existing agent positions + if ii == 1 + while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2) 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 + else + candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2)); + end + + % Make sure that the candidate position is within the + % domain + if ~tc.domain.contains(candidatePos) + continue; + end + + % Make sure that the candidate position does not crowd + % the sensing objective and create boring scenarios + if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2) + continue; + end + + % Make sure that there exist unobstructed lines of sight at + % appropriate ranges to form a connected communications + % graph between the agents + connections = false(1, ii - 1); + for jj = 1:(ii - 1) + if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange + % Check new agent position against all existing + % agent positions for communications range + connections(jj) = true; + for kk = 1:size(tc.obstacles, 1) + if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos) + connections(jj) = false; + end 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.obstacles, 1) - inside = false; - if tc.obstacles{jj, 1}.contains(tc.agents{ii, 1}.pos) - % Found a violation, stop checking - inside = true; + end + + % New agent must be connected to an existing agent to + % be valid + if ii ~= 1 && ~any(connections) + continue; + end + + % Initialize candidate agent + candidateGeometry = rectangularPrism; + newAgent = 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)); + + % Make sure candidate agent doesn't collide with + % domain, obstacles, or any existing agents + violation = false; + for jj = 1:size(newAgent.collisionGeometry.vertices, 1) + % Check if collision geometry exits domain + if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) + violation = true; + break; + end + + % Check if collision geometry enters obstacle + for kk = 1:size(tc.obstacles, 1) + if tc.obstacles{kk}.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) + violation = true; break; end end - - % Agent is inside obstacle, try again - if inside - continue; - end - - % Create a collision geometry for this agent - 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 - for jj = 1:(ii - 1) - % Check if previously defined agents collide with - % this one - colliding = false; - if candidateGeometry.contains(tc.agents{jj, 1}.pos) - % Found a violation, stop checking - colliding = true; + + % Check if collision geometry enters other + % collision geometry + for kk = 1:(ii - 1) + if tc.agents{kk}.collisionGeometry.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) + violation = true; break; end end - - % Agent is colliding with another, try again - if ii ~= 1 && colliding - continue; - end - - % Allow to proceed since no obstacle/collision - % violations were found - posInvalid = false; end - end - - % Collect all agent positions - posArray = arrayfun(@(x) x{1}.pos, tc.agents, 'UniformOutput', false); - posArray = reshape([posArray{:}], size(tc.agents, 1), 3); - - % Communications checks - adjacency = false(size(tc.agents, 1), size(tc.agents, 1)); - for ii = 1:size(tc.agents, 1) - % Compute distance from each to all agents - for jj = 1:(size(tc.agents, 1)) - if norm(posArray(ii, 1:3) - posArray(jj, 1:3)) <= tc.comRange - adjacency(ii, jj) = true; - end + if violation + continue; end + + % Candidate agent is valid, store to pass in to sim + initInvalid = false; + tc.agents{ii} = newAgent; end - - % Check connectivity - G = graph(adjacency); - connected = all(conncomp(G) == 1); - nIter = nIter + 1; end % Initialize the simulation @@ -215,7 +223,7 @@ classdef test_miSim < matlab.unittest.TestCase % Plot obstacles for ii = 1:size(tc.testClass.obstacles, 1) - tc.testClass.obstacles{ii, 1}.plotWireframe(f); + tc.testClass.obstacles{ii}.plotWireframe(f); end % Plot objective gradient @@ -223,8 +231,8 @@ classdef test_miSim < matlab.unittest.TestCase % Plot agents and their collision geometries for ii = 1:size(tc.testClass.agents, 1) - f = tc.testClass.agents{ii, 1}.plot(f); - f = tc.testClass.agents{ii, 1}.collisionGeometry.plotWireframe(f); + f = tc.testClass.agents{ii}.plot(f); + f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f); end end end diff --git a/validators/agentsCrowdObjective.m b/validators/agentsCrowdObjective.m new file mode 100644 index 0000000..be93221 --- /dev/null +++ b/validators/agentsCrowdObjective.m @@ -0,0 +1,11 @@ +function c = agentsCrowdObjective(objective, positions, protectedRange) + arguments (Input) + objective (1, 1) {mustBeA(objective, 'sensingObjective')}; + positions (:, 3) double; % this could be expanded to handle n obstacles in 1 call + protectedRange (1, 1) double; + end + arguments (Output) + c (:, 1) logical; + end + c = vecnorm(positions(:, 1:2) - objective.groundPos, 2, 2) <= protectedRange; +end \ No newline at end of file diff --git a/validators/mustBeAgents.m b/validators/arguments/mustBeAgents.m similarity index 100% rename from validators/mustBeAgents.m rename to validators/arguments/mustBeAgents.m diff --git a/validators/mustBeDcm.m b/validators/arguments/mustBeDcm.m similarity index 100% rename from validators/mustBeDcm.m rename to validators/arguments/mustBeDcm.m diff --git a/validators/mustBeGeometry.m b/validators/arguments/mustBeGeometry.m similarity index 100% rename from validators/mustBeGeometry.m rename to validators/arguments/mustBeGeometry.m diff --git a/validators/domainContainsObstacle.m b/validators/domainContainsObstacle.m new file mode 100644 index 0000000..a2d64ae --- /dev/null +++ b/validators/domainContainsObstacle.m @@ -0,0 +1,21 @@ +function c = domainContainsObstacle(domain, obstacle) + arguments (Input) + domain (1, 1) {mustBeGeometry}; + obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call + end + arguments (Output) + c (1, 1) logical; + end + + switch class(domain) + case 'rectangularPrism' + switch class(obstacle) + case 'rectangularPrism' + c = all(domain.minCorner <= obstacle.minCorner) && all(domain.maxCorner >= obstacle.maxCorner); + otherwise + error("%s not implemented for obstacles of class %s", coder.mfunctionname, class(domain)); + end + otherwise + error("%s not implemented for domains of class %s", coder.mfunctionname, class(domain)); + end +end \ No newline at end of file diff --git a/validators/obstacleCoversObjective.m b/validators/obstacleCoversObjective.m new file mode 100644 index 0000000..672a4d0 --- /dev/null +++ b/validators/obstacleCoversObjective.m @@ -0,0 +1,13 @@ +function c = obstacleCoversObjective(objective, obstacle) + arguments (Input) + objective (1, 1) {mustBeA(objective, 'sensingObjective')}; + obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call + end + arguments (Output) + c (1, 1) logical; + end + + % Check if the obstacle contains the objective's ground position if the + % ground position were raised to the obstacle's center's height + c = obstacle.contains([objective.groundPos, obstacle.center(3)]); +end \ No newline at end of file diff --git a/validators/obstacleCrowdsObjective.m b/validators/obstacleCrowdsObjective.m new file mode 100644 index 0000000..941aa69 --- /dev/null +++ b/validators/obstacleCrowdsObjective.m @@ -0,0 +1,11 @@ +function c = obstacleCrowdsObjective(objective, obstacle, protectedRange) + arguments (Input) + objective (1, 1) {mustBeA(objective, 'sensingObjective')}; + obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call + protectedRange (1, 1) double; + end + arguments (Output) + c (1, 1) logical; + end + c = norm(obstacle.distance([objective.groundPos, obstacle.center(3)])) <= protectedRange; +end \ No newline at end of file