Vector3.Angle - more accurate and numerically stable at small angles version

[allista]

Hi everyone.

I'm not an engineer, so maybe this is common knowledge already, but during TCA development I've encountered a problem with numerical instability of Vector3[d].Angle methods at small angles. Both methods use the same straightforward implementation:

public static double Angle(Vector3d from, Vector3d to)
{
	return Math.Acos(Math.Min(Math.Max(Vector3d.Dot(from.normalized, to.normalized), -1), 1)) * 57.2957795130823;
}

Which is fine, unless rounding errors start producing cosine values > 1 that get clamped, which results in angle "measurements" like this:

*this, of course, depends on the vectors themselves, as most of the error comes from normalization.

So I've started looking and, soon enough, found a paper reciting a much more numerically stable approach:

https://people.eecs.berkeley.edu/~wkahan/Mindless.pdf (page 47, top, don't mind the title )

What's more, as it uses arctangent of direct quotient of sine and cosine, we can use the standard atan2, which is robust at all angles:

public static float Angle2(Vector3 a, Vector3 b)
{
    var abm = a*b.magnitude;
    var bam = b*a.magnitude;
    return 2 * Mathf.Atan2((abm-bam).magnitude, (abm+bam).magnitude) * Mathf.Rad2Deg;
}

No normalization, no clamping, and much, much more reasonable results:

So if you happen to have the need for accurate angle measurements in Unity, this is a cheap and effective solution.

[drtedastro]

Nice work.

Pure, simple is usually best.

Cheers.

[RandomKerbal]

PID controller.