Added rotation matrix transformation and changed to vectors