Upload New File

Matlab Code/generateGreedySolutionFast.m

+function [plcSel, score] = generateGreedySolutionFast(plcs, SG, env, agents, reqConn, sig, plotArea, bddSet, parOpt)
+
+N = size(plcs{1,1},1);
+[Tk,Tj] = size(plcs);
+plcSel = cell(1,Tk);
+score = zeros(1,Tk);
+
+if parOpt
+    if reqConn
+        fprintf("selecting parallelized constrained samples\n")
+    else
+        fprintf("selecting parallelized unconstrained samples\n")
+    end
+else
+    if reqConn
+        fprintf("selecting serialized constrained samples\n")
+    else
+        fprintf("selecting serialized unconstrained samples\n")
+    end
+end
+
+if reqConn
+    
+    for k=1:Tk
+        %             G = SG{k};
+        verts = [];
+        while length(verts) < agents
+            maxUtil = 0;
+            nextNode = -1;
+            for i=1:Tj*N % iterate through all nodes and pick best one
+                
+                if ismember(i,verts)
+                    continue;
+                end
+                
+                ags = covertIdxForm([verts;i], Tj);
+                if length(unique(ags)) < length(ags) % if multiple agents are being used
+                    continue;
+                end
+                
+                [iags,itrj] = covertIdxForm(i, Tj);
+                if ~inpolygon(plcs{k,itrj}(iags,1), plcs{k,itrj}(iags,2), bddSet{k}(:,1), bddSet{k}(:,2)) % check if point is in bounding box ([xmin xmax ymin ymax])
+                    continue; % if connectivity is required, can only pick samples from within the constrained bounding set
+                end
+                
+                util = trajectoryUtility([verts;i], plcs, env, k, sig, plotArea); % dont care about connectivity here
+                if util > maxUtil
+                    maxUtil = util;
+                    nextNode = i;
+                end
+            end
+            % add the best node
+            if nextNode ~= -1
+                verts = [verts;nextNode];
+            else
+                break;
+            end
+        end
+        if ~isempty(verts)
+            score(k) = trajectoryUtility(verts, plcs, env, k, sig, plotArea);
+            plcSel{k} = verts;
+        else
+            score(k) = 0;
+            plcSel{k} = [];
+        end
+        fprintf("samples selected for time slice %d\n", k);
+    end
+    
+else % connectivity is not required
+    
+    for k=1:Tk
+        %             G = SG{k};
+        verts = [];
+        while length(verts) < agents
+            maxUtil = 0;
+            for i=1:Tj*N % iterate through all nodes and pick best one
+                if ismember(i,verts)
+                    continue;
+                end
+                ags = covertIdxForm([verts;i], Tj);
+                if length(unique(ags)) < length(ags) % if multiple agents are being used
+                    continue;
+                end
+                util = trajectoryUtility([verts;i], plcs, env, k, sig, plotArea); % dont care about connectivity here
+                if util > maxUtil
+                    maxUtil = util;
+                    nextNode = i;
+                end
+            end
+            % add the best node
+            if nextNode ~= -1
+                verts = [verts;nextNode];
+            end
+        end
+        score(k) = trajectoryUtility(verts, plcs, env, k, sig, plotArea);
+        plcSel{k} = verts;
+        fprintf("samples selected for time slice %d\n", k);
+    end
+    
+end
+