| | | 1 | | //======================================================================= |
| | | 2 | | // FixedMath.Trigonometry.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | |
| | | 11 | | namespace FixedMathSharp; |
| | | 12 | | |
| | | 13 | | /// <content> |
| | | 14 | | /// Trigonometric, logarithmic, and related constants/lookup tables for fixed-point math, |
| | | 15 | | /// along with sine/cosine/asin approximation coefficients used by FixedMath. |
| | | 16 | | /// </content> |
| | | 17 | | public static partial class FixedMath |
| | | 18 | | { |
| | | 19 | | #region Fields and Constants |
| | | 20 | | |
| | 2 | 21 | | private static readonly int[] s_pow10Lookup = { |
| | 2 | 22 | | 1, // 10^0 |
| | 2 | 23 | | 10, // 10^1 |
| | 2 | 24 | | 100, // 10^2 |
| | 2 | 25 | | 1000, // 10^3 |
| | 2 | 26 | | 10000, // 10^4 |
| | 2 | 27 | | 100000, // 10^5 |
| | 2 | 28 | | 1000000, // 10^6 |
| | 2 | 29 | | 10000000, // 10^7 |
| | 2 | 30 | | 100000000, // 10^8 |
| | 2 | 31 | | 1000000000, // 10^9 |
| | 2 | 32 | | }; |
| | | 33 | | |
| | | 34 | | /// <summary> |
| | | 35 | | /// Provides a lookup table of integer powers of 10 from 10^0 to 10^9. |
| | | 36 | | /// </summary> |
| | | 37 | | /// <remarks> |
| | | 38 | | /// This array can be used to efficiently retrieve the value of 10 raised to an integer |
| | | 39 | | /// exponent within the supported range, avoiding repeated calculations. |
| | | 40 | | /// The index corresponds to the exponent. |
| | | 41 | | /// </remarks> |
| | 14 | 42 | | public static ReadOnlySpan<int> Pow10Lookup => s_pow10Lookup; |
| | | 43 | | |
| | | 44 | | // Trigonometric and logarithmic constants |
| | | 45 | | |
| | | 46 | | internal const double PI_DOUBLE = 3.14159265358979323846d; |
| | | 47 | | /// <summary> |
| | | 48 | | /// Represents the mathematical constant π (pi). |
| | | 49 | | /// </summary> |
| | | 50 | | /// <remarks>The value is approximately 3.14159265358979323846.</remarks> |
| | | 51 | | internal const long PI_LONG = (long)(PI_DOUBLE * ONE_L); |
| | | 52 | | |
| | | 53 | | internal const double LN2_DOUBLE = 0.6931471805599453d; |
| | | 54 | | /// <summary> |
| | | 55 | | /// Represents the mathematical constant natural logarithm of 2 (ln(2)). |
| | | 56 | | /// </summary> |
| | | 57 | | /// <remarks>The value is approximately 0.6931471805599453.</remarks> |
| | | 58 | | internal const long LN2_LONG = (long)(LN2_DOUBLE * ONE_L); |
| | | 59 | | |
| | | 60 | | // Asin Padé approximations |
| | | 61 | | internal const double PADE_A1_DOUBLE = 0.183320102d; |
| | | 62 | | internal const long PADE_A1_LONG = (long)(PADE_A1_DOUBLE * ONE_L); |
| | | 63 | | internal const double PADE_A2_DOUBLE = 0.0218804099d; |
| | | 64 | | internal const long PADE_A2_LONG = (long)(PADE_A2_DOUBLE * ONE_L); |
| | | 65 | | |
| | | 66 | | // Minimax sine coefficients for [0, pi/4]. |
| | | 67 | | internal const long SIN_COEFF_3_LONG = 715827922L; |
| | | 68 | | internal const long SIN_COEFF_5_LONG = 35789249L; |
| | | 69 | | internal const long SIN_COEFF_7_LONG = 841334L; |
| | | 70 | | |
| | | 71 | | // Nearest Q32.32 Taylor coefficients for cosine on [0, pi/4]. |
| | | 72 | | internal const long COS_COEFF_2_LONG = 2147483648L; // round(2^32 / 2!) |
| | | 73 | | internal const long COS_COEFF_4_LONG = 178956971L; // round(2^32 / 4!) |
| | | 74 | | internal const long COS_COEFF_6_LONG = 5965232L; // round(2^32 / 6!) |
| | | 75 | | internal const long COS_COEFF_8_LONG = 106522L; // round(2^32 / 8!) |
| | | 76 | | |
| | | 77 | | /// <summary> |
| | | 78 | | /// Gets the conservative absolute approximation-error bound for |
| | | 79 | | /// <see cref="Sin(Fixed64)"/> and <see cref="Cos(Fixed64)"/> when the |
| | | 80 | | /// input is already canonical in [-π, π]. |
| | | 81 | | /// </summary> |
| | | 82 | | /// <remarks> |
| | | 83 | | /// The bound includes the degree-8 cosine remainder on [0, π/4], |
| | | 84 | | /// coefficient quantization, fixed-point Horner rounding, and the tuned |
| | | 85 | | /// sine approximation error. Raw-neighborhood and principal-range tests |
| | | 86 | | /// validate range-reduction seams independently of exact anchors. |
| | | 87 | | /// |
| | | 88 | | /// This bound does not include phase error accumulated while reducing a |
| | | 89 | | /// large multi-turn input by the fixed-point approximation of 2π. A |
| | | 90 | | /// consumer propagating a strict error budget must canonicalize its |
| | | 91 | | /// angle before turns accumulate or account for that phase error too. |
| | | 92 | | /// |
| | | 93 | | /// Consumers that propagate sine/cosine error through rotations must |
| | | 94 | | /// also account for their own multiply, add, and normalization error. |
| | | 95 | | /// </remarks> |
| | 4397 | 96 | | public static Fixed64 CanonicalSinCosErrorBound => Fixed64.FromRaw(DEFAULT_TOLERANCE_L * 8); |
| | | 97 | | |
| | 2 | 98 | | private static readonly long[] s_pow2PositiveFractionLookup = |
| | 2 | 99 | | { |
| | 2 | 100 | | 6074001000L, |
| | 2 | 101 | | 5107605667L, |
| | 2 | 102 | | 4683695048L, |
| | 2 | 103 | | 4485121744L, |
| | 2 | 104 | | 4389014833L, |
| | 2 | 105 | | 4341736423L, |
| | 2 | 106 | | 4318288544L, |
| | 2 | 107 | | 4306612134L, |
| | 2 | 108 | | 4300785774L, |
| | 2 | 109 | | 4297875550L, |
| | 2 | 110 | | 4296421177L, |
| | 2 | 111 | | 4295694175L, |
| | 2 | 112 | | 4295330720L, |
| | 2 | 113 | | 4295149004L, |
| | 2 | 114 | | 4295058149L, |
| | 2 | 115 | | 4295012722L, |
| | 2 | 116 | | 4294990009L, |
| | 2 | 117 | | 4294978653L, |
| | 2 | 118 | | 4294972974L, |
| | 2 | 119 | | 4294970135L, |
| | 2 | 120 | | 4294968716L, |
| | 2 | 121 | | 4294968006L, |
| | 2 | 122 | | 4294967651L, |
| | 2 | 123 | | 4294967473L, |
| | 2 | 124 | | 4294967385L, |
| | 2 | 125 | | 4294967340L, |
| | 2 | 126 | | 4294967318L, |
| | 2 | 127 | | 4294967307L, |
| | 2 | 128 | | 4294967302L, |
| | 2 | 129 | | 4294967299L, |
| | 2 | 130 | | 4294967297L, |
| | 2 | 131 | | 4294967297L |
| | 2 | 132 | | }; |
| | | 133 | | |
| | 2 | 134 | | private static readonly long[] s_pow2NegativeFractionLookup = |
| | 2 | 135 | | { |
| | 2 | 136 | | 3037000500L, |
| | 2 | 137 | | 3611622603L, |
| | 2 | 138 | | 3938502376L, |
| | 2 | 139 | | 4112874773L, |
| | 2 | 140 | | 4202935003L, |
| | 2 | 141 | | 4248701965L, |
| | 2 | 142 | | 4271771996L, |
| | 2 | 143 | | 4283353945L, |
| | 2 | 144 | | 4289156690L, |
| | 2 | 145 | | 4292061010L, |
| | 2 | 146 | | 4293513907L, |
| | 2 | 147 | | 4294240540L, |
| | 2 | 148 | | 4294603903L, |
| | 2 | 149 | | 4294785595L, |
| | 2 | 150 | | 4294876445L, |
| | 2 | 151 | | 4294921870L, |
| | 2 | 152 | | 4294944583L, |
| | 2 | 153 | | 4294955939L, |
| | 2 | 154 | | 4294961618L, |
| | 2 | 155 | | 4294964457L, |
| | 2 | 156 | | 4294965876L, |
| | 2 | 157 | | 4294966586L, |
| | 2 | 158 | | 4294966941L, |
| | 2 | 159 | | 4294967119L, |
| | 2 | 160 | | 4294967207L, |
| | 2 | 161 | | 4294967252L, |
| | 2 | 162 | | 4294967274L, |
| | 2 | 163 | | 4294967285L, |
| | 2 | 164 | | 4294967290L, |
| | 2 | 165 | | 4294967293L, |
| | 2 | 166 | | 4294967295L, |
| | 2 | 167 | | 4294967295L |
| | 2 | 168 | | }; |
| | | 169 | | |
| | | 170 | | /// <summary> |
| | | 171 | | /// Squared magnitudes at or below this value use component scaling so |
| | | 172 | | /// fixed-point squaring cannot dominate the normalized direction's |
| | | 173 | | /// relative error. |
| | | 174 | | /// </summary> |
| | 2 | 175 | | internal static readonly Fixed64 ScaleSafeMagnitudeSquaredThreshold = Fixed64.FromFraction(1, 256); |
| | | 176 | | |
| | | 177 | | /// <summary> |
| | | 178 | | /// Magnitudes at or below this value normalize in scale-relative |
| | | 179 | | /// coordinates so quantizing the final scalar length cannot distort |
| | | 180 | | /// component ratios. |
| | | 181 | | /// </summary> |
| | 2 | 182 | | internal static readonly Fixed64 ScaleSafeMagnitudeThreshold = Fixed64.FromFraction(1, 16); |
| | | 183 | | |
| | | 184 | | #endregion |
| | | 185 | | |
| | | 186 | | #region FixedTrigonometry Operations |
| | | 187 | | |
| | | 188 | | /// <summary> |
| | | 189 | | /// Raises the base number b to the power of exp. |
| | | 190 | | /// Uses logarithms to compute power efficiently for fixed-point values. |
| | | 191 | | /// </summary> |
| | | 192 | | /// <exception cref="DivideByZeroException"> |
| | | 193 | | /// The base was Fixed64.Zero, with a negative expFixed64.Onent |
| | | 194 | | /// </exception> |
| | | 195 | | /// <exception cref="ArgumentOutOfRangeException"> |
| | | 196 | | /// The base was negative, with a non-Fixed64.Zero expFixed64.Onent |
| | | 197 | | /// </exception> |
| | | 198 | | public static Fixed64 Pow(Fixed64 b, Fixed64 exp) |
| | | 199 | | { |
| | 14 | 200 | | if (b == Fixed64.One) |
| | 1 | 201 | | return Fixed64.One; |
| | | 202 | | |
| | 13 | 203 | | if (exp.m_rawValue == 0) |
| | 1 | 204 | | return Fixed64.One; |
| | | 205 | | |
| | 12 | 206 | | if (b.m_rawValue == 0) |
| | | 207 | | { |
| | 2 | 208 | | if (exp.m_rawValue < 0) |
| | 1 | 209 | | throw new DivideByZeroException("Cannot raise 0 to a negative power."); |
| | | 210 | | |
| | 1 | 211 | | return Fixed64.Zero; |
| | | 212 | | } |
| | | 213 | | |
| | 10 | 214 | | Fixed64 log2 = Log2(b); // Calculate logarithm base 2 |
| | 10 | 215 | | return Pow2(exp * log2); // Raise 2 to the power of log2 result |
| | | 216 | | } |
| | | 217 | | |
| | | 218 | | /// <summary> |
| | | 219 | | /// Raises 2 to the power of x. |
| | | 220 | | /// Provides high accuracy for small values of x. |
| | | 221 | | /// </summary> |
| | | 222 | | public static Fixed64 Pow2(Fixed64 x) |
| | | 223 | | { |
| | 54 | 224 | | if (x.m_rawValue == 0) |
| | 2 | 225 | | return Fixed64.One; |
| | | 226 | | |
| | 52 | 227 | | bool neg = x.m_rawValue < 0; |
| | 52 | 228 | | if (neg) |
| | 18 | 229 | | x = -x; |
| | | 230 | | |
| | 52 | 231 | | if (x == Fixed64.One) |
| | 6 | 232 | | return neg ? Fixed64.One / Fixed64.Two : Fixed64.Two; |
| | | 233 | | |
| | 46 | 234 | | int integerPart = (int)(x.m_rawValue >> SHIFT_AMOUNT_I); |
| | 46 | 235 | | long fractionalRaw = x.m_rawValue & MAX_SHIFTED_AMOUNT_UI; |
| | | 236 | | |
| | 46 | 237 | | if (neg) |
| | | 238 | | { |
| | 15 | 239 | | if (integerPart >= SHIFT_AMOUNT_I) |
| | 1 | 240 | | return Fixed64.MinIncrement; |
| | | 241 | | |
| | 14 | 242 | | Fixed64 result = Pow2Fractional(fractionalRaw, s_pow2NegativeFractionLookup); |
| | 14 | 243 | | return Fixed64.FromRaw(ShiftRightRounded(result.m_rawValue, integerPart)); |
| | | 244 | | } |
| | | 245 | | |
| | 31 | 246 | | if (integerPart >= 31) |
| | 2 | 247 | | return Fixed64.MaxValue; |
| | | 248 | | |
| | 29 | 249 | | Fixed64 positiveResult = Pow2Fractional(fractionalRaw, s_pow2PositiveFractionLookup); |
| | 29 | 250 | | long shifted = positiveResult.m_rawValue << integerPart; |
| | | 251 | | |
| | 29 | 252 | | return Fixed64.FromRaw(shifted); |
| | | 253 | | } |
| | | 254 | | |
| | | 255 | | /// <summary> |
| | | 256 | | /// Returns the base-2 logarithm of a specified number. |
| | | 257 | | /// Provides at least 9 decimals of accuracy. |
| | | 258 | | /// </summary> |
| | | 259 | | /// <remarks> |
| | | 260 | | /// This implementation is based on Clay. S. Turner's fast binary logarithm algorithm |
| | | 261 | | /// (C. S. Turner, "A Fast Binary Logarithm Algorithm", IEEE Signal Processing Mag., pp. 124,140, Sep. 2010.) |
| | | 262 | | /// </remarks> |
| | | 263 | | public static Fixed64 Log2(Fixed64 x) |
| | | 264 | | { |
| | 59 | 265 | | if (x.m_rawValue <= 0) |
| | 1 | 266 | | throw new ArgumentOutOfRangeException(nameof(x), "Cannot compute logarithm of non-positive number."); |
| | | 267 | | |
| | 58 | 268 | | long b = 1U << (SHIFT_AMOUNT_I - 1); // Initial value for binary logarithm |
| | 58 | 269 | | long rawX = x.m_rawValue; |
| | 58 | 270 | | int shift = FloorLog2((ulong)rawX) - SHIFT_AMOUNT_I; |
| | 58 | 271 | | long y = (long)shift << SHIFT_AMOUNT_I; |
| | | 272 | | |
| | 58 | 273 | | if (shift > 0) |
| | 37 | 274 | | rawX >>= shift; |
| | 21 | 275 | | else if (shift < 0) |
| | 17 | 276 | | rawX <<= -shift; |
| | | 277 | | |
| | 58 | 278 | | Fixed64 z = Fixed64.FromRaw(rawX); // Remaining fraction |
| | | 279 | | |
| | 3828 | 280 | | for (int i = 0; i < SHIFT_AMOUNT_I; i++) |
| | | 281 | | { |
| | 1856 | 282 | | z = FastMul(z, z); |
| | 1856 | 283 | | if (z.m_rawValue >= (ONE_L << 1)) |
| | | 284 | | { |
| | 137 | 285 | | z = Fixed64.FromRaw(z.m_rawValue >> 1); |
| | 137 | 286 | | y += b; |
| | | 287 | | } |
| | 1856 | 288 | | b >>= 1; |
| | | 289 | | } |
| | | 290 | | |
| | 58 | 291 | | return Fixed64.FromRaw(y); |
| | | 292 | | } |
| | | 293 | | |
| | | 294 | | /// <summary> |
| | | 295 | | /// Returns the natural logarithm of a specified fixed-point number. |
| | | 296 | | /// Provides at least 7 decimals of accuracy. |
| | | 297 | | /// </summary> |
| | | 298 | | public static Fixed64 Ln(Fixed64 x) |
| | | 299 | | { |
| | 12 | 300 | | if (x.m_rawValue <= 0) |
| | 1 | 301 | | throw new ArgumentOutOfRangeException(nameof(x), "Cannot compute logarithm of non-positive number."); |
| | | 302 | | |
| | 11 | 303 | | return FastMul(Log2(x), Fixed64.Ln2); |
| | | 304 | | } |
| | | 305 | | |
| | | 306 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 307 | | private static Fixed64 Pow2Fractional(long fractionalRaw, long[] lookup) |
| | | 308 | | { |
| | 43 | 309 | | Fixed64 result = Fixed64.One; |
| | 43 | 310 | | long mask = 1L << (SHIFT_AMOUNT_I - 1); |
| | | 311 | | |
| | 2838 | 312 | | for (int i = 0; i < SHIFT_AMOUNT_I; i++) |
| | | 313 | | { |
| | 1376 | 314 | | if ((fractionalRaw & mask) != 0) |
| | 93 | 315 | | result = FastMul(result, Fixed64.FromRaw(lookup[i])); |
| | | 316 | | |
| | 1376 | 317 | | mask >>= 1; |
| | | 318 | | } |
| | | 319 | | |
| | 43 | 320 | | return result; |
| | | 321 | | } |
| | | 322 | | |
| | | 323 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 324 | | private static long ShiftRightRounded(long value, int shift) |
| | | 325 | | { |
| | 14 | 326 | | if (shift == 0) |
| | 2 | 327 | | return value; |
| | | 328 | | |
| | 12 | 329 | | long half = 1L << (shift - 1); |
| | 12 | 330 | | return (value + half) >> shift; |
| | | 331 | | } |
| | | 332 | | |
| | | 333 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 334 | | private static int FloorLog2(ulong value) |
| | | 335 | | { |
| | 10841 | 336 | | int result = 0; |
| | | 337 | | |
| | 10841 | 338 | | if (value >= 1UL << 32) |
| | | 339 | | { |
| | 10656 | 340 | | value >>= 32; |
| | 10656 | 341 | | result = 32; |
| | | 342 | | } |
| | | 343 | | |
| | 10841 | 344 | | if (value >= 1UL << 16) |
| | | 345 | | { |
| | 214 | 346 | | value >>= 16; |
| | 214 | 347 | | result += 16; |
| | | 348 | | } |
| | | 349 | | |
| | 10841 | 350 | | if (value >= 1UL << 8) |
| | | 351 | | { |
| | 326 | 352 | | value >>= 8; |
| | 326 | 353 | | result += 8; |
| | | 354 | | } |
| | | 355 | | |
| | 10841 | 356 | | if (value >= 1UL << 4) |
| | | 357 | | { |
| | 448 | 358 | | value >>= 4; |
| | 448 | 359 | | result += 4; |
| | | 360 | | } |
| | | 361 | | |
| | 10841 | 362 | | if (value >= 1UL << 2) |
| | | 363 | | { |
| | 579 | 364 | | value >>= 2; |
| | 579 | 365 | | result += 2; |
| | | 366 | | } |
| | | 367 | | |
| | 10841 | 368 | | if (value >= 1UL << 1) |
| | 848 | 369 | | result++; |
| | | 370 | | |
| | 10841 | 371 | | return result; |
| | | 372 | | } |
| | | 373 | | |
| | | 374 | | /// <summary> |
| | | 375 | | /// Returns the square root of a specified fixed-point number. |
| | | 376 | | /// </summary> |
| | | 377 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 378 | | public static Fixed64 Sqrt(Fixed64 x) |
| | | 379 | | { |
| | 10802 | 380 | | if (x.m_rawValue < 0) |
| | 1 | 381 | | throw new ArgumentOutOfRangeException(nameof(x), "Cannot compute square root of a negative number."); |
| | | 382 | | |
| | 10801 | 383 | | ulong num = (ulong)x.m_rawValue; |
| | 10801 | 384 | | if (num == 0UL) |
| | 18 | 385 | | return Fixed64.Zero; |
| | | 386 | | |
| | 10783 | 387 | | ulong result = 0UL; |
| | 10783 | 388 | | ulong bit = 1UL << (FloorLog2(num) & ~1); |
| | | 389 | | |
| | | 390 | | // Perform the square root calculation using bitwise shifts |
| | 64698 | 391 | | for (int i = 0; i < 2; ++i) |
| | | 392 | | { |
| | | 393 | | // Calculate the top bits of the square root result |
| | 378893 | 394 | | while (bit != 0) |
| | | 395 | | { |
| | 357327 | 396 | | if (num >= result + bit) |
| | | 397 | | { |
| | 65997 | 398 | | num -= result + bit; |
| | 65997 | 399 | | result = (result >> 1) + bit; |
| | | 400 | | } |
| | | 401 | | else |
| | | 402 | | { |
| | 291330 | 403 | | result >>= 1; |
| | | 404 | | } |
| | | 405 | | |
| | 357327 | 406 | | bit >>= 2; |
| | | 407 | | } |
| | | 408 | | |
| | 21566 | 409 | | if (i == 0) |
| | | 410 | | { |
| | | 411 | | // Process it again to get the remaining bits |
| | 10783 | 412 | | if (num > ((1UL << SHIFT_AMOUNT_I) - 1)) |
| | | 413 | | { |
| | | 414 | | // Handle large remainders by adjusting the result |
| | 5 | 415 | | num -= result; |
| | 5 | 416 | | num = (num << SHIFT_AMOUNT_I) - (ulong)Fixed64.Half.m_rawValue; |
| | 5 | 417 | | result = (result << SHIFT_AMOUNT_I) + (ulong)Fixed64.Half.m_rawValue; |
| | | 418 | | } |
| | | 419 | | else |
| | | 420 | | { |
| | 10778 | 421 | | num <<= SHIFT_AMOUNT_I; |
| | 10778 | 422 | | result <<= SHIFT_AMOUNT_I; |
| | | 423 | | } |
| | | 424 | | |
| | 10783 | 425 | | bit = 1UL << (SHIFT_AMOUNT_I - 2); |
| | | 426 | | } |
| | | 427 | | } |
| | | 428 | | |
| | | 429 | | // Rounding: round up if necessary |
| | 10783 | 430 | | if (num > result && (num - result) > (result >> 1)) |
| | 1196 | 431 | | ++result; |
| | | 432 | | |
| | 10783 | 433 | | return Fixed64.FromRaw((long)result); |
| | | 434 | | } |
| | | 435 | | |
| | | 436 | | /// <summary> |
| | | 437 | | /// Converts a value in radians to degrees. |
| | | 438 | | /// </summary> |
| | | 439 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 440 | | public static Fixed64 RadToDeg(Fixed64 rad) => |
| | 40 | 441 | | Fixed64.MultiplyDivide(rad, Fixed64.OneEighty, Fixed64.Pi, out _); |
| | | 442 | | |
| | | 443 | | /// <summary> |
| | | 444 | | /// Converts a value in degrees to radians. |
| | | 445 | | /// </summary> |
| | | 446 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 447 | | public static Fixed64 DegToRad(Fixed64 deg) => |
| | 1148 | 448 | | Fixed64.MultiplyDivide(deg, Fixed64.Pi, Fixed64.OneEighty, out _); |
| | | 449 | | |
| | | 450 | | /// <summary> |
| | | 451 | | /// Computes the sine of a given angle in radians using complementary |
| | | 452 | | /// reduced-range polynomial approximations. |
| | | 453 | | /// </summary> |
| | | 454 | | /// <param name="x">The angle in radians.</param> |
| | | 455 | | /// <returns>The sine of the given angle, in fixed-point format.</returns> |
| | | 456 | | /// <remarks> |
| | | 457 | | /// The input is normalized to [-π, π], reflected into [0, π/2], and |
| | | 458 | | /// evaluated as sine on [0, π/4] or cosine on [0, π/4]. Exact quadrant |
| | | 459 | | /// anchors remain exact without introducing a discontinuity beside them. |
| | | 460 | | /// </remarks> |
| | | 461 | | public static Fixed64 Sin(Fixed64 x) |
| | | 462 | | { |
| | | 463 | | // Check for special cases |
| | 94097 | 464 | | if (x == Fixed64.Zero) return Fixed64.Zero; // sin(0) = 0 |
| | 79582 | 465 | | if (x == Fixed64.HalfPi) return Fixed64.One; // sin(π/2) = 1 |
| | 45786 | 466 | | if (x == -Fixed64.HalfPi) return -Fixed64.One; // sin(-π/2) = -1 |
| | 45747 | 467 | | if (x == Fixed64.Pi) return Fixed64.Zero; // sin(π) = 0 |
| | 46949 | 468 | | if (x == -Fixed64.Pi) return Fixed64.Zero; // sin(-π) = 0 |
| | 44529 | 469 | | if (x == Fixed64.TwoPi || x == -Fixed64.TwoPi) return Fixed64.Zero; // sin(2π) = 0 |
| | | 470 | | |
| | | 471 | | // Normalize x to [-π, π] |
| | 44521 | 472 | | x %= Fixed64.TwoPi; |
| | 44521 | 473 | | if (x < -Fixed64.Pi) |
| | 11274 | 474 | | x += Fixed64.TwoPi; |
| | 33247 | 475 | | else if (x > Fixed64.Pi) |
| | 2058 | 476 | | x -= Fixed64.TwoPi; |
| | | 477 | | |
| | 44521 | 478 | | bool flip = false; |
| | 44521 | 479 | | if (x < Fixed64.Zero) |
| | | 480 | | { |
| | 16298 | 481 | | x = -x; |
| | 16298 | 482 | | flip = true; |
| | | 483 | | } |
| | | 484 | | |
| | 44521 | 485 | | if (x > Fixed64.HalfPi) |
| | 19003 | 486 | | x = Fixed64.Pi - x; |
| | | 487 | | |
| | 44521 | 488 | | Fixed64 result = SinReduced(x); |
| | | 489 | | |
| | 44521 | 490 | | return flip ? -result : result; |
| | | 491 | | } |
| | | 492 | | |
| | | 493 | | /// <summary> |
| | | 494 | | /// Computes the cosine of a given angle in radians using a sine-based identity transformation. |
| | | 495 | | /// </summary> |
| | | 496 | | /// <param name="x">The angle in radians.</param> |
| | | 497 | | /// <returns>The cosine of the given angle, in fixed-point format.</returns> |
| | | 498 | | /// <remarks> |
| | | 499 | | /// - Instead of directly approximating cosine, this function derives <c>cos(x)</c> using |
| | | 500 | | /// the identity <c>cos(x) = sin(x + π/2)</c>. |
| | | 501 | | /// - The underlying sine function uses complementary reduced-range |
| | | 502 | | /// sine and cosine polynomials so quadrant anchors remain continuous. |
| | | 503 | | /// - The function automatically normalizes input values to the range [-π, π] for stability. |
| | | 504 | | /// </remarks> |
| | | 505 | | public static Fixed64 Cos(Fixed64 x) |
| | | 506 | | { |
| | 39190 | 507 | | long xl = x.m_rawValue; |
| | 39190 | 508 | | long rawAngle = xl + (xl > 0 ? -Fixed64.Pi.m_rawValue - Fixed64.HalfPi.m_rawValue : Fixed64.HalfPi.m_rawValue); |
| | 39190 | 509 | | return Sin(Fixed64.FromRaw(rawAngle)); |
| | | 510 | | } |
| | | 511 | | |
| | | 512 | | /// <summary> |
| | | 513 | | /// Calculates the hypotenuse of a right triangle given sides a and b using the Pythagorean theorem: sqrt(a^2 + b^2) |
| | | 514 | | /// </summary> |
| | | 515 | | /// <param name="a">The length of side a.</param> |
| | | 516 | | /// <param name="b">The length of side b.</param> |
| | | 517 | | /// <returns>The length of the hypotenuse.</returns> |
| | | 518 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 519 | | public static Fixed64 GetHypotenuse(Fixed64 a, Fixed64 b) |
| | | 520 | | { |
| | 5 | 521 | | Fixed64 squareSum = a * a + b * b; |
| | 5 | 522 | | return squareSum == Fixed64.MaxValue |
| | 5 | 523 | | ? GetScaledMagnitude(a, b, Fixed64.Zero, Fixed64.Zero) |
| | 5 | 524 | | : Sqrt(squareSum); |
| | | 525 | | } |
| | | 526 | | |
| | | 527 | | internal static Fixed64 GetScaledMagnitude(Fixed64 x, Fixed64 y, Fixed64 z, Fixed64 w) |
| | | 528 | | { |
| | 9751 | 529 | | x = Abs(x); |
| | 9751 | 530 | | y = Abs(y); |
| | 9751 | 531 | | z = Abs(z); |
| | 9751 | 532 | | w = Abs(w); |
| | 9751 | 533 | | Fixed64 scale = Max(Max(x, y), Max(z, w)); |
| | 9751 | 534 | | if (scale == Fixed64.Zero) |
| | 51 | 535 | | return Fixed64.Zero; |
| | | 536 | | |
| | 9700 | 537 | | x /= scale; |
| | 9700 | 538 | | y /= scale; |
| | 9700 | 539 | | z /= scale; |
| | 9700 | 540 | | w /= scale; |
| | 9700 | 541 | | return scale * Sqrt(x * x + y * y + z * z + w * w); |
| | | 542 | | } |
| | | 543 | | |
| | | 544 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 545 | | internal static bool TryGetScaledMagnitude( |
| | | 546 | | Fixed64 x, |
| | | 547 | | Fixed64 y, |
| | | 548 | | Fixed64 z, |
| | | 549 | | Fixed64 w, |
| | | 550 | | out Fixed64 magnitude) |
| | | 551 | | { |
| | 67 | 552 | | if (!Fixed64.IsMagnitudeRepresentable(x, y, z, w)) |
| | | 553 | | { |
| | 40 | 554 | | magnitude = Fixed64.MaxValue; |
| | 40 | 555 | | return false; |
| | | 556 | | } |
| | | 557 | | |
| | 27 | 558 | | magnitude = GetScaledMagnitude(x, y, z, w); |
| | 27 | 559 | | return true; |
| | | 560 | | } |
| | | 561 | | |
| | | 562 | | /// <summary> |
| | | 563 | | /// Calculates the cosine value corresponding to a given sine value, assuming the angle is in the first or |
| | | 564 | | /// second quadrant. |
| | | 565 | | /// </summary> |
| | | 566 | | /// <remarks> |
| | | 567 | | /// This method returns the principal (non-negative) value of the cosine. |
| | | 568 | | /// If the input is outside the valid range for sine values, the result may not be meaningful. |
| | | 569 | | /// </remarks> |
| | | 570 | | /// <param name="sin">The sine of the angle. Must be in the range [-1, 1].</param> |
| | | 571 | | /// <returns>The cosine of the angle, computed as the positive square root of (1 - sin²).</returns> |
| | 1 | 572 | | public static Fixed64 SinToCos(Fixed64 sin) => Sqrt(Fixed64.One - sin * sin); |
| | | 573 | | |
| | | 574 | | /// <summary> |
| | | 575 | | /// Returns the tangent of x. |
| | | 576 | | /// </summary> |
| | | 577 | | /// <remarks> |
| | | 578 | | /// This function is not well-tested. It may be wildly inaccurate. |
| | | 579 | | /// </remarks> |
| | | 580 | | public static Fixed64 Tan(Fixed64 x) |
| | | 581 | | { |
| | | 582 | | // Check for special cases |
| | 25 | 583 | | if (x == Fixed64.Zero) return Fixed64.Zero; |
| | 29 | 584 | | if (x == Fixed64.PiOver4) return Fixed64.One; |
| | 18 | 585 | | if (x == -Fixed64.PiOver4) return -Fixed64.One; |
| | | 586 | | |
| | | 587 | | // Normalize x to [-π/2, π/2] |
| | 16 | 588 | | x %= Fixed64.Pi; |
| | 16 | 589 | | if (x < -Fixed64.HalfPi) |
| | 1 | 590 | | x += Fixed64.Pi; |
| | 15 | 591 | | else if (x > Fixed64.HalfPi) |
| | 1 | 592 | | x -= Fixed64.Pi; |
| | | 593 | | |
| | 16 | 594 | | bool flip = x < Fixed64.Zero; |
| | 16 | 595 | | if (flip) |
| | 6 | 596 | | x = -x; |
| | | 597 | | |
| | 16 | 598 | | Fixed64 sin = SinReduced(x); |
| | 16 | 599 | | Fixed64 cos = SinReduced(Fixed64.HalfPi - x); |
| | 16 | 600 | | Fixed64 result = sin / cos; |
| | | 601 | | |
| | 16 | 602 | | return flip ? -result : result; |
| | | 603 | | } |
| | | 604 | | |
| | | 605 | | /// <summary> |
| | | 606 | | /// Computes sine on [0, π/2] using complementary approximations on [0, π/4]. |
| | | 607 | | /// </summary> |
| | | 608 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 609 | | private static Fixed64 SinReduced(Fixed64 x) |
| | | 610 | | { |
| | 44553 | 611 | | if (x > Fixed64.PiOver4) |
| | 20981 | 612 | | return CosReduced(Fixed64.HalfPi - x); |
| | | 613 | | |
| | 23572 | 614 | | Fixed64 x2 = x * x; |
| | 23572 | 615 | | Fixed64 x4 = x2 * x2; |
| | | 616 | | |
| | 23572 | 617 | | return x * (Fixed64.One |
| | 23572 | 618 | | - x2 * Fixed64.SinCoeff3 |
| | 23572 | 619 | | + x4 * Fixed64.SinCoeff5 |
| | 23572 | 620 | | - x4 * x2 * Fixed64.SinCoeff7); |
| | | 621 | | } |
| | | 622 | | |
| | | 623 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 624 | | private static Fixed64 CosReduced(Fixed64 x) |
| | | 625 | | { |
| | 20981 | 626 | | Fixed64 x2 = x * x; |
| | 20981 | 627 | | return Fixed64.One - x2 * ( |
| | 20981 | 628 | | Fixed64.CosCoeff2 - x2 * ( |
| | 20981 | 629 | | Fixed64.CosCoeff4 - x2 * ( |
| | 20981 | 630 | | Fixed64.CosCoeff6 - x2 * ( |
| | 20981 | 631 | | Fixed64.CosCoeff8)))); |
| | | 632 | | } |
| | | 633 | | |
| | | 634 | | /// <summary> |
| | | 635 | | /// Returns the arc-sine of a fixed-point number x, which is the angle in radians |
| | | 636 | | /// whose sine is x, using a combination of a Taylor series expansion and trigonometric identities. |
| | | 637 | | /// |
| | | 638 | | /// For values of x near ±1, the identity asin(x) = π/2 - acos(x) is used for stability. |
| | | 639 | | /// For values of x near 0, a Taylor series expansion is used. |
| | | 640 | | /// </summary> |
| | | 641 | | /// <param name="x">The input value (sine) whose arcsine is to be computed. Should be in the range [-1, 1].</param> |
| | | 642 | | /// <returns>The arc-sine of x in radians.</returns> |
| | | 643 | | /// <exception cref="ArithmeticException">Thrown if x is outside the domain [-1, 1].</exception> |
| | | 644 | | public static Fixed64 Asin(Fixed64 x) |
| | | 645 | | { |
| | | 646 | | // Ensure x is within the domain [-1, 1] |
| | 15 | 647 | | if (x < -Fixed64.One || x > Fixed64.One) |
| | 2 | 648 | | throw new ArithmeticException("Input out of domain for Asin: " + x); |
| | | 649 | | |
| | | 650 | | // Handle boundary cases for -1 and 1 |
| | 14 | 651 | | if (x == Fixed64.One) return Fixed64.HalfPi; // asin(1) = π/2 |
| | 13 | 652 | | if (x == -Fixed64.One) return -Fixed64.HalfPi; // asin(-1) = -π/2 |
| | | 653 | | |
| | | 654 | | // Special case handling for asin(0.5) -> π/6 and asin(-0.5) -> -π/6 |
| | 14 | 655 | | if (x == Fixed64.Half) return Fixed64.PiOver6; |
| | 9 | 656 | | if (x == -Fixed64.Half) return -Fixed64.PiOver6; |
| | | 657 | | |
| | | 658 | | // For values close to 0, use a Padé approximation for better precision |
| | 7 | 659 | | if (x.Abs() < Fixed64.Half) |
| | | 660 | | { |
| | | 661 | | // Padé approximation of asin(x) for |x| < 0.5 |
| | 5 | 662 | | Fixed64 xSquared = x * x; |
| | 5 | 663 | | Fixed64 numerator = x * (Fixed64.One + (xSquared * (Fixed64.PadeA1 + (xSquared * Fixed64.PadeA2)))); |
| | 5 | 664 | | return numerator; |
| | | 665 | | } |
| | | 666 | | |
| | 2 | 667 | | return x > Fixed64.Zero |
| | 2 | 668 | | ? Fixed64.HalfPi - Acos(x) |
| | 2 | 669 | | : -Fixed64.HalfPi + Acos(-x); |
| | | 670 | | } |
| | | 671 | | |
| | | 672 | | /// <summary> |
| | | 673 | | /// Returns the arccosine of the specified number x, calculated using a combination of the atan and sqrt functions. |
| | | 674 | | /// </summary> |
| | | 675 | | /// <param name="x">The input value whose arccosine is to be computed. Should be in the range [-1, 1].</param> |
| | | 676 | | /// <returns>The arccosine of x in radians.</returns> |
| | | 677 | | /// <exception cref="ArgumentOutOfRangeException">Thrown if x is outside the domain [-1, 1].</exception> |
| | | 678 | | public static Fixed64 Acos(Fixed64 x) |
| | | 679 | | { |
| | 45 | 680 | | if (Abs(x) > Fixed64.One) |
| | 4 | 681 | | throw new ArithmeticException("Input out of domain for Acos: " + x); |
| | | 682 | | |
| | | 683 | | // For values near 1 or -1, the result is directly known. |
| | 49 | 684 | | if (x == Fixed64.One) return Fixed64.Zero; // acos(1) = 0 |
| | 38 | 685 | | if (x == -Fixed64.One) return Fixed64.Pi; // acos(-1) = π |
| | 35 | 686 | | if (x == Fixed64.Zero) return Fixed64.HalfPi; // acos(0) = π/2 |
| | | 687 | | |
| | | 688 | | // Compute using the relationship acos(x) = atan(sqrt(1 - x^2) / x) + π/2 when x is negative |
| | 21 | 689 | | var sqrtTerm = Sqrt(Fixed64.One - x * x); // sqrt(1 - x^2) |
| | 21 | 690 | | var atanTerm = Atan(sqrtTerm / x); |
| | | 691 | | |
| | 21 | 692 | | return x < Fixed64.Zero |
| | 21 | 693 | | ? atanTerm + Fixed64.Pi // acos(-x) = atan(...) + π |
| | 21 | 694 | | : atanTerm; // Otherwise, return just atan(sqrt(...)) |
| | | 695 | | } |
| | | 696 | | |
| | | 697 | | /// <summary> |
| | | 698 | | /// Returns the arctangent of the specified number, using a more accurate approximation for larger values. |
| | | 699 | | /// This function has at least 7 decimals of accuracy. |
| | | 700 | | /// </summary> |
| | | 701 | | public static Fixed64 Atan(Fixed64 z) |
| | | 702 | | { |
| | 153 | 703 | | if (z == Fixed64.Zero) return Fixed64.Zero; |
| | 155 | 704 | | if (z == Fixed64.One) return Fixed64.PiOver4; |
| | 117 | 705 | | if (z == -Fixed64.One) return -Fixed64.PiOver4; |
| | | 706 | | |
| | 105 | 707 | | bool neg = z < Fixed64.Zero; |
| | 118 | 708 | | if (neg) z = -z; |
| | | 709 | | |
| | | 710 | | |
| | | 711 | | Fixed64 adjustedResult; |
| | | 712 | | // Adjust series for z > 1 using the identity atan(z) = π/2 - atan(1/z) |
| | 105 | 713 | | if (z > Fixed64.One) |
| | 27 | 714 | | adjustedResult = Fixed64.HalfPi - Atan(Fixed64.One / z); |
| | | 715 | | // For z in (0.5, 1], use a transformation to improve convergence: atan(z) = π/4 - atan((1 - z) / (1 + z)) |
| | 78 | 716 | | else if (z > Fixed64.Half) |
| | | 717 | | { |
| | | 718 | | |
| | 21 | 719 | | Fixed64 transformedZ = (Fixed64.One - z) / (Fixed64.One + z); |
| | 21 | 720 | | adjustedResult = Fixed64.PiOver4 - Atan(transformedZ); |
| | | 721 | | } |
| | | 722 | | // For z in (0, 0.5], use the standard Taylor series expansion around 0 for better precision on small values. |
| | | 723 | | else |
| | | 724 | | { |
| | 57 | 725 | | Fixed64 zSq = z * z; |
| | | 726 | | |
| | 57 | 727 | | Fixed64 result = z; |
| | 57 | 728 | | Fixed64 term = z; |
| | 57 | 729 | | int sign = -1; |
| | | 730 | | |
| | 510 | 731 | | for (int i = 3; i < 15; i += 2) |
| | | 732 | | { |
| | 239 | 733 | | term *= zSq; |
| | 239 | 734 | | Fixed64 nextTerm = term / i; |
| | 239 | 735 | | if (nextTerm.Abs() < Fixed64.Epsilon) |
| | | 736 | | break; |
| | | 737 | | |
| | 198 | 738 | | result += nextTerm * sign; |
| | 198 | 739 | | sign = -sign; |
| | | 740 | | } |
| | | 741 | | |
| | 57 | 742 | | adjustedResult = result; |
| | | 743 | | } |
| | | 744 | | |
| | 105 | 745 | | return neg ? -adjustedResult : adjustedResult; |
| | | 746 | | } |
| | | 747 | | |
| | | 748 | | /// <summary> |
| | | 749 | | /// Computes the angle whose tangent is the quotient of two specified numbers. |
| | | 750 | | /// </summary> |
| | | 751 | | /// <remarks> |
| | | 752 | | /// Uses a fixed-point arithmetic approximation for the arc tangent function, which is more efficient than using flo |
| | | 753 | | /// especially on systems where floating-point operations are expensive. |
| | | 754 | | /// </remarks> |
| | | 755 | | /// <param name="y">The y-coordinate of the point to which the angle is measured.</param> |
| | | 756 | | /// <param name="x">The x-coordinate of the point to which the angle is measured.</param> |
| | | 757 | | /// <returns>An angle, θ, measured in radians, such that -π ≤ θ ≤ π, and tan(θ) = y / x, |
| | | 758 | | /// taking into account the quadrants of the inputs to determine the sign of the result.</returns> |
| | | 759 | | public static Fixed64 Atan2(Fixed64 y, Fixed64 x) |
| | | 760 | | { |
| | 46 | 761 | | if (x == Fixed64.Zero) |
| | | 762 | | { |
| | 7 | 763 | | if (y > Fixed64.Zero) |
| | 3 | 764 | | return Fixed64.HalfPi; |
| | 4 | 765 | | if (y == Fixed64.Zero) |
| | 2 | 766 | | return Fixed64.Zero; |
| | 2 | 767 | | return -Fixed64.HalfPi; |
| | | 768 | | } |
| | | 769 | | |
| | 39 | 770 | | Fixed64 atan = Atan(y / x); |
| | | 771 | | |
| | | 772 | | // Adjust based on the quadrant |
| | 39 | 773 | | if (x < Fixed64.Zero) |
| | | 774 | | { |
| | 7 | 775 | | if (y >= Fixed64.Zero) |
| | | 776 | | { |
| | | 777 | | // Second quadrant |
| | 3 | 778 | | return atan + Fixed64.Pi; |
| | | 779 | | } |
| | | 780 | | else |
| | | 781 | | { |
| | | 782 | | // Third quadrant |
| | 4 | 783 | | return atan - Fixed64.Pi; |
| | | 784 | | } |
| | | 785 | | } |
| | | 786 | | |
| | | 787 | | // First or fourth quadrant |
| | 32 | 788 | | return atan; |
| | | 789 | | } |
| | | 790 | | |
| | | 791 | | #endregion |
| | | 792 | | } |