Fix NaN resulting from non-clamped input to simd_asin in angular motor solver

[ryanhcode]

Fixes a significant NaN issue caused by a non-clamped input to simd_asin in joint_constraint_builder.rs.

Beforehand, as seen below, slight imprecision in the imaginary components of the error quaternion could cause a value out of bounds to be passed into simd_asin, causing a NaN that further propagates to the body. This occurs very frequently with moving nested revolute joints.

Logged data from the code during debugging showcasing the issue:

ang error [1.0000001, -1.6205013e-5, -1.5124679e-6, 9.6172094e-5]
ang dist NaN

[ThierryBerger]

Thanks for the report! Do you have a reproduction test for this ?

[ryanhcode]

@Vrixyz

Angular error is set from let mut ang_err = frame1.rotation.inverse() * frame2.rotation; currently, and then computed from the asin of one of the imaginary components. This evidently can cause the input to be slightly out of the -1.0 to 1.0 range, and therefore produce a NaN.

I do not have a reproduction test for this exact scenario. I could work on creating one if necessary, but I'm uncertain how specific of a test is needed for an issue of this nature.

[ryanhcode]

Should be good now.

[sebcrozet]

Thank you!