From 50eaad95041e0ded85fc12b242d2f15b240ae12f Mon Sep 17 00:00:00 2001 From: krdee1 Date: Wed, 24 Dec 2025 16:00:42 -0800 Subject: [PATCH] fixed comms LOS obstruction by obstacles --- @miSim/initialize.m | 6 +- @miSim/updateAdjacency.m | 41 +++++++---- geometries/@rectangularPrism/containsLine.m | 81 +++++++++++++++------ test/test_miSim.m | 50 ++++++++++++- 4 files changed, 135 insertions(+), 43 deletions(-) diff --git a/@miSim/initialize.m b/@miSim/initialize.m index 7b6807f..c4366ea 100644 --- a/@miSim/initialize.m +++ b/@miSim/initialize.m @@ -32,8 +32,10 @@ function obj = initialize(obj, domain, objective, agents, minAlt, timestep, part % Add an additional obstacle spanning the domain's footprint to % represent the minimum allowable altitude obj.minAlt = minAlt; - obj.obstacles{end + 1, 1} = rectangularPrism; - obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); + if obj.minAlt > 0 + obj.obstacles{end + 1, 1} = rectangularPrism; + obj.obstacles{end, 1} = obj.obstacles{end, 1}.initialize([obj.domain.minCorner; obj.domain.maxCorner(1:2), obj.minAlt], "OBSTACLE", "Minimum Altitude Domain Constraint"); + end % Define objective obj.objective = objective; diff --git a/@miSim/updateAdjacency.m b/@miSim/updateAdjacency.m index 09be91b..b254f05 100644 --- a/@miSim/updateAdjacency.m +++ b/@miSim/updateAdjacency.m @@ -7,26 +7,41 @@ function obj = updateAdjacency(obj) end % Initialize assuming only self-connections - A = logical(eye(size(obj.agents, 1))); + A = true(size(obj.agents, 1)); % Check lower triangle off-diagonal connections 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]) - % 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 - % need extra handling for cases with no obstacles - if isempty(obj.obstacles) - A(ii, jj) = true; + % Check that agents are not out of range + if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) > min([obj.agents{ii}.comRange, obj.agents{jj}.comRange]); + A(ii, jj) = false; % comm range violation + continue; + end + + % Check that agents do not have their line of sight obstructed + for kk = 1:size(obj.obstacles, 1) + if obj.obstacles{kk}.containsLine(obj.agents{jj}.pos, obj.agents{ii}.pos) + A(ii, jj) = false; end end + + + + % if norm(obj.agents{ii}.pos - obj.agents{jj}.pos) <= min([obj.agents{ii}.comRange, obj.agents{jj}.comRange]) + % % Make sure that obstacles don't obstruct the line + % % of sight, breaking the connection + % for kk = 1:size(obj.obstacles, 1) + % if A(ii, jj) && obj.obstacles{kk}.containsLine(obj.agents{ii}.pos, obj.agents{jj}.pos) + % A(ii, jj) = false; + % 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'; + obj.adjacency = A & A'; end \ No newline at end of file diff --git a/geometries/@rectangularPrism/containsLine.m b/geometries/@rectangularPrism/containsLine.m index 2e2194f..d5be933 100644 --- a/geometries/@rectangularPrism/containsLine.m +++ b/geometries/@rectangularPrism/containsLine.m @@ -9,33 +9,66 @@ function c = containsLine(obj, pos1, pos2) end d = pos2 - pos1; - - % edge case where the line is parallel to the geometry - if abs(d) < 1e-12 - % check if it happens to start or end inside or outside of - % the geometry - if obj.contains(pos1) || obj.contains(pos2) - c = true; - else - c = false; - end + + % endpoint contained (trivial case) + if obj.contains(pos1) || obj.contains(pos2) + c = true; return; end - - tmin = -inf; - tmax = inf; - - % Standard case + + % parameterize the line segment to check for an intersection + tMin = 0; + tMax = 1; for ii = 1:3 - t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii); - t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii); - tmin = max(tmin, min(t1, t2)); - tmax = min(tmax, max(t1, t2)); - if tmin > tmax - c = false; - return; + % line is parallel to geometry + if abs(d(ii)) < 1e-12 + if pos1(ii) < obj.minCorner(ii) || pos1(ii) > obj.maxCorner(ii) + c = false; + return; + end + else + t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii); + t2 = (obj.maxCorner(ii) - pos1(ii)) / d(ii); + + tLow = min(t1, t2); + tHigh = max(t1, t2); + + tMin = max(tMin, tLow); + tMax = min(tMax, tHigh); + + if tMin > tMax + c = false; + return; + end end end + c = true; - c = (tmax >= 0) && (tmin <= 1); - end \ No newline at end of file + % if abs(d) < 1e-12 + % % check if it happens to start or end inside or outside of + % % the geometry + % if obj.contains(pos1) || obj.contains(pos2) + % c = true; + % else + % c = false; + % end + % return; + % end + % + % tMin = -inf; + % tMax = inf; + % + % % Standard case + % for ii = 1:3 + % t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii); + % t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii); + % tMin = max(tMin, min(t1, t2)); + % tMax = min(tMax, max(t1, t2)); + % if tMin > tMax + % c = false; + % return; + % end + % end + % + % c = (tMax >= 0) && (tMin <= 1); +end \ No newline at end of file diff --git a/test/test_miSim.m b/test/test_miSim.m index a0d5b5b..a3d3f77 100644 --- a/test/test_miSim.m +++ b/test/test_miSim.m @@ -490,7 +490,7 @@ classdef test_miSim < matlab.unittest.TestCase end function test_obstacle_avoidance(tc) % Fixed single obstacle - % Fixed single agent initial conditions + % Fixed two agents initial conditions % Exaggerated large collision geometries % make basic domain l = 10; % domain size @@ -514,8 +514,8 @@ classdef test_miSim < matlab.unittest.TestCase % Initialize agents tc.agents = {agent; agent;}; - tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 3*radius, 1, sprintf("Agent %d", 1), false); - tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0] - [0, 1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 3*radius, 2, sprintf("Agent %d", 2), false); + tc.agents{1} = tc.agents{1}.initialize(tc.domain.center - d + [0, radius * 1.5, 0], zeros(1,3), 0, 0, geometry1, sensor, @gradientAscent, 5*radius, 1, sprintf("Agent %d", 1), false); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - d - [0, radius * 1.5, 0] - [0, 1, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, 5*radius, 2, sprintf("Agent %d", 2), false); % Initialize obstacles obstacleLength = 1; @@ -523,11 +523,53 @@ classdef test_miSim < matlab.unittest.TestCase tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, tc.minAlt; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1"); % Initialize the simulation - tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, tc.makeVideo); + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, tc.minAlt, tc.timestep, tc.partitoningFreq, 100, tc.obstacles, tc.makeVideo); % Run the simulation tc.testClass.run(); end + + function test_obstacle_blocks_comms_LOS(tc) + % Fixed single obstacle + % Fixed two agents initial conditions + % Exaggerated large communications radius + % make basic domain + l = 10; % domain size + tc.domain = tc.domain.initialize([zeros(1, 3); l * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain"); + + % make basic sensing objective + tc.domain.objective = tc.domain.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], [8, 5]), tc.domain, tc.discretizationStep, tc.protectedRange); + + % Initialize agent collision geometry + radius = .25; + d = 2; + geometry1 = spherical; + geometry2 = geometry1; + geometry1 = geometry1.initialize(tc.domain.center - [d, 0, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1)); + geometry2 = geometry2.initialize(tc.domain.center - [0, d, 0], radius, REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", 1)); + + % Initialize agent sensor model + sensor = sigmoidSensor; + alphaDist = l/2; % half of domain length/width + sensor = sensor.initialize(alphaDist, 3, NaN, NaN, 15, 3); + + % Initialize agents + commsRadius = 5; + 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, commsRadius, 1, sprintf("Agent %d", 1), false); + tc.agents{2} = tc.agents{2}.initialize(tc.domain.center - [0, d, 0], zeros(1,3), 0, 0, geometry2, sensor, @gradientAscent, commsRadius, 2, sprintf("Agent %d", 2), false); + + % Initialize obstacles + obstacleLength = 1.5; + tc.obstacles{1} = rectangularPrism; + tc.obstacles{1} = tc.obstacles{1}.initialize([tc.domain.center(1:2) - obstacleLength, 0; tc.domain.center(1:2) + obstacleLength, tc.domain.maxCorner(3)], REGION_TYPE.OBSTACLE, "Obstacle 1"); + + % Initialize the simulation + tc.testClass = tc.testClass.initialize(tc.domain, tc.domain.objective, tc.agents, 0, tc.timestep, tc.partitoningFreq, 125, tc.obstacles, tc.makeVideo); + + % No communications link should be established + tc.assertEqual(tc.testClass.adjacency, logical(eye(2))); + end end methods