vector_plane_intersect(Vo,V,C)

function P = vector_plane_intersect(Vo,V,C)
%% A function to calculation the intersection of a vector and a plane
%
% Val Schmidt
% Center for Coastal and Ocean Mapping
% University of New Hampshire
% 2012
%
% input:
%   Vo: 1x3 vector or nx3 matrix of vector starting locations. 
%   V:  nx3 matrix of vector directions (need not be normalized)
%   C:  1x3 vector of plane coefficients z = C(1)*x+C(2)*y+C(3)
%
% output:
%   P:  nx3 matrix of intersection points (or Inf if the vector is parallel
%       to the plane
% 
% Give a vector in 3-D space parameterized by a point and a pointing
% vector, and a list of coefficients that describe a plane,
% vector_plane_instsect calculates at what point the vector will intersect
% the plane. 

%%
% A line in 3 D space is parameterized by 
%
% x = xo + n*t;
% y = yo + n*t;
% z = zo + n*t;
% 
% A plane in 3D space is parameterized by 
%
% z = C(1)*x+C(2)*y+C(3)
%
% To find the point where the line intersects the plane we substitute the
% equations for x, y and z into the plane equation above and solve for t.
% Then evaluate the x, y and z equations at that point. 

t = (Vo(:,3) - C(1).*Vo(:,1) - C(2).*Vo(:,2) - C(3)) ./ ...
    (C(1).*V(:,1)+C(2).*V(:,2) - V(:,3));

% Vo might be a point of a set of points. If it's a point, replicate it. 
if length(Vo) == 3
    Vo = repmat(Vo(:)',size(V,1),1);
end

P = Vo+V.*repmat(t(:),1,3);