Numerical instability in UnitQuaternion::rotation_between_axis for small angles

[patowen]

The current implementation of UnitQuaternion::rotation_between_axis, due to its use of scaled_rotation_between_axis, takes the cosine of the angle between the two input vectors, followed by taking the arccosine. For small angles, cosine is approximately quadratic, and I think this has the effect of only having half the precision that the floating point type allows.

There is a possible simpler implementation for rotation_between_axis that's more stable for small angles (would need adapting for more general types):

fn rotation_between_axis_stable<N: RealField + Copy>(
    from: &na::UnitVector3<N>,
    to: &na::UnitVector3<N>,
    epsilon: N, // Could use default_epsilon() instead
) -> Option<na::UnitQuaternion<N>> {
    let angle_bisector = na::UnitVector3::try_new(from.into_inner() + to.into_inner(), epsilon)?;
    Some(na::UnitQuaternion::new_unchecked(
        na::Quaternion::from_parts(from.dot(&angle_bisector), from.cross(&angle_bisector)),
    ))
}

For a concrete example of the precision difference, the following is some sample code that should be able to be run in any project that just imports nalgebra:

use nalgebra as na;

pub fn rotation_between_axis_stable<N: na::RealField + Copy>(
    from: &na::UnitVector3<N>,
    to: &na::UnitVector3<N>,
    epsilon: N,
) -> Option<na::UnitQuaternion<N>> {
    let angle_bisector = na::UnitVector3::try_new(from.into_inner() + to.into_inner(), epsilon)?;
    Some(na::UnitQuaternion::new_unchecked(
        na::Quaternion::from_parts(from.dot(&angle_bisector), from.cross(&angle_bisector)),
    ))
}

fn main() {
    let from = na::UnitVector3::new_normalize(na::Vector3::new(1.0f32, 1.0, 3.0));
    let to = na::UnitVector3::new_normalize(na::Vector3::new(1.0f32, 1.0, 3.000001));
    let current_impl_result = na::UnitQuaternion::rotation_between_axis(&from, &to).unwrap();
    let stable_impl_result = rotation_between_axis_stable(&from, &to, 1e-5).unwrap();
    println!("             Expected: {:.8?}", to);
    println!("Actual (current impl): {:.8?}", current_impl_result * from);
    println!(" Actual (stable impl): {:.8?}", stable_impl_result * from);
}

which results in

             Expected: [[0.30151126, 0.30151126, 0.90453410]]
Actual (current impl): [[0.30129048, 0.30129048, 0.90468115]]
 Actual (stable impl): [[0.30151123, 0.30151123, 0.90453404]]