function Sol = heuristic(Rx,Ry,Rz,Sol,n) %Proposed heuristic based obstacle avoidance approach: Refer onenote global Comp kx = cell2mat(Rx(:,3)); ky = cell2mat(Ry(:,3)); kz = cell2mat(Rz(:,3)); for i = 1:size(kx,1) Fxy = kx(i,:).* ky; Case3a = find(~any(Fxy,2)); Case3b = find(any(Fxy==2 | Fxy==1,2)); Case3c = find(any(Fxy>2,2) & ~any(Fxy==2 | Fxy==1,2)); if ~isempty(Case3a) Sol{n,1} = Rx(i,1); % 1elem Sol{n,2} = Ry(Case3a,1); % >1 elem Sol{n,3} = Rz(:,1); % >1 elem Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 2.1]; n = n+1; % Ry(Case3a,:) = []; % ky(Case3a,:) = []; end if ~isempty(Case3c) for ix = 1:length(Case3c) On = find(Fxy(ix,:)>2); flg = intersect_pair3(Rx{i,2},Ry{Case3c(ix),2},Rz,On,2); if flg Sol{n,1} = Rx(i,1); Sol{n,2} = Ry(Case3c(ix),1); Sol{n,3} = Rz(:,1); Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 2.2]; n = n+1; % Ry(Case3c(ix),:) = []; % ky(Case3c(ix),:) = []; else Case3b = [Case3b;Case3c(ix)]; end end end if ~isempty(Case3b) for ix = 1:length(Case3b) Fxyz = Fxy(Case3b(ix),:).* kz; Case4a = find(~any(Fxyz,2)); if ~isempty(Case4a) Sol{n,1} = Rx(i,1); Sol{n,2} = Ry(Case3b(ix),1); Sol{n,3} = Rz(Case4a,1); Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 3.1]; n = n+1; % Rz(Case4a,:) = []; % kz(Case4a,:) = []; end Case4c = find(any(Fxyz>2,2) & ~any(Fxyz==2 | Fxyz==1,2)); if ~isempty(Case4c) for iy = 1:length(Case4c) On = find(Fxyz(iy,:)>2); flg1 = intersect_pair3(Rx{i,2},Ry{Case3b(ix),2},Rz{Case4c(iy),2},On,3); if flg1 Sol{n,1} = Rx(i,1); Sol{n,2} = Ry(Case3b(ix),1); Sol{n,3} = Rz(Case4c(iy),1); Comp = [Comp; toc size(Sol{n,1},1)*size(Sol{n,2},1)*size(Sol{n,3},1) 3.2]; n = n+1; end end end end end end end