function R = rotationMatrix(phi,theta,psi)
% function R = rotationMatrix(phi,theta,psi)
%
% Three dimensional rotation matrix
%
% INPUT:
%  φ    θ    ψ
% phi theta psi
% rotates about axis:
%  x    y    z
%
% OUTPUT:
% rotation matrix R
% x_prime = R*x;
%
% http://mathworld.wolfram.com/RotationMatrix.html
% http://en.wikipedia.org/wiki/Rotation_matrix
%
% T.Hilmer, UH
% 2010.03.15  version 1

% If this isn't efficient enough, write something using quaternions.
% Instructions for calculating (and translating between) the different
% rotation representations are here:
% http://en.wikipedia.org/wiki/Rotation_representation_(mathematics)

% The axes of the rotation depend on the specific convention being used.
% For the x-convention the rotation are about the X, Y and Z axes with
% angles φ, θ and ψ, the individual matrices are as follows:

% These matrices represent counterclockwise rotations of an object relative
% to fixed coordinate axes, by an angle of θ. The direction of the rotation
% is as follows: Rx rotates the y-axis towards the z-axis, Ry rotates the
% z-axis towards the x-axis, and Rz rotates the x-axis towards the y-axis.

% I think the only difference between rotating the axis vs rotating the
% object is sin, -sin ....

Rx = [ 1 0 0; 0 cos(phi) -sin(phi) ; 0 sin(phi) cos(phi) ];

Ry = [ cos(theta) 0 sin(theta); 0 1 0; -sin(theta) 0 cos(theta) ];

Rz = [ cos(psi) -sin(psi) 0; sin(psi) cos(psi) 0; 0 0 1];

% Euler's rotation theorem:
% 3 successive rotations can be achieved with a single rotation:
R = Rz*Ry*Rx; % ORDER OF ROTATIONS MATTERS !
% R2 = Rx*Ry*Rz;
% Which describes a single axis of rotation (vector) and rotation angle   

% Length-preserving transformations in R3 preserve the dot product, and
% thus the angle between vectors.  Preserving length is equivalent to
% preserving angle.

% R3'*R == I
% det(R) == 1

% There's talk about numerical accuracy when doing rotations; I'm not sure
% if this is a concern:

dI = R'*R - eye(size(R));

if max(abs(dI(:))) > 1E-9 || abs(det(R)-1) > 1E-9
    error('rotation matrix not orthogonal')
end