Fixed head offset matrix calculation. #41, #38