calc_distance_3D_point_from_3D_line

% distance 3D point from 3D line

 Usage:
   d = calc_distance_3D_point_from_3D_line(X,L)

   X  - 4 x 1 homogeneous vector of 3D point
   L  - 6 x 1 Plücker coordinates 

   d  - Euclidean distance, inf if distance infinite

 Wolfgang Förstner 2010/02/14
 wfoerstn@uni-bonn.de 

 See also calc_distance_3D_point_from_linesegment, calc_distance_3D_point_from_triangle

0001 %% distance 3D point from 3D line
0002 %
0003 % Usage:
0004 %   d = calc_distance_3D_point_from_3D_line(X,L)
0005 %
0006 %   X  - 4 x 1 homogeneous vector of 3D point
0007 %   L  - 6 x 1 Plücker coordinates
0008 %
0009 %   d  - Euclidean distance, inf if distance infinite
0010 %
0011 % Wolfgang Förstner 2010/02/14
0012 % wfoerstn@uni-bonn.de
0013 %
0014 % See also calc_distance_3D_point_from_linesegment, calc_distance_3D_point_from_triangle
0015 
0016 function d = calc_distance_3D_point_from_3D_line(X,L)
0017 
0018 % homogeneous parts
0019 Xh = X(4);
0020 Lh = L(1:3);
0021 % check for elements at infinity
0022 if Xh == 0 || norm(Lh) == 0
0023     d = inf;
0024 else
0025     % distance
0026     d = norm(calc_S(X(1:3))*Lh-Xh*L(4:6))/norm(Xh*Lh);
0027 end