function d = cv_dist_pt_plane(point,plane)
%
% cv_dist_pt_plane - distance from point to plane
% Call: d = cv_dist_pt_plane(point,plane);
% On input:
%     point: 3D point
%     plane: plane given by 3 points (matrix is 3x3, one point each row)
% On output:
%     d: distance from point to plane
% Author:
%     Tom Henderson
%     7 January 2000
% Custom Functions Used:
%     none
% Method:
%     least square regression solution for point in plane that minimizes the
% distance from the point to the plane
% Testing:
% > d(1,1) = cv_dist_pt_plane([0 0 0],[0,0,0;0 1 0;0 0 1]);
% > d(1,2) = cv_dist_pt_plane([1 0 0],[0,0,0;0 1 0;0 0 1]);
% > d(1,3) = cv_dist_pt_plane([0 0 0],[1,0,0;0 1 0;0 0 1]);
% > d(1,4) = cv_dist_pt_plane([0 0 0],[2,0,0;2 1 0;2 0 1]);
% > d
%
% d =
%
%         0    1.0000    0.5774    2.0000
%
pt1 = plane(1,:);
pt2 = plane(2,:);
pt3 = plane(3,:);
b = zeros(2,1);
A = zeros(2,2);

b(1) = point(1)*(pt2(1)-pt1(1))-pt1(1)*(pt2(1)-pt1(1)) ...
   + point(2)*(pt2(2)-pt1(2))-pt1(2)*(pt2(2)-pt1(2)) ...
   + point(3)*(pt2(3)-pt1(3))-pt1(3)*(pt2(3)-pt1(3));
b(2) = point(1)*(pt3(1)-pt1(1))-pt1(1)*(pt3(1)-pt1(1)) ...
   + point(2)*(pt3(2)-pt1(2))-pt1(2)*(pt3(2)-pt1(2)) ...
   + point(3)*(pt3(3)-pt1(3))-pt1(3)*(pt3(3)-pt1(3));

A(1,1) = (pt2(1)-pt1(1))^2 + (pt2(2)-pt1(2))^2 + (pt2(3)-pt1(3))^2;
A(1,2) = (pt3(1)-pt1(1))*(pt2(1)-pt1(1)) ...
   + (pt3(2)-pt1(2))*(pt2(2)-pt1(2)) ...
   + (pt3(3)-pt1(3))*(pt2(3)-pt1(3));
A(2,1) = A(1,2);
A(2,2) = (pt3(1)-pt1(1))^2 + (pt3(2)-pt1(2))^2 + (pt3(3)-pt1(3))^2;
t = A\b;
closest_pt = pt1 + t(1)*(pt2-pt1) + t(2)*(pt3-pt1);
d = cv_dist_pt_pt(closest_pt, point);
