gtMathsAngleAxis2OriMat.m

function g = gtMathsAngleAxis2OriMat(theta, r)
% GTMATHSANGLEAXIS2ORIMAT  Convert angle/axis rotations to orientation matrices.
%
%     g = gtMathsAngleAxis2OriMat(theta, r)
%     ---------------------------------------------------------------------
%
%     INPUT:
%       theta = <double>  Rotation angles, in degrees (1xN array).
%       r     = <double>  Rotation axes, as column vectors (3xN array).
%
%     OUTPUT:
%       g     = <double>  Orientation matrices (3x3xN array).
%
%     Version 001 27-06-2013 by YGuilhem

% Check size of input
if (size(theta, 1) ~= 1)
    gtError('gtMathsAngleAxis2OriMat:wrong_input_angles', ...
        'Rotation angles should be sized as a 1xN array!');
elseif (size(r, 1) ~= 3)
    gtError('gtMathsAngleAxis2OriMat:wrong_input_axes', ...
        'Rotation axes should be sized as a 3xN array (column vectors)!');
elseif (size(theta, 2) ~= size(r, 2))
    gtError('gtMathsAngleAxis2OriMat:wrong_input', ...
        'There should be as many rotation angles (1xN array) as rotation axes (3xN array)!');
end

% Normalize rotation axis vectors
n = sqrt(sum(r.*r, 1));
r = r ./ n([1 1 1], :);

% Compute r1, r2, r3 and r^2
%r1 = r(1, :);
%r2 = r(2, :);
%r3 = r(3, :);
rr = r.^2;

% Conpute cosines and sines
c = cosd(theta);
s = sind(theta);

% Diagonal components
g(1, :) = rr(1, :) .* (1 - c) + c; % g11
g(5, :) = rr(2, :) .* (1 - c) + c; % g22
g(9, :) = rr(3, :) .* (1 - c) + c; % g33

% Anti diagonal components
g(4, :) = r(1, :) .* r(2, :) .* (1 - c) - r(3, :) .* s; % g12
g(2, :) = r(2, :) .* r(1, :) .* (1 - c) + r(3, :) .* s; % g21

g(7, :) = r(1, :) .* r(3, :) .* (1 - c) + r(2, :) .* s; % g13
g(3, :) = r(3, :) .* r(1, :) .* (1 - c) - r(2, :) .* s; % g31

g(8, :) = r(2, :) .* r(3, :) .* (1 - c) - r(1, :) .* s; % g23
g(6, :) = r(3, :) .* r(2, :) .* (1 - c) + r(1, :) .* s; % g32

g = reshape(g, 3, 3, []);

end % end of function