diff --git a/agent.m b/agent.m index 9034c9b..c922223 100644 --- a/agent.m +++ b/agent.m @@ -114,9 +114,10 @@ classdef agent % Network connections end - function [obj, f] = plot(obj, f) + 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) @@ -128,22 +129,22 @@ classdef agent f = firstPlotSetup(f); % 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); - 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(5))]; + o = [o; copyobj(o(1), f.Children(1).Children(4))]; end obj.scatterPoints = o; % Plot collision geometry - [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(f); + [obj.collisionGeometry, f] = obj.collisionGeometry.plotWireframe(ind, f); end end end \ No newline at end of file diff --git a/firstPlotSetup.m b/firstPlotSetup.m index 7806d77..5094972 100644 --- a/firstPlotSetup.m +++ b/firstPlotSetup.m @@ -1,9 +1,15 @@ 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) - tiledlayout(f, 5, 4, "TileSpacing", "tight", "Padding", "compact"); + tiledlayout(f, 5, 5, "TileSpacing", "tight", "Padding", "compact"); % 3D view - nexttile(1, [4, 4]); + nexttile(1, [4, 5]); axes(f.Children(1).Children(1)); axis(f.Children(1).Children(1), "image"); grid(f.Children(1).Children(1), "on"); @@ -12,7 +18,7 @@ function f = firstPlotSetup(f) title(f.Children(1).Children(1), "3D View"); % Communications graph - nexttile(17, [1, 1]); + nexttile(21, [1, 1]); axes(f.Children(1).Children(1)); axis(f.Children(1).Children(1), "image"); grid(f.Children(1).Children(1), "off"); @@ -28,7 +34,7 @@ function f = firstPlotSetup(f) set(f.Children(1).Children(1), 'YColor', 'none'); % Top-down view - nexttile(18, [1, 1]); + nexttile(22, [1, 1]); axes(f.Children(1).Children(1)); axis(f.Children(1).Children(1), "image"); grid(f.Children(1).Children(1), "on"); @@ -37,7 +43,7 @@ function f = firstPlotSetup(f) title(f.Children(1).Children(1), "Top-down View"); % Side-on view - nexttile(19, [1, 1]); + nexttile(23, [1, 1]); axes(f.Children(1).Children(1)); axis(f.Children(1).Children(1), "image"); grid(f.Children(1).Children(1), "on"); @@ -46,12 +52,27 @@ function f = firstPlotSetup(f) title(f.Children(1).Children(1), "Side-on View"); % Front-on view - nexttile(20, [1, 1]); + 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 \ No newline at end of file diff --git a/geometries/rectangularPrism.m b/geometries/rectangularPrism.m index c2ebcb2..414bd3e 100644 --- a/geometries/rectangularPrism.m +++ b/geometries/rectangularPrism.m @@ -163,9 +163,10 @@ classdef rectangularPrism c = (tmax >= 0) && (tmin <= 1); end - function [obj, f] = plotWireframe(obj, f) + 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) @@ -181,17 +182,20 @@ classdef rectangularPrism 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"); + % 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 - % 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))]; + % 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; diff --git a/miSim.m b/miSim.m index cb99076..ac66900 100644 --- a/miSim.m +++ b/miSim.m @@ -10,11 +10,20 @@ classdef miSim 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) @@ -52,29 +61,35 @@ classdef miSim % 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.domain, f] = obj.domain.plotWireframe(obj.spatialPlotIndices); % Plot obstacles for ii = 1:size(obj.obstacles, 1) - [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(f); + [obj.obstacles{ii}, f] = obj.obstacles{ii}.plotWireframe(obj.spatialPlotIndices, f); end % Plot objective gradient - f = obj.objective.plot(f); + 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(f); + [obj.agents{ii}, f] = obj.agents{ii}.plot(obj.spatialPlotIndices, f); end % Plot communication links - [obj, f] = obj.plotConnections(f); + [obj, f] = obj.plotConnections(obj.spatialPlotIndices, f); % Plot abstract network graph - [obj, f] = obj.plotGraph(f); + [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) @@ -120,6 +135,31 @@ classdef miSim % 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 + + % 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{:}); + + % Get highest performance value at each point + [~, idx] = max(agentPerformances, [], 3); + + % 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{:}); + + % 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) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; @@ -171,15 +211,20 @@ classdef miSim 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, f) + 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) @@ -202,23 +247,57 @@ classdef miSim 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"); + 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 - % 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))]; + % 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] = plotGraph(obj, f) + 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) @@ -230,9 +309,21 @@ classdef miSim G = graph(obj.adjacency, 'omitselfloops'); % Plot graph object - hold(f.Children(1).Children(4), 'on'); - obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2); - hold(f.Children(1).Children(4), 'off'); + 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 diff --git a/sensingModels/fixedCardinalSensor.m b/sensingModels/fixedCardinalSensor.m index d08a3cd..544b979 100644 --- a/sensingModels/fixedCardinalSensor.m +++ b/sensingModels/fixedCardinalSensor.m @@ -56,5 +56,20 @@ classdef fixedCardinalSensor % Prevent out of bounds locations from ever possibly being selected neighborValues(outOfBounds) = 0; end + function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'fixedCardinalSensor')}; + agentPos (1, 3) double; + agentPan (1, 1) double; + agentTilt (1, 1) double; + targetPos (:, 3) double; + end + arguments (Output) + value (:, 1) double; + end + + value = 0.5 * ones(size(targetPos, 1), 1); + + end end end \ No newline at end of file diff --git a/sensingModels/sigmoidSensor.m b/sensingModels/sigmoidSensor.m index ff6a429..a52c1c3 100644 --- a/sensingModels/sigmoidSensor.m +++ b/sensingModels/sigmoidSensor.m @@ -1,16 +1,72 @@ -function accuracy = sigmoid(sensorPos, targetPos) - arguments (Input) - sensorPos (1, 3) double; - targetPos (:, 3) double; +classdef sigmoidSensor + properties (SetAccess = private, GetAccess = public) + % Sensor parameters + alphaDist; + betaDist; + alphaPan; + betaPan; + alphaTilt; + betaTilt; end - arguments (Output) - accuracy (:, 3) double; + + methods (Access = public) + function obj = initialize(obj, alphaDist, betaDist, alphaPan, betaPan, alphaTilt, betaTilt) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'sigmoidSensor')} + alphaDist (1, 1) double; + betaDist (1, 1) double; + alphaPan (1, 1) double; + betaPan (1, 1) double; + alphaTilt (1, 1) double; + betaTilt (1, 1) double; + end + arguments (Output) + obj (1, 1) {mustBeA(obj, 'sigmoidSensor')} + end + + obj.alphaDist = alphaDist; + obj.betaDist = betaDist; + obj.alphaPan = alphaPan; + obj.betaPan = betaPan; + obj.alphaTilt = alphaTilt; + obj.betaTilt = betaTilt; + end + function [neighborValues, neighborPos] = sense(obj, objectiveFunction, domain, pos) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}; + objectiveFunction (1, 1) {mustBeA(objectiveFunction, 'function_handle')}; + domain (1, 1) {mustBeGeometry}; + pos (1, 3) double; + end + arguments (Output) + neighborValues (4, 1) double; + neighborPos (4, 3) double; + end + + + end + function value = sensorPerformance(obj, agentPos, agentPan, agentTilt, targetPos) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'sigmoidSensor')}; + agentPos (1, 3) double; + agentPan (1, 1) double; + agentTilt (1, 1) double; + targetPos (:, 3) double; + end + arguments (Output) + value (:, 1) double; + end + + d = vecnorm(agentPos - targetPos, 2, 2); + panAngle = atan2(targetPos(:, 2) - agentPos(2), targetPos(:, 1) - agentPos(1)) - agentPan; + tiltAngle = atan2(targetPos(:, 3) - agentPos(3), d) - agentTilt; + + % Membership functions + mu_d = 1 - (1 ./ (1 + exp(-obj.betaDist .* (d - obj.alphaDist)))); % distance + mu_p = (1 ./ (1 + exp(-obj.betaPan .* (panAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (panAngle - obj.alphaPan)))); % pan + mu_t = (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle + obj.alphaPan)))) - (1 ./ (1 + exp(-obj.betaPan .* (tiltAngle - obj.alphaPan)))); % tilt + + value = mu_d .* mu_p .* mu_t; + end end - - - -end - -function distanceMembership() - end \ No newline at end of file diff --git a/sensingObjective.m b/sensingObjective.m index b468018..5ca08bd 100644 --- a/sensingObjective.m +++ b/sensingObjective.m @@ -46,9 +46,10 @@ classdef sensingObjective idx = obj.values == max(obj.values, [], "all"); obj.groundPos = [obj.X(idx), obj.Y(idx)]; end - function f = plot(obj, f) + function f = plot(obj, ind, f) arguments (Input) obj (1,1) {mustBeA(obj, 'sensingObjective')}; + ind (1, :) double = NaN; f (1,1) {mustBeA(f, 'matlab.ui.Figure')} = figure; end arguments (Output) @@ -58,24 +59,27 @@ 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 + % Plot gradient on the "floor" of the domain + if isnan(ind) 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 = surf(f.CurrentAxes, 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.CurrentAxes, "off"); + + else + hold(f.Children(1).Children(ind(1)), "on"); + o = surf(f.Children(1).Children(ind(1)), 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(ind(1)), "off"); + end + + % Add to other perspectives + if size(ind, 2) > 1 + for ii = 2:size(ind, 2) + copyobj(o, f.Children(1).Children(ind(ii))); + end end end end diff --git a/test_miSim.m b/test_miSim.m index ac8ed1c..35bf4e4 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -4,7 +4,7 @@ classdef test_miSim < matlab.unittest.TestCase % Domain domain = rectangularPrism; % domain geometry - maxIter = 1000; + maxIter = 250; timestep = 0.05 % Obstacles @@ -358,8 +358,8 @@ classdef test_miSim < matlab.unittest.TestCase candidateGeometry = 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)); % Initialize candidate agent sensor model - sensor = fixedCardinalSensor; - sensor.initialize(tc.sensingLength); + sensor = sigmoidSensor; + sensor = sensor.initialize(1, 1, 1, 1, 1, 1); % Initialize candidate agent newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), 0, 0, candidateGeometry, sensor, @gradientAscent, tc.comRange, ii, sprintf("Agent %d", ii)); @@ -415,5 +415,34 @@ classdef test_miSim < matlab.unittest.TestCase % Run simulation loop [tc.testClass, f] = tc.testClass.run(f); end + function test_basic_partitioning(tc) + % place agents a fixed distance +/- X from the domain's center + d = 1; + c = 0.1; + + % make basic domain + tc.domain = tc.domain.initialize([zeros(1, 3); 10 * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); + + % make basic sensing objective + tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], tc.domain.center(1:2), eye(2)), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep); + + % Initialize agent collision geometry + geometry1 = rectangularPrism; + geometry2 = geometry1; + geometry1 = geometry1.initialize([tc.domain.center + [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center + [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1)); + geometry2 = geometry2.initialize([tc.domain.center - [d, 0, 0] - tc.collisionRanges(1) * ones(1, 3); tc.domain.center - [d, 0, 0] + tc.collisionRanges(1) * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 2)); + + % Initialize agent sensor model + sensor = sigmoidSensor; + sensor = sensor.initialize(1, 1, 1, 1, 1, 1); + + % Initialize agents + tc.agents = {agent; agent}; + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center + [d, 0, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*d, 1, sprintf("Agent %d", 1)); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [d, 0, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*d, 2, sprintf("Agent %d", 2)); + + % Initialize the simulation + [tc.testClass, f] = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter); + end end end \ No newline at end of file