fixed init generation being really slow
This commit is contained in:
@@ -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')};
|
||||
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="validators/arguments" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="b7c7eec5-a318-4c17-adb2-b13a21bf0609" type="Reference"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="1" type="DIR_SIGNIFIER"/>
|
||||
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="domainContainsObstacle.m" type="File"/>
|
||||
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="obstacleCoversObjective.m" type="File"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="arguments" type="File"/>
|
||||
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="obstacleCrowdsObjective.m" type="File"/>
|
||||
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="agentsCrowdObjective.m" type="File"/>
|
||||
272
test_miSim.m
272
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;
|
||||
badCandidate = true;
|
||||
while badCandidate
|
||||
% Instantiate a rectangular prism obstacle
|
||||
tc.obstacles{ii} = rectangularPrism;
|
||||
|
||||
% Randomly come up with dimensions until they
|
||||
% fit within the domain
|
||||
candidateMinCorner = [-Inf(1, 2), 0];
|
||||
candidateMaxCorner = Inf(1, 3);
|
||||
% Randomly generate min corner for the obstacle
|
||||
candidateMinCorner = tc.domain.random();
|
||||
candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
|
||||
|
||||
% 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
|
||||
% Randomly select a corresponding maximum corner that
|
||||
% satisfies min/max obstacle size specifications
|
||||
candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
|
||||
|
||||
% 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
|
||||
% Initialize obstacle
|
||||
tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
|
||||
|
||||
% 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
|
||||
% 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));
|
||||
end
|
||||
|
||||
% 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;
|
||||
% 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
|
||||
end
|
||||
|
||||
% Check connectivity
|
||||
G = graph(adjacency);
|
||||
connected = all(conncomp(G) == 1);
|
||||
nIter = nIter + 1;
|
||||
% Candidate agent is valid, store to pass in to sim
|
||||
initInvalid = false;
|
||||
tc.agents{ii} = newAgent;
|
||||
end
|
||||
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
|
||||
|
||||
11
validators/agentsCrowdObjective.m
Normal file
11
validators/agentsCrowdObjective.m
Normal file
@@ -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
|
||||
21
validators/domainContainsObstacle.m
Normal file
21
validators/domainContainsObstacle.m
Normal file
@@ -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
|
||||
13
validators/obstacleCoversObjective.m
Normal file
13
validators/obstacleCoversObjective.m
Normal file
@@ -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
|
||||
11
validators/obstacleCrowdsObjective.m
Normal file
11
validators/obstacleCrowdsObjective.m
Normal file
@@ -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
|
||||
Reference in New Issue
Block a user