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

improvement for obstacle-agent collision detection

Browse Source
This commit is contained in:
Kevin Dee
2026-01-28 00:15:59 -08:00
parent bf875cbfce
commit fea1b1686d
2 changed files with 15 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
@miSim/validate.m
View File

@@ -15,12 +15,14 @@ function validate(obj)
end end
%% Obstacle Validators %% Obstacle Validators
AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, "UniformOutput", false); % Agent-Obstacle Collision Detection
AO_collisions = vertcat(AO_collisions{:}); for jj = 1:size(obj.obstacles, 1)
if any(AO_collisions) for kk = 1:size(obj.agents, 1)
[idx, idy] = find(AO_collisions); P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
for ii = 1:size(idx, 1) d = obj.agents{kk}.pos - P;
error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii)); if dot(d, d) <= obj.agents{kk}.collisionGeometry.radius^2
error("%s colliding with %s", obj.agents{kk}.label, obj.obstacles{jj}.label); % this will cause quadprog to fail
end
end end
end end

8
test/parametricTestSuite.m
View File

@@ -178,9 +178,13 @@ classdef parametricTestSuite < matlab.unittest.TestCase
% Check if the obstacle collides with an agent % Check if the obstacle collides with an agent
if ~retry if ~retry
AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obstacles), agents, "UniformOutput", false); for kk = 1:size(agents, 1)
if any(vertcat(AO_collisions{:})) P = min(max(agents{kk}.pos, obstacles{jj}.minCorner), obstacles{jj}.maxCorner);
d = agents{kk}.pos - P;
if dot(d, d) <= agents{kk}.collisionGeometry.radius^2
retry = true; retry = true;
break;
end
end end
end end