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

fixed bug allowing obstructed coms connections

Browse Source
This commit is contained in:
Kevin D
2025-10-27 20:45:37 -07:00
parent 5c6eeed6fd
commit db0ce2d42d
3 changed files with 243 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

201
test_miSim.m
View File

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