function [d, U] = getDistance(crd1, crd2);

% GETDISTANCE  Compute the distance between two 3D-structures.
%    First, translate both structures to the origin, and then find an 
%    optimal rotation of one of the structures that minimizes the
%    distance between the two.
%
%    [d, U] = getDistance(crd1, crd2) returns the distance, along with
%    the optimal rotation matrix.

if (size(crd1, 1) ~= size(crd2, 1))
   error('Coordinate matrices must be of the same size');
end
if (size(crd1, 2) ~= size(crd2, 2))
   error('Coordinate space must have the same dimension');
end

N   = size(crd1, 1);    % The number of points
dim = size(crd1, 2);    % The dimension of the space

% Translate both structures to origin
r1 = transOrigin(crd1);
r2 = transOrigin(crd2);

% Construct the non-symmetric matrix R.  Note that the below is 
% equivalent to R(i,j) = r2(:,i) * r1(:,j) for i,j=1:3
R = r2' * r1;

[B, Lambda, A] = svd(R);   % Singular value decomposition of R
L = diag(Lambda);          % Store the eigenvalues in an array
U = zeros(dim, dim);       % Form the rotation matrix U
for i=1:dim
   U = U + (B(:,i) * A(:,i)');
end

% Check determinant of the rotation matrix
if (det(U) < 0)
   minL = Inf;
   for i=1:dim                      % Find the minimum non-zero eigenvalue
       if (L(i)<minL & L(i)>0)
          minL = L(i); min_ind = i;
       end
   end
   L(min_ind) = -L(min_ind);        % Negate the smallest eigenvalue

   % If asked for rotation matrix, recompute it and return
   if (nargout == 2)
       B(:,min_ind) = -B(:,min_ind);
       U = U + 2*B(:,min_ind) * A(:,min_ind)';
   end
end

% Compute the square of the distance
dsq = 0;
for i=1:N
   dsq = dsq + (r1(i,:)*r1(i,:)' + r2(i,:)*r2(i,:)');
end
dsq = dsq - 2 * sum(L);
d = sqrt(dsq);               % Return the distance


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function x = transOrigin(coor)

% Translate the coordinates stored in vector COOR to the origin

len = size(coor, 1);
center = sum(coor, 1) ./ len;
for i = 1:len
    x(i, :) = coor(i, :) - center;
end