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
34function [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
73endfunction
74