% QUATERNION2MATRIX - Quaternion to a 4x4 homogeneous transformation matrix
%
%  Usage:  T = quaternion2matrix(Q)
%
%  Argument: Q - a quaternion in the form [w xi yj zk]
%  Returns:  T - 4x4 Homogeneous rotation matrix
% 
% See also MATRIX2QUATERNION, NEWQUATERNION, QUATERNIONROTATE

% Copyright (c) 2008 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% pk at csse uwa edu au
% http://www.csse.uwa.edu.au/
% 
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
% 
% The above copyright notice and this permission notice shall be included in 
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

function T = quaternion2matrix(Q)
    
    Q = Q/norm(Q); % Ensure Q has unit norm
    
    % Set up convenience variables
    w = Q(1); x = Q(2); y = Q(3); z = Q(4);
    w2 = w^2; x2 = x^2; y2 = y^2; z2 = z^2;
    xy = x*y; xz = x*z; yz = y*z;
    wx = w*x; wy = w*y; wz = w*z;
    
    T = [w2+x2-y2-z2 , 2*(xy - wz) , 2*(wy + xz) ,  0
         2*(wz + xy) , w2-x2+y2-z2 , 2*(yz - wx) ,  0
         2*(xz - wy) , 2*(wx + yz) , w2-x2-y2+z2 ,  0
              0      ,       0     ,       0     ,  1];