fixed init generation being really slow

geometries/rectangularPrism.m

@@ -74,6 +74,8 @@ classdef rectangularPrism
             arguments (Output)
                 d (:, 1) double
             end
+            assert(~obj.contains(pos), "Cannot determine distance for a point inside of the geometry");
+            
             cPos = NaN(1, 3);
             for ii = 1:3
                 if pos(ii) < obj.minCorner(ii)
@@ -94,6 +96,8 @@ classdef rectangularPrism
             arguments (Output)
                 d (:, 1) double
             end
+            assert(obj.contains(pos), "Cannot determine interior distance for a point outside of the geometry");
+
             % find minimum distance to any face
             d = min([pos(1) - obj.minCorner(1), ...
                      pos(2) - obj.minCorner(2), ...
@@ -112,6 +116,47 @@ classdef rectangularPrism
             end
             c = all(pos >= repmat(obj.minCorner, size(pos, 1), 1), 2) & all(pos <= repmat(obj.maxCorner, size(pos, 1), 1), 2);
         end
+        function c = containsLine(obj, pos1, pos2)
+            arguments (Input)
+                obj (1, 1) {mustBeA(obj, 'rectangularPrism')};
+                pos1 (1, 3) double;
+                pos2 (1, 3) double;
+            end
+            arguments (Output)
+                c (1, 1) logical
+            end
+            
+            d = pos2 - pos1;
+
+            % edge case where the line is parallel to the geometry
+            if abs(d) < 1e-12
+                % check if it happens to start or end inside or outside of
+                % the geometry
+                if obj.contains(pos1) || obj.contains(pos2)
+                    c = true;
+                else
+                    c = false;
+                end
+                return;
+            end
+
+            tmin = -inf;        
+            tmax = inf;
+
+            % Standard case
+            for ii = 1:3
+                t1 = (obj.minCorner(ii) - pos1(ii)) / d(ii);
+                t2 = (obj.maxCorner(ii) - pos2(ii)) / d(ii);
+                tmin = max(tmin, min(t1, t2));
+                tmax = min(tmax, max(t1, t2));
+                if tmin > tmax
+                    c = false;
+                    return;
+                end
+            end
+
+            c = (tmax >= 0) && (tmin <= 1);
+        end
         function f = plotWireframe(obj, f)
             arguments (Input)
                 obj (1, 1) {mustBeA(obj, 'rectangularPrism')};

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info Ref="validators/arguments" Type="Relative"/>

resources/project/EEtUlUb-dLAdf0KpMVivaUlztwA/jjRWXxv1fmTGw_ntc54vNxAyLDMp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="b7c7eec5-a318-4c17-adb2-b13a21bf0609" type="Reference"/>

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>

resources/project/IkDHDbHimostG0Ka3Qk97pof68k/bkRZvUTstiBe1YFWETJX1U9n5uQp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="1" type="DIR_SIGNIFIER"/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/6yzCMKkTsLiObJh9i87v9JChC2Yp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="domainContainsObstacle.m" type="File"/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/G6lGXsTbuwZ-AEQ27UXCqCsFttEp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="obstacleCoversObjective.m" type="File"/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kd.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/IkDHDbHimostG0Ka3Qk97pof68kp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="arguments" type="File"/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/gE3UtRAYb2Mvi_5_27MbNbrZKNcp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="obstacleCrowdsObjective.m" type="File"/>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUd.xml

@@ -0,0 +1,6 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info>
+    <Category UUID="FileClassCategory">
+        <Label UUID="design"/>
+    </Category>
+</Info>

resources/project/M3axfhPiVFfHfWnq8PF7Q8OtKAU/mjV-hfj-qf8MzZBB6uhFVn0raaUp.xml

@@ -0,0 +1,2 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Info location="agentsCrowdObjective.m" type="File"/>

test_miSim.m

@@ -1,61 +1,72 @@
 classdef test_miSim < matlab.unittest.TestCase
     properties (Access = private)
         testClass = miSim;
+
         % Domain
-        domain = rectangularPrism;
+        domain = rectangularPrism; % domain geometry
 
         % Obstacles
-        minNumObstacles = 1;
+        minNumObstacles = 1; % Minimum number of obstacles to be randomly generated
-        maxNumObstacles = 3;
+        maxNumObstacles = 3; % Maximum number of obstacles to be randomly generated
+        minObstacleSize = 1; % Minimum size of a randomly generated obstacle
+        maxObstacleSize = 6; % Maximum size of a randomly generated obstacle
         obstacles = cell(1, 0);
-        minObstacleDimension = 1;
         
         % Objective
+        objectiveDiscretizationStep = 0.01; % Step at which the objective function is solved in X and Y space
+        protectedRange = 1; % Minimum distance between the sensing objective and the edge of the domain
         objective = sensingObjective;
-        objectiveFunction = @(x, y) 0;
-        objectiveDiscretizationStep = 0.01;
-        protectedRange = 1;
 
         % Agents
-        minAgents = 3;
+        minAgents = 3; % Minimum number of agents to be randomly generated
-        maxAgents = 9;
+        maxAgents = 9; % Maximum number of agents to be randomly generated
-        agents = cell(1, 0);
+        agents = cell(0, 1);
         
         % Collision
-        minCollisionRange = 0.1;
+        minCollisionRange = 0.1; % Minimum randomly generated collision geometry size
-        maxCollisionRange = 0.5;
+        maxCollisionRange = 0.5; % Maximum randomly generated collision geometry size
         collisionRanges = NaN;
 
         % Communications
-        comRange = 5;
+        comRange = 5; % Maximum range between agents that forms a communications link
     end
 
     % Setup for each test
     methods (TestMethodSetup)
         % Generate a random domain
         function tc = setDomain(tc)
-            % random integer-sized domain ranging from [0, 5] to [0, 25] in all dimensions
+            % random integer-sized cube domain ranging from [0, 5 -> 25]
+            % in all dimensions
             L = ceil(5 + rand * 10 + rand * 10);
             tc.domain = tc.domain.initialize([zeros(1, 3); L * ones(1, 3)], REGION_TYPE.DOMAIN, "Domain");
         end
         % Generate a random sensing objective within that domain
         function tc = setSensingObjective(tc)
+            % Using a bivariate normal distribution
+            % Set peak position (mean)
             mu = tc.domain.minCorner;
             while tc.domain.interiorDistance(mu) < tc.protectedRange
                 mu = tc.domain.random();
             end
             mu(3) = 0;
-            assert(tc.domain.contains(mu));
+
+            % Set standard deviations of bivariate distribution
             sig = [2 + rand * 2, 1; 1, 2 + rand * 2];
-            tc.objectiveFunction = @(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig);
+
-            tc.objective = tc.objective.initialize(tc.objectiveFunction, tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
+            % Define objective
+            tc.objective = tc.objective.initialize(@(x, y) mvnpdf([x(:), y(:)], mu(1:2), sig), tc.domain.footprint, tc.domain.minCorner(3), tc.objectiveDiscretizationStep);
         end
-        % Instantiate agents, they will be initialized under different
+        % Instantiate agents
-        % parameters in individual test cases
         function tc = setAgents(tc)
+            % Agents will be initialized under different parameters in
+            % individual test cases
+
+            % Instantiate a random number of agents according to parameters
             for ii = 1:randi([tc.minAgents, tc.maxAgents])
                 tc.agents{ii, 1} = agent;
             end
+
+            % Define random collision ranges for each agent
             tc.collisionRanges = tc.minCollisionRange + rand(size(tc.agents, 1), 1) * (tc.maxCollisionRange - tc.minCollisionRange);
         end
     end
@@ -66,140 +77,137 @@ classdef test_miSim < matlab.unittest.TestCase
             % randomly create 2-3 obstacles
             nGeom = tc.minNumObstacles + randi(tc.maxNumObstacles - tc.minNumObstacles);
             tc.obstacles = cell(nGeom, 1);
+
+            % Iterate over obstacles to initialize
             for ii = 1:size(tc.obstacles, 1)
-                % Instantiate a rectangular prism obstacle
+                badCandidate = true;
-                tc.obstacles{ii, 1} = rectangularPrism;
+                while badCandidate
-
+                    % Instantiate a rectangular prism obstacle
-                % Randomly come up with dimensions until they
+                    tc.obstacles{ii} = rectangularPrism;
-                % fit within the domain
-                candidateMinCorner = [-Inf(1, 2), 0];
-                candidateMaxCorner = Inf(1, 3);
-
-                % make sure obstacles are not too small in any dimension
-                tooSmall = true;
-                while tooSmall
-                    % make sure the obstacles don't contain the sensing
-                    % objective or encroach on it too much
-                    obstructs = true;
-                    while obstructs
     
-                        % Make sure the obstacle is in the domain
+                    % Randomly generate min corner for the obstacle
-                        while any(candidateMinCorner < tc.domain.minCorner) 
+                    candidateMinCorner = tc.domain.random();
-                            candidateMinCorner = tc.domain.minCorner(1:3) + [(tc.domain.maxCorner(1:2) - tc.domain.minCorner(1:2)) .* rand(1, 2), 0]; % random spots on the ground
+                    candidateMinCorner = [candidateMinCorner(1:2), 0]; % bind obstacles to floor of domain
-                        end
-                        while any(candidateMaxCorner > tc.domain.maxCorner)
-                            candidateMaxCorner = [candidateMinCorner(1:2), 0] + ((tc.domain.maxCorner(1:3) - tc.domain.minCorner(1:3)) .* rand(1, 3) ./ 2); % halved to keep from being excessively large
-                        end
     
-                        % once a domain-valid obstacle has been found, make
+                    % Randomly select a corresponding maximum corner that
-                        % sure it doesn't obstruct the sensing target
+                    % satisfies min/max obstacle size specifications
-                        if all(candidateMinCorner(1:2) <= tc.objective.groundPos) && all(candidateMaxCorner(1:2) >= tc.objective.groundPos)
+                    candidateMaxCorner = candidateMinCorner + tc.minObstacleSize + rand(1, 3) * (tc.maxObstacleSize - tc.minObstacleSize);
-                            % reset to try again
+                
-                            candidateMinCorner = [-Inf(1, 2), 0];
+                    % Initialize obstacle
-                            candidateMaxCorner = Inf(1, 3);
+                    tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
-                        else
+
-                            obstructs = false;
+                    % Make sure that the obstacles are fully contained by 
-                        end
+                    % the domain
+                    if ~domainContainsObstacle(tc.domain, tc.obstacles{ii})
+                        continue;
                     end
-                    if min(candidateMaxCorner - candidateMinCorner) >= tc.minObstacleDimension
+
-                        tooSmall = false;
+                    % Make sure that the obstacles don't cover the sensing
-                    else
+                    % objective
-                        candidateMinCorner = [-Inf(1, 2), 0];
+                    if obstacleCoversObjective(tc.objective, tc.obstacles{ii})
-                        candidateMaxCorner = Inf(1, 3);
+                        continue;
                     end
+
+                    % Make sure that the obstacles aren't too close to the
+                    % sensing objective
+                    if obstacleCrowdsObjective(tc.objective, tc.obstacles{ii}, tc.protectedRange)
+                        continue;
+                    end
+                    
+                    badCandidate = false;
                 end
-
-                % Reduce infinite dimensions to the domain
-                candidateMinCorner(isinf(candidateMinCorner)) = tc.domain.minCorner(isinf(candidateMinCorner));
-                candidateMaxCorner(isinf(candidateMaxCorner)) = tc.domain.maxCorner(isinf(candidateMaxCorner));
-
-                % Initialize obstacle geometry
-                tc.obstacles{ii} = tc.obstacles{ii}.initialize([candidateMinCorner; candidateMaxCorner], REGION_TYPE.OBSTACLE, sprintf("Column obstacle %d", ii));
             end
-            
+
-            % Repeat this until a connected set of agent initial conditions
+            % Add agents individually, ensuring that each addition does not
-            % is found by random chance
+            % invalidate the initialization setup
-            nIter = 0;
+            for ii = 1:size(tc.agents, 1)
-            connected = false;
+                initInvalid = true;
-            while ~connected
+                while initInvalid
-                % Randomly place agents in the domain
+                    candidatePos = [tc.objective.groundPos, 0];
-                for ii = 1:size(tc.agents, 1)
+                    % Generate a random position for the agent based on
-                    posInvalid = true;
+                    % existing agent positions
-                    while posInvalid
+                    if ii == 1
-                        % Initialize the agent into a random spot in the
+                        while agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
-                        % domain (that is not too close to the sensing
-                        % objective)
-                        boringInit = true;
-                        while boringInit
                             candidatePos = tc.domain.random();
-                            if norm(candidatePos(1:2) - tc.objective.groundPos) >= norm(tc.domain.footprint(4, :) - tc.domain.footprint(1, :))/2
+                        end
-                                boringInit = false;
+                    else
+                        candidatePos = tc.agents{randi(ii - 1)}.pos + sign(randn([1, 3])) .* (rand(1, 3) .* tc.comRange/sqrt(2));
+                    end
+
+                    % Make sure that the candidate position is within the
+                    % domain
+                    if ~tc.domain.contains(candidatePos)
+                        continue;
+                    end
+
+                    % Make sure that the candidate position does not crowd
+                    % the sensing objective and create boring scenarios
+                    if agentsCrowdObjective(tc.objective, candidatePos, mean(tc.domain.dimensions) / 2)
+                        continue;
+                    end
+
+                    % Make sure that there exist unobstructed lines of sight at
+                    % appropriate ranges to form a connected communications 
+                    % graph between the agents
+                    connections = false(1, ii - 1);
+                    for jj = 1:(ii - 1)
+                        if norm(tc.agents{jj}.pos - candidatePos) <= tc.comRange
+                            % Check new agent position against all existing
+                            % agent positions for communications range
+                            connections(jj) = true;
+                            for kk = 1:size(tc.obstacles, 1)
+                                if tc.obstacles{kk}.containsLine(tc.agents{jj}.pos, candidatePos)
+                                    connections(jj) = false;
+                                end
                             end
                         end
-                        candidateGeometry = rectangularPrism;
+                    end
-                        tc.agents{ii} = 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));
+
-    
+                    % New agent must be connected to an existing agent to
-                        % Check obstacles to confirm that none are violated
+                    % be valid
-                        for jj = 1:size(tc.obstacles, 1)
+                    if ii ~= 1 && ~any(connections)
-                            inside = false;
+                        continue;
-                            if tc.obstacles{jj, 1}.contains(tc.agents{ii, 1}.pos)
+                    end
-                                % Found a violation, stop checking
+
-                                inside = true;
+                    % Initialize candidate agent
+                    candidateGeometry = rectangularPrism;
+                    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
+                    violation = false;
+                    for jj = 1:size(newAgent.collisionGeometry.vertices, 1)
+                        % Check if collision geometry exits domain
+                        if ~tc.domain.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                            violation = true;
+                            break;
+                        end
+
+                        % Check if collision geometry enters obstacle
+                        for kk = 1:size(tc.obstacles, 1)
+                            if tc.obstacles{kk}.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
+                                violation = true;
                                 break;
                             end
                         end
-    
+                        
-                        % Agent is inside obstacle, try again
+                        % Check if collision geometry enters other
-                        if inside
+                        % collision geometry
-                            continue;
+                        for kk = 1:(ii - 1)
-                        end
+                            if tc.agents{kk}.collisionGeometry.contains(newAgent.collisionGeometry.vertices(jj, 1:3))
-    
+                                violation = true;
-                        % Create a collision geometry for this agent
-                        candidateGeometry = rectangularPrism;
-                        candidateGeometry = candidateGeometry.initialize([tc.agents{ii}.pos - 0.1 * ones(1, 3); tc.agents{ii}.pos + 0.1 * ones(1, 3)], REGION_TYPE.COLLISION, sprintf("Agent %d collision volume", ii));
-    
-                        % Check previously placed agents for collisions
-                        for jj = 1:(ii - 1)
-                            % Check if previously defined agents collide with
-                            % this one
-                            colliding = false;
-                            if candidateGeometry.contains(tc.agents{jj, 1}.pos)
-                                % Found a violation, stop checking
-                                colliding = true;
                                 break;
                             end
                         end
-    
-                        % Agent is colliding with another, try again
-                        if ii ~= 1 && colliding
-                            continue;
-                        end
-    
-                        % Allow to proceed since no obstacle/collision
-                        % violations were found
-                        posInvalid = false;
                     end
-                end
+                    if violation
-    
+                        continue;
-                % Collect all agent positions
-                posArray = arrayfun(@(x) x{1}.pos, tc.agents, 'UniformOutput', false);
-                posArray = reshape([posArray{:}], size(tc.agents, 1), 3);  
-    
-                % Communications checks
-                adjacency = false(size(tc.agents, 1), size(tc.agents, 1));
-                for ii = 1:size(tc.agents, 1)
-                    % Compute distance from each to all agents
-                    for jj = 1:(size(tc.agents, 1))
-                        if norm(posArray(ii, 1:3) - posArray(jj, 1:3)) <= tc.comRange
-                            adjacency(ii, jj) = true;
-                        end
                     end
+
+                    % Candidate agent is valid, store to pass in to sim
+                    initInvalid = false;
+                    tc.agents{ii} = newAgent;
                 end
-    
-                % Check connectivity
-                G = graph(adjacency);
-                connected = all(conncomp(G) == 1);
-                nIter = nIter + 1;
             end
 
             % Initialize the simulation
@@ -215,7 +223,7 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Plot obstacles
             for ii = 1:size(tc.testClass.obstacles, 1)
-                tc.testClass.obstacles{ii, 1}.plotWireframe(f);
+                tc.testClass.obstacles{ii}.plotWireframe(f);
             end
 
             % Plot objective gradient
@@ -223,8 +231,8 @@ classdef test_miSim < matlab.unittest.TestCase
 
             % Plot agents and their collision geometries
             for ii = 1:size(tc.testClass.agents, 1)
-                f = tc.testClass.agents{ii, 1}.plot(f);
+                f = tc.testClass.agents{ii}.plot(f);
-                f = tc.testClass.agents{ii, 1}.collisionGeometry.plotWireframe(f);
+                f = tc.testClass.agents{ii}.collisionGeometry.plotWireframe(f);
             end
         end
     end

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

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

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

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