diff --git a/agent.m b/agent.m index 58d254f..bf3967e 100644 --- a/agent.m +++ b/agent.m @@ -21,6 +21,9 @@ classdef agent % Collision collisionGeometry; + % FOV cone + fovGeometry; + % Communication comRange = NaN; @@ -57,6 +60,10 @@ classdef agent 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 obj = run(obj, sensingObjective, domain, partitioning) arguments (Input) @@ -113,7 +120,13 @@ classdef agent end end - % Network connections + % 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) @@ -146,6 +159,9 @@ classdef agent % 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 \ No newline at end of file diff --git a/geometries/REGION_TYPE.m b/geometries/REGION_TYPE.m index aa81a07..d323ae2 100644 --- a/geometries/REGION_TYPE.m +++ b/geometries/REGION_TYPE.m @@ -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) diff --git a/geometries/cone.m b/geometries/cone.m index af1a9b1..52bba95 100644 --- a/geometries/cone.m +++ b/geometries/cone.m @@ -1,22 +1,82 @@ classdef cone - %CONE Summary of this class goes here - % Detailed explanation goes here + % Conical geometry + properties (SetAccess = private, GetAccess = public) + % Meta + tag = REGION_TYPE.INVALID; + label = ""; - properties - Property1 + % Spatial + center = NaN; + radius = NaN; + height = NaN; + + % Plotting + surface; + n = 32; end methods - function obj = cone(inputArg1,inputArg2) - %CONE Construct an instance of this class - % Detailed explanation goes here - obj.Property1 = inputArg1 + inputArg2; - end + 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 - function outputArg = method1(obj,inputArg) - %METHOD1 Summary of this method goes here - % Detailed explanation goes here - outputArg = obj.Property1 + inputArg; + 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 \ No newline at end of file diff --git a/miSim.m b/miSim.m index b17604f..a887a75 100644 --- a/miSim.m +++ b/miSim.m @@ -205,6 +205,13 @@ classdef miSim [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) diff --git a/sensingModels/sigmoidSensor.m b/sensingModels/sigmoidSensor.m index 175b9b6..5f2ee0c 100644 --- a/sensingModels/sigmoidSensor.m +++ b/sensingModels/sigmoidSensor.m @@ -7,6 +7,8 @@ classdef sigmoidSensor betaPan = NaN; alphaTilt = NaN; betaTilt = NaN; + + r = NaN; end methods (Access = public) @@ -30,6 +32,8 @@ classdef sigmoidSensor obj.betaPan = betaPan; obj.alphaTilt = alphaTilt; obj.betaTilt = betaTilt; + + obj.r = obj.alphaDist; end function [values, positions] = sense(obj, agent, sensingObjective, domain, partitioning) arguments (Input)