improvement for obstacle-agent collision detection

2026-01-28 00:15:59 -08:00
parent bf875cbfce
commit fea1b1686d
2 changed files with 15 additions and 9 deletions
--- 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