From d0a060f404af43f104af25485e885a7db2b51bb2 Mon Sep 17 00:00:00 2001 From: Kevin D Date: Sun, 26 Oct 2025 19:45:32 -0700 Subject: [PATCH] fixed collision detection in initialization --- geometries/rectangularPrism.m | 14 ++++----- test_miSim.m | 51 +++++++++++++++++++++++---------- validators/geometryIntersects.m | 17 +++++++++++ 3 files changed, 60 insertions(+), 22 deletions(-) create mode 100644 validators/geometryIntersects.m diff --git a/geometries/rectangularPrism.m b/geometries/rectangularPrism.m index a1b6001..eba190b 100644 --- a/geometries/rectangularPrism.m +++ b/geometries/rectangularPrism.m @@ -13,6 +13,10 @@ classdef rectangularPrism vertices = NaN(8, 3); + edges = [1 2; 2 3; 3 4; 4 1; % bottom square + 5 6; 6 8; 8 7; 7 5; % top square + 1 5; 2 6; 3 8; 4 7]; % vertical edges + footprint = NaN(4, 2); end @@ -169,14 +173,10 @@ classdef rectangularPrism % Create axes if they don't already exist f = firstPlotSetup(f); - edges = [1 2; 2 3; 3 4; 4 1; % bottom square - 5 6; 6 8; 8 7; 7 5; % top square - 1 5; 2 6; 3 8; 4 7]; % vertical edges - % Create plotting inputs from vertices and edges - X = [obj.vertices(edges(:,1),1), obj.vertices(edges(:,2),1)]'; - Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]'; - Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]'; + X = [obj.vertices(obj.edges(:,1),1), obj.vertices(obj.edges(:,2),1)]'; + 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 hold(f.CurrentAxes, "on"); diff --git a/test_miSim.m b/test_miSim.m index 42ced91..3c8c6d8 100644 --- a/test_miSim.m +++ b/test_miSim.m @@ -96,6 +96,19 @@ classdef test_miSim < matlab.unittest.TestCase % 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}) @@ -174,7 +187,7 @@ classdef test_miSim < matlab.unittest.TestCase 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)), ii, sprintf("Agent %d", ii)); % Make sure candidate agent doesn't collide with - % domain, obstacles, or any existing agents + % domain violation = false; for jj = 1:size(newAgent.collisionGeometry.vertices, 1) % Check if collision geometry exits domain @@ -182,22 +195,30 @@ classdef test_miSim < matlab.unittest.TestCase violation = true; break; end + end + if violation + continue; + end - % Check if collision geometry enters obstacle - for kk = 1:size(tc.obstacles, 1) - if tc.obstacles{kk}.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) - violation = true; - break; - 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 - - % Check if collision geometry enters other - % collision geometry - for kk = 1:(ii - 1) - if tc.agents{kk}.collisionGeometry.contains(newAgent.collisionGeometry.vertices(jj, 1:3)) - 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 diff --git a/validators/geometryIntersects.m b/validators/geometryIntersects.m new file mode 100644 index 0000000..278f4c5 --- /dev/null +++ b/validators/geometryIntersects.m @@ -0,0 +1,17 @@ +function c = geometryIntersects(g1, g2) + c = false; + % Check if g2 contains g1 + for jj = 1:size(g1.edges, 1) + if g2.containsLine(g1.vertices(g1.edges(jj, 1), 1:3), g1.vertices(g1.edges(jj, 2), 1:3)) + c = true; + return; + end + end + % Check if g1 contains g2 + for jj = 1:size(g2.edges, 1) + if g1.containsLine(g2.vertices(g2.edges(jj, 1), 1:3), g2.vertices(g2.edges(jj, 2), 1:3)) + c = true; + return; + end + end +end \ No newline at end of file