% RAY2RAYDIST   Minimum distance between two 3D rays
%
% Usage: d = ray2raydist(p1, v1, p2, v2)
%
% Arguments:
%       p1, p2 - 3D points that lie on rays 1 and 2.
%       v1, v2 - 3D vectors defining the direction of each ray.
%
% Returns:
%            d - The minimum distance between the rays.
%
% Each ray is defined by a point on the ray and a vector giving the direction
% of the ray.  Thus a point on ray 1 will be given by  p1 + alpha*v1  where
% alpha is some scalar.

% Peter Kovesi
% peterkovesi.com
% June 2016

function d = ray2raydist(p1, v1, p2, v2)
    
    % Get vector perpendicular to both rays
    n = cross(v1, v2); n = n(:);
    nnorm = sqrt(n'*n);

    % Check if lines are parallel. If so, form a vector perpendicular to v1
    % that is within the plane formed by the parallel rays.
    if nnorm < eps; 
        n = cross(v1, p1-p2); % Vector perpendicular to plane formed by pair
                              % of rays.
        n = cross(v1, n);     % Vector perpendicular to v1 within the plane
        n = n(:);             % formed by the 2 rays.
        nnorm = sqrt(n'*n);

        if nnorm < eps
            d = 0;
            return
        end
    end                     
    
    n = n/nnorm;        % Unit vector
    
    % The vector joining the two closest points on the rays is:
    %    d*n = p2 + beta*v2 - (p1 + alpha*v1) 
    % for some unknown values of alpha and beta.   
    %
    % Take dot product of n on both sides
    %    d*n.n = p2.n + beta*v2.n - p1.n - alpha*v1.n
    %
    % as n is prependicular to v1 and v2 this reduces to
    %   d = (p2 - p1).n
    d = abs((p2(:)' - p1(:)')*n);