rigidBodyDynamics: Simplify handling of quaternions by maintaining a unit...

rigidBodyDynamics: Simplify handling of quaternions by maintaining a unit quaternion in the joint state field 'q'

'w' is now obtained from 'v' using the relation w = sqrt(1 - |sqr(v)|)
and 'v' is stored in the joint state field 'q' and integrated in the
usual manner but corrected using quaternion transformations.