Add convinient method to calculate the ortho matrix