gtMathsRod2OriMat.m

function g = gtMathsRod2OriMat(r)
% GTMATHSROD2ROTMAT  Convert Rodrigues vectors to orientation matrices.
%
%     g = gtMathsRod2OriMat(r)
%     ---------------------------------------------------------------------
%
%     INPUT:
%       r = <double>  Rodrigues vectors, as column vectors (3xN array).
%
%     OUTPUT:
%       g = <double>  Orientation matrices (3x3xN array).
%                     See Randle & Engler, 2000 for more details about g.
%
%     Version 002 11-04-2013 by YGuilhem
%       Refactoring for optimized performance (inspired by Peter's functions)
%
%     Version 001 15-06-2012 by YGuilhem

% If one single entry
if (numel(r) == 3)
    rs1 = r(1)^2;
    rs2 = r(2)^2;
    rs3 = r(3)^2; 

    r1r2 = r(1)*r(2);
    r1r3 = r(1)*r(3);
    r2r3 = r(2)*r(3);

    g = [ rs1 - rs2 - rs3 + 1, 2*(r1r2 + r(3)),     2*(r1r3 - r(2))      ; ...
          2*(r1r2 - r(3)),     rs2 - rs1 - rs3 + 1, 2*(r2r3 + r(1))      ; ...
          2*(r1r3 + r(2)),     2*(r2r3 - r(1)),     rs3 - rs1 - rs2 + 1] ;
 
    g = g/(1 + rs1 + rs2 + rs3);
else
    % Otherwise vectorize computation
    if (size(r, 1) ~= 3)
        gtError('gtMathsRod2OriMat:wrong_input_size', ...
                'Input array r should be sized 3xN!');
    end
    N = size(r, 2);
    g = zeros(9, N, class(r));

    % Diagonals (1,1) (2,2) (3,3)
    r2 = r.^2;
    g(1, :) = r2(1, :);
    g(5, :) = r2(2, :);
    g(9, :) = r2(3, :);

    % Off diagonals (1,2) (2,1)
    rr = r(1, :).*r(2, :);
    g(2, :) = rr - r(3, :);
    g(4, :) = rr + r(3, :);

    % Off diagonals (1,3) (3,1)
    rr = r(1, :).*r(3, :);
    g(3, :) = rr + r(2, :);
    g(7, :) = rr - r(2, :);

    % Off diagonals (2,3) (3,2)
    rr = r(2, :).*r(3, :);
    g(6, :) = rr - r(1, :);
    g(8, :) = rr + r(1, :);

    % Multiply all by two
    g = g*2;

    % Add term to diagonals
    r2s = sum(r2, 1);
    g(1, :) = g(1, :) - r2s + 1;
    g(5, :) = g(5, :) - r2s + 1;
    g(9, :) = g(9, :) - r2s + 1;

    % Divide all
    r2s = 1 + r2s;
    g = g./r2s(ones(9, 1), :);
    
    % Reshape for output
    g = reshape(g, 3, 3, []);
end

end % of function