diff --git a/agent.m b/agent.m index eb8a4d4..b75af2b 100644 --- a/agent.m +++ b/agent.m @@ -20,13 +20,14 @@ classdef agent end methods (Access = public) - function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, comRange, index, label) + function obj = initialize(obj, pos, vel, cBfromC, collisionGeometry, sensingFunction, 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) {mustBeGeometry}; + sensingFunction (1, 1) {mustBeA(sensingFunction, 'function_handle')} comRange (1, 1) double = NaN; index (1, 1) double = NaN; label (1, 1) string = ""; diff --git a/miSim.m b/miSim.m index c154bcf..e60cdff 100644 --- a/miSim.m +++ b/miSim.m @@ -3,6 +3,8 @@ classdef miSim % Simulation parameters properties (SetAccess = private, GetAccess = public) + timestep = NaN; % delta time interval for simulation iterations + maxIter = NaN; % maximum number of simulation iterations domain = rectangularPrism; objective = sensingObjective; obstacles = cell(0, 1); % geometries that define obstacles within the domain @@ -11,33 +13,58 @@ classdef miSim end methods (Access = public) - function obj = initialize(obj, domain, objective, agents, obstacles) + function obj = initialize(obj, domain, objective, agents, timestep, maxIter, obstacles) arguments (Input) obj (1, 1) {mustBeA(obj, 'miSim')}; domain (1, 1) {mustBeGeometry}; objective (1, 1) {mustBeA(objective, 'sensingObjective')}; agents (:, 1) cell {mustBeAgents}; + timestep (:, 1) double = 0.05; + maxIter (:, 1) double = 1000; obstacles (:, 1) cell {mustBeGeometry} = cell(0, 1); end arguments (Output) obj (1, 1) {mustBeA(obj, 'miSim')}; end - %% Define domain + % Define simulation time parameters + obj.timestep = timestep; + obj.maxIter = maxIter; + + % Define domain obj.domain = domain; - %% Add geometries representing obstacles within the domain + % Add geometries representing obstacles within the domain obj.obstacles = obstacles; - %% Define objective + % Define objective obj.objective = objective; - %% Define agents + % Define agents obj.agents = agents; - %% Compute adjacency matrix + % Compute adjacency matrix obj = obj.updateAdjacency(); + end + function obj = run(obj) + arguments (Input) + obj (1, 1) {mustBeA(obj, 'miSim')}; + end + arguments (Output) + obj (1, 1) {mustBeA(obj, 'miSim')}; + end + keyboard + % Iterate over agents to simulate their motion + for ii = 1:size(obj.agents, 1) + obj.agents{ii} + end + + % Update adjacency matrix + obj = obj.updateAdjacency; + + % Update plots + end function obj = updateAdjacency(obj) arguments (Input) @@ -54,7 +81,13 @@ classdef miSim 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]) - A(ii, jj) = true; + % 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 end end end @@ -86,7 +119,7 @@ classdef miSim % Plot the connections hold(f.CurrentAxes, "on"); - o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 1, 'LineStyle', '--'); + o = plot3(X, Y, Z, 'Color', 'g', 'LineWidth', 2, 'LineStyle', '--'); hold(f.CurrentAxes, "off"); % Check if this is a tiled layout figure @@ -111,11 +144,10 @@ classdef miSim % Check if this is a tiled layout figure if strcmp(f.Children(1).Type, 'tiledlayout') - o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k'); + o = plot(f.Children(1).Children(4), G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2); else - o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k'); + o = plot(f.CurrentAxes, G, 'LineStyle', '--', 'EdgeColor', 'g', 'NodeColor', 'k', 'LineWidth', 2); end - end end diff --git a/test_miSim.m b/test_miSim.m index 71842d9..f949ac8 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -4,6 +4,8 @@ classdef test_miSim < matlab.unittest.TestCase % Domain domain = rectangularPrism; % domain geometry + maxIter = 1000; + timestep = 0.05 % Obstacles minNumObstacles = 1; % Minimum number of obstacles to be randomly generated @@ -74,7 +76,7 @@ classdef test_miSim < matlab.unittest.TestCase methods (Test) % Test methods function misim_initialization(tc) - % randomly create 2-3 obstacles + % randomly create obstacles nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles); tc.obstacles = cell(nGeom, 1); @@ -184,7 +186,7 @@ classdef test_miSim < matlab.unittest.TestCase % Initialize candidate agent candidateGeometry = rectangularPrism; - newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),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)), tc.comRange, ii, sprintf("Agent %d", ii)); + newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),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)), @(r) 0.5, tc.comRange, ii, sprintf("Agent %d", ii)); % Make sure candidate agent doesn't collide with % domain @@ -232,7 +234,7 @@ classdef test_miSim < matlab.unittest.TestCase end % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.obstacles); + tc.testClass = tc.testClass.initialize(tc.domain, tc.objective, tc.agents, tc.timestep, tc.maxIter, tc.obstacles); % Plot domain f = tc.testClass.domain.plotWireframe; @@ -262,5 +264,198 @@ classdef test_miSim < matlab.unittest.TestCase % Plot abstract network graph f = tc.testClass.plotGraph(f); end + function misim_run(tc) + % randomly create obstacles + nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles); + tc.obstacles = cell(nGeom, 1); + + % Iterate over obstacles to initialize + for ii = 1:size(tc.obstacles, 1) + badCandidate = true; + while badCandidate + % Instantiate a rectangular prism obstacle + tc.obstacles{ii} = rectangularPrism; + + % Randomly generate min corner for the obstacle + candidateMinCorner = tc.domain.random(); + candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain + + % Randomly select a corresponding maximum corner that + % satisfies min/max obstacle size specifications + candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize); + + % Initialize obstacle + tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii)); + + % Check if the obstacle intersects with any existing + % obstacles + violation = false; + for kk = 1:(ii - 1) + if geometryIntersects(tc.obstacles{kk}, tc.obstacles{ii}) + violation = true; + break; + end + end + if violation + continue; + end + + % Make sure that the obstacles are fully contained by + % the domain + if ~domainContainsObstacle(tc.domain, tc.obstacles{ii}) + continue; + end + + % Make sure that the obstacles don't cover the sensing + % objective + if obstacleCoversObjective(tc.objective, tc.obstacles{ii}) + continue; + end + + % Make sure that the obstacles aren't too close to the + % sensing objective + if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange) + continue; + end + + badCandidate = false; + end + end + + % Add agents individually, ensuring that each addition does not + % invalidate the initialization setup + for ii = 1:size(tc.agents, 1) + initInvalid = true; + while initInvalid + candidatePos = [tc.objective.groundPos, 0]; + % Generate a random position for the agent based on + % existing agent positions + if ii == 1 + while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2) + candidatePos = tc.domain.random(); + end + else + candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2)); + end + + % Make sure that the candidate position is within the + % domain + if ~tc.domain.contains(candidatePos) + continue; + end + + % Make sure that the candidate position does not crowd + % the sensing objective and create boring scenarios + if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2) + continue; + end + + % Make sure that there exist unobstructed lines of sight at + % appropriate ranges to form a connected communications + % graph between the agents + connections = false(1, ii - 1); + for jj = 1:(ii - 1) + if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange + % Check new agent position against all existing + % agent positions for communications range + connections(jj) = true; + for kk = 1:size(tc.obstacles, 1) + if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos) + connections(jj) = false; + end + end + end + end + + % New agent must be connected to an existing agent to + % be valid + if ii ~= 1 && ~any(connections) + continue; + end + + % Initialize candidate agent + candidateGeometry = rectangularPrism; + newAgent = tc.agents{ii}.initialize(candidatePos, zeros(1,3), eye(3),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)), @(r) 0.5, tc.comRange, ii, sprintf("Agent %d", ii)); + + % Make sure candidate agent doesn't collide with + % domain + violation = false; + for jj = 1:size(newAgent.collisionGeometry.vertices, 1) + % Check if collision geometry exits domain + if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) + violation = true; + break; + end + end + if violation + continue; + end + + % Make sure candidate doesn't collide with obstacles + violation = false; + for kk = 1:size(tc.obstacles, 1) + if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry) + violation = true; + break; + end + end + if violation + continue; + end + + % Make sure candidate doesn't collide with existing + % agents + violation = false; + for kk = 1:(ii - 1) + if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry) + violation = true; + break; + end + end + if violation + continue; + end + + % Candidate agent is valid, store to pass in to sim + initInvalid = false; + tc.agents{ii} = newAgent; + end + 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); + + % Run simulation loop + tc.testClass.run(); + + end end end \ No newline at end of file