Finished implementation of rotation matrix. Need tests.