octave_packages/linear-algebra-2.2.0/rotv.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{r} = } rotv ( v, ang )
  18 ## @cindex
  19 ## The functionrotv calculates a Matrix of rotation about @var{v} w/ angle |v|
  20 ## r = rotv(v [,ang])
  21 ##
  22 ## Returns the rotation matrix w/ axis v, and angle, in radians, norm(v) or
  23 ## ang (if present).
  24 ##
  25 ## rotv(v) == w'*w + cos(a) * (eye(3)-w'*w) - sin(a) * crossmat(w)
  26 ##
  27 ## where a = norm (v) and w = v/a.
  28 ##
  29 ## v and ang may be vertically stacked : If 'v' is 2x3, then
  30 ## rotv( v ) == [rotv(v(1,:)); rotv(v(2,:))]
  31 ##
  32 ## @example
  33 ##
  34 ## @end example
  35 ## @seealso{rotparams, rota, rot}
  36 ## @end deftypefn
  37
  38 function r = rotv(v ,ang)
  39
  40   if nargin > 1
  41     v = v.*((ang(:)./sqrt(sum(v'.^2))')*ones(1,3));
  42   end
  43   ## For checking only
  44   ## v00 = v ;
  45   ## static toto = floor(rand(1)*100) ;
  46   ## toto
  47   a = sqrt(sum(v'.^2))' ;
  48   oka = find(a!=0);
  49   if all(size(oka)),
  50     v(oka,:) = v(oka,:)./(a(oka)*ones(1,3)) ;
  51   end
  52   ## ca = cos(a);
  53   ## sa = sin(a);
  54
  55   N = size(v,1) ; N3 = 3*N ;
  56   r = (reshape( v', N3,1 )*ones(1,3)).*kron(v,ones(3,1)) ;
  57   r += kron(cos(a),ones(3,3)) .* (kron(ones(N,1),eye(3))-r) ;
  58
  59   ## kron(cos(a),ones(3,3)) .* (kron(ones(N,1),eye(3))-r0)
  60   ## cos(a)
  61
  62   tmp = zeros(N3,3) ;
  63   tmp( 2:3:N3,1 ) =  v(:,3) ;
  64   tmp( 1:3:N3,2 ) = -v(:,3) ;
  65   tmp( 3:3:N3,1 ) = -v(:,2) ;
  66   tmp( 1:3:N3,3 ) =  v(:,2) ;
  67   tmp( 2:3:N3,3 ) = -v(:,1) ;
  68   tmp( 3:3:N3,2 ) =  v(:,1) ;
  69   ## keyboard
  70   r -= kron(sin(a),ones(3)) .* tmp ;
  71
  72 endfunction
  73
  74 ## For checking only
  75 ## r2 = zeros(N3,3) ;
  76 ## for i=1:size(v,1),
  77 ##   v0 = v00(i,:);
  78 ##   t = norm(v0);
  79 ##   if t, v0 = v0/t; end;
  80 ##   r2(3*i-2:3*i,:) = v0'*v0 + cos(t)*(eye(3)-v0'*v0) + -sin(t)*[0, -v0(3), v0(2);v0(3), 0, -v0(1);-v0(2), v0(1), 0];
  81 ## end
  82 ## max(abs(r2(:)-r(:)))