Compare commits
3 Commits
main
...
26264ec1b9
| Author | SHA1 | Date | |
|---|---|---|---|
| 26264ec1b9 | |||
| 437257691c | |||
| b0cd420aa3 |
3
.gitignore
vendored
3
.gitignore
vendored
@@ -38,6 +38,3 @@ codegen/
|
||||
# SimBiology backup files
|
||||
*.sbproj.backup
|
||||
*.sbproj.bak
|
||||
|
||||
# Sandbox contents
|
||||
sandbox/*
|
||||
95
agent.m
95
agent.m
@@ -6,35 +6,24 @@ classdef agent
|
||||
|
||||
% Sensor
|
||||
sensingFunction = @(r) 0.5; % probability of detection as a function of range
|
||||
sensingLength = 0.05; % length parameter used by sensing function
|
||||
|
||||
% State
|
||||
lastPos = NaN(1, 3); % position from previous timestep
|
||||
pos = NaN(1, 3); % current position
|
||||
vel = NaN(1, 3); % current velocity
|
||||
cBfromC = NaN(3); % current DCM body from sim cartesian (assume fixed for now)
|
||||
pos = NaN(1, 3);
|
||||
vel = NaN(1, 3);
|
||||
cBfromC = NaN(3); % DCM body from sim cartesian (assume fixed for now)
|
||||
|
||||
% Collision
|
||||
collisionGeometry;
|
||||
|
||||
% Communication
|
||||
comRange = NaN;
|
||||
|
||||
% Plotting
|
||||
scatterPoints;
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, sensingLength, comRange, index, label)
|
||||
function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, index, label)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
pos (1, 3) double;
|
||||
vel (1, 3) double;
|
||||
cBfromC (3, 3) double {mustBeDcm};
|
||||
collisionGeometry (1, 1) {mustBeGeometry};
|
||||
sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')} = @(r) 0.5;
|
||||
sensingLength (1, 1) double = NaN;
|
||||
comRange (1, 1) double = NaN;
|
||||
collisionGeometry (1, 1) {mustBeConstraintGeometries};
|
||||
index (1, 1) double = NaN;
|
||||
label (1, 1) string = "";
|
||||
end
|
||||
@@ -46,72 +35,15 @@ classdef agent
|
||||
obj.vel = vel;
|
||||
obj.cBfromC = cBfromC;
|
||||
obj.collisionGeometry = collisionGeometry;
|
||||
obj.sensingFunction = sensingFunction;
|
||||
obj.sensingLength = sensingLength;
|
||||
obj.comRange = comRange;
|
||||
obj.index = index;
|
||||
obj.label = label;
|
||||
end
|
||||
function obj = run(obj, objectiveFunction, domain)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
end
|
||||
|
||||
% Do sensing to determine target position
|
||||
nextPos = obj.sensingFunction(objectiveFunction, domain, obj.pos, obj.sensingLength);
|
||||
|
||||
% Move to next position
|
||||
% (dynamics not modeled at this time)
|
||||
obj.lastPos = obj.pos;
|
||||
obj.pos = nextPos;
|
||||
|
||||
% Calculate movement
|
||||
d = obj.pos - obj.collisionGeometry.center;
|
||||
|
||||
% Reinitialize collision geometry in the new position
|
||||
obj.collisionGeometry = obj.collisionGeometry.initialize([obj.collisionGeometry.minCorner; obj.collisionGeometry.maxCorner] + d, obj.collisionGeometry.tag, obj.collisionGeometry.label);
|
||||
end
|
||||
function updatePlots(obj)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
end
|
||||
arguments (Output)
|
||||
end
|
||||
|
||||
% Scatterplot point positions
|
||||
for ii = 1:size(obj.scatterPoints, 1)
|
||||
obj.scatterPoints(ii).XData = obj.pos(1);
|
||||
obj.scatterPoints(ii).YData = obj.pos(2);
|
||||
obj.scatterPoints(ii).ZData = obj.pos(3);
|
||||
end
|
||||
|
||||
% Find change in agent position since last timestep
|
||||
deltaPos = obj.pos - obj.lastPos;
|
||||
|
||||
% Collision geometry edges
|
||||
for jj = 1:size(obj.collisionGeometry.lines, 2)
|
||||
% Update plotting
|
||||
for ii = 1:size(obj.collisionGeometry.lines(:, jj), 1)
|
||||
obj.collisionGeometry.lines(ii, jj).XData = obj.collisionGeometry.lines(ii, jj).XData + deltaPos(1);
|
||||
obj.collisionGeometry.lines(ii, jj).YData = obj.collisionGeometry.lines(ii, jj).YData + deltaPos(2);
|
||||
obj.collisionGeometry.lines(ii, jj).ZData = obj.collisionGeometry.lines(ii, jj).ZData + deltaPos(3);
|
||||
end
|
||||
end
|
||||
|
||||
% Network connections
|
||||
end
|
||||
function [obj, f] = plot(obj, f)
|
||||
function f = plot(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
@@ -120,21 +52,8 @@ classdef agent
|
||||
|
||||
% Plot points representing the agent position
|
||||
hold(f.CurrentAxes, "on");
|
||||
o = scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
|
||||
scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 50);
|
||||
hold(f.CurrentAxes, "off");
|
||||
|
||||
% Check if this is a tiled layout figure
|
||||
if strcmp(f.Children(1).Type, 'tiledlayout')
|
||||
% Add to other perspectives
|
||||
o = [o; copyobj(o(1), f.Children(1).Children(2))];
|
||||
o = [o; copyobj(o(1), f.Children(1).Children(3))];
|
||||
o = [o; copyobj(o(1), f.Children(1).Children(5))];
|
||||
end
|
||||
|
||||
obj.scatterPoints = o;
|
||||
|
||||
% Plot collision geometry
|
||||
[obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(f);
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -1,49 +1,8 @@
|
||||
function f = firstPlotSetup(f)
|
||||
if isempty(f.CurrentAxes)
|
||||
tiledlayout(f, 4, 3, "TileSpacing", "tight", "Padding", "compact");
|
||||
|
||||
% Top-down view
|
||||
nexttile(1, [1, 2]);
|
||||
axes(f.Children(1).Children(1));
|
||||
axis(f.Children(1).Children(1), "image");
|
||||
grid(f.Children(1).Children(1), "on");
|
||||
view(f.Children(1).Children(1), 0, 90);
|
||||
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
|
||||
title(f.Children(1).Children(1), "Top-down Perspective");
|
||||
|
||||
% Communications graph
|
||||
nexttile(3, [1, 1]);
|
||||
axes(f.Children(1).Children(1));
|
||||
axis(f.Children(1).Children(1), "image");
|
||||
grid(f.Children(1).Children(1), "off");
|
||||
view(f.Children(1).Children(1), 0, 0);
|
||||
title(f.Children(1).Children(1), "Network Graph");
|
||||
|
||||
% 3D view
|
||||
nexttile(4, [2, 2]);
|
||||
axes(f.Children(1).Children(1));
|
||||
axis(f.Children(1).Children(1), "image");
|
||||
grid(f.Children(1).Children(1), "on");
|
||||
view(f.Children(1).Children(1), 3);
|
||||
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
|
||||
title(f.Children(1).Children(1), "3D Perspective");
|
||||
|
||||
% Side-on view
|
||||
nexttile(6, [2, 1]);
|
||||
axes(f.Children(1).Children(1));
|
||||
axis(f.Children(1).Children(1), "image");
|
||||
grid(f.Children(1).Children(1), "on");
|
||||
view(f.Children(1).Children(1), 90, 0);
|
||||
ylabel(f.Children(1).Children(1), "Y"); zlabel(f.Children(1).Children(1), "Z");
|
||||
title(f.Children(1).Children(1), "Side-on Perspective");
|
||||
|
||||
% Front-on view
|
||||
nexttile(10, [1, 2]);
|
||||
axes(f.Children(1).Children(1));
|
||||
axis(f.Children(1).Children(1), "image");
|
||||
grid(f.Children(1).Children(1), "on");
|
||||
view(f.Children(1).Children(1), 0, 0);
|
||||
xlabel(f.Children(1).Children(1), "X"); zlabel(f.Children(1).Children(1), "Z");
|
||||
title(f.Children(1).Children(1), "Front-on Perspective");
|
||||
axes(f);
|
||||
axis(f.CurrentAxes, "equal");
|
||||
grid(f.CurrentAxes, "on");
|
||||
view(f.CurrentAxes, 3);
|
||||
end
|
||||
end
|
||||
@@ -1,200 +0,0 @@
|
||||
classdef rectangularPrism
|
||||
% Rectangular prism geometry
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
% Meta
|
||||
tag = REGION_TYPE.INVALID;
|
||||
label = "";
|
||||
|
||||
% Spatial
|
||||
minCorner = NaN(1, 3);
|
||||
maxCorner = NaN(1, 3);
|
||||
dimensions = NaN(1, 3);
|
||||
center = NaN;
|
||||
footprint = NaN(4, 2);
|
||||
|
||||
% Graph
|
||||
vertices = NaN(8, 3);
|
||||
edges = [1 2; 2 3; 3 4; 4 1; % bottom square
|
||||
5 6; 6 8; 8 7; 7 5; % top square
|
||||
1 5; 2 6; 3 8; 4 7]; % vertical edges
|
||||
|
||||
% Plotting
|
||||
lines;
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, bounds, tag, label)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
bounds (2, 3) double;
|
||||
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
||||
label (1, 1) string = "";
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
end
|
||||
|
||||
obj.tag = tag;
|
||||
obj.label = label;
|
||||
|
||||
%% Define geometry bounds by LL corner and UR corner
|
||||
obj.minCorner = bounds(1, 1:3);
|
||||
obj.maxCorner = bounds(2, 1:3);
|
||||
|
||||
% Compute L, W, H
|
||||
obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
|
||||
|
||||
% Compute center
|
||||
obj.center = obj.minCorner + obj.dimensions ./ 2;
|
||||
|
||||
% Compute vertices
|
||||
obj.vertices = [obj.minCorner;
|
||||
obj.maxCorner(1), obj.minCorner(2:3);
|
||||
obj.maxCorner(1:2), obj.minCorner(3);
|
||||
obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
|
||||
obj.minCorner(1:2), obj.maxCorner(3);
|
||||
obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
|
||||
obj.minCorner(1), obj.maxCorner(2:3)
|
||||
obj.maxCorner;];
|
||||
|
||||
% Compute footprint
|
||||
obj.footprint = [obj.minCorner(1:2); ...
|
||||
[obj.minCorner(1), obj.maxCorner(2)]; ...
|
||||
[obj.maxCorner(1), obj.minCorner(2)]; ...
|
||||
obj.maxCorner(1:2)];
|
||||
end
|
||||
function r = random(obj)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
end
|
||||
arguments (Output)
|
||||
r (1, 3) double
|
||||
end
|
||||
r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
|
||||
end
|
||||
function d = distance(obj, pos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
pos (:, 3) double;
|
||||
end
|
||||
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)
|
||||
cPos(ii) = obj.minCorner(ii);
|
||||
elseif pos(ii) > obj.maxCorner(ii)
|
||||
cPos(ii) = obj.maxCorner(ii);
|
||||
else
|
||||
cPos(ii) = pos(ii);
|
||||
end
|
||||
end
|
||||
d = norm(cPos - pos);
|
||||
end
|
||||
function d = interiorDistance(obj, pos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
pos (:, 3) double;
|
||||
end
|
||||
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), ...
|
||||
pos(3) - obj.minCorner(3), ...
|
||||
obj.maxCorner(1) - pos(1), ...
|
||||
obj.maxCorner(2) - pos(2), ...
|
||||
obj.maxCorner(3) - pos(3)]);
|
||||
end
|
||||
function c = contains(obj, pos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
pos (:, 3) double;
|
||||
end
|
||||
arguments (Output)
|
||||
c (:, 1) logical
|
||||
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 [obj, f] = plotWireframe(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Create axes if they don't already exist
|
||||
f = firstPlotSetup(f);
|
||||
|
||||
% Create plotting inputs from vertices and edges
|
||||
X = [obj.vertices(obj.edges(:,1),1), obj.vertices(obj.edges(:,2),1)]';
|
||||
Y = [obj.vertices(obj.edges(:,1),2), obj.vertices(obj.edges(:,2),2)]';
|
||||
Z = [obj.vertices(obj.edges(:,1),3), obj.vertices(obj.edges(:,2),3)]';
|
||||
|
||||
% Plot the boundaries of the geometry
|
||||
hold(f.CurrentAxes, "on");
|
||||
o = plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
||||
hold(f.CurrentAxes, "off");
|
||||
|
||||
% Check if this is a tiled layout figure
|
||||
if strcmp(f.Children(1).Type, 'tiledlayout')
|
||||
% Add to other perspectives
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
|
||||
end
|
||||
|
||||
obj.lines = o;
|
||||
end
|
||||
end
|
||||
end
|
||||
106
geometries/rectangularPrismConstraint.m
Normal file
106
geometries/rectangularPrismConstraint.m
Normal file
@@ -0,0 +1,106 @@
|
||||
classdef rectangularPrismConstraint
|
||||
% Rectangular prism constraint geometry
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
tag = REGION_TYPE.INVALID;
|
||||
label = "";
|
||||
|
||||
minCorner = NaN(3, 1);
|
||||
maxCorner = NaN(3, 1);
|
||||
|
||||
dimensions = NaN(3, 1);
|
||||
|
||||
center = NaN;
|
||||
|
||||
vertices = NaN(8, 3);
|
||||
|
||||
footprint = NaN(2, 4);
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, bounds, tag, label)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
|
||||
bounds (3, 2) double;
|
||||
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
||||
label (1, 1) string = "";
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
|
||||
end
|
||||
|
||||
obj.tag = tag;
|
||||
obj.label = label;
|
||||
|
||||
%% Define geometry bounds by LL corner and UR corner
|
||||
obj.minCorner = bounds(:, 1);
|
||||
obj.maxCorner = bounds(:, 2);
|
||||
|
||||
% Compute L, W, H
|
||||
obj.dimensions = [obj.maxCorner(1) - obj.minCorner(1), obj.maxCorner(2) - obj.minCorner(2), obj.maxCorner(3) - obj.minCorner(3)];
|
||||
|
||||
% Compute center
|
||||
obj.center = obj.minCorner + obj.dimensions ./ 2;
|
||||
|
||||
% Compute vertices
|
||||
obj.vertices = [obj.minCorner';
|
||||
obj.maxCorner(1), obj.minCorner(2:3)';
|
||||
obj.maxCorner(1:2)', obj.minCorner(3);
|
||||
obj.minCorner(1), obj.maxCorner(2), obj.minCorner(3);
|
||||
obj.minCorner(1:2)', obj.maxCorner(3);
|
||||
obj.maxCorner(1), obj.minCorner(2), obj.maxCorner(3);
|
||||
obj.minCorner(1), obj.maxCorner(2:3)'
|
||||
obj.maxCorner';];
|
||||
|
||||
% Compute footprint
|
||||
obj.footprint = [obj.minCorner(1:2, 1), ...
|
||||
[obj.minCorner(1, 1); obj.maxCorner(2, 1)], ...
|
||||
[obj.maxCorner(1, 1); obj.minCorner(2, 1)], ...
|
||||
obj.maxCorner(1:2, 1)];
|
||||
end
|
||||
function r = random(obj)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
|
||||
end
|
||||
arguments (Output)
|
||||
r (1, 3) double
|
||||
end
|
||||
r = (obj.vertices(1, 1:3) + rand(1, 3) .* obj.vertices(8, 1:3) - obj.vertices(1, 1:3))';
|
||||
end
|
||||
function c = contains(obj, pos)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
|
||||
pos (:, 3) double;
|
||||
end
|
||||
arguments (Output)
|
||||
c (1, 1) logical
|
||||
end
|
||||
c = all(pos >= repmat(obj.minCorner', size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner', size(pos, 1), 1), 2);
|
||||
end
|
||||
function f = plotWireframe(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrismConstraint')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Create axes if they don't already exist
|
||||
f = firstPlotSetup(f);
|
||||
|
||||
edges = [1 2; 2 3; 3 4; 4 1; % bottom square
|
||||
5 6; 6 8; 8 7; 7 5; % top square
|
||||
1 5; 2 6; 3 8; 4 7]; % vertical edges
|
||||
|
||||
% Create plotting inputs from vertices and edges
|
||||
X = [obj.vertices(edges(:,1),1), obj.vertices(edges(:,2),1)]';
|
||||
Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]';
|
||||
Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]';
|
||||
|
||||
% Plot the boundaries of the constraint geometry
|
||||
hold(f.CurrentAxes, "on");
|
||||
plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
||||
hold(f.CurrentAxes, "off");
|
||||
end
|
||||
end
|
||||
end
|
||||
224
miSim.m
224
miSim.m
@@ -3,241 +3,37 @@ classdef miSim
|
||||
|
||||
% Simulation parameters
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
timestep = NaN; % delta time interval for simulation iterations
|
||||
maxIter = NaN; % maximum number of simulation iterations
|
||||
domain = rectangularPrism;
|
||||
domain = rectangularPrismConstraint;
|
||||
objective = sensingObjective;
|
||||
obstacles = cell(0, 1); % geometries that define obstacles within the domain
|
||||
constraintGeometries = cell(0, 1); % geometries that define constraints within the domain
|
||||
agents = cell(0, 1); % agents that move within the domain
|
||||
adjacency = NaN; % Adjacency matrix representing communications network graph
|
||||
end
|
||||
|
||||
properties (Access = private)
|
||||
v = VideoWriter(fullfile('sandbox', strcat(string(datetime('now'), 'yyyy_MM_dd_HH_mm_ss'), '_miSimHist')));
|
||||
connectionsPlot; % objects for lines connecting agents in spatial plots
|
||||
graphPlot; % objects for abstract network graph plot
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles)
|
||||
function obj = initialize(obj, domain, objective, agents, constraintGeometries)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
domain (1, 1) {mustBeConstraintGeometries};
|
||||
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
|
||||
agents (:, 1) cell {mustBeAgents};
|
||||
timestep (:, 1) double = 0.05;
|
||||
maxIter (:, 1) double = 1000;
|
||||
obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1);
|
||||
constraintGeometries (:, 1) cell {mustBeConstraintGeometries} = cell(0, 1);
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Define simulation time parameters
|
||||
obj.timestep = timestep;
|
||||
obj.maxIter = maxIter;
|
||||
|
||||
% Define domain
|
||||
%% Define domain
|
||||
obj.domain = domain;
|
||||
|
||||
% Add geometries representing obstacles within the domain
|
||||
obj.obstacles = obstacles;
|
||||
%% Add constraint geometries against the domain
|
||||
obj.constraintGeometries = constraintGeometries;
|
||||
|
||||
% Define objective
|
||||
%% Define objective
|
||||
obj.objective = objective;
|
||||
|
||||
% Define agents
|
||||
%% Define agents
|
||||
obj.agents = agents;
|
||||
|
||||
% Compute adjacency matrix
|
||||
obj = obj.updateAdjacency();
|
||||
|
||||
% Set up initial plot
|
||||
% Set up axes arrangement
|
||||
% Plot domain
|
||||
[obj.domain, f] = obj.domain.plotWireframe();
|
||||
|
||||
% Set plotting limits to focus on the domain
|
||||
xlim([obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
|
||||
ylim([obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
|
||||
zlim([obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
|
||||
|
||||
% Plot obstacles
|
||||
for ii = 1:size(obj.obstacles, 1)
|
||||
[obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(f);
|
||||
end
|
||||
|
||||
% Plot objective gradient
|
||||
f = obj.objective.plot(f);
|
||||
|
||||
% Plot agents and their collision geometries
|
||||
for ii = 1:size(obj.agents, 1)
|
||||
[obj.agents{ii}, f] = obj.agents{ii}.plot(f);
|
||||
end
|
||||
|
||||
% Plot communication links
|
||||
[obj, f] = obj.plotConnections(f);
|
||||
|
||||
% Plot abstract network graph
|
||||
[obj, f] = obj.plotGraph(f);
|
||||
end
|
||||
function [obj, f] = run(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Create axes if they don't already exist
|
||||
f = firstPlotSetup(f);
|
||||
|
||||
% Set up times to iterate over
|
||||
times = linspace(0, obj.timestep * obj.maxIter, obj.maxIter+1)';
|
||||
|
||||
% Start video writer
|
||||
obj.v.FrameRate = 1/obj.timestep;
|
||||
obj.v.Quality = 90;
|
||||
obj.v.open();
|
||||
|
||||
for ii = 1:size(times, 1)
|
||||
% Display current sim time
|
||||
t = times(ii);
|
||||
fprintf("Sim Time: %4.2f (%d/%d)\n", t, ii, obj.maxIter)
|
||||
|
||||
% Iterate over agents to simulate their motion
|
||||
for jj = 1:size(obj.agents, 1)
|
||||
obj.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain);
|
||||
end
|
||||
|
||||
% Update adjacency matrix
|
||||
obj = obj.updateAdjacency;
|
||||
|
||||
% Update plots
|
||||
[obj, f] = obj.updatePlots(f);
|
||||
|
||||
% Write frame in to video
|
||||
I = getframe(f);
|
||||
obj.v.writeVideo(I);
|
||||
end
|
||||
|
||||
% Close video file
|
||||
obj.v.close();
|
||||
end
|
||||
function [obj, f] = updatePlots(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Update agent positions, collision geometries
|
||||
for ii = 1:size(obj.agents, 1)
|
||||
obj.agents{ii}.updatePlots();
|
||||
end
|
||||
|
||||
% The remaining updates might be possible to do in a clever way
|
||||
% that moves existing lines instead of clearing and
|
||||
% re-plotting, which is much better for performance boost
|
||||
|
||||
% Update agent connections plot
|
||||
delete(obj.connectionsPlot);
|
||||
[obj, f] = obj.plotConnections(f);
|
||||
|
||||
% Update network graph plot
|
||||
delete(obj.graphPlot);
|
||||
[obj, f] = obj.plotGraph(f);
|
||||
|
||||
drawnow;
|
||||
end
|
||||
function obj = updateAdjacency(obj)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
end
|
||||
|
||||
% Initialize assuming only self-connections
|
||||
A = logical(eye(size(obj.agents, 1)));
|
||||
|
||||
% Check lower triangle off-diagonal connections
|
||||
for ii = 2:size(A, 1)
|
||||
for jj = 1:(ii - 1)
|
||||
if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) <= min([obj.agents{ii}.comRange, obj.agents{jj}.comRange])
|
||||
% Make sure that obstacles don't obstruct the line
|
||||
% of sight, breaking the connection
|
||||
for kk = 1:size(obj.obstacles, 1)
|
||||
if ~obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos)
|
||||
A(ii, jj) = true;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
obj.adjacency = A | A';
|
||||
end
|
||||
function [obj, f] = plotConnections(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Iterate over lower triangle off-diagonal region of the
|
||||
% adjacency matrix to plot communications links between agents
|
||||
X = []; Y = []; Z = [];
|
||||
for ii = 2:size(obj.adjacency, 1)
|
||||
for jj = 1:(ii - 1)
|
||||
if obj.adjacency(ii, jj)
|
||||
X = [X; obj.agents{ii}.pos(1), obj.agents{jj}.pos(1)];
|
||||
Y = [Y; obj.agents{ii}.pos(2), obj.agents{jj}.pos(2)];
|
||||
Z = [Z; obj.agents{ii}.pos(3), obj.agents{jj}.pos(3)];
|
||||
end
|
||||
end
|
||||
end
|
||||
X = X'; Y = Y'; Z = Z';
|
||||
|
||||
% Plot the connections
|
||||
hold(f.CurrentAxes, "on");
|
||||
o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
|
||||
hold(f.CurrentAxes, "off");
|
||||
|
||||
% Check if this is a tiled layout figure
|
||||
if strcmp(f.Children(1).Type, 'tiledlayout')
|
||||
% Add to other plots
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
|
||||
end
|
||||
|
||||
obj.connectionsPlot = o;
|
||||
end
|
||||
function [obj, f] = plotGraph(obj, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Form graph from adjacency matrix
|
||||
G = graph(obj.adjacency, 'omitselfloops');
|
||||
|
||||
% Plot graph object
|
||||
obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="sensingFunctions" Type="Relative"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="9c9ce3cb-5989-41e8-a20d-358a95c08b20" type="Reference"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="sandbox" Type="Relative"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="ff13e617-a2ad-49b1-a9b5-668ac2cffc4a" type="Reference"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="validators/arguments" Type="Relative"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="b7c7eec5-a318-4c17-adb2-b13a21bf0609" type="Reference"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="mustBeGeometry.m" type="File"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="1" type="DIR_SIGNIFIER"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="domainContainsObstacle.m" type="File"/>
|
||||
@@ -1,6 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="obstacleCoversObjective.m" type="File"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="arguments" type="File"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="mustBeConstraintGeometries.m" type="File"/>
|
||||
@@ -1,6 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="obstacleCrowdsObjective.m" type="File"/>
|
||||
@@ -1,6 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="agentsCrowdObjective.m" type="File"/>
|
||||
@@ -1,6 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info>
|
||||
<Category UUID="FileClassCategory">
|
||||
<Label UUID="design"/>
|
||||
</Category>
|
||||
</Info>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="rectangularPrism.m" type="File"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="rectangularPrismConstraint.m" type="File"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="1" type="DIR_SIGNIFIER"/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
@@ -1,2 +0,0 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="sandbox" type="File"/>
|
||||
@@ -1,44 +0,0 @@
|
||||
function nextPos = basicGradientAscent(objectiveFunction, domain, pos, r)
|
||||
arguments (Input)
|
||||
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
pos (1, 3) double;
|
||||
r (1, 1) double;
|
||||
end
|
||||
arguments (Output)
|
||||
nextPos(1, 3) double;
|
||||
end
|
||||
|
||||
% Evaluate objective at position offsets +/-[r, 0, 0] and +/-[0, r, 0]
|
||||
currentPos = pos(1:2);
|
||||
neighborPos = [currentPos(1) + r, currentPos(2); ... % (+x)
|
||||
currentPos(1), currentPos(2) + r; ... % (+y)
|
||||
currentPos(1) - r, currentPos(2); ... % (-x)
|
||||
currentPos(1), currentPos(2) - r; ... % (-y)
|
||||
];
|
||||
|
||||
% Check for neighbor positions that fall outside of the domain
|
||||
outOfBounds = false(size(neighborPos, 1), 1);
|
||||
for ii = 1:size(neighborPos, 1)
|
||||
if ~domain.contains([neighborPos(ii, :), 0])
|
||||
outOfBounds(ii) = true;
|
||||
end
|
||||
end
|
||||
|
||||
% Replace out of bounds positions with inoffensive in-bounds positions
|
||||
neighborPos(outOfBounds, 1:3) = repmat(pos, sum(outOfBounds), 1);
|
||||
|
||||
% Sense values at selected positions
|
||||
neighborValues = [objectiveFunction(neighborPos(1, 1), neighborPos(1, 2)), ... % (+x)
|
||||
objectiveFunction(neighborPos(2, 1), neighborPos(2, 2)), ... % (+y)
|
||||
objectiveFunction(neighborPos(3, 1), neighborPos(3, 2)), ... % (-x)
|
||||
objectiveFunction(neighborPos(4, 1), neighborPos(4, 2)), ... % (-y)
|
||||
];
|
||||
|
||||
% Prevent out of bounds locations from ever possibly being selected
|
||||
neighborValues(outOfBounds) = 0;
|
||||
|
||||
% Select next position by maximum sensed value
|
||||
nextPos = neighborPos(neighborValues == max(neighborValues), :);
|
||||
nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
|
||||
end
|
||||
@@ -16,7 +16,7 @@ classdef sensingObjective
|
||||
arguments (Input)
|
||||
obj (1,1) {mustBeA(obj, 'sensingObjective')};
|
||||
objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')};
|
||||
footprint (:, 2) double;
|
||||
footprint (2, :) double;
|
||||
groundAlt (1, 1) double = 0;
|
||||
discretizationStep (1, 1) double = 1;
|
||||
end
|
||||
@@ -27,10 +27,10 @@ classdef sensingObjective
|
||||
obj.groundAlt = groundAlt;
|
||||
|
||||
% Extract footprint limits
|
||||
xMin = min(footprint(:, 1));
|
||||
xMax = max(footprint(:, 1));
|
||||
yMin = min(footprint(:, 2));
|
||||
yMax = max(footprint(:, 2));
|
||||
xMin = min(footprint(1, :));
|
||||
xMax = max(footprint(1, :));
|
||||
yMin = min(footprint(2, :));
|
||||
yMax = max(footprint(2, :));
|
||||
|
||||
xGrid = unique([xMin:discretizationStep:xMax, xMax]);
|
||||
yGrid = unique([yMin:discretizationStep:yMax, yMax]);
|
||||
@@ -58,25 +58,12 @@ classdef sensingObjective
|
||||
% Create axes if they don't already exist
|
||||
f = firstPlotSetup(f);
|
||||
|
||||
% Check if this is a tiled layout figure
|
||||
if strcmp(f.Children(1).Type, 'tiledlayout')
|
||||
% Plot gradient on the "floor" of the domain
|
||||
hold(f.Children(1).Children(3), "on");
|
||||
o = surf(f.Children(1).Children(3), obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
|
||||
o.HitTest = 'off';
|
||||
o.PickableParts = 'none';
|
||||
hold(f.Children(1).Children(3), "off");
|
||||
|
||||
% Add to other perspectives
|
||||
copyobj(o, f.Children(1).Children(5));
|
||||
else
|
||||
% Plot gradient on the "floor" of the domain
|
||||
hold(f.CurrentAxes, "on");
|
||||
o = surf(obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
|
||||
o.HitTest = 'off';
|
||||
o.PickableParts = 'none';
|
||||
s = surf(obj.X, obj.Y, repmat(obj.groundAlt, size(obj.X)), obj.values ./ max(obj.values, [], "all"), 'EdgeColor', 'none');
|
||||
s.HitTest = 'off';
|
||||
s.PickableParts = 'none';
|
||||
hold(f.CurrentAxes, "off");
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
450
test_miSim.m
450
test_miSim.m
@@ -1,75 +1,51 @@
|
||||
classdef test_miSim < matlab.unittest.TestCase
|
||||
properties (Access = private)
|
||||
testClass = miSim;
|
||||
|
||||
% Domain
|
||||
domain = rectangularPrism; % domain geometry
|
||||
maxIter = 1000;
|
||||
timestep = 0.05
|
||||
domain = rectangularPrismConstraint;
|
||||
|
||||
% Obstacles
|
||||
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);
|
||||
constraintGeometries = cell(1, 0);
|
||||
|
||||
% 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;
|
||||
|
||||
% Agents
|
||||
minAgents = 3; % Minimum number of agents to be randomly generated
|
||||
maxAgents = 9; % Maximum number of agents to be randomly generated
|
||||
sensingLength = 0.05; % length parameter used by sensing function
|
||||
agents = cell(0, 1);
|
||||
minAgents = 3;
|
||||
maxAgents = 9;
|
||||
agents = cell(1, 0);
|
||||
|
||||
% Collision
|
||||
minCollisionRange = 0.1; % Minimum randomly generated collision geometry size
|
||||
maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
|
||||
minCollisionRange = 0.1;
|
||||
maxCollisionRange = 0.5;
|
||||
collisionRanges = NaN;
|
||||
|
||||
% Communications
|
||||
comRange = 5; % Maximum range between agents that forms a communications link
|
||||
comRange = 5;
|
||||
end
|
||||
|
||||
% Setup for each test
|
||||
methods (TestMethodSetup)
|
||||
% Generate a random domain
|
||||
function tc = setDomain(tc)
|
||||
% 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");
|
||||
% random integer-sized domain within [-10, 10] in all dimensions
|
||||
tc.domain = tc.domain.initialize(ceil([rand * -10, rand * 10; rand * -10, rand * 10; rand * -10, rand * 10]), 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();
|
||||
sig = [3, 1; 1, 4];
|
||||
tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1, 1:2), sig);
|
||||
tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3, 1), tc.objectiveDiscretizationStep);
|
||||
end
|
||||
mu(3) = 0;
|
||||
|
||||
% Set standard deviations of bivariate distribution
|
||||
sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
|
||||
|
||||
% 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
|
||||
% Instantiate agents, they will be initialized under different
|
||||
% parameters in individual test cases
|
||||
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
|
||||
@@ -77,329 +53,149 @@ classdef test_miSim < matlab.unittest.TestCase
|
||||
methods (Test)
|
||||
% Test methods
|
||||
function misim_initialization(tc)
|
||||
% randomly create obstacles
|
||||
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
||||
tc.obstacles = cell(nGeom, 1);
|
||||
% randomly create 2-3 constraint geometries
|
||||
nGeom = 1 + randi(2);
|
||||
tc.constraintGeometries = cell(nGeom, 1);
|
||||
for ii = 1:size(tc.constraintGeometries, 1)
|
||||
% Instantiate a rectangular prism constraint that spans the
|
||||
% domain's height
|
||||
tc.constraintGeometries{ii, 1} = rectangularPrismConstraint;
|
||||
|
||||
% Iterate over obstacles to initialize
|
||||
for ii = 1:size(tc.obstacles, 1)
|
||||
badCandidate = true;
|
||||
while badCandidate
|
||||
% Instantiate a rectangular prism obstacle
|
||||
tc.obstacles{ii} = rectangularPrism;
|
||||
% Randomly come up with constraint geometries until they
|
||||
% fit within the domain
|
||||
candidateMinCorner = -Inf(3, 1);
|
||||
candidateMaxCorner = Inf(3, 1);
|
||||
|
||||
% Randomly generate min corner for the obstacle
|
||||
candidateMinCorner = tc.domain.random();
|
||||
candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
|
||||
% make sure the obstacles don't contain the sensing objective
|
||||
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);
|
||||
|
||||
% 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;
|
||||
% Make sure the obstacle is in the domain
|
||||
while any(candidateMinCorner(1:2, 1) < tc.domain.minCorner(1:2, 1))
|
||||
candidateMinCorner = tc.domain.minCorner(1:3, 1) + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1); -Inf]; % random spots on the ground
|
||||
end
|
||||
end
|
||||
if violation
|
||||
continue;
|
||||
while any(candidateMaxCorner(1:2, 1) > tc.domain.maxCorner(1:2, 1))
|
||||
candidateMaxCorner = [candidateMinCorner(1:2, 1); 0] + [(tc.domain.maxCorner(1:2, 1) - tc.domain.minCorner(1:2, 1)) .* rand(2, 1) ./ 2; Inf]; % halved to keep from being excessively large
|
||||
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.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
|
||||
end
|
||||
|
||||
% 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();
|
||||
end
|
||||
% once a domain-valid obstacle has been found, make
|
||||
% sure it doesn't obstruct the sensing target
|
||||
if all(candidateMinCorner(1:2, 1)' <= tc.objective.groundPos) && all(candidateMaxCorner(1:2, 1)' >= tc.objective.groundPos)
|
||||
% reset to try again
|
||||
candidateMinCorner = -Inf(3, 1);
|
||||
candidateMaxCorner = Inf(3, 1);
|
||||
else
|
||||
candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
|
||||
obstructs = false;
|
||||
end
|
||||
end
|
||||
|
||||
% Make sure that the candidate position is within the
|
||||
% domain
|
||||
if ~tc.domain.contains(candidatePos)
|
||||
% Reduce infinite dimensions to the domain
|
||||
candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
|
||||
candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
|
||||
|
||||
% Initialize constraint geometry
|
||||
tc.constraintGeometries{ii, 1} = tc.constraintGeometries{ii, 1}.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
|
||||
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
|
||||
candidatePos = tc.domain.random();
|
||||
candidateGeometry = rectangularPrismConstraint;
|
||||
tc.agents{ii, 1} = tc.agents{ii, 1}.initialize(candidatePos, zeros(1, 3), eye(3), candidateGeometry.initialize([candidatePos - tc.collisionRanges(ii, 1) * ones(1, 3); candidatePos + tc.collisionRanges(ii, 1) * 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.constraintGeometries, 1)
|
||||
inside = false;
|
||||
if tc.constraintGeometries{jj, 1}.contains(tc.agents{ii, 1}.pos)
|
||||
% Found a violation, stop checking
|
||||
inside = true;
|
||||
break;
|
||||
end
|
||||
end
|
||||
|
||||
% Agent is inside obstacle, try again
|
||||
if inside
|
||||
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
|
||||
% Create a collision geometry for this agent
|
||||
candidateGeometry = rectangularPrismConstraint;
|
||||
candidateGeometry = candidateGeometry.initialize([tc.agents{ii, 1}.pos - 0.1 * ones(1, 3); tc.agents{ii, 1}.pos + 0.1 * ones(1, 3)]', REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
|
||||
|
||||
% 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);
|
||||
% Check previously placed agents for collisions
|
||||
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
|
||||
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)), @(r) 0.5, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
|
||||
|
||||
% Make sure candidate agent doesn't collide with
|
||||
% domain
|
||||
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;
|
||||
% 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;
|
||||
break;
|
||||
end
|
||||
end
|
||||
if violation
|
||||
|
||||
% Agent is colliding with another, try again
|
||||
if ii ~= 1 && colliding
|
||||
continue;
|
||||
end
|
||||
|
||||
% Make sure candidate doesn't collide with obstacles
|
||||
violation = false;
|
||||
for kk = 1:size(tc.obstacles, 1)
|
||||
if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
|
||||
violation = true;
|
||||
break;
|
||||
% Allow to proceed since no obstacle/collision
|
||||
% violations were found
|
||||
posInvalid = false;
|
||||
end
|
||||
end
|
||||
if violation
|
||||
continue;
|
||||
end
|
||||
|
||||
% Make sure candidate doesn't collide with existing
|
||||
% agents
|
||||
violation = false;
|
||||
for kk = 1:(ii - 1)
|
||||
if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
|
||||
violation = true;
|
||||
break;
|
||||
% 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
|
||||
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);
|
||||
end
|
||||
|
||||
% Initialize the simulation
|
||||
[tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
|
||||
end
|
||||
function misim_run(tc)
|
||||
% randomly create obstacles
|
||||
nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
|
||||
tc.obstacles = cell(nGeom, 1);
|
||||
tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.constraintGeometries);
|
||||
|
||||
% Iterate over obstacles to initialize
|
||||
for ii = 1:size(tc.obstacles, 1)
|
||||
badCandidate = true;
|
||||
while badCandidate
|
||||
% Instantiate a rectangular prism obstacle
|
||||
tc.obstacles{ii} = rectangularPrism;
|
||||
% Plot domain
|
||||
f = tc.testClass.domain.plotWireframe;
|
||||
|
||||
% Randomly generate min corner for the obstacle
|
||||
candidateMinCorner = tc.domain.random();
|
||||
candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
|
||||
% Set plotting limits to focus on the domain
|
||||
xlim([tc.testClass.domain.minCorner(1) - 0.5, tc.testClass.domain.maxCorner(1) + 0.5]);
|
||||
ylim([tc.testClass.domain.minCorner(2) - 0.5, tc.testClass.domain.maxCorner(2) + 0.5]);
|
||||
zlim([tc.testClass.domain.minCorner(3) - 0.5, tc.testClass.domain.maxCorner(3) + 0.5]);
|
||||
|
||||
% 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;
|
||||
% Plot constraint geometries
|
||||
for ii = 1:size(tc.testClass.constraintGeometries, 1)
|
||||
tc.testClass.constraintGeometries{ii, 1}.plotWireframe(f);
|
||||
end
|
||||
|
||||
% Make sure that the obstacles are fully contained by
|
||||
% the domain
|
||||
if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
|
||||
continue;
|
||||
end
|
||||
% Plot objective gradient
|
||||
f = tc.testClass.objective.plot(f);
|
||||
|
||||
% Make sure that the obstacles don't cover the sensing
|
||||
% objective
|
||||
if obstacleCoversObjective(tc.objective, tc.obstacles{ii})
|
||||
continue;
|
||||
% 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);
|
||||
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
|
||||
end
|
||||
|
||||
% 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();
|
||||
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
|
||||
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)), @basicGradientAscent, tc.sensingLength, tc.comRange, ii, sprintf("Agent %d", ii));
|
||||
|
||||
% Make sure candidate agent doesn't collide with
|
||||
% domain
|
||||
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
|
||||
end
|
||||
if violation
|
||||
continue;
|
||||
end
|
||||
|
||||
% Make sure candidate doesn't collide with obstacles
|
||||
violation = false;
|
||||
for kk = 1:size(tc.obstacles, 1)
|
||||
if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
|
||||
violation = true;
|
||||
break;
|
||||
end
|
||||
end
|
||||
if violation
|
||||
continue;
|
||||
end
|
||||
|
||||
% Make sure candidate doesn't collide with existing
|
||||
% agents
|
||||
violation = false;
|
||||
for kk = 1:(ii - 1)
|
||||
if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
|
||||
violation = true;
|
||||
break;
|
||||
end
|
||||
end
|
||||
if violation
|
||||
continue;
|
||||
end
|
||||
|
||||
% Candidate agent is valid, store to pass in to sim
|
||||
initInvalid = false;
|
||||
tc.agents{ii} = newAgent;
|
||||
end
|
||||
end
|
||||
|
||||
% Initialize the simulation
|
||||
[tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles);
|
||||
|
||||
% Run simulation loop
|
||||
[tc.testClass, f] = tc.testClass.run(f);
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -1,11 +0,0 @@
|
||||
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
|
||||
@@ -1,10 +0,0 @@
|
||||
function mustBeGeometry(geometry)
|
||||
validGeometries = ["rectangularPrism";];
|
||||
if isa(geometry, 'cell')
|
||||
for ii = 1:size(geometry, 1)
|
||||
assert(isa(geometry{ii}, validGeometries), "Geometry in index %d is not a valid geometry class", ii);
|
||||
end
|
||||
else
|
||||
assert(isa(geometry, validGeometries), "Geometry is not a valid geometry class");
|
||||
end
|
||||
end
|
||||
@@ -1,21 +0,0 @@
|
||||
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
|
||||
@@ -1,17 +0,0 @@
|
||||
function c = geometryIntersects(g1, g2)
|
||||
c = false;
|
||||
% Check if g2 contains g1
|
||||
for jj = 1:size(g1.edges, 1)
|
||||
if g2.containsLine(g1.vertices(g1.edges(jj, 1), 1:3), g1.vertices(g1.edges(jj, 2), 1:3))
|
||||
c = true;
|
||||
return;
|
||||
end
|
||||
end
|
||||
% Check if g1 contains g2
|
||||
for jj = 1:size(g2.edges, 1)
|
||||
if g1.containsLine(g2.vertices(g2.edges(jj, 1), 1:3), g2.vertices(g2.edges(jj, 2), 1:3))
|
||||
c = true;
|
||||
return;
|
||||
end
|
||||
end
|
||||
end
|
||||
10
validators/mustBeConstraintGeometries.m
Normal file
10
validators/mustBeConstraintGeometries.m
Normal file
@@ -0,0 +1,10 @@
|
||||
function mustBeConstraintGeometries(constraintGeometry)
|
||||
validGeometries = ["rectangularPrismConstraint";];
|
||||
if isa(constraintGeometry, 'cell')
|
||||
for ii = 1:size(constraintGeometry, 1)
|
||||
assert(isa(constraintGeometry{ii}, validGeometries), "Constraint geometry in index %d is not a valid constraint geometry class", ii);
|
||||
end
|
||||
else
|
||||
assert(isa(constraintGeometry, validGeometries), "Constraint geometry is not a valid constraint geometry class");
|
||||
end
|
||||
end
|
||||
@@ -1,13 +0,0 @@
|
||||
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
|
||||
@@ -1,11 +0,0 @@
|
||||
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