function [min_d,t] = LEM_trajMinDist(O1,V1,ts1,te1,O2,V2,ts2,te2)
% LEM_trajDist - determines the closest time/distance of two trajectories
% On input:
%     O1 (1x3 vector): Origin vector of segment 1
%     V1 (1x3 vector): Velocity vector of segment 1
%     ts1 (float): Start time of segment 1
%     te1 (float): End time of segment 1
%     O2 (1x3 vector): Origin vector of segment 2
%     V2 (1x3 vector): Velocity vector of segment 2
%     ts2 (float): Start time of segment 2
%     te2 (float): End time of segment 2
% On output:
%     min_d (float): minimum distance between two segment
%     t : time of minimum distance between segments (-1 if parallel)
% Call:
%     [min_d,t] = LEM_trajDist(O1,V1,Ts1,Te1,O2,V2,Ts2,Te2);
% Author:
%     B. Russon
%     UU
%     Spring 2020

O12 = O1 + (te1-ts1)*V1;
dist1 = norm(O12-O1);
O22 = O2 + (te2-ts2)*V2;
dist2 = norm(O22-O2);
Vr = V1-V2;

if (~any(Vr)) % all elements are zero - Trajectories are parallel
    t = -1;
else
    %Find closest time
    t = -((O1-O2-V1*ts1+V2*ts2)*(Vr)')/(Vr*Vr');
    %Compare to boundaries of trajectory
%     if t < ts1
%         t = ts1;
%     elseif t > te1
%         t = te1;
%     end
%     if t < ts2
%         t = ts2;
%     elseif t > te2
%         t = te2;
%     end
end

%Use the same for parallel, because if trajectories are parallel
%the distance will be the same for all times, including t = -1.
[min_d,~,~] = LEM_trajDist(O1,V1,ts1,O2,V2,ts2,t);
