Rotation matrices and vectors are now keyed to the relevant degree of freedom
Removed uneccessary using statements to Mathdotnet