## Copyright (C) 1998, 1999, 2000, 2002, 2005, 2006, 2007
##               Auburn University.  All rights reserved.
##
## This program is free software; you can redistribute it and/or modify it
## under the terms of the GNU General Public License as published by
## the Free Software Foundation; either version 3 of the License, or (at
## your option) any later version.
##
## This program is distributed in the hope that it will be useful, but
## WITHOUT ANY WARRANTY; without even the implied warranty of
## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
## General Public License for more details.
##
## You should have received a copy of the GNU General Public License
## along with this program; see the file COPYING.  If not, see
## <http://www.gnu.org/licenses/>.

## -*- texinfo -*-
## @deftypefn {Function File} {[@var{a}, @var{b}, @var{c}, @var{d}] =} quaternion (w)
## @deftypefnx {Function File} {[@var{vv}, @var{theta}] =} quaternion (w)
## @deftypefnx {Function File} {@var{w} =} quaternion (@var{a}, @var{b}, @var{c}, @var{d})
## @deftypefnx {Function File} {@var{w} =} quaternion (@var{vv}, @var{theta})
## Construct or extract a quaternion
##
## @example
## w = a*i + b*j + c*k + d
## @end example
##
## @noindent
## from given data.
## @end deftypefn

## Author: A. S. Hodel <a.s.hodel@eng.auburn.edu>
## Adapted-By: jwe

function [a, b, c, d] = quaternion (w, x, y, z)

  switch (nargin)
    case(1)
      if (! (isvector (w) && length (w) == 4))
        error ("input vector must be of length 4)");
      endif
      ## extract data
      switch (nargout)
        case(4)
          a = w(1);
          b = w(2);
          c = w(3);
          d = w(4);

        case(2)
          if (abs (norm (w) - 1) > 1e-12)
            warning ("quaternion: ||w||=%e, setting=1 for vv, theta", norm(w));
            w = w/norm(w);
          endif
          [a, b, c, d] = quaternion (w);
          theta = acos (d) * 2;
          if (abs (theta) > pi)
            theta = theta - sign (theta) * pi;
          endif
          sin_th_2 = norm ([a, b, c]);

          if (sin_th_2 != 0)
            vv = [a, b, c] / sin_th_2;
          else
            vv = [a, b, c];
          endif
          a = vv;
          b = theta;
        otherwise
          print_usage ();
      endswitch

  case(2)
    if (nargout > 1)
      print_usage ();
    endif
    vv = w;
    theta = x;

    if (! isvector (vv) || length (vv) != 3)
      error ("vv must be a length three vector");
    elseif (! isscalar (theta))
      error ("theta must be a scalar");
    elseif (norm (vv) == 0)
      error ("quaternion: vv is zero");
    elseif (abs (norm (vv) - 1) > 1e-12)
      warning ("quaternion: ||vv|| != 1, normalizing")
      vv = vv / norm (vv);
    endif

    if (abs (theta) > 2*pi)
      warning ("quaternion: |theta| > 2 pi, normalizing")
      theta = rem (theta, 2*pi);
    endif
    vv = vv * sin (theta / 2);
    d = cos (theta / 2);
    a = quaternion (vv(1), vv(2), vv(3), d);

  case(4)
    if (nargout > 1)
      print_usage ();
    endif
    if (! (isscalar (w) && isscalar (x) && isscalar (y) && isscalar (z)))
      error ("input values must be scalars");
    endif
    a = [w, x, y, z];

  otherwise
    print_usage ();

  endswitch

endfunction