Add 6th stage and refactor common rotation conversaions into MathUtility