octave_packages/linear-algebra-2.2.0/rotparams.m

   1 ## Copyright (C) 2002 Etienne Grossmann <etienne@egdn.net>
   2 ##
   3 ## This program is free software; you can redistribute it and/or modify it under
   4 ## the terms of the GNU General Public License as published by the Free Software
   5 ## Foundation; either version 3 of the License, or (at your option) any later
   6 ## version.
   7 ##
   8 ## This program is distributed in the hope that it will be useful, but WITHOUT
   9 ## ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  10 ## FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
  11 ## details.
  12 ##
  13 ## You should have received a copy of the GNU General Public License along with
  14 ## this program; if not, see <http://www.gnu.org/licenses/>.
  15
  16 ## -*- texinfo -*-
  17 ## @deftypefn{Function File} {[@var{vstacked}, @var{astacked}] =} rotparams (@var{rstacked})
  18 ## @cindex
  19 ##  The function w = rotparams (r)            - Inverse to rotv().
  20 ##  Using, @var{w}    = rotparams(@var{r})  is such that
  21 ##  rotv(w)*r' == eye(3).
  22 ##
  23 ##  If used as, [v,a]=rotparams(r) ,  idem, with v (1 x 3) s.t. w == a*v.
  24 ##
  25 ##     0 <= norm(w)==a <= pi
  26 ##
  27 ##     :-O !!  Does not check if 'r' is a rotation matrix.
  28 ##
  29 ##  Ignores matrices with zero rows or with NaNs. (returns 0 for them)
  30 ##
  31 ## @seealso{rotv}
  32 ## @end deftypefn
  33
  34 function [vstacked, astacked] = rotparams (rstacked)
  35
  36   N = size (rstacked,1) / 3;
  37
  38   ## ang = 0 ;
  39   ## if length(varargin),
  40   ##   if strcmp(varargin{1},'ang'),    ang = 1;  end
  41   ## end
  42   ok = all ( ! isnan (rstacked') ) & any ( rstacked' );
  43   ok = min ( reshape (ok,3,N) );
  44   ok = find (ok) ;
  45   ## keyboard
  46   vstacked = zeros (N,3);
  47   astacked = zeros (N,1);
  48   for j = ok,
  49     r = rstacked(3*j-2:3*j,:);
  50     [v,f] = eig (r);
  51     f = diag(f);
  52
  53     [m,i] = min (abs (real (f)-1));
  54     v = v(:,i);
  55
  56     w = null (v');
  57     u = w(:,1);
  58     a = u'*r*u;
  59     if a<1,
  60       a = real (acos (u'*r*u));
  61     else
  62       a = 0;
  63     endif
  64     ## Check orientation
  65     x=r*u;
  66     if v'*[0 -u(3) u(2); u(3) 0 -u(1);-u(2) u(1) 0]*x < 0,  v=-v; endif
  67
  68
  69     if nargout <= 1,   v = v*a; endif
  70     vstacked(j,:) = -v';
  71     astacked(j) = a;
  72   endfor
  73 endfunction