Q2M

 Generate a rotation matrix from a quaternion xi+yj+zk+w,
 where Q = [x y z], and w = 1-x^2-y^2-z^2.
 See: http://skal.planet-d.net/demo/matrixfaq.htm
 _______________________________________________________________________
 Copyright (C) 2005 Wellcome Department of Imaging Neuroscience

0001 function M = Q2M(Q)
0002 % Generate a rotation matrix from a quaternion xi+yj+zk+w,
0003 % where Q = [x y z], and w = 1-x^2-y^2-z^2.
0004 % See: http://skal.planet-d.net/demo/matrixfaq.htm
0005 % _______________________________________________________________________
0006 % Copyright (C) 2005 Wellcome Department of Imaging Neuroscience
0007 
0008 %
0009 % $Id: Q2M.m 253 2005-10-13 15:31:34Z guillaume $
0010 
0011 
0012 Q = Q(1:3); % Assume rigid body
0013 w = sqrt(1 - sum(Q.^2));
0014 x = Q(1); y = Q(2); z = Q(3);
0015 if w<1e-7,
0016     w = 1/sqrt(x*x+y*y+z*z);
0017     x = x*w;
0018     y = y*w;
0019     z = z*w;
0020     w = 0;
0021 end;
0022 xx = x*x; yy = y*y; zz = z*z; ww = w*w;
0023 xy = x*y; xz = x*z; xw = x*w;
0024 yz = y*z; yw = y*w; zw = z*w;
0025 M = [...
0026 (xx-yy-zz+ww)      2*(xy-zw)      2*(xz+yw) 0
0027     2*(xy+zw) (-xx+yy-zz+ww)      2*(yz-xw) 0
0028     2*(xz-yw)      2*(yz+xw) (-xx-yy+zz+ww) 0
0029            0              0              0  1];
0030 return;
0031