% 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];