Added the calcQuaternion function to get Quaternion from a Rotation Matrix