function pln = fitplane(r)

% Input r is an nx3 array of vectors stacked vertically.
% Script computes best fit plane to the set of vectors.

% Reduce to 3 equations, 3 unknowns and solve via Cramer's rule.

sr = size(r);
if sr(1) < 3
  fprintf('Cannot form plane\n');
  pln = [1000 1000 1000];
  return;
end;

%if sr(1) == 3
%  fprintf('Warning only 3 points used to fit plane.\n');
%end;

a1 = sum(r(:,1).^2);
b1 = sum(r(:,1).*r(:,2));
c1 = sum(r(:,1));
a2 = sum(r(:,1).*r(:,2));
b2 = sum(r(:,2).^2);
c2 = sum(r(:,2));
a3 = sum(r(:,1));
b3 = sum(r(:,2));
c3 = length(r(:,1));

d1 = sum(r(:,1).*r(:,3)); 
d2 = sum(r(:,2).*r(:,3));
d3 = sum(r(:,3));

D = det([a1 b1 c1; a2 b2 c2; a3 b3 c3]);

a = det([d1 b1 c1; d2 b2 c2; d3 b3 c3])/D;
b = det([a1 d1 c1; a2 d2 c2; a3 d3 c3])/D;
c = det([a1 b1 d1; a2 b2 d2; a3 b3 d3])/D;

pln = [a b c];  

% z = ax + by + c is the equation of the plane.

