Added function to sort elements by increasing distance then angle

sort_dist_angle.m

+function [sig,order] = sort_dist_angle(dist,angle)
+%SORT_DIST_ANGLE Summary of this function goes here
+%   Detailed explanation goes here
+
+sz = size(dist);
+
+[~,sig] = sort(dist(:)','ascend');
+
+dist  =  dist(sig);
+angle = angle(sig);
+
+for cur_dist = unique(dist)
+    cur_ind = dist==cur_dist;
+    
+      sig_cur_ind =   sig(cur_ind);
+     dist_cur_ind =  dist(cur_ind);
+    angle_cur_ind = angle(cur_ind);
+    
+    [~,cur_sig] = sort(angle_cur_ind,'ascend');
+    
+      sig(cur_ind) =   sig_cur_ind(cur_sig);
+     dist(cur_ind) =  dist_cur_ind(cur_sig);
+    angle(cur_ind) = angle_cur_ind(cur_sig);
+end
+
+order = zeros(sz);
+order(sig) = 1:numel(dist);
+
+end
\ No newline at end of file