gtLinePlaneIntersection.m

function isec=gtLinePlaneIntersection(l,p)
%
% FUNCTION is=gtLinePlaneIntersection(l,p)
%
% Computes the intersection of a line with several planes.
%
% INPUT:  l:  line parameters   (1x6) [l0x l0y l0z ldirx ldiry ldirz]
%         p:  planes parameters (nx6) [p0x p0y p0z pnormx pnormy pnormz]
%    
%   ,where ldir and pnorm do not need to be normalized.
%
% OUTPUT: isec: intersection points respectively (nx3) [X Y Z]
%


% Equations:
% t  = (plane_0-line_0)*plane_dir/(plane_dir*line_dir)
% isec = line_0+line_dir*t

% For one plane only:
%  t=((p(1)-l(1))*p(4)+(p(2)-l(2))*p(5)+(p(3)-l(3))*p(6))/...
%	   (l(4)*p(4)+l(5)*p(5)+l(6)*p(6));
%  isec=l(1:3)+l(4:6)*t;
%

l0=repmat(l(1:3),size(p,1),1);
ld=repmat(l(4:6),size(p,1),1);

t=sum((p(:,1:3)-l0).*p(:,4:6),2)./(p(:,4:6)*l(4:6)');

isec=l0+ld.*[t t t];

end