function q=dcm2q(R)
% DCM2Q(R) converts direction cosine matrices into quaternions.
%
%     The resultant quaternion(s) will perform the equivalent vector
%     transformation as the input DCM(s), i.e.:
%
%       qconj(Q)*V*Q = R*V
%
%     where R is the DCM, V is a vector, and Q is the quaternion.  Note that
%     for purposes of quaternion-vector multiplication, a vector is treated
%     as a quaterion with a scalar element of zero.
%
%     If the input is a 3x3xN array, the output will be a vector of
%     quaternions where input direction cosine matrix R(:,:,k) corresponds
%     to the output quaternion Q(k,:).
%
% See also Q2DCM.

% Release: $Name: quaternions-1_2_2 $
% $Revision: 1.9 $
% $Date: 2002/01/21 06:46:20 $
 
% Copyright (C) 2000-2002, Jay A. St. Pierre.  All rights reserved.
% Mdified 2007, Heinz-Peter Brunke and Lars Tøffner-Clausen
%      to numerically stable method given by Shepperd,
%      "Quanternion from rotation matrix". 
%      However, the definition of DCM differs between Shepperd and
%      St. Pierre (one is the others inverse), hence the quaternion
%      has been conjugated compared to Shepperd's original description.


if nargin~=1
  error('dcm2q() requires one input argument');
else
  size_R=size(R);
  if ( size_R(1)~=3 | size_R(2)~=3 | length(size_R)>3 )
    error(['Invalid input: must be a 3x3xN array'])
  end
end

% Allocate output variable
q = zeros(size(R,3), 4);

% Create Nx3 array of diagonals
diags = [squeeze(R(1,1,:)) squeeze(R(2,2,:)) squeeze(R(3,3,:))];

% Find largest values in each of N diagonals and traces
[major, par] = max([diags sum(diags,2)], [], 2);

% For all DCM with trace being larger
i = find(par == 4);
if(numel(i) > 0)
  q(i,4) = sqrt(1 + major(i)) * 0.5;
  r = 0.25 ./ q(i,4);
  q(i,2) = squeeze(R(3,1,i) - R(1,3,i)) .* r;
  q(i,1) = squeeze(R(2,3,i) - R(3,2,i)) .* r;
  q(i,3) = squeeze(R(1,2,i) - R(2,1,i)) .* r;
end%if

% For all DCM with upper-left being larger
i = find(par == 1);
if(numel(i) > 0)
  q(i,1) = sqrt(1 + R(1,1,i) - R(2,2,i) - R(3,3,i)) * 0.5;
  r = 0.25 ./ q(i,1);
  q(i,3) = squeeze(R(3,1,i) + R(1,3,i)) .* r;
  q(i,4) = squeeze(R(2,3,i) - R(3,2,i)) .* r;
  q(i,2) = squeeze(R(1,2,i) + R(2,1,i)) .* r;
end%if

% For all DCM with middle being larger
i = find(par == 2);
if(numel(i) > 0)
  q(i,2) = sqrt(1 - R(1,1,i) + R(2,2,i) - R(3,3,i)) * 0.5;
  r = 0.25 ./ q(i,2);
  q(i,4) = squeeze(R(3,1,i) - R(1,3,i)) .* r;
  q(i,3) = squeeze(R(2,3,i) + R(3,2,i)) .* r;
  q(i,1) = squeeze(R(1,2,i) + R(2,1,i)) .* r;
end%if

% For all DCM with lower-right being larger
i = find(par == 3);
if(numel(i) > 0)
  q(i,3) = sqrt(1 - R(1,1,i) - R(2,2,i) + R(3,3,i)) * 0.5;
  r = 0.25 ./ q(i,3);
  q(i,1) = squeeze(R(3,1,i) + R(1,3,i)) .* r;
  q(i,2) = squeeze(R(2,3,i) + R(3,2,i)) .* r;
  q(i,4) = squeeze(R(1,2,i) - R(2,1,i)) .* r;
end%if
