reorganized code into separate files

util/validators/agentsCrowdObjective.m

@@ -0,0 +1,11 @@
+function c = agentsCrowdObjective(objective, positions, protectedRange)
+    arguments (Input)
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+        positions (:, 3) double; % this could be expanded to handle n obstacles in 1 call
+        protectedRange (1, 1) double;
+    end
+    arguments (Output)
+        c (:, 1) logical;
+    end
+    c = vecnorm(positions(:, 1:2) - objective.groundPos, 2, 2) <= protectedRange;
+end

util/validators/arguments/mustBeGeometry.m

@@ -0,0 +1,10 @@
+function mustBeGeometry(geometry)
+    validGeometries = ["rectangularPrism";];
+    if isa(geometry, 'cell')
+        for ii = 1:size(geometry, 1)
+            assert(any(arrayfun(@(x) isa(geometry{ii}, x), validGeometries)), "Geometry in index %d is not a valid geometry class", ii);
+        end
+    else
+        assert(any(arrayfun(@(x) isa(geometry, x), validGeometries)), "Geometry is not a valid geometry class");
+    end
+end

util/validators/arguments/mustBeSensor.m

@@ -0,0 +1,10 @@
+function mustBeSensor(sensorModel)
+    validSensorModels = ["fixedCardinalSensor"; "sigmoidSensor";];
+    if isa(sensorModel, 'cell')
+        for ii = 1:size(sensorModel, 1)
+            assert(any(arrayfun(@(x) isa(sensorModel{ii}, x), validSensorModels)), "Sensor in index %d is not a valid sensor class", ii);
+        end
+    else
+        assert(any(arrayfun(@(x) isa(sensorModel, x), validSensorModels)), "Sensor is not a valid sensor class");
+    end
+end

util/validators/domainContainsObstacle.m

@@ -0,0 +1,21 @@
+function c = domainContainsObstacle(domain, obstacle)
+    arguments (Input)
+        domain (1, 1) {mustBeGeometry};
+        obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
+    end
+    arguments (Output)
+        c (1, 1) logical;
+    end
+
+    switch class(domain)
+        case 'rectangularPrism'
+            switch class(obstacle)
+                case 'rectangularPrism'
+                    c = all(domain.minCorner <= obstacle.minCorner) && all(domain.maxCorner >= obstacle.maxCorner);
+                otherwise
+                    error("%s not implemented for obstacles of class %s", coder.mfunctionname, class(domain));
+            end
+        otherwise
+            error("%s not implemented for domains of class %s", coder.mfunctionname, class(domain));
+    end
+end

util/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

util/validators/obstacleCoversObjective.m

@@ -0,0 +1,13 @@
+function c = obstacleCoversObjective(objective, obstacle)
+    arguments (Input)
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+        obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
+    end
+    arguments (Output)
+        c (1, 1) logical;
+    end
+
+    % Check if the obstacle contains the objective's ground position if the
+    % ground position were raised to the obstacle's center's height
+    c = obstacle.contains([objective.groundPos, obstacle.center(3)]);
+end

util/validators/obstacleCrowdsObjective.m

@@ -0,0 +1,11 @@
+function c = obstacleCrowdsObjective(objective, obstacle, protectedRange)
+    arguments (Input)
+        objective (1, 1) {mustBeA(objective, 'sensingObjective')};
+        obstacle (1, 1) {mustBeGeometry}; % this could be expanded to handle n obstacles in 1 call
+        protectedRange (1, 1) double;
+    end
+    arguments (Output)
+        c (1, 1) logical;
+    end
+    c = norm(obstacle.distance([objective.groundPos, obstacle.center(3)])) <= protectedRange;
+end