Matlab: How to output a matrix which is sorted by distance

Question

I have a cluster of points in 3D point clouds, says

    A = [ 1 4 3;
    1 2 3;
    1 6 3;
    1 5 3];

The distance matrix then was found:

    D= pdist(A);
    Z= squareform(D);
    Z =

 0     2     2     1
 2     0     4     3
 2     4     0     1
 1     3     1     0

I would like to sort the points so that the sum of the distance travelled through the points will be the smallest, and output in another matrix. This is similar to TSP problem but in a 3D model. Is there any function can do this? Your help is really appreciated in advance.

Answer 1

This could be one approach and must be efficient enough for a wide range of datasizes -

D = pdist(A);
Z = squareform(D);  %// Get distance matrix

N = size(A,1);      %// Store the size of the input array for later usage
Z(1:N+1:end) = Inf; %// Set diagonals as Infinites as we intend to find
                    %// minimum along each row

%// Starting point and initialize an array to store the indices according
%// to the sorted requirements set in the question
idx = 1;
out_idx = zeros(N,1);
out_idx(1) = idx;

%// Perform an iterative search to look for nearest one starting from point-1
for k = 2:N
    start_ind = idx;
    [~,idx] = min(Z(start_ind,:));
    Z(:,start_ind) = Inf;
    out_idx(k) = idx;
end

%// Now that you have the list of indices based on the next closest one, 
%// sort the input array based on those indices and have the desired output 
out = A(out_idx,:)

Sample run for given input -

A =
     1     4     3
     1     2     3
     1     6     3
     1     5     3
     1     2     3
out =
     1     4     3
     1     5     3
     1     6     3
     1     2     3
     1     2     3