fixed collision detection in initialization

geometries/rectangularPrism.m

@@ -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)]';
+            X = [obj.vertices(obj.edges(:,1),1), obj.vertices(obj.edges(:,2),1)]';
-            Y = [obj.vertices(edges(:,1),2), obj.vertices(edges(:,2),2)]';
+            Y = [obj.vertices(obj.edges(:,1),2), obj.vertices(obj.edges(:,2),2)]';
-            Z = [obj.vertices(edges(:,1),3), obj.vertices(edges(:,2),3)]';
+            Z = [obj.vertices(obj.edges(:,1),3), obj.vertices(obj.edges(:,2),3)]';
 
             % Plot the boundaries of the geometry
             hold(f.CurrentAxes, "on");

test_miSim.m

@@ -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
+                    % Make sure candidate doesn't collide with obstacles
-                        for kk = 1:size(tc.obstacles, 1)
+                    violation = false;
-                            if tc.obstacles{kk}.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                    for kk = 1:size(tc.obstacles, 1)
-                                violation = true;
+                        if geometryIntersects(tc.obstacles{kk}, newAgent.collisionGeometry)
-                                break;
+                            violation = true;
-                            end
+                            break;
                         end
-                        
+                    end
-                        % Check if collision geometry enters other
+                    if violation
-                        % collision geometry
+                        continue;
-                        for kk = 1:(ii - 1)
+                    end
-                            if tc.agents{kk}.collisionGeometry.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+
-                                violation = true;
+                    % Make sure candidate doesn't collide with existing
-                                break;
+                    % agents
-                            end
+                    violation = false;
+                    for kk = 1:(ii - 1)
+                        if geometryIntersects(tc.agents{kk}.collisionGeometry, newAgent.collisionGeometry)
+                            violation = true;
+                            break;
                         end
                     end
                     if violation

validators/geometryIntersects.m

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