Compare commits
40 Commits
5debb2b5f4
...
sensor-mod
| Author | SHA1 | Date | |
|---|---|---|---|
| a2eb95381d | |||
| 3d35179579 | |||
| 9e948072e8 | |||
| 74088a13f3 | |||
| 8b14bfc5ce | |||
| c7510812cb | |||
| b63bbadfb4 | |||
| f50beeab5b | |||
| d36d2cab3e | |||
| 2b58646aca | |||
| 0621e0a07a | |||
| 7fb9d13781 | |||
| faa8bad596 | |||
| 8955d4d29b | |||
| f953cd3882 | |||
| b2787e1e53 | |||
| 8af5e87272 | |||
| d0a060f404 | |||
| 2f0647caf3 | |||
| 5a9ac0c2d5 | |||
| c61d627f0d | |||
| da3f732250 | |||
| 7fff074a5c | |||
| 66a5dfe1e6 | |||
| c5a7634d37 | |||
| bbefb6111b | |||
| ade795b3ae | |||
| db0ce2d42d | |||
| 5c6eeed6fd | |||
| f8a36eec4b | |||
| 22fab26485 | |||
| c64fc3eae5 | |||
| fdbd90afdf | |||
| b82c87520a | |||
| 78538ab586 | |||
| c5a2b644d8 | |||
| 26264ec1b9 | |||
| 437257691c | |||
| 371656fb39 | |||
| b0cd420aa3 |
33
.gitattributes
vendored
Normal file
33
.gitattributes
vendored
Normal file
@@ -0,0 +1,33 @@
|
||||
* text=auto
|
||||
|
||||
*.fig binary
|
||||
*.mat binary
|
||||
*.mdl binary diff merge=mlAutoMerge
|
||||
*.mdlp binary
|
||||
*.mex* binary
|
||||
*.mlapp binary
|
||||
*.mldatx binary merge=mlAutoMerge
|
||||
*.mlproj binary
|
||||
*.mlx binary
|
||||
*.p binary
|
||||
*.plprj binary
|
||||
*.sbproj binary
|
||||
*.sfx binary
|
||||
*.sldd binary
|
||||
*.slreqx binary merge=mlAutoMerge
|
||||
*.slmx binary merge=mlAutoMerge
|
||||
*.sltx binary
|
||||
*.slxc binary
|
||||
*.slx binary merge=mlAutoMerge
|
||||
*.slxp binary
|
||||
|
||||
## MATLAB Project metadata files use LF line endings
|
||||
/resources/project/**/*.xml text eol=lf
|
||||
|
||||
## Other common binary file types
|
||||
*.docx binary
|
||||
*.exe binary
|
||||
*.jpg binary
|
||||
*.pdf binary
|
||||
*.png binary
|
||||
*.xlsx binary
|
||||
43
.gitignore
vendored
Normal file
43
.gitignore
vendored
Normal file
@@ -0,0 +1,43 @@
|
||||
# Autosave files
|
||||
*.asv
|
||||
*.m~
|
||||
*.autosave
|
||||
*.slx.r*
|
||||
*.mdl.r*
|
||||
|
||||
# Derived content-obscured files
|
||||
*.p
|
||||
|
||||
# Compiled MEX files
|
||||
*.mex*
|
||||
|
||||
# Packaged app and toolbox files
|
||||
*.mlappinstall
|
||||
*.mltbx
|
||||
|
||||
# Deployable archives
|
||||
*.ctf
|
||||
|
||||
# Generated helpsearch folders
|
||||
helpsearch*/
|
||||
|
||||
# Code generation folders
|
||||
slprj/
|
||||
sccprj/
|
||||
codegen/
|
||||
|
||||
# Cache files
|
||||
*.slxc
|
||||
|
||||
# Cloud based storage dotfile
|
||||
.MATLABDriveTag
|
||||
|
||||
# buildtool cache folder
|
||||
.buildtool/
|
||||
|
||||
# SimBiology backup files
|
||||
*.sbproj.backup
|
||||
*.sbproj.bak
|
||||
|
||||
# Sandbox contents
|
||||
sandbox/*
|
||||
@@ -1 +1,4 @@
|
||||
# miSim
|
||||
*Multi-agent Interconnectivity Simulation*
|
||||
|
||||
<img width="992" height="665" alt="image" src="https://github.com/user-attachments/assets/133a283b-63fe-46a8-b1cf-81f6ab1cccfc" />
|
||||
|
||||
132
agent.m
132
agent.m
@@ -5,25 +5,44 @@ classdef agent
|
||||
label = "";
|
||||
|
||||
% Sensor
|
||||
sensingFunction = @(r) 0.5; % probability of detection as a function of range
|
||||
sensorModel;
|
||||
sensingLength = 0.05; % length parameter used by sensing function
|
||||
|
||||
% Guidance
|
||||
guidanceModel;
|
||||
|
||||
% State
|
||||
pos = NaN(1, 3);
|
||||
vel = NaN(1, 3);
|
||||
cBfromC = NaN(3); % DCM body from sim cartesian (assume fixed for now)
|
||||
lastPos = NaN(1, 3); % position from previous timestep
|
||||
pos = NaN(1, 3); % current position
|
||||
vel = NaN(1, 3); % current velocity
|
||||
pan = NaN; % pan angle
|
||||
tilt = NaN; % tilt angle
|
||||
|
||||
% Collision
|
||||
collisionGeometry;
|
||||
|
||||
% FOV cone
|
||||
fovGeometry;
|
||||
|
||||
% Communication
|
||||
comRange = NaN;
|
||||
|
||||
% Plotting
|
||||
scatterPoints;
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, index, label)
|
||||
function obj = initialize(obj, pos, vel, pan, tilt, collisionGeometry, sensorModel, guidanceModel, comRange, 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) {mustBeConstraintGeometries};
|
||||
pan (1, 1) double;
|
||||
tilt (1, 1) double;
|
||||
collisionGeometry (1, 1) {mustBeGeometry};
|
||||
sensorModel (1, 1) {mustBeSensor}
|
||||
guidanceModel (1, 1) {mustBeA(guidanceModel, 'function_handle')};
|
||||
comRange (1, 1) double = NaN;
|
||||
index (1, 1) double = NaN;
|
||||
label (1, 1) string = "";
|
||||
end
|
||||
@@ -33,17 +52,90 @@ classdef agent
|
||||
|
||||
obj.pos = pos;
|
||||
obj.vel = vel;
|
||||
obj.cBfromC = cBfromC;
|
||||
obj.pan = pan;
|
||||
obj.tilt = tilt;
|
||||
obj.collisionGeometry = collisionGeometry;
|
||||
obj.sensorModel = sensorModel;
|
||||
obj.guidanceModel = guidanceModel;
|
||||
obj.comRange = comRange;
|
||||
obj.index = index;
|
||||
obj.label = label;
|
||||
|
||||
% Initialize FOV cone
|
||||
obj.fovGeometry = cone;
|
||||
obj.fovGeometry = obj.fovGeometry.initialize([obj.pos(1:2), 0], obj.sensorModel.r, obj.pos(3), REGION_TYPE.FOV, sprintf("%s FOV", obj.label));
|
||||
end
|
||||
function f = plot(obj, f)
|
||||
function obj = run(obj, sensingObjective, domain, partitioning)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
sensingObjective (1, 1) {mustBeA(sensingObjective, 'sensingObjective')};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
partitioning (:, :) double;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
end
|
||||
|
||||
% Do sensing
|
||||
[sensedValues, sensedPositions] = obj.sensorModel.sense(obj, sensingObjective, domain, partitioning);
|
||||
|
||||
% Determine next planned position
|
||||
nextPos = obj.guidanceModel(sensedValues, sensedPositions, obj.pos);
|
||||
|
||||
% 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
|
||||
|
||||
% Update FOV geometry surfaces
|
||||
for jj = 1:size(obj.fovGeometry.surface, 2)
|
||||
% Update each plot
|
||||
obj.fovGeometry.surface(jj).XData = obj.fovGeometry.surface(jj).XData + deltaPos(1);
|
||||
obj.fovGeometry.surface(jj).YData = obj.fovGeometry.surface(jj).YData + deltaPos(2);
|
||||
obj.fovGeometry.surface(jj).ZData = obj.fovGeometry.surface(jj).ZData + deltaPos(3);
|
||||
end
|
||||
end
|
||||
function [obj, f] = plot(obj, ind, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'agent')};
|
||||
ind (1, :) double = NaN;
|
||||
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
|
||||
|
||||
@@ -51,9 +143,25 @@ classdef agent
|
||||
f = firstPlotSetup(f);
|
||||
|
||||
% Plot points representing the agent position
|
||||
hold(f.CurrentAxes, "on");
|
||||
scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 50);
|
||||
hold(f.CurrentAxes, "off");
|
||||
hold(f.Children(1).Children(end), "on");
|
||||
o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
|
||||
hold(f.Children(1).Children(end), "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(4))];
|
||||
end
|
||||
|
||||
obj.scatterPoints = o;
|
||||
|
||||
% Plot collision geometry
|
||||
[obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f);
|
||||
|
||||
% Plot FOV geometry
|
||||
[obj.fovGeometry, f] = obj.fovGeometry.plot(ind, f);
|
||||
end
|
||||
end
|
||||
end
|
||||
@@ -1,8 +1,78 @@
|
||||
function f = firstPlotSetup(f)
|
||||
arguments (Input)
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
if isempty(f.CurrentAxes)
|
||||
axes(f);
|
||||
axis(f.CurrentAxes, "equal");
|
||||
grid(f.CurrentAxes, "on");
|
||||
view(f.CurrentAxes, 3);
|
||||
tiledlayout(f, 5, 5, "TileSpacing", "tight", "Padding", "compact");
|
||||
|
||||
% 3D view
|
||||
nexttile(1, [4, 5]);
|
||||
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 View");
|
||||
|
||||
% Communications graph
|
||||
nexttile(21, [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, 90);
|
||||
title(f.Children(1).Children(1), "Network Graph");
|
||||
set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
|
||||
set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
|
||||
set(f.Children(1).Children(1), 'XTickLabel', {});
|
||||
set(f.Children(1).Children(1), 'YTickLabel', {});
|
||||
set(f.Children(1).Children(1), 'XTick', []);
|
||||
set(f.Children(1).Children(1), 'YTick', []);
|
||||
set(f.Children(1).Children(1), 'XColor', 'none');
|
||||
set(f.Children(1).Children(1), 'YColor', 'none');
|
||||
|
||||
% Top-down view
|
||||
nexttile(22, [1, 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), 0, 90);
|
||||
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
|
||||
title(f.Children(1).Children(1), "Top-down View");
|
||||
|
||||
% Side-on view
|
||||
nexttile(23, [1, 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 View");
|
||||
|
||||
% Front-on view
|
||||
nexttile(24, [1, 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), 0, 0);
|
||||
xlabel(f.Children(1).Children(1), "X"); zlabel(f.Children(1).Children(1), "Z");
|
||||
title(f.Children(1).Children(1), "Front-on View");
|
||||
|
||||
% Partitioning
|
||||
nexttile(25, [1, 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), 0, 90);
|
||||
xlabel(f.Children(1).Children(1), "X"); ylabel(f.Children(1).Children(1), "Y");
|
||||
title(f.Children(1).Children(1), "Domain Partitioning");
|
||||
set(f.Children(1).Children(1), 'XTickLabelMode', 'manual');
|
||||
set(f.Children(1).Children(1), 'YTickLabelMode', 'manual');
|
||||
set(f.Children(1).Children(1), 'XTickLabel', {});
|
||||
set(f.Children(1).Children(1), 'YTickLabel', {});
|
||||
set(f.Children(1).Children(1), 'XTick', []);
|
||||
set(f.Children(1).Children(1), 'YTick', []);
|
||||
end
|
||||
end
|
||||
@@ -8,6 +8,7 @@ classdef REGION_TYPE
|
||||
DOMAIN (1, [0, 0, 0]); % domain region
|
||||
OBSTACLE (2, [255, 127, 127]); % obstacle region
|
||||
COLLISION (3, [255, 255, 128]); % collision avoidance region
|
||||
FOV (4, [255, 165, 0]); % field of view region
|
||||
end
|
||||
methods
|
||||
function obj = REGION_TYPE(id, color)
|
||||
82
geometries/cone.m
Normal file
82
geometries/cone.m
Normal file
@@ -0,0 +1,82 @@
|
||||
classdef cone
|
||||
% Conical geometry
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
% Meta
|
||||
tag = REGION_TYPE.INVALID;
|
||||
label = "";
|
||||
|
||||
% Spatial
|
||||
center = NaN;
|
||||
radius = NaN;
|
||||
height = NaN;
|
||||
|
||||
% Plotting
|
||||
surface;
|
||||
n = 32;
|
||||
end
|
||||
|
||||
methods
|
||||
function obj = initialize(obj, center, radius, height, tag, label)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
||||
center (1, 3) double;
|
||||
radius (1, 1) double;
|
||||
height (1, 1) double;
|
||||
tag (1, 1) REGION_TYPE = REGION_TYPE.INVALID;
|
||||
label (1, 1) string = "";
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
||||
end
|
||||
|
||||
obj.center = center;
|
||||
obj.radius = radius;
|
||||
obj.height = height;
|
||||
obj.tag = tag;
|
||||
obj.label = label;
|
||||
end
|
||||
function [obj, f] = plot(obj, ind, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
||||
ind (1, :) double = NaN;
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'cone')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')};
|
||||
end
|
||||
|
||||
% Create axes if they don't already exist
|
||||
f = firstPlotSetup(f);
|
||||
|
||||
% Plot cone
|
||||
[X, Y, Z] = cylinder([obj.radius, 0], obj.n);
|
||||
|
||||
% Scale to match height
|
||||
Z = Z * obj.height;
|
||||
|
||||
% Move to center location
|
||||
X = X + obj.center(1);
|
||||
Y = Y + obj.center(2);
|
||||
Z = Z + obj.center(3);
|
||||
|
||||
% Plot
|
||||
if isnan(ind)
|
||||
o = surf(f.CurrentAxes, X, Y, Z);
|
||||
else
|
||||
hold(f.Children(1).Children(ind(1)), "on");
|
||||
o = surf(f.Children(1).Children(ind(1)), X, Y, Z, ones([size(Z), 1]) .* reshape(obj.tag.color, 1, 1, 3), 'FaceAlpha', 0.25, 'EdgeColor', 'none');
|
||||
hold(f.Children(1).Children(ind(1)), "off");
|
||||
end
|
||||
|
||||
% Copy to other requested tiles
|
||||
if numel(ind) > 1
|
||||
for ii = 2:size(ind, 2)
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
|
||||
end
|
||||
end
|
||||
|
||||
obj.surface = o;
|
||||
end
|
||||
end
|
||||
end
|
||||
204
geometries/rectangularPrism.m
Normal file
204
geometries/rectangularPrism.m
Normal file
@@ -0,0 +1,204 @@
|
||||
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, ind, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
|
||||
ind (1, :) double = NaN;
|
||||
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 into 3D view
|
||||
if isnan(ind)
|
||||
o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
||||
else
|
||||
hold(f.Children(1).Children(ind(1)), "on");
|
||||
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
|
||||
hold(f.Children(1).Children(ind(1)), "off");
|
||||
end
|
||||
|
||||
% Copy to other requested tiles
|
||||
if numel(ind) > 1
|
||||
for ii = 2:size(ind, 2)
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
|
||||
end
|
||||
end
|
||||
|
||||
obj.lines = o;
|
||||
end
|
||||
end
|
||||
end
|
||||
26
guidanceModels/gradientAscent.m
Normal file
26
guidanceModels/gradientAscent.m
Normal file
@@ -0,0 +1,26 @@
|
||||
function nextPos = gradientAscent(sensedValues, sensedPositions, pos, rate)
|
||||
arguments (Input)
|
||||
sensedValues (:, 1) double;
|
||||
sensedPositions (:, 3) double;
|
||||
pos (1, 3) double;
|
||||
rate (1, 1) double = 0.1;
|
||||
end
|
||||
arguments (Output)
|
||||
nextPos(1, 3) double;
|
||||
end
|
||||
|
||||
% As a default, maintain current position
|
||||
if size(sensedValues, 1) == 0 && size(sensedPositions, 1) == 0
|
||||
nextPos = pos;
|
||||
return;
|
||||
end
|
||||
|
||||
% Select next position by maximum sensed value
|
||||
nextPos = sensedPositions(sensedValues == max(sensedValues), :);
|
||||
nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one
|
||||
|
||||
% rate-limit motion
|
||||
v = nextPos - pos;
|
||||
nextPos = pos + (v / norm(v, 2)) * rate;
|
||||
|
||||
end
|
||||
343
miSim.m
343
miSim.m
@@ -3,37 +3,352 @@ classdef miSim
|
||||
|
||||
% Simulation parameters
|
||||
properties (SetAccess = private, GetAccess = public)
|
||||
domain = rectangularPrismConstraint;
|
||||
timestep = NaN; % delta time interval for simulation iterations
|
||||
partitioningFreq = NaN; % number of simulation timesteps at which the partitioning routine is re-run
|
||||
maxIter = NaN; % maximum number of simulation iterations
|
||||
domain = rectangularPrism;
|
||||
objective = sensingObjective;
|
||||
constraintGeometries = cell(0, 1); % geometries that define constraints within the domain
|
||||
obstacles = cell(0, 1); % geometries that define obstacles within the domain
|
||||
agents = cell(0, 1); % agents that move within the domain
|
||||
adjacency = NaN; % Adjacency matrix representing communications network graph
|
||||
partitioning = NaN;
|
||||
end
|
||||
|
||||
properties (Access = private)
|
||||
% Plot objects
|
||||
connectionsPlot; % objects for lines connecting agents in spatial plots
|
||||
graphPlot; % objects for abstract network graph plot
|
||||
partitionPlot; % objects for partition plot
|
||||
|
||||
% Indicies for various plot types in the main tiled layout figure
|
||||
spatialPlotIndices = [6, 4, 3, 2];
|
||||
objectivePlotIndices = [6, 4];
|
||||
networkGraphIndex = 5;
|
||||
partitionGraphIndex = 1;
|
||||
end
|
||||
|
||||
methods (Access = public)
|
||||
function obj = initialize(obj, domain, objective, agents, constraintGeometries)
|
||||
function [obj, f] = initialize(obj, domain, objective, agents, timestep, partitoningFreq, maxIter, obstacles)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
domain (1, 1) {mustBeConstraintGeometries};
|
||||
domain (1, 1) {mustBeGeometry};
|
||||
objective (1, 1) {mustBeA(objective, 'sensingObjective')};
|
||||
agents (:, 1) cell {mustBeAgents};
|
||||
constraintGeometries (:, 1) cell {mustBeConstraintGeometries} = cell(0, 1);
|
||||
agents (:, 1) cell;
|
||||
timestep (:, 1) double = 0.05;
|
||||
partitoningFreq (:, 1) double = 0.25
|
||||
maxIter (:, 1) double = 1000;
|
||||
obstacles (:, 1) cell {mustBeGeometry} = 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
|
||||
obj.domain = domain;
|
||||
obj.partitioningFreq = partitoningFreq;
|
||||
|
||||
% Add geometries representing obstacles within the domain
|
||||
obj.obstacles = obstacles;
|
||||
|
||||
% Define objective
|
||||
obj.objective = objective;
|
||||
|
||||
% Define agents
|
||||
obj.agents = agents;
|
||||
|
||||
% Compute adjacency matrix
|
||||
obj = obj.updateAdjacency();
|
||||
|
||||
% Create initial partitioning
|
||||
obj = obj.partition();
|
||||
|
||||
% Set up initial plot
|
||||
% Set up axes arrangement
|
||||
% Plot domain
|
||||
[obj.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices);
|
||||
|
||||
% Plot obstacles
|
||||
for ii = 1:size(obj.obstacles, 1)
|
||||
[obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f);
|
||||
end
|
||||
|
||||
% Plot objective gradient
|
||||
f = obj.objective.plot(obj.objectivePlotIndices, f);
|
||||
|
||||
% Plot agents and their collision geometries
|
||||
for ii = 1:size(obj.agents, 1)
|
||||
[obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f);
|
||||
end
|
||||
|
||||
% Plot communication links
|
||||
[obj, f] = obj.plotConnections(obj.spatialPlotIndices, f);
|
||||
|
||||
% Plot abstract network graph
|
||||
[obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
|
||||
|
||||
% Plot domain partitioning
|
||||
[obj, f] = obj.plotPartitions(obj.partitionGraphIndex, 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)';
|
||||
partitioningTimes = times(obj.partitioningFreq:obj.partitioningFreq:size(times, 1));
|
||||
|
||||
% Start video writer
|
||||
v = setupVideoWriter(obj.timestep);
|
||||
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)
|
||||
|
||||
% Check if it's time for new partitions
|
||||
updatePartitions = false;
|
||||
if ismember(t, partitioningTimes)
|
||||
updatePartitions = true;
|
||||
obj = obj.partition();
|
||||
end
|
||||
|
||||
% Iterate over agents to simulate their motion
|
||||
for jj = 1:size(obj.agents, 1)
|
||||
obj.agents{jj} = obj.agents{jj}.run(obj.objective, obj.domain, obj.partitioning);
|
||||
end
|
||||
|
||||
% Update adjacency matrix
|
||||
obj = obj.updateAdjacency;
|
||||
|
||||
% Update plots
|
||||
[obj, f] = obj.updatePlots(f, updatePartitions);
|
||||
|
||||
% Write frame in to video
|
||||
I = getframe(f);
|
||||
v.writeVideo(I);
|
||||
end
|
||||
|
||||
% Close video file
|
||||
v.close();
|
||||
end
|
||||
function obj = partition(obj)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
end
|
||||
arguments (Output)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
end
|
||||
|
||||
%% Define domain
|
||||
obj.domain = domain;
|
||||
% Assess sensing performance of each agent at each sample point
|
||||
% in the domain
|
||||
agentPerformances = cellfun(@(x) reshape(x.sensorModel.sensorPerformance(x.pos, x.pan, x.tilt, [obj.objective.X(:), obj.objective.Y(:), zeros(size(obj.objective.X(:)))]), size(obj.objective.X)), obj.agents, 'UniformOutput', false);
|
||||
agentPerformances = cat(3, agentPerformances{:});
|
||||
|
||||
%% Add constraint geometries against the domain
|
||||
obj.constraintGeometries = constraintGeometries;
|
||||
% Get highest performance value at each point
|
||||
[~, idx] = max(agentPerformances, [], 3);
|
||||
|
||||
%% Define objective
|
||||
obj.objective = objective;
|
||||
% Collect agent indices in the same way
|
||||
agentInds = cellfun(@(x) x.index * ones(size(obj.objective.X)), obj.agents, 'UniformOutput', false);
|
||||
agentInds = cat(3, agentInds{:});
|
||||
|
||||
%% Define agents
|
||||
obj.agents = agents;
|
||||
% Get highest performing agent's index
|
||||
[m,n,~] = size(agentInds);
|
||||
[i,j] = ndgrid(1:m, 1:n);
|
||||
obj.partitioning = agentInds(sub2ind(size(agentInds), i, j, idx));
|
||||
end
|
||||
function [obj, f] = updatePlots(obj, f, updatePartitions)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure;
|
||||
updatePartitions (1, 1) logical = false;
|
||||
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(obj.spatialPlotIndices, f);
|
||||
|
||||
% Update network graph plot
|
||||
delete(obj.graphPlot);
|
||||
[obj, f] = obj.plotGraph(obj.networkGraphIndex, f);
|
||||
|
||||
% Update partitioning plot
|
||||
if updatePartitions
|
||||
delete(obj.partitionPlot);
|
||||
[obj, f] = obj.plotPartitions(obj.partitionGraphIndex, f);
|
||||
end
|
||||
|
||||
% reset plot limits to fit domain
|
||||
for ii = 1:size(obj.spatialPlotIndices, 2)
|
||||
xlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(1), obj.domain.maxCorner(1)]);
|
||||
ylim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(2), obj.domain.maxCorner(2)]);
|
||||
zlim(f.Children(1).Children(obj.spatialPlotIndices(ii)), [obj.domain.minCorner(3), obj.domain.maxCorner(3)]);
|
||||
end
|
||||
|
||||
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
|
||||
% need extra handling for cases with no obstacles
|
||||
if isempty(obj.obstacles)
|
||||
A(ii, jj) = true;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
obj.adjacency = A | A';
|
||||
end
|
||||
function [obj, f] = plotConnections(obj, ind, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
ind (1, :) double = NaN;
|
||||
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
|
||||
if isnan(ind)
|
||||
hold(f.CurrentAxes, "on");
|
||||
o = plot3(f.CurrentAxes, X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
|
||||
hold(f.CurrentAxes, "off");
|
||||
else
|
||||
hold(f.Children(1).Children(ind(1)), "on");
|
||||
o = plot3(f.Children(1).Children(ind(1)), X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
|
||||
hold(f.Children(1).Children(ind(1)), "off");
|
||||
end
|
||||
|
||||
% Copy to other plots
|
||||
if size(ind, 2) > 1
|
||||
for ii = 2:size(ind, 2)
|
||||
o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
|
||||
end
|
||||
end
|
||||
|
||||
obj.connectionsPlot = o;
|
||||
end
|
||||
function [obj, f] = plotPartitions(obj, ind, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
ind (1, :) double = NaN;
|
||||
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
|
||||
|
||||
if isnan(ind)
|
||||
hold(f.CurrentAxes, 'on');
|
||||
o = imagesc(f.CurrentAxes, obj.partitioning);
|
||||
hold(f.CurrentAxes, 'off');
|
||||
else
|
||||
hold(f.Children(1).Children(ind(1)), 'on');
|
||||
o = imagesc(f.Children(1).Children(ind(1)), obj.partitioning);
|
||||
hold(f.Children(1).Children(ind(1)), 'on');
|
||||
if size(ind, 2) > 1
|
||||
for ii = 2:size(ind, 2)
|
||||
o = [o, copyobj(o(1), f.Children(1).Children(ind(ii)))];
|
||||
end
|
||||
end
|
||||
end
|
||||
obj.partitionPlot = o;
|
||||
|
||||
end
|
||||
function [obj, f] = plotGraph(obj, ind, f)
|
||||
arguments (Input)
|
||||
obj (1, 1) {mustBeA(obj, 'miSim')};
|
||||
ind (1, :) double = NaN;
|
||||
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
|
||||
if isnan(ind)
|
||||
hold(f.CurrentAxes, 'on');
|
||||
o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
|
||||
hold(f.CurrentAxes, 'off');
|
||||
else
|
||||
hold(f.Children(1).Children(ind(1)), 'on');
|
||||
o = plot(f.Children(1).Children(ind(1)), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
|
||||
hold(f.Children(1).Children(ind(1)), 'off');
|
||||
if size(ind, 2) > 1
|
||||
for ii = 2:size(ind, 2)
|
||||
o = [o; copyobj(o(1), f.Children(1).Children(ind(ii)))];
|
||||
end
|
||||
end
|
||||
end
|
||||
obj.graphPlot = o;
|
||||
end
|
||||
end
|
||||
|
||||
|
||||
2
miSim.prj
Normal file
2
miSim.prj
Normal file
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<MATLABProject xmlns="http://www.mathworks.com/MATLABProjectFile"/>
|
||||
@@ -1,106 +0,0 @@
|
||||
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
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="sensingModels" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="420d04e4-3880-4a45-8609-11cb30d87302" type="Reference"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="geometries" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="5e8de0f6-dbb2-499b-a851-32a867fd50c2" type="Reference"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="7d58d2d3-2500-426a-b99d-912a79467335" type="Reference"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="guidanceModels" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="1d8d2b42-2863-4985-9cf2-980917971eba" type="Reference"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="sandbox" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="ff13e617-a2ad-49b1-a9b5-668ac2cffc4a" type="Reference"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Ref="validators" Type="Relative"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="5f96e524-3aac-4fa1-95df-67fd6ce02ff3" type="Reference"/>
|
||||
@@ -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,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="mustBeGeometry.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="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="mustBeAgents.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="mustBeDcm.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="mustBeSensor.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="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"/>
|
||||
@@ -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="rectangularPrism.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="REGION_TYPE.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="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="cone.m" type="File"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="Test" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="test" type="Label"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="Other" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="other" type="Label"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="Convenience" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="convenience" type="Label"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="None" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="none" type="Label"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="Derived" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="derived" type="Label"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="Design" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="design" type="Label"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info Name="Artifact" ReadOnly="READ_ONLY"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="artifact" type="Label"/>
|
||||
2
resources/project/Project.xml
Normal file
2
resources/project/Project.xml
Normal file
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info MetadataType="fixedPathV2"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info DataType="None" Name="Classification" ReadOnly="1" SingleValued="1"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info location="FileClassCategory" type="Category"/>
|
||||
@@ -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="test_miSim.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="setupVideoWriter.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="firstPlotSetup.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="validators" 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="geometries" 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="startup.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="README.md" 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=".gitignore" 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="sandbox" 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=".gitattributes" 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="agent.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="sensingObjective.m" type="File"/>
|
||||
@@ -0,0 +1,2 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<Info/>
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user