1function r = toRotMat(q)
2%ROTATE convert normalized quaternion q to rotation matrix
4if length(q(:))~=4
5 error('Only a single quaternion expressed as a matlab vector of length 4 can be processed');
6end
8r = [(1 - 2*(q(3)^2 + q(4)^2)), 2*(q(2)*q(3) - q(1)*q(4)), 2*(q(2)*q(4) + q(1)*q(3)); ...
9 2*(q(2)*q(3) + q(1)*q(4)), (1 - 2*(q(2)^2 + q(4)^2)), 2*(q(3)*q(4) - q(1)*q(2)); ...
10 2*(q(2)*q(4) - q(1)*q(3)), 2*(q(3)*q(4) + q(1)*q(2)), (1 - 2*(q(2)^2 + q(3)^2))];
12end