V_ROTATION Encode and decode rotation matrices (1) r=v_rotation(x,y,angle) creates a matrix that rotates vectors in the plane containing x and y. A small positive angle moves x towards y. (2) [x,y,ang]=v_rotation(r) is the inverse of (1) above. The input argument r is assumed to be a rotation matrix: little error checking is done. (3) r=v_rotation(axis,angle)=v_rotation(axis*ang) only works for a 3-dimensional vector axis and creates a rotation of angle radians around axis. (4) [axis,ang]=v_rotation(r) is the inverse of (3) above for a 3x3 input matrix r
0001 function [r,p,q]=v_rotation(x,y,z) 0002 %V_ROTATION Encode and decode rotation matrices 0003 % (1) r=v_rotation(x,y,angle) creates a matrix that rotates vectors in the 0004 % plane containing x and y. A small positive angle moves x towards y. 0005 % (2) [x,y,ang]=v_rotation(r) is the inverse of (1) above. The input argument r 0006 % is assumed to be a rotation matrix: little error checking is done. 0007 % (3) r=v_rotation(axis,angle)=v_rotation(axis*ang) only works for a 3-dimensional 0008 % vector axis and creates a rotation of angle radians around axis. 0009 % (4) [axis,ang]=v_rotation(r) is the inverse of (3) above for a 3x3 input matrix r 0010 0011 % Copyright (C) Mike Brookes 1998 0012 % Version: $Id: v_rotation.m 10865 2018-09-21 17:22:45Z dmb $ 0013 % 0014 % VOICEBOX is a MATLAB toolbox for speech processing. 0015 % Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html 0016 % 0017 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0018 % This program is free software; you can redistribute it and/or modify 0019 % it under the terms of the GNU General Public License as published by 0020 % the Free Software Foundation; either version 2 of the License, or 0021 % (at your option) any later version. 0022 % 0023 % This program is distributed in the hope that it will be useful, 0024 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0025 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0026 % GNU General Public License for more details. 0027 % 0028 % You can obtain a copy of the GNU General Public License from 0029 % http://www.gnu.org/copyleft/gpl.html or by writing to 0030 % Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA. 0031 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0032 0033 l=length(x(:)); 0034 if nargin>2 0035 x=x(:); x=x/sqrt(x'*x); 0036 y=y(:); y=y-y'*x*x; y=y/sqrt(y'*y); 0037 r=eye(l)+(cos(z)-1)*(x*x'+y*y')+sin(z)*(y*x'-x*y'); 0038 elseif l==3 0039 x=x(:); 0040 lx=sqrt(x'*x); 0041 if nargin==1 0042 y=lx; 0043 end 0044 x=x/lx; 0045 xx=x*x'; 0046 s=zeros(3,3); 0047 s([6 7 2])=x; 0048 s([8 3 4])=-x; 0049 r=xx+cos(y)*(eye(3)-xx)+sin(y)*s; 0050 else 0051 [v,d]=eig(x); 0052 [e,j]=sort(real(diag(d))); 0053 j1=j(1); 0054 an=angle(d(j1,j1)); 0055 q=an; 0056 sq=sqrt(2); 0057 r=imag(v(:,j1))*sq; 0058 if r==0 0059 p=v(:,j1); 0060 r=v(:,j(2)); 0061 else 0062 p=real(v(:,j1))*sq; 0063 end 0064 if nargout==2 0065 r=cross(r,p); 0066 p=an; 0067 end 0068 end