diff --git a/include/fpm/math.hpp b/include/fpm/math.hpp index 16c3217..4aed82a 100644 --- a/include/fpm/math.hpp +++ b/include/fpm/math.hpp @@ -559,6 +559,58 @@ inline fixed tan(fixed x) noexcept return sin(x) / cx; } +namespace detail { + +// Calculates atan(x) assuming that x is in the range [0,1] +template +fixed atan_sanitized(fixed x) noexcept +{ + using Fixed = fixed; + assert(x >= Fixed(0) && x <= Fixed(1)); + + constexpr auto fA = Fixed::template from_fixed_point<63>( 716203666280654660ll); // 0.0776509570923569 + constexpr auto fB = Fixed::template from_fixed_point<63>(-2651115102768076601ll); // -0.287434475393028 + constexpr auto fC = Fixed::template from_fixed_point<63>( 9178930894564541004ll); // 0.995181681698119 (PI/4 - A - B) + + const auto xx = x * x; + return ((fA*xx + fB)*xx + fC)*x; +} + +// Calculate atan(y / x), assuming x != 0. +// +// If x is very, very small, y/x can easily overflow the fixed-point range. +// If q = y/x and q > 1, atan(q) would calculate atan(1/q) as intermediate step +// anyway. We can shortcut that here and avoid the loss of information, thus +// improving the accuracy of atan(y/x) for very small x. +template +fixed atan_div(fixed y, fixed x) noexcept +{ + using Fixed = fixed; + assert(x != Fixed(0)); + + // Make sure y and x are positive. + // If y / x is negative (when y or x, but not both, are negative), negate the result to + // keep the correct outcome. + if (y < Fixed(0)) { + if (x < Fixed(0)) { + return atan_div(-y, -x); + } + return -atan_div(-y, x); + } + if (x < Fixed(0)) { + return -atan_div(y, -x); + } + assert(y >= Fixed(0)); + assert(x > Fixed(0)); + + if (y > x) { + return Fixed::half_pi() - detail::atan_sanitized(x / y); + } + return detail::atan_sanitized(y / x); +} + +} + template fixed atan(fixed x) noexcept { @@ -570,15 +622,10 @@ fixed atan(fixed x) noexcept if (x > Fixed(1)) { - return Fixed::half_pi() - atan(Fixed(1) / x); + return Fixed::half_pi() - detail::atan_sanitized(Fixed(1) / x); } - constexpr auto fA = Fixed::template from_fixed_point<63>( 716203666280654660ll); // 0.0776509570923569 - constexpr auto fB = Fixed::template from_fixed_point<63>(-2651115102768076601ll); // -0.287434475393028 - constexpr auto fC = Fixed::template from_fixed_point<63>( 9178930894564541004ll); // 0.995181681698119 (PI/4 - A - B) - - const auto xx = x * x; - return ((fA*xx + fB)*xx + fC)*x; + return detail::atan_sanitized(x); } template @@ -592,7 +639,7 @@ fixed asin(fixed x) noexcept { return copysign(Fixed::half_pi(), x); } - return atan(x / sqrt(yy)); + return detail::atan_div(x, sqrt(yy)); } template @@ -606,7 +653,7 @@ fixed acos(fixed x) noexcept return Fixed::pi(); } const auto yy = Fixed(1) - x * x; - return Fixed(2)*atan(sqrt(yy) / (Fixed(1) + x)); + return Fixed(2)*detail::atan_div(sqrt(yy), Fixed(1) + x); } template @@ -618,7 +665,9 @@ fixed atan2(fixed y, fixed x) noexcept assert(y != Fixed(0)); return (y > Fixed(0)) ? Fixed::half_pi() : -Fixed::half_pi(); } - auto ret = atan(y / x); + + auto ret = detail::atan_div(y, x); + if (x < Fixed(0)) { return (y >= Fixed(0)) ? ret + Fixed::pi() : ret - Fixed::pi(); diff --git a/tests/trigonometry.cpp b/tests/trigonometry.cpp index 90d050a..19d8607 100644 --- a/tests/trigonometry.cpp +++ b/tests/trigonometry.cpp @@ -132,4 +132,29 @@ TEST(trigonometry, atan2) #ifndef NDEBUG EXPECT_DEATH(atan2(P(0), P(0)), ""); #endif -} \ No newline at end of file +} + +// Naively, atan2(y, x) does y / x which would overflow for near-zero x with Q16.16. +// Test that we've got protections in place for this. +TEST(trigonometry, atan2_near_zero) +{ + constexpr auto MAX_ERROR_PERC = 0.025; + using P = fpm::fixed_16_16; + + const auto x = P::from_raw_value(1); + const auto y = P(100); + + // Positive x + { + auto atan2_real = std::atan2(static_cast(y), static_cast(x)); + auto atan2_fixed = static_cast(atan2(y, x)); + EXPECT_TRUE(HasMaximumError(atan2_fixed, atan2_real, MAX_ERROR_PERC)); + } + + // Negative x + { + auto atan2_real = std::atan2(static_cast(y), static_cast(-x)); + auto atan2_fixed = static_cast(atan2(y, -x)); + EXPECT_TRUE(HasMaximumError(atan2_fixed, atan2_real, MAX_ERROR_PERC)); + } +}