Added some methods in RotationMatrix and starting writing tests.