Return type is T which can be an integral type, float or double. By
dividing by 2 we avoid float operation on the first case and do the
right thing on the second and third.
Return type is float, so operate on float types everywhere.
Fixes this warning while building for PX4:
../../libraries/AP_Math/AP_Math.cpp: In instantiation of 'float safe_asin(T) [with T = double]':
../../libraries/AP_Math/AP_Math.cpp:56:48: required from here
../../libraries/AP_Math/AP_Math.cpp:44:11: warning: implicit conversion from 'float' to 'double' to match other operand of binary expression [-Wdouble-promotion]
if (v >= 1.0f) {
^
../../libraries/AP_Math/AP_Math.cpp:47:11: warning: implicit conversion from 'float' to 'double' to match other operand of binary expression [-Wdouble-promotion]
if (v <= -1.0f) {
^
We are calling fabsf(), which returns a float. We should use the epsilon
from float type, not from the argument type passed to fabsf().
On the other hand when the double version is instantiated we do want to
use the std::numeric_limits<double>::epsilon() value.
This adds a branch to the function, but it's removed when the function
is intantiated by the compiler since the type is known at compile-time.
Fixes this warning when building for PX4:
../../libraries/AP_Math/AP_Math.cpp: In instantiation of 'typename std::enable_if<std::is_floating_point<typename std::common_type<_Tp, _Up>::type>::value, bool>::type is_equal(Arithmetic1, Arithmetic2) [with Arithmetic1 = double; Arithmetic2 = double; typename std::enable_if<std::is_floating_point<typename std::common_type<_Tp, _Up>::type>::value, bool>::type = bool]':
../../libraries/AP_Math/AP_Math.cpp:23:66: required from here
../../libraries/AP_Math/AP_Math.cpp:17:29: warning: implicit conversion from 'float' to 'double' to match other operand of binary expression [-Wdouble-promotion]
return fabsf(v_1 - v_2) < std::numeric_limits<decltype(v_1 - v_2)>::epsilon();
^
When using wrap_180_cd() we are adding a small float (180 * 100) to a
possibly big number. This may lose float precision as illustrated by the
unit test failing:
OUT: ../../libraries/AP_Math/tests/test_math.cpp:195: Failure
OUT: Value of: wrap_180_cd(-3600000000.f)
OUT: Actual: -80
OUT: Expected: 0.f
OUT: Which is: 0