| | | 1 | | //======================================================================= |
| | | 2 | | // WideGeometry.AxisFrame.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.Runtime.CompilerServices; |
| | | 9 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Provides shared helper methods for computing support points, anchors, and axis-aligned |
| | | 14 | | /// transformations used by GJK/EPA-based wide (batched) collision detection routines. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static partial class WideGeometry |
| | | 17 | | { |
| | 1 | 18 | | internal static readonly Signed192 CenteredAxisDenominator = |
| | 1 | 19 | | Signed192.Signed(FixedMath.ONE_L * 2L); |
| | | 20 | | |
| | | 21 | | #region Nested Types |
| | | 22 | | |
| | | 23 | | private enum CenteredAxisSupportKind |
| | | 24 | | { |
| | | 25 | | Capsule, |
| | | 26 | | Cylinder, |
| | | 27 | | Cone, |
| | | 28 | | } |
| | | 29 | | |
| | | 30 | | #endregion |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// Returns the exact representable size of a normalized scalar interval. |
| | | 34 | | /// </summary> |
| | | 35 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 36 | | internal static Fixed64 GetIntervalSize(Fixed64 min, Fixed64 max) |
| | | 37 | | { |
| | 66 | 38 | | ulong span = unchecked((ulong)max.m_rawValue - (ulong)min.m_rawValue); |
| | 66 | 39 | | if (span > long.MaxValue) |
| | 4 | 40 | | throw new System.OverflowException("The interval size is outside the representable Fixed64 range."); |
| | | 41 | | |
| | 62 | 42 | | return Fixed64.FromRaw((long)span); |
| | | 43 | | } |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Returns the smallest representable half-extent that contains both ends |
| | | 47 | | /// of a normalized scalar interval around its lattice midpoint. |
| | | 48 | | /// </summary> |
| | | 49 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 50 | | internal static Fixed64 GetIntervalScope(Fixed64 min, Fixed64 max) |
| | | 51 | | { |
| | 50 | 52 | | ulong span = unchecked((ulong)max.m_rawValue - (ulong)min.m_rawValue); |
| | 50 | 53 | | ulong scope = (span >> 1) + (span & 1UL); |
| | 50 | 54 | | if (scope > long.MaxValue) |
| | 2 | 55 | | throw new System.OverflowException("The interval scope is outside the representable Fixed64 range."); |
| | | 56 | | |
| | 48 | 57 | | return Fixed64.FromRaw((long)scope); |
| | | 58 | | } |
| | | 59 | | |
| | | 60 | | /// <summary> |
| | | 61 | | /// Returns the representable absolute magnitude of a scalar extent. |
| | | 62 | | /// </summary> |
| | | 63 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 64 | | internal static Fixed64 GetExtentMagnitude(Fixed64 extent) |
| | | 65 | | { |
| | 47 | 66 | | if (extent == Fixed64.MinValue) |
| | 2 | 67 | | throw new System.OverflowException("The extent magnitude is outside the representable Fixed64 range."); |
| | | 68 | | |
| | 45 | 69 | | return extent.Abs(); |
| | | 70 | | } |
| | | 71 | | |
| | | 72 | | /// <summary> |
| | | 73 | | /// Returns the smallest scalar half-extent whose symmetric interval covers |
| | | 74 | | /// the requested size magnitude, including the minimum scalar input. |
| | | 75 | | /// </summary> |
| | | 76 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 77 | | internal static Fixed64 GetHalfSizeMagnitude(Fixed64 size) |
| | | 78 | | { |
| | 441 | 79 | | ulong magnitude = size.m_rawValue < 0L |
| | 441 | 80 | | ? unchecked(0UL - (ulong)size.m_rawValue) |
| | 441 | 81 | | : (ulong)size.m_rawValue; |
| | 441 | 82 | | ulong half = (magnitude >> 1) + (magnitude & 1UL); |
| | 441 | 83 | | return Fixed64.FromRaw((long)half); |
| | | 84 | | } |
| | | 85 | | |
| | | 86 | | /// <summary> |
| | | 87 | | /// Returns an exact full-domain AABB union-volume growth metric. |
| | | 88 | | /// </summary> |
| | | 89 | | internal static long GetVolumeExpansionCost( |
| | | 90 | | Vector3d min, |
| | | 91 | | Vector3d max, |
| | | 92 | | Vector3d otherMin, |
| | | 93 | | Vector3d otherMax) |
| | | 94 | | { |
| | 5 | 95 | | Signed192 volume = GetIntervalVolume(min, max); |
| | 5 | 96 | | Vector3d unionMin = Vector3d.Min(min, otherMin); |
| | 5 | 97 | | Vector3d unionMax = Vector3d.Max(max, otherMax); |
| | 5 | 98 | | Signed192 unionVolume = GetIntervalVolume(unionMin, unionMax); |
| | 5 | 99 | | Signed192 growth = WideArithmetic.SubtractSigned192(unionVolume, volume); |
| | | 100 | | |
| | | 101 | | // Q32.32 axis spans produce a Q96.96 product. Shifting by 96 floors |
| | | 102 | | // the exact volume to integer world units. Any remaining bit above the |
| | | 103 | | // signed 63-bit result range maps to the public metric's upper bound. |
| | 5 | 104 | | if (growth.High > 0x0000_0000_7FFF_FFFFUL) // 2,147,483,647: highest 31-bit word that fits after the 96-bit shif |
| | 1 | 105 | | return long.MaxValue; |
| | | 106 | | |
| | 4 | 107 | | ulong floor = (growth.High << 32) | (growth.Middle >> 32); |
| | 4 | 108 | | return (long)floor; |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 112 | | private static Signed192 GetIntervalVolume(Vector3d min, Vector3d max) |
| | | 113 | | { |
| | 10 | 114 | | ulong x = unchecked((ulong)max.X.m_rawValue - (ulong)min.X.m_rawValue); |
| | 10 | 115 | | ulong y = unchecked((ulong)max.Y.m_rawValue - (ulong)min.Y.m_rawValue); |
| | 10 | 116 | | ulong z = unchecked((ulong)max.Z.m_rawValue - (ulong)min.Z.m_rawValue); |
| | 10 | 117 | | Fixed64.Multiply64To128(x, y, out ulong productHigh, out ulong productLow); |
| | 10 | 118 | | Fixed64.Multiply64To128(productLow, z, out ulong lowHigh, out ulong low); |
| | 10 | 119 | | Fixed64.Multiply64To128(productHigh, z, out ulong high, out ulong highLow); |
| | 10 | 120 | | return WideArithmetic.AddSigned192( |
| | 10 | 121 | | new Signed192(0UL, lowHigh, low), |
| | 10 | 122 | | new Signed192(high, highLow, 0UL)); |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | /// <summary> |
| | | 126 | | /// Compares an exact 2D distance with the exact sum of two non-negative radii. |
| | | 127 | | /// </summary> |
| | | 128 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 129 | | internal static int CompareDistanceToRadiusSum( |
| | | 130 | | Vector2d first, |
| | | 131 | | Vector2d second, |
| | | 132 | | Fixed64 firstRadius, |
| | | 133 | | Fixed64 secondRadius) |
| | | 134 | | { |
| | 69 | 135 | | Signed192 squaredDistance = GetDifferenceDotProduct2D( |
| | 69 | 136 | | first.X, second.X, first.Y, second.Y, |
| | 69 | 137 | | first.X, second.X, first.Y, second.Y); |
| | 69 | 138 | | return WideArithmetic.CompareMagnitude( |
| | 69 | 139 | | squaredDistance, |
| | 69 | 140 | | GetSquaredRadiusSum(firstRadius, secondRadius)); |
| | | 141 | | } |
| | | 142 | | |
| | | 143 | | /// <summary> |
| | | 144 | | /// Compares an exact 3D distance with the exact sum of two non-negative radii. |
| | | 145 | | /// </summary> |
| | | 146 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 147 | | internal static int CompareDistanceToRadiusSum( |
| | | 148 | | Vector3d first, |
| | | 149 | | Vector3d second, |
| | | 150 | | Fixed64 firstRadius, |
| | | 151 | | Fixed64 secondRadius) |
| | | 152 | | { |
| | 390 | 153 | | Signed192 squaredDistance = GetDifferenceDotProduct3D( |
| | 390 | 154 | | first.X, second.X, first.Y, second.Y, first.Z, second.Z, |
| | 390 | 155 | | first.X, second.X, first.Y, second.Y, first.Z, second.Z); |
| | 390 | 156 | | return WideArithmetic.CompareMagnitude( |
| | 390 | 157 | | squaredDistance, |
| | 390 | 158 | | GetSquaredRadiusSum(firstRadius, secondRadius)); |
| | | 159 | | } |
| | | 160 | | |
| | | 161 | | /// <summary> |
| | | 162 | | /// Returns whether an axis interval contains the centered extent of a non-negative radius. |
| | | 163 | | /// </summary> |
| | | 164 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 165 | | internal static bool ContainsCenteredExtent( |
| | | 166 | | Fixed64 min, |
| | | 167 | | Fixed64 max, |
| | | 168 | | Fixed64 center, |
| | | 169 | | Fixed64 radius) |
| | | 170 | | { |
| | 16 | 171 | | if (center < min || center > max) |
| | 5 | 172 | | return false; |
| | | 173 | | |
| | 11 | 174 | | ulong radiusRaw = (ulong)radius.m_rawValue; |
| | 11 | 175 | | ulong minimumClearance = unchecked((ulong)center.m_rawValue - (ulong)min.m_rawValue); |
| | 11 | 176 | | ulong maximumClearance = unchecked((ulong)max.m_rawValue - (ulong)center.m_rawValue); |
| | 11 | 177 | | return minimumClearance >= radiusRaw && maximumClearance >= radiusRaw; |
| | | 178 | | } |
| | | 179 | | |
| | | 180 | | /// <summary> |
| | | 181 | | /// Attempts to return the rounded distance between two 2D endpoints without |
| | | 182 | | /// narrowing their component differences. |
| | | 183 | | /// </summary> |
| | | 184 | | internal static bool TryGetDistance(Vector2d start, Vector2d end, out Fixed64 distance) |
| | | 185 | | { |
| | 4 | 186 | | Signed192 squaredDistance = GetDifferenceDotProduct2D( |
| | 4 | 187 | | end.X, start.X, end.Y, start.Y, |
| | 4 | 188 | | end.X, start.X, end.Y, start.Y); |
| | 4 | 189 | | return TryRoundDistance(squaredDistance, out distance); |
| | | 190 | | } |
| | | 191 | | |
| | | 192 | | /// <summary> |
| | | 193 | | /// Attempts to return the rounded distance between two 3D endpoints without |
| | | 194 | | /// narrowing their component differences. |
| | | 195 | | /// </summary> |
| | | 196 | | internal static bool TryGetDistance(Vector3d start, Vector3d end, out Fixed64 distance) |
| | | 197 | | { |
| | 24 | 198 | | Signed192 squaredDistance = GetDifferenceDotProduct3D( |
| | 24 | 199 | | end.X, start.X, end.Y, start.Y, end.Z, start.Z, |
| | 24 | 200 | | end.X, start.X, end.Y, start.Y, end.Z, start.Z); |
| | 24 | 201 | | return TryRoundDistance(squaredDistance, out distance); |
| | | 202 | | } |
| | | 203 | | |
| | | 204 | | /// <summary> |
| | | 205 | | /// Interpolates one coordinate from an exact nonnegative numerator and |
| | | 206 | | /// denominator with one final round-half-to-even conversion. |
| | | 207 | | /// </summary> |
| | | 208 | | internal static Fixed64 InterpolateCoordinate( |
| | | 209 | | Fixed64 start, |
| | | 210 | | Fixed64 end, |
| | | 211 | | Signed192 numerator, |
| | | 212 | | Signed192 denominator) |
| | | 213 | | { |
| | 209 | 214 | | if (start == end) |
| | 60 | 215 | | return start; |
| | | 216 | | |
| | 149 | 217 | | Signed192 remaining = WideArithmetic.SubtractSigned192(denominator, numerator); |
| | 149 | 218 | | Signed320 weighted = WideArithmetic.AddSigned320( |
| | 149 | 219 | | WideArithmetic.MultiplySigned192( |
| | 149 | 220 | | Signed192.Signed(start.m_rawValue), |
| | 149 | 221 | | remaining), |
| | 149 | 222 | | WideArithmetic.MultiplySigned192( |
| | 149 | 223 | | Signed192.Signed(end.m_rawValue), |
| | 149 | 224 | | numerator)); |
| | 149 | 225 | | return Fixed64.GetSignedRawRatio(weighted, denominator); |
| | | 226 | | } |
| | | 227 | | |
| | | 228 | | private static bool TryRoundDistance(Signed192 squaredDistance, out Fixed64 distance) |
| | | 229 | | { |
| | 28 | 230 | | Signed192 root = WideArithmetic.GetFloorSquareRoot( |
| | 28 | 231 | | Signed320.ExtendValue(squaredDistance), |
| | 28 | 232 | | out Signed192 remainder); |
| | 28 | 233 | | WideArithmetic.GetMagnitude(root, out ulong high, out ulong middle, out ulong low); |
| | | 234 | | // A 2D/3D Fixed64 endpoint difference cannot produce a distance root |
| | | 235 | | // beyond the middle word; only its representable low-word limit varies. |
| | 28 | 236 | | if (middle != 0UL || low > (ulong)long.MaxValue) |
| | | 237 | | { |
| | 3 | 238 | | distance = Fixed64.MaxValue; |
| | 3 | 239 | | return false; |
| | | 240 | | } |
| | | 241 | | |
| | | 242 | | // For integer n = root^2 + remainder, sqrt(n) rounds upward exactly |
| | | 243 | | // when remainder is at least root + 1. A half-way tie is impossible. |
| | 25 | 244 | | if (WideArithmetic.CompareMagnitude(remainder, root) > 0) |
| | | 245 | | { |
| | 11 | 246 | | if (low == (ulong)long.MaxValue) |
| | | 247 | | { |
| | 1 | 248 | | distance = Fixed64.MaxValue; |
| | 1 | 249 | | return false; |
| | | 250 | | } |
| | | 251 | | |
| | 10 | 252 | | low++; |
| | | 253 | | } |
| | | 254 | | |
| | 24 | 255 | | distance = Fixed64.FromRaw((long)low); |
| | 24 | 256 | | return true; |
| | | 257 | | } |
| | | 258 | | |
| | | 259 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 260 | | private static Signed192 GetSquaredRadiusSum(Fixed64 firstRadius, Fixed64 secondRadius) |
| | | 261 | | { |
| | 459 | 262 | | ulong radiusSum = unchecked((ulong)firstRadius.m_rawValue + (ulong)secondRadius.m_rawValue); |
| | 459 | 263 | | Fixed64.Multiply64To128(radiusSum, radiusSum, out ulong middle, out ulong low); |
| | 459 | 264 | | return new Signed192(0UL, middle, low); |
| | | 265 | | } |
| | | 266 | | |
| | | 267 | | /// <summary> |
| | | 268 | | /// Returns the exact dot product of two two-dimensional endpoint differences. |
| | | 269 | | /// </summary> |
| | | 270 | | internal static Signed192 GetDifferenceDotProduct2D( |
| | | 271 | | Fixed64 leftEndX, |
| | | 272 | | Fixed64 leftStartX, |
| | | 273 | | Fixed64 leftEndY, |
| | | 274 | | Fixed64 leftStartY, |
| | | 275 | | Fixed64 rightEndX, |
| | | 276 | | Fixed64 rightStartX, |
| | | 277 | | Fixed64 rightEndY, |
| | | 278 | | Fixed64 rightStartY) |
| | | 279 | | { |
| | 248792 | 280 | | ulong high = 0UL; |
| | 248792 | 281 | | ulong middle = 0UL; |
| | 248792 | 282 | | ulong low = 0UL; |
| | 248792 | 283 | | AccumulateDifferenceProduct( |
| | 248792 | 284 | | leftEndX.m_rawValue, |
| | 248792 | 285 | | leftStartX.m_rawValue, |
| | 248792 | 286 | | rightEndX.m_rawValue, |
| | 248792 | 287 | | rightStartX.m_rawValue, |
| | 248792 | 288 | | ref high, |
| | 248792 | 289 | | ref middle, |
| | 248792 | 290 | | ref low); |
| | 248792 | 291 | | AccumulateDifferenceProduct( |
| | 248792 | 292 | | leftEndY.m_rawValue, |
| | 248792 | 293 | | leftStartY.m_rawValue, |
| | 248792 | 294 | | rightEndY.m_rawValue, |
| | 248792 | 295 | | rightStartY.m_rawValue, |
| | 248792 | 296 | | ref high, |
| | 248792 | 297 | | ref middle, |
| | 248792 | 298 | | ref low); |
| | 248792 | 299 | | return new Signed192(high, middle, low); |
| | | 300 | | } |
| | | 301 | | |
| | | 302 | | /// <summary> |
| | | 303 | | /// Returns the exact dot product of two three-dimensional endpoint differences. |
| | | 304 | | /// </summary> |
| | | 305 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 306 | | internal static Signed192 GetDifferenceDotProduct3D( |
| | | 307 | | Fixed64 leftEndX, |
| | | 308 | | Fixed64 leftStartX, |
| | | 309 | | Fixed64 leftEndY, |
| | | 310 | | Fixed64 leftStartY, |
| | | 311 | | Fixed64 leftEndZ, |
| | | 312 | | Fixed64 leftStartZ, |
| | | 313 | | Fixed64 rightEndX, |
| | | 314 | | Fixed64 rightStartX, |
| | | 315 | | Fixed64 rightEndY, |
| | | 316 | | Fixed64 rightStartY, |
| | | 317 | | Fixed64 rightEndZ, |
| | | 318 | | Fixed64 rightStartZ) |
| | | 319 | | { |
| | 30585 | 320 | | if (TrySubtractRaw(leftEndX.m_rawValue, leftStartX.m_rawValue, out long leftX) |
| | 30585 | 321 | | && TrySubtractRaw(leftEndY.m_rawValue, leftStartY.m_rawValue, out long leftY) |
| | 30585 | 322 | | && TrySubtractRaw(leftEndZ.m_rawValue, leftStartZ.m_rawValue, out long leftZ) |
| | 30585 | 323 | | && TrySubtractRaw(rightEndX.m_rawValue, rightStartX.m_rawValue, out long rightX) |
| | 30585 | 324 | | && TrySubtractRaw(rightEndY.m_rawValue, rightStartY.m_rawValue, out long rightY) |
| | 30585 | 325 | | && TrySubtractRaw(rightEndZ.m_rawValue, rightStartZ.m_rawValue, out long rightZ)) |
| | | 326 | | { |
| | 30187 | 327 | | ulong narrowHigh = 0UL; |
| | 30187 | 328 | | ulong narrowMiddle = 0UL; |
| | 30187 | 329 | | ulong narrowLow = 0UL; |
| | 30187 | 330 | | AccumulateRawProduct(leftX, rightX, false, ref narrowHigh, ref narrowMiddle, ref narrowLow); |
| | 30187 | 331 | | AccumulateRawProduct(leftY, rightY, false, ref narrowHigh, ref narrowMiddle, ref narrowLow); |
| | 30187 | 332 | | AccumulateRawProduct(leftZ, rightZ, false, ref narrowHigh, ref narrowMiddle, ref narrowLow); |
| | 30187 | 333 | | return new Signed192(narrowHigh, narrowMiddle, narrowLow); |
| | | 334 | | } |
| | | 335 | | |
| | 398 | 336 | | ulong high = 0UL; |
| | 398 | 337 | | ulong middle = 0UL; |
| | 398 | 338 | | ulong low = 0UL; |
| | 398 | 339 | | AccumulateDifferenceProduct( |
| | 398 | 340 | | leftEndX.m_rawValue, |
| | 398 | 341 | | leftStartX.m_rawValue, |
| | 398 | 342 | | rightEndX.m_rawValue, |
| | 398 | 343 | | rightStartX.m_rawValue, |
| | 398 | 344 | | ref high, |
| | 398 | 345 | | ref middle, |
| | 398 | 346 | | ref low); |
| | 398 | 347 | | AccumulateDifferenceProduct( |
| | 398 | 348 | | leftEndY.m_rawValue, |
| | 398 | 349 | | leftStartY.m_rawValue, |
| | 398 | 350 | | rightEndY.m_rawValue, |
| | 398 | 351 | | rightStartY.m_rawValue, |
| | 398 | 352 | | ref high, |
| | 398 | 353 | | ref middle, |
| | 398 | 354 | | ref low); |
| | 398 | 355 | | AccumulateDifferenceProduct( |
| | 398 | 356 | | leftEndZ.m_rawValue, |
| | 398 | 357 | | leftStartZ.m_rawValue, |
| | 398 | 358 | | rightEndZ.m_rawValue, |
| | 398 | 359 | | rightStartZ.m_rawValue, |
| | 398 | 360 | | ref high, |
| | 398 | 361 | | ref middle, |
| | 398 | 362 | | ref low); |
| | 398 | 363 | | return new Signed192(high, middle, low); |
| | | 364 | | } |
| | | 365 | | |
| | | 366 | | /// <summary> |
| | | 367 | | /// Returns whether an exact squared direction rounds to zero in Q32.32. |
| | | 368 | | /// </summary> |
| | | 369 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 370 | | internal static bool IsSquaredLengthDegenerate(Signed192 value) => |
| | 287 | 371 | | value.High == 0UL |
| | 287 | 372 | | && value.Middle == 0UL |
| | 287 | 373 | | && value.Low <= (1UL << (FixedMath.SHIFT_AMOUNT_I - 1)); |
| | | 374 | | |
| | | 375 | | /// <summary> |
| | | 376 | | /// Returns the exact 2D cross product of two endpoint differences. |
| | | 377 | | /// </summary> |
| | | 378 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 379 | | internal static Signed192 GetDifferenceCrossProduct2D( |
| | | 380 | | Fixed64 leftEndX, |
| | | 381 | | Fixed64 leftStartX, |
| | | 382 | | Fixed64 leftEndY, |
| | | 383 | | Fixed64 leftStartY, |
| | | 384 | | Fixed64 rightEndX, |
| | | 385 | | Fixed64 rightStartX, |
| | | 386 | | Fixed64 rightEndY, |
| | | 387 | | Fixed64 rightStartY) |
| | | 388 | | { |
| | 4730 | 389 | | ulong high = 0UL; |
| | 4730 | 390 | | ulong middle = 0UL; |
| | 4730 | 391 | | ulong low = 0UL; |
| | 4730 | 392 | | AccumulateDifferenceProduct( |
| | 4730 | 393 | | leftEndX.m_rawValue, |
| | 4730 | 394 | | leftStartX.m_rawValue, |
| | 4730 | 395 | | rightEndY.m_rawValue, |
| | 4730 | 396 | | rightStartY.m_rawValue, |
| | 4730 | 397 | | ref high, |
| | 4730 | 398 | | ref middle, |
| | 4730 | 399 | | ref low); |
| | 4730 | 400 | | AccumulateDifferenceProduct( |
| | 4730 | 401 | | leftEndY.m_rawValue, |
| | 4730 | 402 | | leftStartY.m_rawValue, |
| | 4730 | 403 | | rightStartX.m_rawValue, |
| | 4730 | 404 | | rightEndX.m_rawValue, |
| | 4730 | 405 | | ref high, |
| | 4730 | 406 | | ref middle, |
| | 4730 | 407 | | ref low); |
| | 4730 | 408 | | return new Signed192(high, middle, low); |
| | | 409 | | } |
| | | 410 | | |
| | | 411 | | /// <summary> |
| | | 412 | | /// Returns all three exact components of a 3D endpoint-difference cross product. |
| | | 413 | | /// </summary> |
| | | 414 | | internal static void GetDifferenceCrossProduct3D( |
| | | 415 | | Fixed64 leftEndX, |
| | | 416 | | Fixed64 leftStartX, |
| | | 417 | | Fixed64 leftEndY, |
| | | 418 | | Fixed64 leftStartY, |
| | | 419 | | Fixed64 leftEndZ, |
| | | 420 | | Fixed64 leftStartZ, |
| | | 421 | | Fixed64 rightEndX, |
| | | 422 | | Fixed64 rightStartX, |
| | | 423 | | Fixed64 rightEndY, |
| | | 424 | | Fixed64 rightStartY, |
| | | 425 | | Fixed64 rightEndZ, |
| | | 426 | | Fixed64 rightStartZ, |
| | | 427 | | out Signed192 x, |
| | | 428 | | out Signed192 y, |
| | | 429 | | out Signed192 z) |
| | | 430 | | { |
| | 3673 | 431 | | if (TrySubtractRaw(leftEndX.m_rawValue, leftStartX.m_rawValue, out long leftX) |
| | 3673 | 432 | | && TrySubtractRaw(leftEndY.m_rawValue, leftStartY.m_rawValue, out long leftY) |
| | 3673 | 433 | | && TrySubtractRaw(leftEndZ.m_rawValue, leftStartZ.m_rawValue, out long leftZ) |
| | 3673 | 434 | | && TrySubtractRaw(rightEndX.m_rawValue, rightStartX.m_rawValue, out long rightX) |
| | 3673 | 435 | | && TrySubtractRaw(rightEndY.m_rawValue, rightStartY.m_rawValue, out long rightY) |
| | 3673 | 436 | | && TrySubtractRaw(rightEndZ.m_rawValue, rightStartZ.m_rawValue, out long rightZ)) |
| | | 437 | | { |
| | 3497 | 438 | | x = GetRawCrossComponent(leftY, leftZ, rightZ, rightY); |
| | 3497 | 439 | | y = GetRawCrossComponent(leftZ, leftX, rightX, rightZ); |
| | 3497 | 440 | | z = GetRawCrossComponent(leftX, leftY, rightY, rightX); |
| | 3497 | 441 | | return; |
| | | 442 | | } |
| | | 443 | | |
| | 176 | 444 | | x = GetDifferenceCrossProduct2D( |
| | 176 | 445 | | leftEndY, leftStartY, leftEndZ, leftStartZ, |
| | 176 | 446 | | rightEndY, rightStartY, rightEndZ, rightStartZ); |
| | 176 | 447 | | y = GetDifferenceCrossProduct2D( |
| | 176 | 448 | | leftEndZ, leftStartZ, leftEndX, leftStartX, |
| | 176 | 449 | | rightEndZ, rightStartZ, rightEndX, rightStartX); |
| | 176 | 450 | | z = GetDifferenceCrossProduct2D( |
| | 176 | 451 | | leftEndX, leftStartX, leftEndY, leftStartY, |
| | 176 | 452 | | rightEndX, rightStartX, rightEndY, rightStartY); |
| | 176 | 453 | | } |
| | | 454 | | |
| | | 455 | | /// <summary> |
| | | 456 | | /// Returns the exact nonnegative squared magnitude of three wide components. |
| | | 457 | | /// </summary> |
| | | 458 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 459 | | internal static Signed320 GetSquaredMagnitude( |
| | | 460 | | Signed192 x, |
| | | 461 | | Signed192 y, |
| | | 462 | | Signed192 z, |
| | | 463 | | out Signed320 xSquare, |
| | | 464 | | out Signed320 ySquare, |
| | | 465 | | out Signed320 zSquare) |
| | | 466 | | { |
| | 10481 | 467 | | xSquare = WideArithmetic.MultiplySigned192(x, x); |
| | 10481 | 468 | | ySquare = WideArithmetic.MultiplySigned192(y, y); |
| | 10481 | 469 | | zSquare = WideArithmetic.MultiplySigned192(z, z); |
| | 10481 | 470 | | return WideArithmetic.AddSigned320(WideArithmetic.AddSigned320(xSquare, ySquare), zSquare); |
| | | 471 | | } |
| | | 472 | | |
| | | 473 | | /// <summary> |
| | | 474 | | /// Applies the inclusive public epsilon threshold to an exact Q128.128 magnitude. |
| | | 475 | | /// </summary> |
| | | 476 | | internal static bool IsQ128MagnitudeAtMostEpsilon(Signed320 value) |
| | | 477 | | { |
| | 943 | 478 | | WideArithmetic.GetMagnitude( |
| | 943 | 479 | | value, |
| | 943 | 480 | | out ulong word4, |
| | 943 | 481 | | out ulong word3, |
| | 943 | 482 | | out ulong word2, |
| | 943 | 483 | | out ulong word1, |
| | 943 | 484 | | out ulong word0); |
| | 943 | 485 | | ulong epsilonRaw = (ulong)Fixed64.Epsilon.m_rawValue; |
| | 943 | 486 | | return WideArithmetic.CompareUnsigned( |
| | 943 | 487 | | word4, |
| | 943 | 488 | | word3, |
| | 943 | 489 | | word2, |
| | 943 | 490 | | word1, |
| | 943 | 491 | | word0, |
| | 943 | 492 | | 0UL, |
| | 943 | 493 | | 0UL, |
| | 943 | 494 | | epsilonRaw >> 32, |
| | 943 | 495 | | epsilonRaw << 32, |
| | 943 | 496 | | 0UL) <= 0; |
| | | 497 | | } |
| | | 498 | | |
| | | 499 | | /// <summary> |
| | | 500 | | /// Compares exact squared distances between two pairs of 3D points. |
| | | 501 | | /// </summary> |
| | | 502 | | internal static int CompareSquaredDistance3D( |
| | | 503 | | Fixed64 leftStartX, |
| | | 504 | | Fixed64 leftEndX, |
| | | 505 | | Fixed64 leftStartY, |
| | | 506 | | Fixed64 leftEndY, |
| | | 507 | | Fixed64 leftStartZ, |
| | | 508 | | Fixed64 leftEndZ, |
| | | 509 | | Fixed64 rightStartX, |
| | | 510 | | Fixed64 rightEndX, |
| | | 511 | | Fixed64 rightStartY, |
| | | 512 | | Fixed64 rightEndY, |
| | | 513 | | Fixed64 rightStartZ, |
| | | 514 | | Fixed64 rightEndZ) |
| | | 515 | | { |
| | 104 | 516 | | Signed192 left = GetDifferenceDotProduct3D( |
| | 104 | 517 | | leftStartX, leftEndX, leftStartY, leftEndY, leftStartZ, leftEndZ, |
| | 104 | 518 | | leftStartX, leftEndX, leftStartY, leftEndY, leftStartZ, leftEndZ); |
| | 104 | 519 | | Signed192 right = GetDifferenceDotProduct3D( |
| | 104 | 520 | | rightStartX, rightEndX, rightStartY, rightEndY, rightStartZ, rightEndZ, |
| | 104 | 521 | | rightStartX, rightEndX, rightStartY, rightEndY, rightStartZ, rightEndZ); |
| | 104 | 522 | | return WideArithmetic.CompareMagnitude(left, right); |
| | | 523 | | } |
| | | 524 | | |
| | | 525 | | /// <summary> |
| | | 526 | | /// Returns the exact sign of the scalar triple product of three raw |
| | | 527 | | /// three-component vectors. |
| | | 528 | | /// </summary> |
| | | 529 | | internal static int GetTripleProductSign( |
| | | 530 | | Fixed64 firstX, |
| | | 531 | | Fixed64 firstY, |
| | | 532 | | Fixed64 firstZ, |
| | | 533 | | Fixed64 secondX, |
| | | 534 | | Fixed64 secondY, |
| | | 535 | | Fixed64 secondZ, |
| | | 536 | | Fixed64 thirdX, |
| | | 537 | | Fixed64 thirdY, |
| | | 538 | | Fixed64 thirdZ) |
| | | 539 | | { |
| | 614 | 540 | | Signed192 minorX = GetDifferenceCrossProduct2D( |
| | 614 | 541 | | secondY, Fixed64.Zero, secondZ, Fixed64.Zero, |
| | 614 | 542 | | thirdY, Fixed64.Zero, thirdZ, Fixed64.Zero); |
| | 614 | 543 | | Signed192 minorY = GetDifferenceCrossProduct2D( |
| | 614 | 544 | | secondZ, Fixed64.Zero, secondX, Fixed64.Zero, |
| | 614 | 545 | | thirdZ, Fixed64.Zero, thirdX, Fixed64.Zero); |
| | 614 | 546 | | Signed192 minorZ = GetDifferenceCrossProduct2D( |
| | 614 | 547 | | secondX, Fixed64.Zero, secondY, Fixed64.Zero, |
| | 614 | 548 | | thirdX, Fixed64.Zero, thirdY, Fixed64.Zero); |
| | 614 | 549 | | Signed320 tripleProduct = WideArithmetic.AddSigned320( |
| | 614 | 550 | | WideArithmetic.AddSigned320( |
| | 614 | 551 | | WideArithmetic.MultiplySigned192(Signed192.Signed(firstX.m_rawValue), minorX), |
| | 614 | 552 | | WideArithmetic.MultiplySigned192(Signed192.Signed(firstY.m_rawValue), minorY)), |
| | 614 | 553 | | WideArithmetic.MultiplySigned192(Signed192.Signed(firstZ.m_rawValue), minorZ)); |
| | 614 | 554 | | return tripleProduct.Sign; |
| | | 555 | | } |
| | | 556 | | |
| | | 557 | | /// <summary> |
| | | 558 | | /// Applies the public 3D segment near-parallel threshold to an exact Q128.128 determinant. |
| | | 559 | | /// </summary> |
| | | 560 | | internal static bool IsSegmentDeterminantNearParallel(Signed320 determinant) |
| | | 561 | | { |
| | 61 | 562 | | WideArithmetic.GetMagnitude( |
| | 61 | 563 | | determinant, |
| | 61 | 564 | | out ulong word4, |
| | 61 | 565 | | out ulong word3, |
| | 61 | 566 | | out ulong word2, |
| | 61 | 567 | | out ulong word1, |
| | 61 | 568 | | out ulong word0); |
| | 61 | 569 | | ulong epsilonRaw = (ulong)Fixed64.Epsilon.m_rawValue; |
| | 61 | 570 | | return WideArithmetic.CompareUnsigned( |
| | 61 | 571 | | word4, |
| | 61 | 572 | | word3, |
| | 61 | 573 | | word2, |
| | 61 | 574 | | word1, |
| | 61 | 575 | | word0, |
| | 61 | 576 | | 0UL, |
| | 61 | 577 | | 0UL, |
| | 61 | 578 | | epsilonRaw >> 32, |
| | 61 | 579 | | epsilonRaw << 32, |
| | 61 | 580 | | 0UL) < 0; |
| | | 581 | | } |
| | | 582 | | |
| | | 583 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 584 | | internal static void AccumulateDifferenceProduct( |
| | | 585 | | long candidate, |
| | | 586 | | long current, |
| | | 587 | | long directionEnd, |
| | | 588 | | long directionStart, |
| | | 589 | | ref ulong sumHigh, |
| | | 590 | | ref ulong sumMiddle, |
| | | 591 | | ref ulong sumLow) |
| | | 592 | | { |
| | 509047 | 593 | | if (candidate == current || directionEnd == directionStart) |
| | 229294 | 594 | | return; |
| | | 595 | | |
| | 279753 | 596 | | bool negativeDifference = candidate < current; |
| | 279753 | 597 | | ulong differenceMagnitude = negativeDifference |
| | 279753 | 598 | | ? unchecked((ulong)current - (ulong)candidate) |
| | 279753 | 599 | | : unchecked((ulong)candidate - (ulong)current); |
| | 279753 | 600 | | bool negativeDirection = directionEnd < directionStart; |
| | 279753 | 601 | | ulong directionMagnitude = negativeDirection |
| | 279753 | 602 | | ? unchecked((ulong)directionStart - (ulong)directionEnd) |
| | 279753 | 603 | | : unchecked((ulong)directionEnd - (ulong)directionStart); |
| | 279753 | 604 | | bool negativeProduct = negativeDifference != negativeDirection; |
| | | 605 | | |
| | 279753 | 606 | | Fixed64.Multiply64To128( |
| | 279753 | 607 | | differenceMagnitude, |
| | 279753 | 608 | | directionMagnitude, |
| | 279753 | 609 | | out ulong productMiddle, |
| | 279753 | 610 | | out ulong productLow); |
| | | 611 | | |
| | 279753 | 612 | | ulong productHigh = 0UL; |
| | 279753 | 613 | | if (negativeProduct) |
| | | 614 | | { |
| | 63606 | 615 | | productLow = unchecked(~productLow + 1UL); |
| | 63606 | 616 | | productMiddle = unchecked(~productMiddle + (productLow == 0UL ? 1UL : 0UL)); |
| | 63606 | 617 | | productHigh = ulong.MaxValue; |
| | | 618 | | } |
| | | 619 | | |
| | 279753 | 620 | | ulong previousLow = sumLow; |
| | 279753 | 621 | | sumLow = unchecked(sumLow + productLow); |
| | 279753 | 622 | | ulong carry = sumLow < previousLow ? 1UL : 0UL; |
| | | 623 | | |
| | 279753 | 624 | | ulong addMiddle = unchecked(productMiddle + carry); |
| | 279753 | 625 | | ulong carryHigh = addMiddle < productMiddle ? 1UL : 0UL; |
| | 279753 | 626 | | ulong previousMiddle = sumMiddle; |
| | 279753 | 627 | | sumMiddle = unchecked(sumMiddle + addMiddle); |
| | 279753 | 628 | | if (sumMiddle < previousMiddle) |
| | 8123 | 629 | | carryHigh = 1UL; |
| | | 630 | | |
| | 279753 | 631 | | sumHigh = unchecked(sumHigh + productHigh + carryHigh); |
| | 279753 | 632 | | } |
| | | 633 | | |
| | | 634 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 635 | | private static Signed192 GetRawCrossComponent( |
| | | 636 | | long leftFirst, |
| | | 637 | | long leftSecond, |
| | | 638 | | long rightFirst, |
| | | 639 | | long rightSecond) |
| | | 640 | | { |
| | 10491 | 641 | | ulong high = 0UL; |
| | 10491 | 642 | | ulong middle = 0UL; |
| | 10491 | 643 | | ulong low = 0UL; |
| | 10491 | 644 | | AccumulateRawProduct(leftFirst, rightFirst, false, ref high, ref middle, ref low); |
| | 10491 | 645 | | AccumulateRawProduct(leftSecond, rightSecond, true, ref high, ref middle, ref low); |
| | 10491 | 646 | | return new Signed192(high, middle, low); |
| | | 647 | | } |
| | | 648 | | |
| | | 649 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 650 | | private static bool TrySubtractRaw(long end, long start, out long difference) |
| | | 651 | | { |
| | 202993 | 652 | | difference = unchecked(end - start); |
| | 202993 | 653 | | return ((end ^ start) & (end ^ difference)) >= 0L; |
| | | 654 | | } |
| | | 655 | | |
| | | 656 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 657 | | private static void AccumulateRawProduct( |
| | | 658 | | long left, |
| | | 659 | | long right, |
| | | 660 | | bool subtract, |
| | | 661 | | ref ulong sumHigh, |
| | | 662 | | ref ulong sumMiddle, |
| | | 663 | | ref ulong sumLow) |
| | | 664 | | { |
| | 111543 | 665 | | if (left == 0L || right == 0L) |
| | 74817 | 666 | | return; |
| | | 667 | | |
| | 36726 | 668 | | ulong leftMagnitude = left < 0L ? unchecked(0UL - (ulong)left) : (ulong)left; |
| | 36726 | 669 | | ulong rightMagnitude = right < 0L ? unchecked(0UL - (ulong)right) : (ulong)right; |
| | 36726 | 670 | | bool negativeProduct = (left < 0L) != (right < 0L) != subtract; |
| | 36726 | 671 | | Fixed64.Multiply64To128(leftMagnitude, rightMagnitude, out ulong productMiddle, out ulong productLow); |
| | | 672 | | |
| | 36726 | 673 | | ulong productHigh = 0UL; |
| | 36726 | 674 | | if (negativeProduct) |
| | | 675 | | { |
| | 14269 | 676 | | productLow = unchecked(~productLow + 1UL); |
| | 14269 | 677 | | productMiddle = unchecked(~productMiddle + (productLow == 0UL ? 1UL : 0UL)); |
| | 14269 | 678 | | productHigh = ulong.MaxValue; |
| | | 679 | | } |
| | | 680 | | |
| | 36726 | 681 | | ulong previousLow = sumLow; |
| | 36726 | 682 | | sumLow = unchecked(sumLow + productLow); |
| | 36726 | 683 | | ulong carry = sumLow < previousLow ? 1UL : 0UL; |
| | | 684 | | |
| | 36726 | 685 | | ulong addMiddle = unchecked(productMiddle + carry); |
| | 36726 | 686 | | ulong carryHigh = addMiddle < productMiddle ? 1UL : 0UL; |
| | 36726 | 687 | | ulong previousMiddle = sumMiddle; |
| | 36726 | 688 | | sumMiddle = unchecked(sumMiddle + addMiddle); |
| | 36726 | 689 | | if (sumMiddle < previousMiddle) |
| | 2244 | 690 | | carryHigh = 1UL; |
| | | 691 | | |
| | 36726 | 692 | | sumHigh = unchecked(sumHigh + productHigh + carryHigh); |
| | 36726 | 693 | | } |
| | | 694 | | |
| | | 695 | | internal static bool TryGetMagnitudeCeiling( |
| | | 696 | | Fixed64 x, |
| | | 697 | | Fixed64 y, |
| | | 698 | | Fixed64 z, |
| | | 699 | | out Fixed64 magnitude) |
| | | 700 | | { |
| | 5 | 701 | | Signed192 rawX = Signed192.Signed(x.m_rawValue); |
| | 5 | 702 | | Signed192 rawY = Signed192.Signed(y.m_rawValue); |
| | 5 | 703 | | Signed192 rawZ = Signed192.Signed(z.m_rawValue); |
| | 5 | 704 | | Signed320 squaredMagnitude = WideArithmetic.AddSigned320( |
| | 5 | 705 | | WideArithmetic.AddSigned320( |
| | 5 | 706 | | WideArithmetic.MultiplySigned192(rawX, rawX), |
| | 5 | 707 | | WideArithmetic.MultiplySigned192(rawY, rawY)), |
| | 5 | 708 | | WideArithmetic.MultiplySigned192(rawZ, rawZ)); |
| | 5 | 709 | | Signed192 root = WideArithmetic.GetFloorSquareRoot( |
| | 5 | 710 | | squaredMagnitude, |
| | 5 | 711 | | out Signed192 remainder); |
| | 5 | 712 | | if (!remainder.IsZero) |
| | | 713 | | { |
| | 3 | 714 | | root = WideArithmetic.AddSigned192( |
| | 3 | 715 | | root, |
| | 3 | 716 | | Signed192.Signed(1L)); |
| | | 717 | | } |
| | | 718 | | |
| | 5 | 719 | | if ((root.High | root.Middle | (root.Low >> 63)) != 0UL) |
| | | 720 | | { |
| | 1 | 721 | | magnitude = default; |
| | 1 | 722 | | return false; |
| | | 723 | | } |
| | | 724 | | |
| | 4 | 725 | | magnitude = Fixed64.FromRaw((long)root.Low); |
| | 4 | 726 | | return true; |
| | | 727 | | } |
| | | 728 | | |
| | | 729 | | internal static FixedQuaternion GetCanonicalAxisRotation( |
| | | 730 | | Vector3d normalizedAxis) |
| | | 731 | | { |
| | 28 | 732 | | if (normalizedAxis == Vector3d.Up) |
| | 2 | 733 | | return FixedQuaternion.Identity; |
| | 26 | 734 | | if (normalizedAxis == Vector3d.Down) |
| | | 735 | | { |
| | 2 | 736 | | return FixedQuaternion.FromAxisAngle( |
| | 2 | 737 | | Vector3d.Right, |
| | 2 | 738 | | Fixed64.Pi); |
| | | 739 | | } |
| | | 740 | | |
| | 24 | 741 | | return new FixedQuaternion( |
| | 24 | 742 | | normalizedAxis.Z, |
| | 24 | 743 | | Fixed64.Zero, |
| | 24 | 744 | | -normalizedAxis.X, |
| | 24 | 745 | | Fixed64.One + normalizedAxis.Y).Normalized; |
| | | 746 | | } |
| | | 747 | | |
| | | 748 | | /// <summary> |
| | | 749 | | /// Returns one outward-rounded endpoint of a centered finite-axis extent, |
| | | 750 | | /// clipped to the representable scalar domain. |
| | | 751 | | /// </summary> |
| | | 752 | | internal static Fixed64 GetCenteredFiniteAxisBoundClippedToDomain( |
| | | 753 | | Fixed64 center, |
| | | 754 | | Fixed64 axisComponent, |
| | | 755 | | Fixed64 fullAxisLength, |
| | | 756 | | Fixed64 radialExtent, |
| | | 757 | | bool minimum) |
| | | 758 | | { |
| | 1928 | 759 | | Signed320 centerNumerator = WideArithmetic.MultiplySigned192( |
| | 1928 | 760 | | Signed192.Signed(center.m_rawValue), |
| | 1928 | 761 | | CenteredAxisDenominator); |
| | 1928 | 762 | | Signed320 extentNumerator = WideArithmetic.AddSigned320( |
| | 1928 | 763 | | WideArithmetic.MultiplySigned192( |
| | 1928 | 764 | | Signed192.Signed(axisComponent.Abs().m_rawValue), |
| | 1928 | 765 | | Signed192.Signed(fullAxisLength.m_rawValue)), |
| | 1928 | 766 | | WideArithmetic.MultiplySigned192( |
| | 1928 | 767 | | Signed192.Signed(radialExtent.m_rawValue), |
| | 1928 | 768 | | CenteredAxisDenominator)); |
| | 1928 | 769 | | Signed320 numerator = minimum |
| | 1928 | 770 | | ? WideArithmetic.SubtractSigned320(centerNumerator, extentNumerator) |
| | 1928 | 771 | | : WideArithmetic.AddSigned320(centerNumerator, extentNumerator); |
| | | 772 | | |
| | 1928 | 773 | | return GetRationalBoundClippedToDomain( |
| | 1928 | 774 | | numerator, |
| | 1928 | 775 | | CenteredAxisDenominator, |
| | 1928 | 776 | | minimum); |
| | | 777 | | } |
| | | 778 | | |
| | | 779 | | internal static Fixed64 GetRationalBoundClippedToDomain( |
| | | 780 | | Signed320 numerator, |
| | | 781 | | Signed192 denominator, |
| | | 782 | | bool minimum) |
| | | 783 | | { |
| | 1960 | 784 | | Signed320 minimumNumerator = WideArithmetic.MultiplySigned192( |
| | 1960 | 785 | | Signed192.Signed(Fixed64.MinValue.m_rawValue), |
| | 1960 | 786 | | denominator); |
| | 1960 | 787 | | Signed320 maximumNumerator = WideArithmetic.MultiplySigned192( |
| | 1960 | 788 | | Signed192.Signed(Fixed64.MaxValue.m_rawValue), |
| | 1960 | 789 | | denominator); |
| | | 790 | | |
| | | 791 | | // Clip the exact endpoint before round-to-even so the subsequent |
| | | 792 | | // one-unit outward correction cannot cross the scalar domain. |
| | 1960 | 793 | | if (WideArithmetic.SubtractSigned320( |
| | 1960 | 794 | | numerator, |
| | 1960 | 795 | | minimumNumerator).Sign < 0) |
| | | 796 | | { |
| | 8 | 797 | | return Fixed64.MinValue; |
| | | 798 | | } |
| | 1952 | 799 | | if (WideArithmetic.SubtractSigned320( |
| | 1952 | 800 | | numerator, |
| | 1952 | 801 | | maximumNumerator).Sign > 0) |
| | | 802 | | { |
| | 10 | 803 | | return Fixed64.MaxValue; |
| | | 804 | | } |
| | | 805 | | |
| | 1942 | 806 | | Fixed64 bound = Fixed64.GetSignedRawRatio( |
| | 1942 | 807 | | numerator, |
| | 1942 | 808 | | denominator); |
| | 1942 | 809 | | Signed320 represented = WideArithmetic.MultiplySigned192( |
| | 1942 | 810 | | Signed192.Signed(bound.m_rawValue), |
| | 1942 | 811 | | denominator); |
| | 1942 | 812 | | int comparison = WideArithmetic.SubtractSigned320(represented, numerator).Sign; |
| | 1942 | 813 | | if (minimum && comparison > 0) |
| | 8 | 814 | | return Fixed64.FromRaw(bound.m_rawValue - 1L); |
| | 1934 | 815 | | if (!minimum && comparison < 0) |
| | 9 | 816 | | return Fixed64.FromRaw(bound.m_rawValue + 1L); |
| | 1925 | 817 | | return bound; |
| | | 818 | | } |
| | | 819 | | } |