Fix issues in calculating transformation matrix