q2r function

Convert quaternion to rotation matrix http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation