diff --git a/@miSim/validate.m b/@miSim/validate.m index 11dbcf6..03a4fff 100644 --- a/@miSim/validate.m +++ b/@miSim/validate.m @@ -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 diff --git a/test/parametricTestSuite.m b/test/parametricTestSuite.m index 94c4dc4..eaa498a 100644 --- a/test/parametricTestSuite.m +++ b/test/parametricTestSuite.m @@ -178,9 +178,13 @@ classdef parametricTestSuite < matlab.unittest.TestCase % Check if the obstacle collides with an agent if ~retry - AO_collisions = cellfun(@(a) cellfun(@(o) o.contains(a.pos), obstacles), agents, "UniformOutput", false); - if any(vertcat(AO_collisions{:})) - retry = true; + for kk = 1:size(agents, 1) + 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; + break; + end end end