function [p1,theta,Q_p] = p_and_theta_from_Q(Q)
% Given a transformation matrix Q find the vector p and the angle theta
% such that Q corresponds to a rotation of the coordinate frame by theta
% around p. We will work by 'similarity transformation'

% If Q is a rotation around an axis p, p will not be changed by the
% rotation: 
% Q*p = p
% Q*p - p = 0
% (Q-I)*p = 0
% Therefore p is in the 'null' space of (Q-I)

I = eye(3);
p1 = null(Q-I);

% We generate an orthonormal frame P such that p2 and p3 are in the 'null'
% space of p1'. Because of the way the 'null' algorithm works the two
% vectors in the null space will be the 2nd and 3rd axis of a right handed
% frame.

P = [p1 null(p1')];

% We know the transformation Q in standard basis, and now we want to know 
% what would be the 'similar' transformation in P space. Here we apply a
% direct 'similarity transformation'.

Q_p = P'*Q*P;
theta = acos(Q_p(2,2));
