kdee/miSim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fixed comms LOS obstruction by obstacles

Browse Source
This commit is contained in:
Kevin Dee
2025-12-24 16:00:42 -08:00
parent 14e372ae55
commit 50eaad9504
4 changed files with 135 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
@miSim/initialize.m
View File

@@ -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;

41
@miSim/updateAdjacency.m
View File

@@ -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

81
geometries/@rectangularPrism/containsLine.m
View File

@@ -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
% 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

50
test/test_miSim.m
View File

@@ -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