octave_packages/quaternion-2.0.0/q2rot.m

   1 ## Copyright (C) 1998, 1999, 2000, 2002, 2005, 2006, 2007 Auburn University
   2 ## Copyright (C) 2010, 2011   Lukas F. Reichlin
   3 ##
   4 ## This program is free software: you can redistribute it and/or modify
   5 ## it under the terms of the GNU General Public License as published by
   6 ## the Free Software Foundation, either version 3 of the License, or
   7 ## (at your option) any later version.
   8 ##
   9 ## This program is distributed in the hope that it will be useful,
  10 ## but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 ## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12 ## GNU General Public License for more details.
  13 ##
  14 ## You should have received a copy of the GNU General Public License
  15 ## along with this program.  If not, see <http://www.gnu.org/licenses/>.
  16
  17 ## -*- texinfo -*-
  18 ## @deftypefn {Function File} {[@var{axis}, @var{angle}] =} q2rot (@var{q})
  19 ## Extract vector/angle form of a unit quaternion @var{q}.
  20 ##
  21 ## @strong{Inputs}
  22 ## @table @var
  23 ## @item q
  24 ## Unit quaternion describing the rotation.
  25 ## @end table
  26 ##
  27 ## @strong{Outputs}
  28 ## @table @var
  29 ## @item axis
  30 ## Eigenaxis as a 3-d unit vector @code{[x, y, z]}.
  31 ## @item angle
  32 ## Rotation angle in radians.  The positive direction is
  33 ## determined by the right-hand rule applied to @var{axis}.
  34 ## @end table
  35 ##
  36 ## @strong{Example}
  37 ## @example
  38 ## @group
  39 ## octave:1> axis = [0, 0, 1]
  40 ## axis =
  41 ##    0   0   1
  42 ## octave:2> angle = pi/4
  43 ## angle =  0.78540
  44 ## octave:3> q = rot2q (axis, angle)
  45 ## q = 0.9239 + 0i + 0j + 0.3827k
  46 ## octave:4> [vv, th] = q2rot (q)
  47 ## vv =
  48 ##    0   0   1
  49 ## th =  0.78540
  50 ## octave:5> theta = th*180/pi
  51 ## theta =  45.000
  52 ## octave:6>
  53 ## @end group
  54 ## @end example
  55 ##
  56 ## @end deftypefn
  57
  58 ## Adapted from: quaternion by A. S. Hodel <a.s.hodel@eng.auburn.edu>
  59 ## Author: Lukas Reichlin <lukas.reichlin@gmail.com>
  60 ## Created: May 2010
  61 ## Version: 0.1
  62
  63 function [vv, theta] = q2rot (q)
  64
  65   if (nargin != 1 || nargout != 2)
  66     print_usage ();
  67   endif
  68
  69   if (! isa (q, "quaternion") || ! isscalar (q.w))
  70     error ("q2rot: require scalar quaternion as input");
  71   endif
  72
  73   if (abs (norm (q) - 1) > 1e-12)
  74     warning ("q2rot: ||q||=%e, setting=1 for vv, theta", norm (q));
  75     q = unit (q);
  76   endif
  77
  78   s = q.s;
  79   vv = [q.x, q.y, q.z];
  80
  81   theta = acos (s) * 2;
  82
  83   if (abs (theta) > pi)
  84     theta = theta - sign (theta) * pi;
  85   endif
  86
  87   sin_th_2 = norm (vv);
  88
  89   if (sin_th_2 != 0)
  90     vv ./= sin_th_2;
  91   endif
  92
  93 endfunction