| | | 1 | | //======================================================================= |
| | | 2 | | // CapsulePlaneRayValue.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.Geometry; |
| | | 12 | | |
| | | 13 | | |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// One positive outward ray root of a quadratic over an independent quadratic |
| | | 17 | | /// point field. The norm polynomial and selected ordinal borrow caller storage. |
| | | 18 | | /// </summary> |
| | | 19 | | internal ref struct CapsulePlaneRayValue |
| | | 20 | | { |
| | | 21 | | // Opposite rays through edge-stationary quadratic witnesses have the largest |
| | | 22 | | // norm: coefficients <3190 bits, normalization adds <=402, and the point |
| | | 23 | | // radicand is <2256. Its complete degree-four norm stays <9600 bits. |
| | | 24 | | internal const int PolynomialWords = 160, SignCount = 5, CellWords = 8; |
| | | 25 | | internal const int StorageWords = SignCount * PolynomialWords + CellWords; |
| | 9067 | 26 | | internal static int GetStorageWords(int polynomialWords) => SignCount * polynomialWords + CellWords; |
| | | 27 | | private readonly int _words; |
| | | 28 | | private readonly Span<ulong> _polynomial, _cell; |
| | | 29 | | private readonly Span<sbyte> _signs; |
| | | 30 | | private CapsulePlaneRayValueMetadata _metadata; |
| | 283 | 31 | | internal CapsulePlaneRayValueMetadata Metadata => _metadata; |
| | 4 | 32 | | internal bool HasValue => _metadata.HasValue; |
| | | 33 | | |
| | | 34 | | internal CapsulePlaneRayValue(Span<ulong> storage, Span<sbyte> signs, int polynomialWords = PolynomialWords) |
| | | 35 | | { |
| | 1814 | 36 | | _words = polynomialWords; |
| | 1814 | 37 | | _polynomial = storage[..(SignCount * polynomialWords)]; |
| | 1814 | 38 | | _cell = storage.Slice(SignCount * polynomialWords, CellWords); |
| | 3628 | 39 | | _signs = signs[..SignCount]; _metadata = default; |
| | 1814 | 40 | | } |
| | | 41 | | |
| | 681 | 42 | | internal void Restore(CapsulePlaneRayValueMetadata metadata) => _metadata = metadata; |
| | | 43 | | |
| | | 44 | | internal void CopyFrom(scoped CapsulePlaneRayValue other) |
| | | 45 | | { |
| | 282 | 46 | | _polynomial.Clear(); |
| | 3384 | 47 | | for (int index = 0; index < SignCount; index++) |
| | | 48 | | { |
| | 1410 | 49 | | ReadOnlySpan<ulong> coefficient = other._polynomial.Slice(index * other._words, other._words); |
| | 1410 | 50 | | coefficient[..WideArithmetic.GetActiveMagnitudeLength(coefficient)].CopyTo(_polynomial.Slice(index * _words, |
| | | 51 | | } |
| | 564 | 52 | | other._signs.CopyTo(_signs); other._cell.CopyTo(_cell); |
| | 282 | 53 | | _metadata = other._metadata; |
| | 282 | 54 | | } |
| | | 55 | | |
| | | 56 | | /// <summary>Retains the positive root of a*t²+2*b*t+c with nonnegative outward derivative.</summary> |
| | | 57 | | internal bool TrySetUpperRoot(scoped ContactQuadratic a, scoped ContactQuadratic b, scoped ContactQuadratic c, |
| | | 58 | | scoped ReadOnlySpan<ulong> pointRoot, int valueShift) |
| | | 59 | | { |
| | 542 | 60 | | _metadata = default; |
| | 542 | 61 | | Span<ulong> rational = stackalloc ulong[3 * _words]; |
| | 542 | 62 | | Span<ulong> radical = stackalloc ulong[3 * _words]; |
| | 1084 | 63 | | Span<sbyte> rationalSigns = stackalloc sbyte[3], radicalSigns = stackalloc sbyte[3]; |
| | 2168 | 64 | | rational.Clear(); radical.Clear(); rationalSigns.Clear(); radicalSigns.Clear(); |
| | 542 | 65 | | SetCoefficient(c, rational, rationalSigns, radical, radicalSigns, 0, 0); |
| | 542 | 66 | | SetCoefficient(b, rational, rationalSigns, radical, radicalSigns, 1, valueShift + 1); |
| | 542 | 67 | | SetCoefficient(a, rational, rationalSigns, radical, radicalSigns, 2, 2 * valueShift); |
| | 542 | 68 | | bool rationalRay = ((byte)radicalSigns[0] | (byte)radicalSigns[1] | (byte)radicalSigns[2]) == 0 |
| | 542 | 69 | | || WideArithmetic.GetActiveMagnitudeLength(pointRoot) == 0; |
| | 542 | 70 | | if (rationalRay) |
| | | 71 | | { |
| | | 72 | | // Rational ray coefficients need only their original quadratic. |
| | | 73 | | // Squaring it would duplicate every root and double its height. |
| | 518 | 74 | | _polynomial.Clear(); _signs.Clear(); |
| | 518 | 75 | | rational.CopyTo(_polynomial); rationalSigns.CopyTo(_signs); |
| | | 76 | | } |
| | | 77 | | else |
| | 283 | 78 | | BuildNorm(rational, rationalSigns, radical, radicalSigns, pointRoot, _polynomial, _signs); |
| | 542 | 79 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(_polynomial, _signs); |
| | 1084 | 80 | | Span<ulong> batchCells = stackalloc ulong[64]; Span<int> shifts = stackalloc int[8]; |
| | 542 | 81 | | FiniteAxisValueRoots roots = WideFiniteAxisIntersection.GetFiniteValueRoots(_polynomial, _signs, batchCells, shi |
| | 542 | 82 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords(_polynomial, _signs)] |
| | 542 | 83 | | Span<ulong> derivativeRational = stackalloc ulong[2 * _words]; |
| | 542 | 84 | | Span<ulong> derivativeRadical = stackalloc ulong[2 * _words]; |
| | 1084 | 85 | | Span<sbyte> derivativeRationalSigns = stackalloc sbyte[2], derivativeRadicalSigns = stackalloc sbyte[2]; |
| | 542 | 86 | | WideFiniteAxisIntersection.DifferentiateFiniteAxisPolynomial(rational, rationalSigns, derivativeRational, deriva |
| | 542 | 87 | | WideFiniteAxisIntersection.DifferentiateFiniteAxisPolynomial(radical, radicalSigns, derivativeRadical, derivativ |
| | 1444 | 88 | | for (int ordinal = 0; ordinal < roots.Count; ordinal++) |
| | | 89 | | { |
| | 550 | 90 | | FiniteAxisValueRoot root = FiniteAxisValueRoots.GetRoot(roots, ordinal, _polynomial, _signs, cell); |
| | | 91 | | // The norm also contains the conjugate field's roots. Unsquared |
| | | 92 | | // admission and the outward derivative identify the intended exit. |
| | 550 | 93 | | if (!rationalRay && SignAtRoot(ref root, rational, rationalSigns, radical, radicalSigns, pointRoot, knownNor |
| | 550 | 94 | | || SignAtRoot(ref root, derivativeRational, derivativeRationalSigns, |
| | 550 | 95 | | derivativeRadical, derivativeRadicalSigns, pointRoot) < 0) |
| | | 96 | | continue; |
| | 370 | 97 | | bool compact = root.DenominatorShift < CellWords * 64; |
| | 370 | 98 | | _cell.Clear(); |
| | 739 | 99 | | if (compact) root.LowerNumerator[..WideArithmetic.GetActiveMagnitudeLength(root.LowerNumerator)].CopyTo(_cel |
| | 370 | 100 | | _metadata = new CapsulePlaneRayValueMetadata(valueShift, ordinal, roots.Count, root.Signs.Length, |
| | 370 | 101 | | root.DenominatorShift, root.IsRational, compact); |
| | 370 | 102 | | return true; |
| | | 103 | | } |
| | 172 | 104 | | return false; |
| | | 105 | | } |
| | | 106 | | |
| | | 107 | | internal int Compare(scoped CapsulePlaneRayValue other) |
| | | 108 | | { |
| | 89 | 109 | | Span<ulong> firstCell = stackalloc ulong[CellCapacity()]; |
| | 89 | 110 | | Span<ulong> secondCell = stackalloc ulong[other.CellCapacity()]; |
| | 89 | 111 | | FiniteAxisValueRoot first = GetRoot(_polynomial, _signs, _cell, _metadata, firstCell), |
| | 89 | 112 | | second = GetRoot(other._polynomial, other._signs, other._cell, other._metadata, secondCell); |
| | 89 | 113 | | int shift = Math.Max(_metadata.ValueShift, other._metadata.ValueShift); |
| | 89 | 114 | | Span<ulong> firstPolynomial = stackalloc ulong[SignCount * _words]; |
| | 89 | 115 | | Span<ulong> secondPolynomial = stackalloc ulong[SignCount * other._words]; |
| | 178 | 116 | | first.Coefficients.CopyTo(firstPolynomial); second.Coefficients.CopyTo(secondPolynomial); |
| | 178 | 117 | | int firstScale = shift - _metadata.ValueShift, secondScale = shift - other._metadata.ValueShift; |
| | 89 | 118 | | first.Coefficients = firstPolynomial[..first.Coefficients.Length]; |
| | 89 | 119 | | second.Coefficients = secondPolynomial[..second.Coefficients.Length]; |
| | 89 | 120 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(firstPolynomial[..first.Coefficients.Length], first |
| | 89 | 121 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(secondPolynomial[..second.Coefficients.Length], sec |
| | 178 | 122 | | first.DenominatorShift += firstScale; second.DenominatorShift += secondScale; |
| | 89 | 123 | | return WideFiniteAxisIntersection.CompareFiniteValueRoots(first, second); |
| | | 124 | | } |
| | | 125 | | |
| | | 126 | | internal int Compare(scoped CapsulePlaneRayValue other, Signed320 divisor, Signed320 otherDivisor) |
| | | 127 | | { |
| | | 128 | | System.Diagnostics.Debug.Assert(divisor.Sign > 0 && otherDivisor.Sign > 0); |
| | 12 | 129 | | Span<ulong> firstCell = stackalloc ulong[CellCapacity()]; |
| | 12 | 130 | | Span<ulong> secondCell = stackalloc ulong[other.CellCapacity()]; |
| | 12 | 131 | | FiniteAxisValueRoot first = GetRoot(_polynomial, _signs, _cell, _metadata, firstCell), |
| | 12 | 132 | | second = GetRoot(other._polynomial, other._signs, other._cell, other._metadata, secondCell); |
| | 12 | 133 | | int shift = Math.Max(_metadata.ValueShift, other._metadata.ValueShift); |
| | 24 | 134 | | int firstScale = shift - _metadata.ValueShift, secondScale = shift - other._metadata.ValueShift; |
| | 12 | 135 | | Span<ulong> firstDenominator = stackalloc ulong[5 + (firstScale + 63) / 64]; |
| | 12 | 136 | | Span<ulong> secondDenominator = stackalloc ulong[5 + (secondScale + 63) / 64]; |
| | 12 | 137 | | Span<ulong> magnitude = stackalloc ulong[5]; |
| | 24 | 138 | | firstDenominator.Clear(); secondDenominator.Clear(); |
| | 12 | 139 | | WideArithmetic.GetMagnitude(divisor, out magnitude[4], out magnitude[3], out magnitude[2], out magnitude[1], out |
| | 12 | 140 | | int sign = 0; |
| | 12 | 141 | | WideArithmetic.AddShiftedSignedMagnitude(magnitude, 1, firstScale, firstDenominator, ref sign); |
| | 12 | 142 | | WideArithmetic.GetMagnitude(otherDivisor, out magnitude[4], out magnitude[3], out magnitude[2], out magnitude[1] |
| | 12 | 143 | | sign = 0; |
| | 12 | 144 | | WideArithmetic.AddShiftedSignedMagnitude(magnitude, 1, secondScale, secondDenominator, ref sign); |
| | | 145 | | // t = alpha*2^ValueShift. A common binary factor cancels, leaving |
| | | 146 | | // original roots divided by D*2^(commonShift-ValueShift). |
| | 12 | 147 | | return WideFiniteAxisIntersection.CompareFiniteValueRoots(first, firstDenominator, second, secondDenominator); |
| | | 148 | | } |
| | | 149 | | |
| | | 150 | | /// <summary>Compares this positive value with an exact quadratic ratio whose denominator is positive.</summary> |
| | | 151 | | internal int CompareQuadratic(scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 152 | | scoped ReadOnlySpan<ulong> root) |
| | | 153 | | { |
| | 448 | 154 | | Span<ulong> values = stackalloc ulong[2 * numerator.FieldWords]; Span<int> signs = stackalloc int[2]; |
| | 448 | 155 | | ContactQuadratic constant = new(values, signs); numerator.CopyTo(constant, -1); |
| | 224 | 156 | | return SignLinear(constant, denominator, root); |
| | | 157 | | } |
| | | 158 | | |
| | | 159 | | /// <summary>Compares this nonnegative value squared with a nonnegative exact quadratic ratio.</summary> |
| | | 160 | | internal int CompareSquaredQuadratic(scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 161 | | scoped ReadOnlySpan<ulong> root) |
| | | 162 | | { |
| | 14 | 163 | | Span<ulong> cell = stackalloc ulong[CellCapacity()]; |
| | 14 | 164 | | FiniteAxisValueRoot parameter = GetRoot(_polynomial, _signs, _cell, _metadata, cell); |
| | 14 | 165 | | int words = Math.Max(parameter.Coefficients.Length / parameter.Signs.Length, |
| | 14 | 166 | | Math.Max(ContactQuadratic.ActiveWords(numerator), ContactQuadratic.ActiveWords(denominator)) |
| | 14 | 167 | | + (2 * _metadata.ValueShift + 63) / 64) + 1; |
| | 28 | 168 | | Span<ulong> rational = stackalloc ulong[3 * words], radical = stackalloc ulong[3 * words]; |
| | 28 | 169 | | Span<sbyte> rationalSigns = stackalloc sbyte[3], radicalSigns = stackalloc sbyte[3]; |
| | 56 | 170 | | rational.Clear(); radical.Clear(); rationalSigns.Clear(); radicalSigns.Clear(); |
| | 14 | 171 | | SetCoefficient(numerator, rational, rationalSigns, radical, radicalSigns, 0, 0); |
| | 28 | 172 | | rationalSigns[0] = (sbyte)-rationalSigns[0]; radicalSigns[0] = (sbyte)-radicalSigns[0]; |
| | 14 | 173 | | SetCoefficient(denominator, rational, rationalSigns, radical, radicalSigns, 2, 2 * _metadata.ValueShift); |
| | 14 | 174 | | return SignAtRoot(ref parameter, rational, rationalSigns, radical, radicalSigns, root); |
| | | 175 | | } |
| | | 176 | | |
| | | 177 | | /// <summary>Exact sign of constant+slope*t, including an independent quadratic coefficient field.</summary> |
| | | 178 | | internal int SignLinear(scoped ContactQuadratic constant, scoped ContactQuadratic slope, |
| | | 179 | | scoped ReadOnlySpan<ulong> root) |
| | | 180 | | { |
| | 746 | 181 | | Span<ulong> cell = stackalloc ulong[CellCapacity()]; |
| | 746 | 182 | | FiniteAxisValueRoot parameter = GetRoot(_polynomial, _signs, _cell, _metadata, cell); |
| | 746 | 183 | | return LinearSign(ref parameter, constant, slope, root, _metadata.ValueShift); |
| | | 184 | | } |
| | | 185 | | |
| | | 186 | | private static int LinearSign(scoped ref FiniteAxisValueRoot parameter, scoped ContactQuadratic constant, |
| | | 187 | | scoped ContactQuadratic slope, scoped ReadOnlySpan<ulong> root, int valueShift) |
| | | 188 | | { |
| | 2464 | 189 | | int _words = Math.Max(parameter.Coefficients.Length / parameter.Signs.Length, 2 * Math.Max(constant.FieldWords, |
| | 4928 | 190 | | Span<ulong> rational = stackalloc ulong[2 * _words], radical = stackalloc ulong[2 * _words]; |
| | 4928 | 191 | | Span<sbyte> rationalSigns = stackalloc sbyte[2], radicalSigns = stackalloc sbyte[2]; |
| | 9856 | 192 | | rational.Clear(); radical.Clear(); rationalSigns.Clear(); radicalSigns.Clear(); |
| | 2464 | 193 | | SetCoefficient(constant, rational, rationalSigns, radical, radicalSigns, 0, 0); |
| | 2464 | 194 | | SetCoefficient(slope, rational, rationalSigns, radical, radicalSigns, 1, valueShift); |
| | 2464 | 195 | | return SignAtRoot(ref parameter, rational, rationalSigns, radical, radicalSigns, root); |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | /// <summary>Rounds capsule-local (q+t*direction)/coordinateScale once at the final coordinate lattice.</summary> |
| | | 199 | | internal bool TryGetPoint(scoped ContactQuadratic qx, scoped ContactQuadratic qy, |
| | | 200 | | scoped ContactQuadratic qz, scoped ContactQuadratic qd, scoped ReadOnlySpan<ulong> pointRoot, |
| | | 201 | | WideDirection3d direction, Signed320 coordinateScale, out Vector3d point) |
| | | 202 | | { |
| | 212 | 203 | | Span<ulong> values = stackalloc ulong[8 * _words]; Span<int> signs = stackalloc int[8]; |
| | 106 | 204 | | ContactQuadratic denominator = ContactQuadratic.At(values, signs, 0, _words); |
| | 106 | 205 | | ContactQuadratic sx = ContactQuadratic.At(values, signs, 1, _words); |
| | 106 | 206 | | ContactQuadratic sy = ContactQuadratic.At(values, signs, 2, _words); |
| | 106 | 207 | | ContactQuadratic sz = ContactQuadratic.At(values, signs, 3, _words); |
| | 106 | 208 | | ContactQuadratic.Scale(qd, Signed576.ExtendValue(coordinateScale), denominator); |
| | 106 | 209 | | ContactQuadratic.Scale(qd, direction.X, sx); |
| | 106 | 210 | | ContactQuadratic.Scale(qd, direction.Y, sy); |
| | 106 | 211 | | ContactQuadratic.Scale(qd, direction.Z, sz); |
| | 106 | 212 | | Span<ulong> cell = stackalloc ulong[CellCapacity()]; |
| | 106 | 213 | | FiniteAxisValueRoot parameter = GetRoot(_polynomial, _signs, _cell, _metadata, cell); |
| | 106 | 214 | | if (!TryRoundCoordinate(ref parameter, qx, sx, denominator, pointRoot, _metadata.ValueShift, out Fixed64 px) |
| | 106 | 215 | | || !TryRoundCoordinate(ref parameter, qy, sy, denominator, pointRoot, _metadata.ValueShift, out Fixed64 py) |
| | 106 | 216 | | || !TryRoundCoordinate(ref parameter, qz, sz, denominator, pointRoot, _metadata.ValueShift, out Fixed64 pz)) |
| | | 217 | | { |
| | 6 | 218 | | point = default; |
| | 6 | 219 | | return false; |
| | | 220 | | } |
| | 100 | 221 | | point = new Vector3d(px, py, pz); |
| | 100 | 222 | | return true; |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | private static bool TryRoundCoordinate(scoped ref FiniteAxisValueRoot parameter, scoped ContactQuadratic constant, |
| | | 226 | | scoped ContactQuadratic slope, scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> root, |
| | | 227 | | int valueShift, out Fixed64 coordinate) |
| | | 228 | | { |
| | 312 | 229 | | coordinate = default; |
| | 312 | 230 | | int _words = Math.Max(constant.FieldWords, Math.Max(slope.FieldWords, denominator.FieldWords)) + 2; |
| | 624 | 231 | | Span<ulong> values = stackalloc ulong[6 * _words]; Span<int> signs = stackalloc int[6]; |
| | 312 | 232 | | ContactQuadratic query = ContactQuadratic.At(values, signs, 0, _words); |
| | 312 | 233 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 1, _words); |
| | 312 | 234 | | ContactQuadratic doubledSlope = ContactQuadratic.At(values, signs, 2, _words); |
| | 624 | 235 | | slope.CopyTo(doubledSlope); doubledSlope.Add(slope); |
| | 312 | 236 | | Signed192 upper = WideArithmetic.AddSigned192(WideArithmetic.AddSigned192(Signed192.Signed(long.MaxValue), |
| | 312 | 237 | | Signed192.Signed(long.MaxValue)), Signed192.Signed(1)); |
| | 312 | 238 | | Signed192 lower = WideArithmetic.SubtractSigned192(WideArithmetic.AddSigned192(Signed192.Signed(long.MinValue), |
| | 312 | 239 | | Signed192.Signed(long.MinValue)), Signed192.Signed(1)); |
| | 312 | 240 | | if (CompareCoordinate(ref parameter, constant, doubledSlope, denominator, root, upper, query, term, valueShift) |
| | 312 | 241 | | || CompareCoordinate(ref parameter, constant, doubledSlope, denominator, root, lower, query, term, valueShif |
| | 6 | 242 | | return false; |
| | | 243 | | // A zero direction retains the original point field; its coordinate |
| | | 244 | | // needs no value-root search after the exact range checks above. |
| | 306 | 245 | | if ((slope.Signs[0] | slope.Signs[1]) == 0) |
| | | 246 | | { |
| | 184 | 247 | | coordinate = ContactQuadratic.RoundRatio(constant, denominator, root); |
| | 184 | 248 | | return true; |
| | | 249 | | } |
| | 244 | 250 | | long lowerFloor = long.MinValue, upperFloor = long.MaxValue; |
| | 122 | 251 | | if (parameter.DenominatorShift < CellWords * 64) |
| | 119 | 252 | | GetCoordinateFloorBounds(parameter, constant, slope, denominator, root, valueShift, |
| | 119 | 253 | | out lowerFloor, out upperFloor); |
| | 122 | 254 | | ulong low = unchecked((ulong)lowerFloor) ^ (1UL << 63); |
| | 122 | 255 | | ulong high = unchecked((ulong)upperFloor) ^ (1UL << 63); |
| | 1097 | 256 | | while (low < high) |
| | | 257 | | { |
| | 975 | 258 | | ulong middle = low + ((high - low) >> 1) + 1; |
| | 975 | 259 | | long raw = unchecked((long)(middle ^ (1UL << 63))); |
| | 975 | 260 | | Signed192 threshold = WideArithmetic.AddSigned192(Signed192.Signed(raw), Signed192.Signed(raw)); |
| | 1383 | 261 | | if (CompareCoordinate(ref parameter, constant, doubledSlope, denominator, root, threshold, query, term, valu |
| | 567 | 262 | | else high = middle - 1; |
| | | 263 | | } |
| | 122 | 264 | | long floor = unchecked((long)(low ^ (1UL << 63))); |
| | 122 | 265 | | Signed192 half = WideArithmetic.AddSigned192(WideArithmetic.AddSigned192(Signed192.Signed(floor), |
| | 122 | 266 | | Signed192.Signed(floor)), Signed192.Signed(1)); |
| | 122 | 267 | | int comparison = CompareCoordinate(ref parameter, constant, doubledSlope, denominator, root, half, query, term, |
| | 122 | 268 | | coordinate = Fixed64.FromRaw(floor + (comparison > 0 || comparison == 0 && (floor & 1) != 0 ? 1L : 0L)); |
| | 122 | 269 | | return true; |
| | | 270 | | } |
| | | 271 | | |
| | | 272 | | [MethodImpl(MethodImplOptions.NoInlining)] // Return interval scratch before exact root signs. |
| | | 273 | | private static void GetCoordinateFloorBounds(scoped FiniteAxisValueRoot parameter, |
| | | 274 | | scoped ContactQuadratic constant, scoped ContactQuadratic slope, scoped ContactQuadratic denominator, |
| | | 275 | | scoped ReadOnlySpan<ulong> root, int valueShift, out long lower, out long upper) |
| | | 276 | | { |
| | | 277 | | // t=2^s*alpha and alpha is in [L,L+1]/2^h. With positive D the |
| | | 278 | | // affine quotient's extrema are its endpoints, even for negative slope. |
| | | 279 | | // Keep both point-field values exact; their independent CQ enclosures |
| | | 280 | | // only narrow the search. Cancellation and range uncertainty fall back. |
| | 119 | 281 | | int commonShift = Math.Max(0, parameter.DenominatorShift - valueShift); |
| | 119 | 282 | | int endpointShift = Math.Max(0, valueShift - parameter.DenominatorShift); |
| | 119 | 283 | | int endpointWords = WideArithmetic.GetActiveMagnitudeLength(parameter.LowerNumerator) |
| | 119 | 284 | | + 1 + (endpointShift + 63) / 64; |
| | 119 | 285 | | int commonWords = commonShift / 64 + 1; |
| | 119 | 286 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(constant), ContactQuadratic.ActiveWords(denominator)) |
| | 119 | 287 | | + commonWords, ContactQuadratic.ActiveWords(slope) + endpointWords) + 1; |
| | 238 | 288 | | Span<ulong> values = stackalloc ulong[6 * words]; Span<int> signs = stackalloc int[6]; |
| | 119 | 289 | | ContactQuadratic numerator = ContactQuadratic.At(values, signs, 0, words); |
| | 119 | 290 | | ContactQuadratic divisor = ContactQuadratic.At(values, signs, 1, words); |
| | 119 | 291 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 2, words); |
| | 238 | 292 | | Span<ulong> common = stackalloc ulong[commonWords], endpoint = stackalloc ulong[endpointWords]; |
| | 238 | 293 | | common.Clear(); common[commonShift / 64] = 1UL << (commonShift & 63); |
| | 119 | 294 | | ContactQuadratic.Scale(denominator, common, 1, divisor); |
| | 238 | 295 | | lower = long.MaxValue; upper = long.MinValue; |
| | 119 | 296 | | Span<ulong> one = stackalloc ulong[1] { 1 }; |
| | 714 | 297 | | for (int index = 0; index < 2; index++) |
| | | 298 | | { |
| | 476 | 299 | | endpoint.Clear(); int sign = 0; |
| | 238 | 300 | | WideArithmetic.AddShiftedSignedMagnitude(parameter.LowerNumerator, 1, endpointShift, endpoint, ref sign); |
| | 238 | 301 | | if (index != 0 && !parameter.IsRational) |
| | 78 | 302 | | WideArithmetic.AddShiftedSignedMagnitude(one, 1, endpointShift, endpoint, ref sign); |
| | 238 | 303 | | ContactQuadratic.Scale(constant, common, 1, numerator); |
| | 476 | 304 | | ContactQuadratic.Scale(slope, endpoint, 1, term); numerator.Add(term); |
| | 476 | 305 | | long endpointLower = long.MinValue, endpointUpper = long.MaxValue; |
| | 238 | 306 | | ContactQuadratic.RefineRoundRatioBounds(numerator, divisor, root, ref endpointLower, ref endpointUpper); |
| | 476 | 307 | | lower = Math.Min(lower, endpointLower); upper = Math.Max(upper, endpointUpper); |
| | | 308 | | } |
| | 119 | 309 | | } |
| | | 310 | | |
| | | 311 | | private static int CompareCoordinate(scoped ref FiniteAxisValueRoot parameter, scoped ContactQuadratic constant, |
| | | 312 | | scoped ContactQuadratic doubledSlope, scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> root, |
| | | 313 | | Signed192 twiceRaw, scoped ContactQuadratic query, scoped ContactQuadratic term, int valueShift) |
| | | 314 | | { |
| | 3436 | 315 | | constant.CopyTo(query); query.Add(constant); |
| | 1718 | 316 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(Signed320.ExtendValue(twiceRaw)), term); |
| | 1718 | 317 | | query.Add(term, -1); |
| | 1718 | 318 | | return LinearSign(ref parameter, query, doubledSlope, root, valueShift); |
| | | 319 | | } |
| | | 320 | | |
| | | 321 | | /// <summary>Rounds t*sqrt(metric)/scale to nonnegative raw depth; scale and metric are positive.</summary> |
| | | 322 | | internal bool TryGetDepth(Signed832 metric, Signed320 scale, out Fixed64 depth, out bool overflow) |
| | | 323 | | { |
| | 274 | 324 | | Span<ulong> cell = stackalloc ulong[CellCapacity()]; FiniteAxisValueRoot parameter = GetRoot(_polynomial, _signs |
| | 274 | 325 | | Span<ulong> metricWords = stackalloc ulong[13]; WideArithmetic.GetMagnitude(metric, metricWords); |
| | 274 | 326 | | Span<ulong> scaleWords = stackalloc ulong[5]; WideArithmetic.GetMagnitude(scale, out scaleWords[4], out scaleWor |
| | 274 | 327 | | Span<ulong> squaredScale = stackalloc ulong[10]; WideArithmetic.MultiplyMagnitudes(scaleWords, scaleWords, squar |
| | 137 | 328 | | int maximum = CompareDepth(ref parameter, metricWords, squaredScale, unchecked((ulong)long.MaxValue << 1), _meta |
| | 143 | 329 | | if (maximum >= 0) { depth = Fixed64.MaxValue; overflow = maximum > 0; return true; } |
| | 270 | 330 | | ulong lower = 0, upper = 1UL << 63; |
| | 135 | 331 | | if (_metadata.HasCompactCell) |
| | | 332 | | { |
| | 134 | 333 | | GetDepthFloorBounds(parameter, metricWords, squaredScale, _metadata.ValueShift, out lower, out upper); |
| | | 334 | | // The search finds the first inadmissible integer, then subtracts one. |
| | 268 | 335 | | lower++; upper++; |
| | | 336 | | } |
| | 1554 | 337 | | while (lower < upper) |
| | | 338 | | { |
| | 1419 | 339 | | ulong middle = lower + ((upper - lower) >> 1); |
| | 1428 | 340 | | if (CompareDepth(ref parameter, metricWords, squaredScale, middle << 1, _metadata.ValueShift) >= 0) lower = |
| | 1410 | 341 | | else upper = middle; |
| | | 342 | | } |
| | 135 | 343 | | ulong floor = lower - 1; |
| | 135 | 344 | | int comparison = CompareDepth(ref parameter, metricWords, squaredScale, (floor << 1) | 1, _metadata.ValueShift); |
| | 135 | 345 | | depth = Fixed64.FromRaw((long)(floor + (comparison > 0 || comparison == 0 && (floor & 1) != 0 ? 1UL : 0))); |
| | 270 | 346 | | overflow = false; return true; |
| | | 347 | | } |
| | | 348 | | |
| | | 349 | | private static void GetDepthFloorBounds(scoped FiniteAxisValueRoot parameter, |
| | | 350 | | scoped ReadOnlySpan<ulong> metric, scoped ReadOnlySpan<ulong> squaredScale, int valueShift, |
| | | 351 | | out ulong lower, out ulong upper) |
| | | 352 | | { |
| | | 353 | | // The retained dyadic cell encloses t. Squaring its nonnegative |
| | | 354 | | // endpoints bounds raw depth; exact signs still decide the final half. |
| | 134 | 355 | | int cellWords = WideArithmetic.GetActiveMagnitudeLength(parameter.LowerNumerator) + 1; |
| | 134 | 356 | | int shift = 2 * (parameter.DenominatorShift - valueShift); |
| | 134 | 357 | | int words = Math.Max(2 * cellWords + metric.Length, squaredScale.Length) |
| | 134 | 358 | | + (Math.Abs(shift) + 63) / 64 + 1; |
| | 268 | 359 | | Span<ulong> endpoint = stackalloc ulong[cellWords], square = stackalloc ulong[2 * cellWords]; |
| | 402 | 360 | | Span<ulong> product = stackalloc ulong[words], numerator = stackalloc ulong[words], denominator = stackalloc ulo |
| | 268 | 361 | | endpoint.Clear(); parameter.LowerNumerator[..(cellWords - 1)].CopyTo(endpoint); |
| | 134 | 362 | | denominator.Clear(); |
| | 134 | 363 | | int sign = 0; |
| | 134 | 364 | | WideArithmetic.AddShiftedSignedMagnitude(squaredScale, 1, Math.Max(0, shift), denominator, ref sign); |
| | 134 | 365 | | lower = upper = 0; |
| | 134 | 366 | | Span<ulong> one = stackalloc ulong[1] { 1 }; |
| | 804 | 367 | | for (int index = 0; index < 2; index++) |
| | | 368 | | { |
| | 268 | 369 | | WideArithmetic.MultiplyMagnitudes(endpoint, endpoint, square); |
| | 268 | 370 | | WideArithmetic.MultiplyMagnitudes(square, metric, product); |
| | 536 | 371 | | numerator.Clear(); sign = 0; |
| | 268 | 372 | | WideArithmetic.AddShiftedSignedMagnitude(product, 1, Math.Max(0, -shift), numerator, ref sign); |
| | 268 | 373 | | ulong floor = WideArithmetic.GetRatioFloorSquareRoot(numerator, denominator, long.MaxValue); |
| | 536 | 374 | | if (index == 0) lower = floor; else upper = floor; |
| | 268 | 375 | | WideArithmetic.AddMagnitudeInto(one, endpoint); |
| | | 376 | | } |
| | 134 | 377 | | } |
| | | 378 | | |
| | | 379 | | private static int CompareDepth(scoped ref FiniteAxisValueRoot parameter, scoped ReadOnlySpan<ulong> metric, |
| | | 380 | | scoped ReadOnlySpan<ulong> squaredScale, ulong twiceRaw, int valueShift) |
| | | 381 | | { |
| | 1691 | 382 | | int _words = Math.Max(PolynomialWords, parameter.Coefficients.Length / parameter.Signs.Length); |
| | 3382 | 383 | | Span<ulong> query = stackalloc ulong[3 * _words]; Span<sbyte> signs = stackalloc sbyte[3]; |
| | 3382 | 384 | | query.Clear(); signs.Clear(); |
| | 1691 | 385 | | int sign = 0; |
| | 1691 | 386 | | WideArithmetic.AddShiftedSignedMagnitude(metric, 1, 2 * valueShift + 2, |
| | 3382 | 387 | | query.Slice(2 * _words, _words), ref sign); signs[2] = (sbyte)sign; |
| | 3382 | 388 | | Span<ulong> threshold = stackalloc ulong[2]; Fixed64.Multiply64To128(twiceRaw, twiceRaw, out threshold[1], out t |
| | 1691 | 389 | | WideArithmetic.MultiplyMagnitudes(threshold, squaredScale, query[.._words]); |
| | 1691 | 390 | | signs[0] = WideArithmetic.GetActiveMagnitudeLength(query[.._words]) == 0 ? (sbyte)0 : (sbyte)-1; |
| | 1691 | 391 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref parameter, query, signs); |
| | | 392 | | } |
| | | 393 | | |
| | 1205 | 394 | | private int CellCapacity() => _metadata.HasCompactCell ? CellWords |
| | 1205 | 395 | | : WideFiniteAxisIntersection.GetFiniteValueRootCellWords(_polynomial[..(_metadata.PolynomialCount * _words)], _s |
| | | 396 | | |
| | | 397 | | private static FiniteAxisValueRoot GetRoot(ReadOnlySpan<ulong> polynomial, ReadOnlySpan<sbyte> polynomialSigns, |
| | | 398 | | ReadOnlySpan<ulong> retainedCell, CapsulePlaneRayValueMetadata metadata, Span<ulong> cell) |
| | | 399 | | { |
| | 1205 | 400 | | int _words = polynomial.Length / polynomialSigns.Length; |
| | 1205 | 401 | | ReadOnlySpan<ulong> coefficients = polynomial[..(metadata.PolynomialCount * _words)]; |
| | 1205 | 402 | | ReadOnlySpan<sbyte> signs = polynomialSigns[..metadata.PolynomialCount]; |
| | 1205 | 403 | | if (!metadata.HasCompactCell) |
| | 4 | 404 | | return WideFiniteAxisIntersection.GetKnownFiniteValueRoot(coefficients, signs, metadata.Ordinal, metadata.Ro |
| | 1201 | 405 | | retainedCell.CopyTo(cell); |
| | 1201 | 406 | | return new FiniteAxisValueRoot { Coefficients = coefficients, Signs = signs, LowerNumerator = cell, |
| | 1201 | 407 | | DenominatorShift = metadata.DenominatorShift, Ordinal = metadata.Ordinal, IsRational = metadata.IsRational } |
| | | 408 | | } |
| | | 409 | | |
| | | 410 | | private static void SetCoefficient(scoped ContactQuadratic value, Span<ulong> rational, Span<sbyte> rationalSigns, |
| | | 411 | | Span<ulong> radical, Span<sbyte> radicalSigns, int index, int shift) |
| | | 412 | | { |
| | 6582 | 413 | | int _words = rational.Length / rationalSigns.Length; |
| | 13164 | 414 | | int rationalSign = 0, radicalSign = 0; |
| | 6582 | 415 | | WideArithmetic.AddShiftedSignedMagnitude(value.Rational, value.Signs[0], shift, |
| | 6582 | 416 | | rational.Slice(index * _words, _words), ref rationalSign); |
| | 6582 | 417 | | WideArithmetic.AddShiftedSignedMagnitude(value.Radical, value.Signs[1], shift, |
| | 6582 | 418 | | radical.Slice(index * _words, _words), ref radicalSign); |
| | 13164 | 419 | | rationalSigns[index] = (sbyte)rationalSign; radicalSigns[index] = (sbyte)radicalSign; |
| | 6582 | 420 | | } |
| | | 421 | | |
| | | 422 | | private static void BuildNorm(scoped ReadOnlySpan<ulong> rational, scoped ReadOnlySpan<sbyte> rationalSigns, |
| | | 423 | | scoped ReadOnlySpan<ulong> radical, scoped ReadOnlySpan<sbyte> radicalSigns, scoped ReadOnlySpan<ulong> root, |
| | | 424 | | Span<ulong> polynomial, Span<sbyte> signs) |
| | | 425 | | { |
| | 906 | 426 | | int words = polynomial.Length / signs.Length; |
| | 1812 | 427 | | Span<ulong> product = stackalloc ulong[words], radicalSquared = stackalloc ulong[polynomial.Length]; |
| | 906 | 428 | | Span<sbyte> radicalSquaredSigns = stackalloc sbyte[signs.Length]; |
| | 906 | 429 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(rational, rationalSigns, rational, rationalSigns, |
| | 906 | 430 | | polynomial, signs, product); |
| | 906 | 431 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(radical, radicalSigns, radical, radicalSigns, |
| | 906 | 432 | | radicalSquared, radicalSquaredSigns, product); |
| | 8436 | 433 | | for (int index = 0; index < signs.Length; index++) |
| | | 434 | | { |
| | 3312 | 435 | | WideArithmetic.MultiplyMagnitudes(radicalSquared.Slice(index * words, words), root, product); |
| | 3312 | 436 | | int sign = signs[index]; |
| | 3312 | 437 | | WideArithmetic.AddShiftedSignedMagnitude(product, -radicalSquaredSigns[index], 0, |
| | 6624 | 438 | | polynomial.Slice(index * words, words), ref sign); signs[index] = (sbyte)sign; |
| | | 439 | | } |
| | 906 | 440 | | } |
| | | 441 | | |
| | | 442 | | private static int SignAtRoot(scoped ref FiniteAxisValueRoot parameter, |
| | | 443 | | scoped ReadOnlySpan<ulong> rational, scoped ReadOnlySpan<sbyte> rationalSigns, |
| | | 444 | | scoped ReadOnlySpan<ulong> radical, scoped ReadOnlySpan<sbyte> radicalSigns, scoped ReadOnlySpan<ulong> root, |
| | | 445 | | bool knownNormRoot = false) |
| | | 446 | | { |
| | 3222 | 447 | | int rationalSign = WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref parameter, rational, rationa |
| | 3222 | 448 | | int radicalSign = WideArithmetic.GetActiveMagnitudeLength(root) == 0 ? 0 |
| | 3222 | 449 | | : WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref parameter, radical, radicalSigns); |
| | 3316 | 450 | | if (rationalSign == 0) return radicalSign; |
| | 5440 | 451 | | if (radicalSign == 0 || rationalSign == radicalSign) return rationalSign; |
| | | 452 | | // The defining norm already proves equal magnitudes. Opposite signs |
| | | 453 | | // admit its unsquared branch without proving that same norm zero again. |
| | 1009 | 454 | | if (knownNormRoot) return 0; |
| | 623 | 455 | | int count = 2 * rationalSigns.Length - 1; |
| | 1246 | 456 | | int inputWords = rational.Length / rationalSigns.Length, active = 0; |
| | 3766 | 457 | | for (int index = 0; index < rationalSigns.Length; index++) |
| | 1260 | 458 | | active = Math.Max(active, Math.Max(WideArithmetic.GetActiveMagnitudeLength(rational.Slice(index * inputWords |
| | 1260 | 459 | | WideArithmetic.GetActiveMagnitudeLength(radical.Slice(index * inputWords, inputWords)))); |
| | 623 | 460 | | int words = 2 * active + WideArithmetic.GetActiveMagnitudeLength(root) + 1; |
| | 1246 | 461 | | Span<ulong> norm = stackalloc ulong[count * words]; Span<sbyte> signs = stackalloc sbyte[count]; |
| | 623 | 462 | | BuildNorm(rational, rationalSigns, radical, radicalSigns, root, norm, signs); |
| | 623 | 463 | | return rationalSign * WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref parameter, norm, signs); |
| | | 464 | | } |
| | | 465 | | } |