implemented partitioning

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");
+            hold(f.Children(1).Children(end), "on");
-            o = scatter3(obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
+            o = scatter3(f.Children(1).Children(end), obj.pos(1), obj.pos(2), obj.pos(3), 'filled', 'ko', 'SizeData', 25);
-            hold(f.CurrentAxes, "off");
+            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

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

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
+            % Plot the boundaries of the geometry into 3D view
-            hold(f.CurrentAxes, "on");
+            if isnan(ind)
-            o = plot3(X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
+                o = plot3(f.CurrentAxes, X, Y, Z, '-', 'Color', obj.tag.color, 'LineWidth', 2);
-            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', obj.tag.color, 'LineWidth', 2);
+                hold(f.Children(1).Children(ind(1)), "off");
+            end
 
-            % Check if this is a tiled layout figure
+            % Copy to other requested tiles
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            if numel(ind) > 1
-                % Add to other perspectives
+                for ii = 2:size(ind, 2)
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
+                    o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
+                end
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
             end
 
             obj.lines = o;

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");
+            if isnan(ind)
-            o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--');
+                hold(f.CurrentAxes, "on");
-            hold(f.CurrentAxes, "off");
+                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
+            % Copy to other plots
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            if size(ind, 2) > 1
-                % Add to other plots
+                for ii = 2:size(ind, 2)
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(2))];
+                    o = [o, copyobj(o(:, 1), f.Children(1).Children(ind(ii)))];
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(3))];
+                end 
-                o = [o, copyobj(o(:, 1), f.Children(1).Children(5))];
             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');
+            if isnan(ind)
-            obj.graphPlot = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2);
+                hold(f.CurrentAxes, 'on');
-            hold(f.Children(1).Children(4), 'off');
+                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
 

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

sensingModels/sigmoidSensor.m

@@ -1,16 +1,72 @@
-function accuracy = sigmoid(sensorPos, targetPos)
+classdef sigmoidSensor
-    arguments (Input)
+    properties (SetAccess = private, GetAccess = public)
-        sensorPos (1, 3) double;
+        % Sensor parameters
-        targetPos (:, 3) double;
+        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

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
+            % Plot gradient on the "floor" of the domain
-            if strcmp(f.Children(1).Type, 'tiledlayout')
+            if isnan(ind)
-                % 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 = 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

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 = sigmoidSensor;
-                    sensor.initialize(tc.sensingLength);
+                    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