From bbefb6111b4ab024eb5d5d137a999439dd108566 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Mon, 27 Oct 2025 22:38:39 -0700 Subject: [PATCH] geometries move in plots as sim runs --- agent.m | 64 +++++++++++++++++++++---- firstPlotSetup.m | 10 ++-- geometries/rectangularPrism.m | 21 +++++---- miSim.m | 65 ++++++++++++++++++++++++-- sensingFunctions/basicGradientAscent.m | 5 +- test_miSim.m | 63 ++----------------------- 6 files changed, 139 insertions(+), 89 deletions(-) diff --git a/agent.m b/agent.m index 3787afe..0b1ef5d 100644 --- a/agent.m +++ b/agent.m @@ -9,15 +9,19 @@ classdef agent sensingLength = 0.05; % length parameter used by sensing function % 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 + cBfromC = NaN(3); % current DCM body from sim cartesian (assume fixed for now) % Collision collisionGeometry; % Communication comRange = NaN; + + % Plotting + scatterPoints; end methods (Access = public) @@ -48,29 +52,66 @@ classdef agent obj.index = index; obj.label = label; end - function obj = run(obj, objectiveFunction) + 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, obj.pos, obj.sensingLength); + 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 f = plot(obj, f) + 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) 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 @@ -85,10 +126,15 @@ classdef agent % Check if this is a tiled layout figure if strcmp(f.Children(1).Type, 'tiledlayout') % Add to other perspectives - copyobj(o, f.Children(1).Children(2)); - copyobj(o, f.Children(1).Children(3)); - copyobj(o, f.Children(1).Children(5)); + 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 \ No newline at end of file diff --git a/firstPlotSetup.m b/firstPlotSetup.m index 3b29be0..422780e 100644 --- a/firstPlotSetup.m +++ b/firstPlotSetup.m @@ -9,7 +9,7 @@ function f = firstPlotSetup(f) 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("Top-down Perspective"); + title(f.Children(1).Children(1), "Top-down Perspective"); % Communications graph nexttile(3, [1, 1]); @@ -17,33 +17,33 @@ function f = firstPlotSetup(f) axis(f.Children(1).Children(1), "image"); grid(f.Children(1).Children(1), "off"); view(f.Children(1).Children(1), 0, 0); - title("Network Graph"); + title(f.Children(1).Children(1), "Network Graph"); % 3D view - title("3D Perspective"); 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 - title("Side-on Perspective"); 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 - title("Front-on Perspective"); 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"); end end \ No newline at end of file diff --git a/geometries/rectangularPrism.m b/geometries/rectangularPrism.m index 8af3d31..c2ebcb2 100644 --- a/geometries/rectangularPrism.m +++ b/geometries/rectangularPrism.m @@ -1,23 +1,25 @@ 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 - footprint = NaN(4, 2); + % Plotting + lines; end methods (Access = public) @@ -161,12 +163,13 @@ classdef rectangularPrism c = (tmax >= 0) && (tmin <= 1); end - function f = plotWireframe(obj, f) + 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 @@ -186,10 +189,12 @@ classdef rectangularPrism % Check if this is a tiled layout figure if strcmp(f.Children(1).Type, 'tiledlayout') % Add to other perspectives - copyobj(o, f.Children(1).Children(2)); - copyobj(o, f.Children(1).Children(3)); - copyobj(o, f.Children(1).Children(5)); + 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 \ No newline at end of file diff --git a/miSim.m b/miSim.m index 37cdcea..e28e8e5 100644 --- a/miSim.m +++ b/miSim.m @@ -13,7 +13,7 @@ classdef miSim end methods (Access = public) - function obj = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles) + function [obj, f] = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; domain (1, 1) {mustBeGeometry}; @@ -25,6 +25,7 @@ classdef miSim end arguments (Output) obj (1, 1) {mustBeA(obj, 'miSim')}; + f (1, 1) {mustBeA(f, 'matlab.ui.Figure')}; end % Define simulation time parameters @@ -46,33 +47,87 @@ classdef miSim % 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 + f = obj.plotNetwork(f); + + % Plot abstract network graph + f = obj.plotGraph(f); end - function obj = run(obj) + 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)'; for ii = 1:size(times, 1) - % Get current sim time + % 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.agents{jj} = obj.agents{jj}.run(obj.objective.objectiveFunction, obj.domain); end % Update adjacency matrix obj = obj.updateAdjacency; % Update plots - + f = obj.updatePlots(f); end + + end + function f = updatePlots(obj, f) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'miSim')}; + f (1, 1) {mustBeA(f, 'matlab.ui.Figure')} = figure; + end + arguments (Output) + f (1, 1) {mustBeA(f, 'matlab.ui.Figure')}; + end + + % Update agent positions, collision geometries, connections + for ii = 1:size(obj.agents, 1) + obj.agents{ii}.updatePlots(); + end + + % Update network graph plot + + drawnow; + end function obj = updateAdjacency(obj) arguments (Input) diff --git a/sensingFunctions/basicGradientAscent.m b/sensingFunctions/basicGradientAscent.m index 9889f28..003f674 100644 --- a/sensingFunctions/basicGradientAscent.m +++ b/sensingFunctions/basicGradientAscent.m @@ -1,6 +1,7 @@ -function nextPos = basicGradientAscent(objectiveFunction, pos, r) +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 @@ -39,5 +40,5 @@ function nextPos = basicGradientAscent(objectiveFunction, pos, r) % Select next position by maximum sensed value nextPos = neighborPos(neighborValues == max(neighborValues), :); - nextPos = nextPos(1, 1:3); % just in case two get selected, simply pick one + nextPos = [nextPos(1, 1:2), pos(3)]; % just in case two get selected, simply pick one end \ No newline at end of file diff --git a/test_miSim.m b/test_miSim.m index 3211893..b8a3a44 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -235,35 +235,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles); - - % Plot domain - f = tc.testClass.domain.plotWireframe; - - % Set plotting limits to focus on the domain - xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]); - ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]); - zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]); - - % Plot obstacles - for ii = 1:size(tc.testClass.obstacles, 1) - tc.testClass.obstacles{ii}.plotWireframe(f); - end - - % Plot objective gradient - f = tc.testClass.objective.plot(f); - - % Plot agents and their collision geometries - for ii = 1:size(tc.testClass.agents, 1) - f = tc.testClass.agents{ii}.plot(f); - f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f); - end - - % Plot communication links - f = tc.testClass.plotNetwork(f); - - % Plot abstract network graph - f = tc.testClass.plotGraph(f); + [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 @@ -424,39 +396,10 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles); - - % Plot domain - f = tc.testClass.domain.plotWireframe; - - % Set plotting limits to focus on the domain - xlim([tc.testClass.domain.minCorner(1), tc.testClass.domain.maxCorner(1)]); - ylim([tc.testClass.domain.minCorner(2), tc.testClass.domain.maxCorner(2)]); - zlim([tc.testClass.domain.minCorner(3), tc.testClass.domain.maxCorner(3)]); - - % Plot obstacles - for ii = 1:size(tc.testClass.obstacles, 1) - tc.testClass.obstacles{ii}.plotWireframe(f); - end - - % Plot objective gradient - f = tc.testClass.objective.plot(f); - - % Plot agents and their collision geometries - for ii = 1:size(tc.testClass.agents, 1) - f = tc.testClass.agents{ii}.plot(f); - f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f); - end - - % Plot communication links - f = tc.testClass.plotNetwork(f); - - % Plot abstract network graph - f = tc.testClass.plotGraph(f); + [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles); % Run simulation loop - tc.testClass.run(); - + [tc.testClass, f] = tc.testClass.run(f); end end end \ No newline at end of file