function [rmsd,angl,t,Q,newB] = viewpoint(A,B,translte)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [rmsd,angl,t,Q,newB] = viewpoint(A,B,translte)
%
% Given data points in the n columns of A and B,
% determine a 3x3 orthogonal matrix Q and a 3x1 vector t
% to minimize || B - Q A - t e^T ||_F
% where  e  is an nx1 vector of ones and "F" denotes
% the Frobenius norm.
% If translte=0, then t is constrained to be zero.
% The function returns
%   rmsd = || B - Q A - t e^T ||_F / sqrt(n)
%   angl = the Euler angles determined by Q
%          yaw   phi = angl(1);
%          pitch theta = angl(2);
%          roll  psi = angl(3);
%   t = the translation (0 if translte=0)
%   Q = the orthogonal matrix
%   newB = Q A + t e^T
%
%  viewpoint.m Dianne P. O'Leary   06/2004
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[m,n]=size(A);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% If translte, translate so that the centroids of A and B 
% are both at (0,0,0).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (translte)
   centroidA = (A*ones(n,1))/n;
   centroidB = (B*ones(n,1))/n;
   Asav = A;
   Bsav = B;
   B = B - centroidB*ones(1,n);
   A = A - centroidA*ones(1,n);
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the rotation matrix Q.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[U,s,V] = svd(B*A');

Q = U*V';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the translation vector t and the rotated matrix.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if (translte)
   t = centroidB - Q * centroidA;
   newB = Q*Asav + t*ones(1,n);
   B = Bsav;
else
   t = zeros(3,1);
   newB = Q * A;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the rmsd between the given and computed 
% coordinates.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

rmsd = norm(B - newB, 'fro')/sqrt(n);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Compute the Euler angles phi, theta, psi.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

angl = compute_angles_from_Q(Q);