Rotation added

Rotation works after switching to matrices instead of quaternions. Must
be something wrong in the quaternion code, will check it out.