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