% QUATERNIONROTATE - Rotates a 3D vector by a quaternion 
%
% Usage:   vnew = quaternionrotate(Q, v)
%
% Arguments: Q - a quaternion in the form [w xi yj zk]
%            v - a vector to rotate, either an inhomogeneous 3-vector or a
%            homogeneous 4-vector
% Returns:   vnew - rotated vector
%
% See also MATRIX2QUATERNION, QUATERNION2MATRIX, NEWQUATERNION

% Copyright (c) 2008 Peter Kovesi
% peterkovesi.com
% 
% 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.

% Code forms the equivalent 3x3 rotation matrix from the quaternion and
% applies it to a vector 
%
% Note that Qw^2 + Qi^2 + Qj^2 + Qk^2 = 1
% So the top-left entry of the rotation matrix of
%   Qw^2 + Qi^2 - Qj^2 - Qk^2
% can be rewritten as
%   Qw^2 + Qi^2 + Qj^2 + Qk^2 - 2Qj^2 - 2Qk^2
% = 1 - 2Qj^2 - 2Qk^2
%
% Similar optimization applies to the other diagonal elements

function vnew = quaternionrotate(Q, v)

    % Copy v to vnew to allocate space.  If v is a 4 element homogeneous
    % vector this also sets the homogeneous scale factor of vnew
    vnew = v;  
    
    Qw = Q(1);  Qi = Q(2);  Qj = Q(3);  Qk = Q(4);
    
    t2 =   Qw*Qi;
    t3 =   Qw*Qj;
    t4 =   Qw*Qk;
    t5 =  -Qi*Qi;
    t6 =   Qi*Qj;
    t7 =   Qi*Qk;
    t8 =  -Qj*Qj;
    t9 =   Qj*Qk;
    t10 = -Qk*Qk;
    vnew(1) = 2*( (t8 + t10)*v(1) + (t6 -  t4)*v(2) + (t3 + t7)*v(3) ) + v(1);
    vnew(2) = 2*( (t4 +  t6)*v(1) + (t5 + t10)*v(2) + (t9 - t2)*v(3) ) + v(2);
    vnew(3) = 2*( (t7 -  t3)*v(1) + (t2 +  t9)*v(2) + (t5 + t8)*v(3) ) + v(3);