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

fixed collision detection in initialization

Browse Source
This commit is contained in:
Kevin D
2025-10-26 19:45:32 -07:00
parent 2f0647caf3
commit d0a060f404
3 changed files with 60 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
geometries/rectangularPrism.m
View File

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

51
test_miSim.m
View File

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

17
validators/geometryIntersects.m Normal file
View File

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