calc_intersect_3D_Lines2Point_finite

% intersection point X of two 3D-lines L and M 
 precondition: L and M are not parallel, otherwise X = [0 0 0 0]'

 see PCV (13.306)

 Usage:
   X = calc_intersect_3D_Lines2Point_finite(L,M)

   L, M - 6x1 Plücker Lines
   X    - 4x1 homogeneous 3D point closest to L and M

 Wolfgang Förstner 7/2017
 wfoerstn@uni-bonn.de 

 See also calc_intersect_3D_Lines2Point, calc_join_3D_Lines2Plane,
 calc_join_3D_Lines2Plane_finite

0001 %% intersection point X of two 3D-lines L and M
0002 % precondition: L and M are not parallel, otherwise X = [0 0 0 0]'
0003 %
0004 % see PCV (13.306)
0005 %
0006 % Usage:
0007 %   X = calc_intersect_3D_Lines2Point_finite(L,M)
0008 %
0009 %   L, M - 6x1 Plücker Lines
0010 %   X    - 4x1 homogeneous 3D point closest to L and M
0011 %
0012 % Wolfgang Förstner 7/2017
0013 % wfoerstn@uni-bonn.de
0014 %
0015 % See also calc_intersect_3D_Lines2Point, calc_join_3D_Lines2Plane,
0016 % calc_join_3D_Lines2Plane_finite
0017 
0018 function X = calc_intersect_3D_Lines2Point_finite(L,M)
0019 
0020 % take safest point at finity
0021 GL = calc_Gamma_reduced(L);
0022 GM = calc_Gamma_reduced(M);
0023 [~,il] = max(abs(GL(:,4)));
0024 [~,ml] = max(abs(GM(:,4)));
0025 P = GL(il,1:3)'/GL(il,4);
0026 Q = GM(ml,1:3)'/GM(ml,4);
0027 
0028 % directions
0029 R = L(1:3);
0030 S = M(1:3);
0031 
0032 % solving linear equation system
0033 RS = [R,-S];
0034 Nm = RS'*RS;
0035 
0036 if abs(det(Nm)) > 10^(-10)
0037     % R not parallel to S: finite point
0038     lm = (Nm)\RS'*(Q-P);
0039     % take midpoint
0040     X = [(P+lm(1)*R+Q+lm(2)*S)/2;1];
0041 else
0042     % point indefinite
0043     X = zeros(4,1);
0044 end