Fixes problems
Added rotation matrix