function [Q,u1,U] = Q_from_p_and_theta(p,theta)
% Find a transformation matrix that represents a rotation of the coordinate
% frame (the basis) by an angle theta around a given vector p. We will work
% by 'similarity transformation'.

% First we normalize p

if size(p,1) == 1
    u1 = p'/norm(p);
else
    u1 = p/norm(p);
end

% We are going to generate an orthonormal frame U such that u1 is a unit
% vector derived by normalizing p, and u2 and u3 are in the 'null' space of 
% u1'. Because of the way the 'null' algorithm works the two vectors in the
% null space will be u2 and u3 of a right handed frame.

U = [u1 null(u1')];

% We generate the transformation matrix for a rotation of the frame 
% counter-clockwise by theta around u1. 

cos_theta = cos(theta);
sin_theta = sin(theta);
Q1 = [1 0 0;0 cos_theta sin_theta ;0 -sin_theta cos_theta];

% Here is the similarity transformation; since we want to rotate our
% standard frame by Q1 around u1 we carry out the usual 3 steps:
% 1. we move into U space by multiplying by the inverse (transpose) of the 
% U base
% 2. we carry out the Q1 rotation in U space
% 3. we come back into standard space by multiplying by the U base

Q = U*Q1*U';

