Major bug fix with rotation matrix