Algorithm 1 3D-SWAP for each UAV i

Input: Pointcloud from local sensors, current position ${\mathbf{p}}_i^{\prime }$, goal position ${\mathbf{g}}_i$    1:while${\mathbf{g}}_i$ not reached do    2: Send position ${\mathbf{p}}_i^{\prime }$ to neighbors    3: $COD\leftarrow initCOD()$    4: for all UAV j within communication range do    5:  Receive its position ${\mathbf{p}}_j^{\prime }$    6:  $({\rho }_j^{\prime },{\phi }_j^{\prime },z_j^{\prime })\leftarrow transformToCylindrical({\mathbf{p}}_j^{\prime })$    7:  $COD\leftarrow UpdateCOD({\rho }_j^{\prime },{\phi }_j^{\prime },z_j^{\prime })$    8: end for    9: for $m=1$ $m=M$ do   10:  Extract point m from local pointcloud   11:  $COD\leftarrow UpdateCOD({\rho }_m,{\phi }_m,z_m)$   12: end for   13: $(s_{xy},s_z)\leftarrow computeState(COD)$   14: ${\mathbf{d}}^g={\mathbf{g}}_i-{\mathbf{p}}_i^{\prime }$   15: if $s_{xy}$ is xy-free then   16:  $\angle {\mathbf{v}}_{xy}^{ref}=\angle {\mathbf{d}}_{xy}^g$ and $||{\mathbf{v}}_{xy}^{ref}||=computeRefSpeed(||{\mathbf{d}}_{xy}^g||,v_{max})$   17: else if $s_{xy}$ is rendezvous then   18:  $\angle {\mathbf{v}}_{xy}^{ref}={\phi }_a$ and $||{\mathbf{v}}_{xy}^{ref}||=v_a$   19: else if $s_{xy}$ is xy-blocked then   20:  ${\mathbf{v}}_{xy}^{ref}=\mathbf{0}$   21: end if   22: if $s_z$ is z-free then   23:  $v_z^{ref}=\pm computeRefSpeed(d_z^g,v_{max})$, depending on whether ${\mathbf{g}}_i$ is above or below   24: else if $s_z$ is z-blocked then   25:  $v_z^{ref}=0$   26: end if   27: ${\mathbf{v}}^{ref}\leftarrow ({\mathbf{v}}_{xy}^{ref},v_z^{ref})$   28: Send ${\mathbf{v}}^{ref}$ to velocity controller   29:end while