octave_packages/quaternion-2.0.0/rot2q.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{q} =} rot2q (@var{axis}, @var{angle})
  19 ## Create unit quaternion @var{q} which describes a rotation of
  20 ## @var{angle} radians about the vector @var{axis}.  This function uses
  21 ## the active convention where the vector @var{axis} is rotated by @var{angle}
  22 ## radians.  If the coordinate frame should be rotated by @var{angle}
  23 ## radians, also called the passive convention, this is equivalent
  24 ## to rotating the @var{axis} by @var{-angle} radians.
  25 ##
  26 ## @strong{Inputs}
  27 ## @table @var
  28 ## @item axis
  29 ## Vector @code{[x, y, z]} describing the axis of rotation.
  30 ## @item angle
  31 ## Rotation angle in radians.  The positive direction is
  32 ## determined by the right-hand rule applied to @var{axis}.
  33 ## @end table
  34 ##
  35 ## @strong{Outputs}
  36 ## @table @var
  37 ## @item q
  38 ## Unit quaternion describing the rotation.
  39 ## @end table
  40 ##
  41 ## @strong{Example}
  42 ## @example
  43 ## @group
  44 ## octave:1> axis = [0, 0, 1];
  45 ## octave:2> angle = pi/4;
  46 ## octave:3> q = rot2q (axis, angle)
  47 ## q = 0.9239 + 0i + 0j + 0.3827k
  48 ## octave:4> v = quaternion (1, 1, 0)
  49 ## v = 0 + 1i + 1j + 0k
  50 ## octave:5> vr = q * v * conj (q)
  51 ## vr = 0 + 0i + 1.414j + 0k
  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 q = rot2q (vv, theta)
  64
  65   if (nargin != 2 || nargout != 1)
  66     print_usage ();
  67   endif
  68
  69   if (! isvector (vv) || length (vv) != 3)
  70     error ("vv must be a length three vector");
  71   endif
  72
  73   if (! isscalar (theta))
  74     error ("theta must be a scalar");
  75   endif
  76
  77   if (norm (vv) == 0)
  78     error ("quaternion: vv is zero");
  79   endif
  80
  81   if (abs (norm (vv) - 1) > 1e-12)
  82     warning ("quaternion: ||vv|| != 1, normalizing")
  83     vv = vv / norm (vv);
  84   endif
  85
  86   if (abs (theta) > 2*pi)
  87     warning ("quaternion: |theta| > 2 pi, normalizing")
  88     theta = rem (theta, 2*pi);
  89   endif
  90
  91   vv = vv * sin (theta / 2);
  92   d = cos (theta / 2);
  93   q = quaternion (d, vv(1), vv(2), vv(3));
  94
  95 endfunction