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
%% Obstacle Validators
AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obj.obstacles), obj.agents, "UniformOutput", false);
AO_collisions = vertcat(AO_collisions{:});
if any(AO_collisions)
[idx, idy] = find(AO_collisions);
for ii = 1:size(idx, 1)
error("Agent(s) %d colliding with obstacle(s) %d", idx(ii), idy(ii));
% Agent-Obstacle Collision Detection
for jj = 1:size(obj.obstacles, 1)
for kk = 1:size(obj.agents, 1)
P = min(max(obj.agents{kk}.pos, obj.obstacles{jj}.minCorner), obj.obstacles{jj}.maxCorner);
d = obj.agents{kk}.pos - P;
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