| | | 1 | | //======================================================================= |
| | | 2 | | // ConvexContactValueRoot.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 | | /// <summary>Exact comparison and materialization of retained convex contact value roots.</summary> |
| | | 14 | | internal static class ConvexContactValueRoot |
| | | 15 | | { |
| | | 16 | | internal static FiniteAxisValueRoot MapSquaredValue(Signed192 rawScale, int valueShift, |
| | | 17 | | scoped ref FiniteAxisValueRoot parameter, scoped ReadOnlySpan<ulong> numerator, |
| | | 18 | | scoped ReadOnlySpan<sbyte> numeratorSigns, scoped ReadOnlySpan<ulong> denominator, |
| | | 19 | | scoped ReadOnlySpan<sbyte> denominatorSigns, ReadOnlySpan<ulong> values, ReadOnlySpan<sbyte> signs, Span<ulong> |
| | | 20 | | { |
| | 201 | 21 | | Span<ulong> scale = stackalloc ulong[3]; |
| | 201 | 22 | | WideArithmetic.GetMagnitude(rawScale, out scale[2], out scale[1], out scale[0]); |
| | 201 | 23 | | return MapSquaredValue(scale, valueShift, ref parameter, numerator, numeratorSigns, |
| | 201 | 24 | | denominator, denominatorSigns, values, signs, cell); |
| | | 25 | | } |
| | | 26 | | |
| | | 27 | | internal static FiniteAxisValueRoot MapSquaredValue(Signed320 rawScale, int valueShift, |
| | | 28 | | scoped ref FiniteAxisValueRoot parameter, scoped ReadOnlySpan<ulong> numerator, |
| | | 29 | | scoped ReadOnlySpan<sbyte> numeratorSigns, scoped ReadOnlySpan<ulong> denominator, |
| | | 30 | | scoped ReadOnlySpan<sbyte> denominatorSigns, ReadOnlySpan<ulong> values, ReadOnlySpan<sbyte> signs, Span<ulong> |
| | | 31 | | { |
| | 250 | 32 | | Span<ulong> scale = stackalloc ulong[5]; |
| | 250 | 33 | | WideArithmetic.GetMagnitude(rawScale, out scale[4], out scale[3], out scale[2], out scale[1], out scale[0]); |
| | 250 | 34 | | return MapSquaredValue(scale, valueShift, ref parameter, numerator, numeratorSigns, |
| | 250 | 35 | | denominator, denominatorSigns, values, signs, cell); |
| | | 36 | | } |
| | | 37 | | |
| | | 38 | | internal static int CompareRootSquared(Signed192 rawScale, int valueShift, |
| | | 39 | | scoped ref FiniteAxisValueRoot root, ConvexContactCandidate candidate) |
| | | 40 | | { |
| | 1374 | 41 | | Span<ulong> scale = stackalloc ulong[3]; |
| | 1374 | 42 | | WideArithmetic.GetMagnitude(rawScale, out scale[2], out scale[1], out scale[0]); |
| | 1374 | 43 | | return CompareRootSquared(scale, valueShift, ref root, candidate); |
| | | 44 | | } |
| | | 45 | | |
| | | 46 | | internal static int CompareRootSquared(Signed320 rawScale, int valueShift, |
| | | 47 | | scoped ref FiniteAxisValueRoot root, ConvexContactCandidate candidate) |
| | | 48 | | { |
| | 161 | 49 | | Span<ulong> scale = stackalloc ulong[5]; |
| | 161 | 50 | | WideArithmetic.GetMagnitude(rawScale, out scale[4], out scale[3], out scale[2], out scale[1], out scale[0]); |
| | 161 | 51 | | return CompareRootSquared(scale, valueShift, ref root, candidate); |
| | | 52 | | } |
| | | 53 | | |
| | | 54 | | internal static bool GetRoundedDepth(Signed192 rawScale, int valueShift, ref FiniteAxisValueRoot root, |
| | | 55 | | out Fixed64 depth, out bool clamped, int gapSign = 1, Fixed64 radiusOffset = default) |
| | | 56 | | { |
| | 310 | 57 | | Span<ulong> scale = stackalloc ulong[3]; |
| | 310 | 58 | | WideArithmetic.GetMagnitude(rawScale, out scale[2], out scale[1], out scale[0]); |
| | 310 | 59 | | return GetRoundedDepth(scale, valueShift, ref root, out depth, out clamped, gapSign, radiusOffset); |
| | | 60 | | } |
| | | 61 | | |
| | | 62 | | internal static bool GetRoundedDepth(Signed320 rawScale, int valueShift, ref FiniteAxisValueRoot root, |
| | | 63 | | out Fixed64 depth, out bool clamped, int gapSign = 1, Fixed64 radiusOffset = default) |
| | | 64 | | { |
| | 136 | 65 | | Span<ulong> scale = stackalloc ulong[5]; |
| | 136 | 66 | | WideArithmetic.GetMagnitude(rawScale, out scale[4], out scale[3], out scale[2], out scale[1], out scale[0]); |
| | 136 | 67 | | return GetRoundedDepth(scale, valueShift, ref root, out depth, out clamped, gapSign, radiusOffset); |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | /// <summary>Compares a positive raw squared gap N/D to (twiceRaw/2)²; D must be positive at the admitted parameter. |
| | | 71 | | internal static int CompareSquaredGapToTwiceRaw(scoped ref FiniteAxisValueRoot parameter, |
| | | 72 | | scoped ReadOnlySpan<ulong> numerator, scoped ReadOnlySpan<sbyte> numeratorSigns, |
| | | 73 | | scoped ReadOnlySpan<ulong> denominator, scoped ReadOnlySpan<sbyte> denominatorSigns, ulong twiceRaw) |
| | | 74 | | { |
| | 3351 | 75 | | Span<ulong> endpoint = stackalloc ulong[2]; |
| | 3351 | 76 | | Fixed64.Multiply64To128(twiceRaw, twiceRaw, out endpoint[1], out endpoint[0]); |
| | 3351 | 77 | | Span<ulong> scale = stackalloc ulong[1] { 1 }; |
| | 3351 | 78 | | return CompareSquaredValueEndpoint(scale, 0, ref parameter, |
| | 3351 | 79 | | numerator, numeratorSigns, denominator, denominatorSigns, endpoint, 2); |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | /// <summary>Maps one admitted stationary parameter to its exact squared-gap value root.</summary> |
| | | 83 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 84 | | internal static FiniteAxisValueRoot MapSquaredValue(scoped ReadOnlySpan<ulong> scale, int valueShift, |
| | | 85 | | scoped ref FiniteAxisValueRoot parameter, scoped ReadOnlySpan<ulong> numerator, |
| | | 86 | | scoped ReadOnlySpan<sbyte> numeratorSigns, scoped ReadOnlySpan<ulong> denominator, |
| | | 87 | | scoped ReadOnlySpan<sbyte> denominatorSigns, |
| | | 88 | | ReadOnlySpan<ulong> values, ReadOnlySpan<sbyte> signs, Span<ulong> cell) |
| | | 89 | | { |
| | 451 | 90 | | Span<ulong> batchCells = stackalloc ulong[64]; |
| | 451 | 91 | | Span<int> batchShifts = stackalloc int[8]; |
| | 451 | 92 | | FiniteAxisValueRoots roots = WideFiniteAxisIntersection.GetFiniteValueRoots(values, signs, batchCells, batchShif |
| | 451 | 93 | | Span<ulong> upper = stackalloc ulong[cell.Length]; |
| | 1454 | 94 | | for (int ordinal = 0; ordinal < roots.Count - 1; ordinal++) |
| | | 95 | | { |
| | 636 | 96 | | FiniteAxisValueRoot root = FiniteAxisValueRoots.GetRoot(roots, ordinal, values, signs, cell); |
| | 636 | 97 | | if (root.IsRational) |
| | | 98 | | { |
| | 47 | 99 | | if (CompareSquaredValueEndpoint(scale, valueShift, ref parameter, |
| | 47 | 100 | | numerator, numeratorSigns, denominator, denominatorSigns, cell, root.DenominatorShift) == 0) |
| | 47 | 101 | | return root; |
| | | 102 | | } |
| | | 103 | | else |
| | | 104 | | { |
| | 589 | 105 | | cell.CopyTo(upper); |
| | 589 | 106 | | WideArithmetic.AddWord(upper, 0, 1); |
| | 589 | 107 | | if (CompareSquaredValueEndpoint(scale, valueShift, ref parameter, |
| | 589 | 108 | | numerator, numeratorSigns, denominator, denominatorSigns, upper, root.DenominatorShift) < 0) |
| | 313 | 109 | | return root; |
| | | 110 | | } |
| | | 111 | | } |
| | | 112 | | System.Diagnostics.Debug.Assert(roots.Count > 0); |
| | 91 | 113 | | return FiniteAxisValueRoots.GetRoot(roots, roots.Count - 1, values, signs, cell); |
| | | 114 | | } |
| | | 115 | | |
| | | 116 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 117 | | private static int CompareSquaredValueEndpoint(scoped ReadOnlySpan<ulong> scale, int valueShift, |
| | | 118 | | scoped ref FiniteAxisValueRoot parameter, scoped ReadOnlySpan<ulong> numerator, |
| | | 119 | | scoped ReadOnlySpan<sbyte> numeratorSigns, scoped ReadOnlySpan<ulong> denominator, |
| | | 120 | | scoped ReadOnlySpan<sbyte> denominatorSigns, scoped ReadOnlySpan<ulong> endpoint, int shift) |
| | | 121 | | { |
| | 3987 | 122 | | int sourceWords = numerator.Length / numeratorSigns.Length; |
| | 3987 | 123 | | int words = (shift + valueShift + sourceWords * 64 + scale.Length * 128 + 127) / 64; |
| | 3987 | 124 | | Span<ulong> query = stackalloc ulong[5 * words]; |
| | 3987 | 125 | | Span<sbyte> signs = stackalloc sbyte[5]; |
| | 3987 | 126 | | Span<ulong> product = stackalloc ulong[words]; |
| | 3987 | 127 | | Span<ulong> scaleSquared = stackalloc ulong[2 * scale.Length]; |
| | 7974 | 128 | | query.Clear(); signs.Clear(); |
| | 3987 | 129 | | WideArithmetic.MultiplyMagnitudes(scale, scale, scaleSquared); |
| | 47844 | 130 | | for (int index = 0; index < 5; index++) |
| | | 131 | | { |
| | 19935 | 132 | | Span<ulong> target = query.Slice(index * words, words); |
| | 19935 | 133 | | int sign = 0; |
| | 19935 | 134 | | WideArithmetic.MultiplyMagnitudes(numerator.Slice(index * sourceWords, sourceWords), scaleSquared, product); |
| | 19935 | 135 | | WideArithmetic.AddShiftedSignedMagnitude(product, numeratorSigns[index], shift, target, ref sign); |
| | 19935 | 136 | | WideArithmetic.MultiplyMagnitudes(denominator.Slice(index * sourceWords, sourceWords), endpoint, product); |
| | 19935 | 137 | | WideArithmetic.AddShiftedSignedMagnitude(product, -denominatorSigns[index], valueShift, target, ref sign); |
| | 19935 | 138 | | signs[index] = (sbyte)sign; |
| | | 139 | | } |
| | 3987 | 140 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref parameter, query, signs); |
| | | 141 | | } |
| | | 142 | | |
| | | 143 | | /// <summary> |
| | | 144 | | /// Compares S/RawScale², where S=2^ValueShift*t, to an analytic squared |
| | | 145 | | /// raw gap. Gap signs are deliberately excluded; the caller ranks them first. |
| | | 146 | | /// Retains certified cell refinement and its metadata when caller storage permits. |
| | | 147 | | /// </summary> |
| | | 148 | | internal static int CompareRootSquared(scoped ReadOnlySpan<ulong> scale, int valueShift, |
| | | 149 | | scoped ref FiniteAxisValueRoot root, ConvexContactCandidate candidate) |
| | | 150 | | { |
| | 1535 | 151 | | Span<ulong> squaredScale = stackalloc ulong[2 * scale.Length]; |
| | 1535 | 152 | | WideArithmetic.MultiplyMagnitudes(scale, scale, squaredScale); |
| | 1535 | 153 | | int scaleWords = WideArithmetic.GetActiveMagnitudeLength(squaredScale); |
| | 1535 | 154 | | int denominatorWords = WideArithmetic.GetActiveMagnitudeLength(candidate.GapDenominator); |
| | 1535 | 155 | | int linearWords = Math.Max( |
| | 1535 | 156 | | WideArithmetic.GetActiveMagnitudeLength(candidate.GapRational) + scaleWords, |
| | 1535 | 157 | | denominatorWords + (valueShift + 63) / 64); |
| | 1535 | 158 | | int radicalWords = WideArithmetic.GetActiveMagnitudeLength(candidate.GapRadical) + scaleWords; |
| | 1535 | 159 | | int words = Math.Max(ConvexContactCandidate.Words, Math.Max(2 * linearWords + 1, |
| | 1535 | 160 | | 2 * radicalWords + WideArithmetic.GetActiveMagnitudeLength(candidate.GapRadicand) + 1)); |
| | 1535 | 161 | | Span<ulong> linear = stackalloc ulong[2 * words]; |
| | 1535 | 162 | | Span<sbyte> linearSigns = stackalloc sbyte[2]; |
| | | 163 | | // R=D*2^ValueShift*t-A*RawScale². Compare R with B*RawScale²*sqrt(C). |
| | 1535 | 164 | | WideArithmetic.MultiplyMagnitudes(candidate.GapRational, squaredScale, linear[..words]); |
| | 1535 | 165 | | linearSigns[0] = (sbyte)-candidate.GapRationalSign; |
| | 1535 | 166 | | linear[words..].Clear(); |
| | | 167 | | // Query scratch follows active coefficient widths, while a retained |
| | | 168 | | // affine candidate can have a wider padded field. Copy only its value. |
| | 1535 | 169 | | candidate.GapDenominator[..denominatorWords].CopyTo(linear[words..]); |
| | 1535 | 170 | | linearSigns[1] = 1; |
| | 1535 | 171 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(linear, 2, valueShift); |
| | 1535 | 172 | | int rationalSign = WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, linear, linearSigns); |
| | 1535 | 173 | | int radicalSign = WideArithmetic.GetActiveMagnitudeLength(candidate.GapRadicand) == 0 |
| | 1535 | 174 | | ? 0 : candidate.GapRadicalSign; |
| | 1535 | 175 | | if (rationalSign != radicalSign) |
| | 598 | 176 | | return rationalSign.CompareTo(radicalSign); |
| | 937 | 177 | | if (rationalSign == 0) |
| | 12 | 178 | | return 0; |
| | | 179 | | |
| | | 180 | | // Active-width bounds include scale multiplication, variable shifting, |
| | | 181 | | // both squares, and the radical product. This is independent of the |
| | | 182 | | // shape-specific candidate builder's coefficient-height proof. |
| | 925 | 183 | | Span<ulong> query = stackalloc ulong[3 * words]; |
| | 925 | 184 | | Span<sbyte> querySigns = stackalloc sbyte[3]; |
| | 925 | 185 | | Span<ulong> product = stackalloc ulong[words]; |
| | 925 | 186 | | Span<ulong> temporary = stackalloc ulong[words]; |
| | 925 | 187 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(linear, linearSigns, linear, linearSigns, |
| | 925 | 188 | | query, querySigns, product); |
| | 925 | 189 | | WideArithmetic.MultiplyMagnitudes(candidate.GapRadical, squaredScale, temporary); |
| | 925 | 190 | | WideArithmetic.MultiplyMagnitudes(temporary, temporary, product); |
| | 925 | 191 | | WideArithmetic.MultiplyMagnitudes(product, candidate.GapRadicand, temporary); |
| | 925 | 192 | | int constantSign = querySigns[0]; |
| | 925 | 193 | | WideArithmetic.AddShiftedSignedMagnitude(temporary, -1, 0, query[..words], ref constantSign); |
| | 925 | 194 | | querySigns[0] = (sbyte)constantSign; |
| | 925 | 195 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(query, querySigns); |
| | 925 | 196 | | return rationalSign * WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, querySigns); |
| | | 197 | | } |
| | | 198 | | |
| | | 199 | | /// <summary>Compares an exact squared value root with a positive-denominator quadratic ratio.</summary> |
| | | 200 | | internal static int CompareRootSquared(Signed192 rawScale, int valueShift, scoped ref FiniteAxisValueRoot root, |
| | | 201 | | scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> radical) |
| | | 202 | | { |
| | 18 | 203 | | Span<ulong> scale = stackalloc ulong[3], square = stackalloc ulong[6]; |
| | 9 | 204 | | WideArithmetic.GetMagnitude(rawScale, out scale[2], out scale[1], out scale[0]); |
| | 9 | 205 | | WideArithmetic.MultiplyMagnitudes(scale, scale, square); |
| | 9 | 206 | | int words = Math.Max(ContactQuadratic.ActiveWords(numerator) + WideArithmetic.GetActiveMagnitudeLength(square), |
| | 9 | 207 | | ContactQuadratic.ActiveWords(denominator) + (valueShift + 63) / 64) + 1; |
| | 18 | 208 | | Span<ulong> a = stackalloc ulong[2 * words], b = stackalloc ulong[2 * words]; |
| | 18 | 209 | | Span<sbyte> aSigns = stackalloc sbyte[2], bSigns = stackalloc sbyte[2]; |
| | 9 | 210 | | WriteLinear(numerator.Rational, numerator.Signs[0], denominator.Rational, denominator.Signs[0], a, aSigns, squar |
| | 9 | 211 | | int firstSign = WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, a, aSigns); |
| | 10 | 212 | | if (WideArithmetic.GetActiveMagnitudeLength(radical) == 0) return firstSign; |
| | 8 | 213 | | WriteLinear(numerator.Radical, numerator.Signs[1], denominator.Radical, denominator.Signs[1], b, bSigns, square, |
| | 8 | 214 | | int secondSign = WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, b, bSigns); |
| | 10 | 215 | | if (firstSign == 0) return secondSign; |
| | 9 | 216 | | if (secondSign == 0 || firstSign == secondSign) return firstSign; |
| | | 217 | | // A(t)+B(t)*sqrt(K) has opposing terms. The original certified |
| | | 218 | | // value root also signs A(t)^2-K*B(t)^2, of degree only two. |
| | 3 | 219 | | int squareWords = 2 * words + WideArithmetic.GetActiveMagnitudeLength(radical) + 1; |
| | 6 | 220 | | Span<ulong> query = stackalloc ulong[3 * squareWords], secondSquare = stackalloc ulong[3 * squareWords]; |
| | 6 | 221 | | Span<sbyte> querySigns = stackalloc sbyte[3], secondSigns = stackalloc sbyte[3]; |
| | 3 | 222 | | Span<ulong> product = stackalloc ulong[squareWords]; |
| | 3 | 223 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(a, aSigns, a, aSigns, query, querySigns, product); |
| | 3 | 224 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(b, bSigns, b, bSigns, secondSquare, secondSigns, produc |
| | 24 | 225 | | for (int index = 0; index < 3; index++) |
| | | 226 | | { |
| | 9 | 227 | | WideArithmetic.MultiplyMagnitudes(secondSquare.Slice(index * squareWords, squareWords), radical, product); |
| | 9 | 228 | | int sign = querySigns[index]; |
| | 9 | 229 | | WideArithmetic.AddShiftedSignedMagnitude(product, -secondSigns[index], 0, query.Slice(index * squareWords, s |
| | 9 | 230 | | querySigns[index] = (sbyte)sign; |
| | | 231 | | } |
| | 3 | 232 | | return firstSign * WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, querySigns); |
| | | 233 | | |
| | | 234 | | static void WriteLinear(ReadOnlySpan<ulong> n, int nSign, ReadOnlySpan<ulong> d, int dSign, Span<ulong> values, |
| | | 235 | | ReadOnlySpan<ulong> square, int words, int valueShift) |
| | | 236 | | { |
| | | 237 | | WideArithmetic.MultiplyMagnitudes(n, square, values[..words]); signs[0] = (sbyte)-nSign; |
| | | 238 | | values[words..].Clear(); d[..WideArithmetic.GetActiveMagnitudeLength(d)].CopyTo(values[words..]); signs[1] = |
| | | 239 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(values, 2, valueShift); |
| | | 240 | | } |
| | | 241 | | } |
| | | 242 | | |
| | | 243 | | internal static Vector3d GetNormalizedDirection(ref FiniteAxisValueRoot root, |
| | | 244 | | scoped ReadOnlySpan<ulong> gradient, scoped ReadOnlySpan<sbyte> gradientSigns, int orientation) => |
| | 454 | 245 | | GetScaledNormalizedDirection(ref root, gradient, gradientSigns, Fixed64.One, orientation); |
| | | 246 | | |
| | | 247 | | internal static Vector3d GetScaledNormalizedDirection(ref FiniteAxisValueRoot root, |
| | | 248 | | scoped ReadOnlySpan<ulong> gradient, scoped ReadOnlySpan<sbyte> gradientSigns, Fixed64 scale, int orientation) |
| | | 249 | | { |
| | 715 | 250 | | int count = gradientSigns.Length / 3; |
| | 715 | 251 | | int words = gradient.Length / gradientSigns.Length; |
| | 715 | 252 | | int squareCount = 2 * count - 1; |
| | 715 | 253 | | int activeWords = 0; |
| | 16586 | 254 | | for (int index = 0; index < gradientSigns.Length; index++) |
| | 7578 | 255 | | activeWords = Math.Max(activeWords, |
| | 7578 | 256 | | WideArithmetic.GetActiveMagnitudeLength(gradient.Slice(index * words, words))); |
| | | 257 | | // Each coefficient sums at most 3*count products; one extra word |
| | | 258 | | // covers that carry. Generated gradients also retain the existing |
| | | 259 | | // proof that their squared coefficients fit twice the input stride. |
| | | 260 | | // Taking the tighter bound avoids padding in every rounding threshold |
| | | 261 | | // without increasing the worst-case stack; a zero gradient needs one word. |
| | 715 | 262 | | int squareWords = Math.Min(2 * words, 2 * activeWords + 1); |
| | 715 | 263 | | Span<ulong> squaredLength = stackalloc ulong[squareCount * squareWords]; |
| | 715 | 264 | | Span<sbyte> squaredLengthSigns = stackalloc sbyte[squareCount]; |
| | 715 | 265 | | BuildSquaredLength(gradient, gradientSigns, squaredLength, squaredLengthSigns); |
| | 715 | 266 | | return new Vector3d( |
| | 715 | 267 | | GetRoundedNormalComponent(ref root, gradient[..(count * words)], gradientSigns[..count], |
| | 715 | 268 | | squaredLength, squaredLengthSigns, scale, orientation), |
| | 715 | 269 | | GetRoundedNormalComponent(ref root, gradient.Slice(count * words, count * words), |
| | 715 | 270 | | gradientSigns.Slice(count, count), squaredLength, squaredLengthSigns, scale, orientation), |
| | 715 | 271 | | GetRoundedNormalComponent(ref root, gradient[(2 * count * words)..], gradientSigns[(2 * count)..], |
| | 715 | 272 | | squaredLength, squaredLengthSigns, scale, orientation)); |
| | | 273 | | } |
| | | 274 | | |
| | | 275 | | /// <summary>Classifies and rounds radiusOffset + gapSign*sqrt(value) as one exact value.</summary> |
| | | 276 | | internal static bool GetRoundedDepth(scoped ReadOnlySpan<ulong> scale, int valueShift, ref FiniteAxisValueRoot root, |
| | | 277 | | out Fixed64 depth, out bool clamped, int gapSign = 1, Fixed64 radiusOffset = default) |
| | | 278 | | { |
| | 892 | 279 | | depth = default; clamped = false; |
| | | 280 | | // Isolated squared-value roots are strictly positive. Exact zero-gap |
| | | 281 | | // features are retained separately by their geometry owner. |
| | | 282 | | System.Diagnostics.Debug.Assert(gapSign is -1 or 1); |
| | 446 | 283 | | Span<ulong> squaredScale = stackalloc ulong[2 * scale.Length]; |
| | 446 | 284 | | WideArithmetic.MultiplyMagnitudes(scale, scale, squaredScale); |
| | 446 | 285 | | if (gapSign < 0 && CompareDepthToTwiceRaw(root, squaredScale, valueShift, 0, gapSign, radiusOffset) < 0) |
| | 11 | 286 | | return false; |
| | 435 | 287 | | int maximum = CompareDepthToTwiceRaw(root, squaredScale, valueShift, |
| | 435 | 288 | | unchecked((ulong)long.MaxValue << 1), gapSign, radiusOffset); |
| | 435 | 289 | | if (maximum >= 0) |
| | | 290 | | { |
| | 4 | 291 | | depth = Fixed64.MaxValue; |
| | 4 | 292 | | clamped = maximum > 0; |
| | 4 | 293 | | return true; |
| | | 294 | | } |
| | | 295 | | // All rounding thresholds lie on this dyadic grid. Once the exact |
| | | 296 | | // selected root occupies one grid cell, none can be strictly inside |
| | | 297 | | // it: subsequent comparisons reuse the cell without rebuilding Sturm |
| | | 298 | | // chains. A root on a grid point is retained as an exact singleton. |
| | 431 | 299 | | WideFiniteAxisIntersection.RefineFiniteValueRoot(ref root, valueShift + 2); |
| | 431 | 300 | | ulong lower = 0; |
| | 431 | 301 | | ulong upper = 1UL << 63; |
| | 27619 | 302 | | while (lower < upper) |
| | | 303 | | { |
| | 27188 | 304 | | ulong midpoint = lower + ((upper - lower) >> 1); |
| | 27188 | 305 | | if (CompareDepthToTwiceRaw(root, squaredScale, valueShift, midpoint << 1, gapSign, radiusOffset) >= 0) |
| | 5271 | 306 | | lower = midpoint + 1; |
| | | 307 | | else |
| | 21917 | 308 | | upper = midpoint; |
| | | 309 | | } |
| | 431 | 310 | | ulong floor = lower - 1; |
| | 431 | 311 | | int comparison = CompareDepthToTwiceRaw(root, squaredScale, valueShift, (floor << 1) | 1, gapSign, radiusOffset) |
| | 431 | 312 | | depth = Fixed64.FromRaw((long)(floor + (comparison > 0 || (comparison == 0 && (floor & 1) != 0) ? 1UL : 0))); |
| | 431 | 313 | | clamped = false; |
| | 431 | 314 | | return true; |
| | | 315 | | } |
| | | 316 | | |
| | | 317 | | private static int CompareDepthToTwiceRaw(FiniteAxisValueRoot root, |
| | | 318 | | ReadOnlySpan<ulong> squaredScale, int valueShift, ulong twiceRaw, int gapSign, Fixed64 radiusOffset) |
| | | 319 | | { |
| | 28091 | 320 | | Signed192 difference = WideArithmetic.SubtractSigned192(new Signed192(0, 0, twiceRaw), |
| | 28091 | 321 | | WideArithmetic.AddSigned192(Signed192.Raw(radiusOffset), Signed192.Raw(radiusOffset))); |
| | 28091 | 322 | | if (gapSign != difference.Sign) |
| | 916 | 323 | | return gapSign.CompareTo(difference.Sign); |
| | | 324 | | // Both the nonnegative output threshold and doubled radius fit 64 |
| | | 325 | | // unsigned bits; their signed difference has the same magnitude bound. |
| | 27175 | 326 | | WideArithmetic.GetMagnitude(difference, out _, out _, out ulong magnitude); |
| | 27175 | 327 | | Fixed64.Multiply64To128(magnitude, magnitude, out ulong high, out ulong low); |
| | 27175 | 328 | | Span<ulong> squaredRaw = stackalloc ulong[2] { low, high }; |
| | 27175 | 329 | | Span<ulong> threshold = stackalloc ulong[squaredScale.Length + 2]; |
| | 27175 | 330 | | WideArithmetic.MultiplyMagnitudes(squaredScale, squaredRaw, threshold); |
| | 27175 | 331 | | return gapSign * WideFiniteAxisIntersection.CompareFiniteValueRootToDyadic(root, threshold, valueShift + 2); |
| | | 332 | | } |
| | | 333 | | |
| | | 334 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 335 | | internal static void BuildSquaredLength(ReadOnlySpan<ulong> gradient, ReadOnlySpan<sbyte> signs, |
| | | 336 | | Span<ulong> squaredLength, Span<sbyte> squaredLengthSigns) |
| | | 337 | | { |
| | 766 | 338 | | int count = signs.Length / 3; |
| | 766 | 339 | | int words = gradient.Length / signs.Length; |
| | 766 | 340 | | Span<ulong> product = stackalloc ulong[squaredLength.Length / squaredLengthSigns.Length]; |
| | 6128 | 341 | | for (int axis = 0; axis < 3; axis++) |
| | | 342 | | { |
| | 2298 | 343 | | ReadOnlySpan<ulong> component = gradient.Slice(axis * count * words, count * words); |
| | 2298 | 344 | | ReadOnlySpan<sbyte> componentSigns = signs.Slice(axis * count, count); |
| | 2298 | 345 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(component, componentSigns, |
| | 2298 | 346 | | component, componentSigns, squaredLength, squaredLengthSigns, product, axis != 0); |
| | | 347 | | } |
| | 766 | 348 | | } |
| | | 349 | | |
| | | 350 | | private static Fixed64 GetRoundedNormalComponent(ref FiniteAxisValueRoot root, |
| | | 351 | | scoped ReadOnlySpan<ulong> component, scoped ReadOnlySpan<sbyte> componentSigns, |
| | | 352 | | scoped ReadOnlySpan<ulong> squaredLength, scoped ReadOnlySpan<sbyte> squaredLengthSigns, Fixed64 scale, int orie |
| | | 353 | | { |
| | 2145 | 354 | | int sign = orientation * WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, component, compo |
| | 2145 | 355 | | if (sign == 0 || scale == Fixed64.Zero) |
| | 479 | 356 | | return Fixed64.Zero; |
| | 1666 | 357 | | int squareCount = squaredLengthSigns.Length; |
| | 1666 | 358 | | int queryWords = squaredLength.Length / squareCount + 2; |
| | 1666 | 359 | | Span<ulong> query = stackalloc ulong[squareCount * queryWords]; |
| | 1666 | 360 | | Span<sbyte> querySigns = stackalloc sbyte[squareCount]; |
| | 1666 | 361 | | int removedShift = PrepareNormalThreshold(root, component, componentSigns, |
| | 1666 | 362 | | squaredLength, squaredLengthSigns, (ulong)scale.m_rawValue, query, querySigns, |
| | 1666 | 363 | | out ulong lowerFloor, out ulong upperFloor); |
| | | 364 | | // The lower floor is already proven to pass its integer threshold. |
| | | 365 | | // Search for the first failing integer strictly above that floor. |
| | 1666 | 366 | | ulong lower = lowerFloor + 1; |
| | 1666 | 367 | | ulong upper = upperFloor + 1; |
| | 1666 | 368 | | ulong previousTwiceRaw = 0; |
| | 21978 | 369 | | while (lower < upper) |
| | | 370 | | { |
| | 20312 | 371 | | ulong midpoint = lower + ((upper - lower) >> 1); |
| | 20312 | 372 | | BuildNormalThreshold(squaredLength, squaredLengthSigns, |
| | 20312 | 373 | | midpoint << 1, ref previousTwiceRaw, ref removedShift, query, querySigns); |
| | 20312 | 374 | | if (WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, querySigns) >= 0) |
| | 8480 | 375 | | lower = midpoint + 1; |
| | | 376 | | else |
| | 11832 | 377 | | upper = midpoint; |
| | | 378 | | } |
| | 1666 | 379 | | ulong floor = lower - 1; |
| | 1666 | 380 | | BuildNormalThreshold(squaredLength, squaredLengthSigns, |
| | 1666 | 381 | | (floor << 1) | 1, ref previousTwiceRaw, ref removedShift, query, querySigns); |
| | 1666 | 382 | | int comparison = WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, querySigns); |
| | 1666 | 383 | | ulong rounded = floor + (comparison > 0 || (comparison == 0 && (floor & 1) != 0) ? 1UL : 0); |
| | 1666 | 384 | | return Fixed64.FromRaw(sign * (long)rounded); |
| | | 385 | | } |
| | | 386 | | |
| | | 387 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 388 | | private static int PrepareNormalThreshold(FiniteAxisValueRoot root, |
| | | 389 | | ReadOnlySpan<ulong> component, ReadOnlySpan<sbyte> componentSigns, |
| | | 390 | | ReadOnlySpan<ulong> squaredLength, ReadOnlySpan<sbyte> squaredLengthSigns, ulong scale, |
| | | 391 | | Span<ulong> query, Span<sbyte> querySigns, out ulong lower, out ulong upper) |
| | | 392 | | { |
| | 1666 | 393 | | int queryWords = query.Length / querySigns.Length; |
| | 1666 | 394 | | Span<ulong> product = stackalloc ulong[queryWords]; |
| | 1666 | 395 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(component, componentSigns, |
| | 1666 | 396 | | component, componentSigns, query, querySigns, product); |
| | 1666 | 397 | | GetNormalFloorBounds(root, query, querySigns, squaredLength, squaredLengthSigns, scale, out lower, out upper); |
| | | 398 | | // Keep the same initialized threshold state as the original first |
| | | 399 | | // search iteration. Bounds consume the unscaled square first, so the |
| | | 400 | | // common normalization does not lose 64..126 useful precision bits. |
| | 2902 | 401 | | if (scale == (ulong)Fixed64.One.m_rawValue) return 66; |
| | 430 | 402 | | Span<ulong> factor = stackalloc ulong[2]; |
| | 430 | 403 | | Fixed64.Multiply64To128(scale << 1, scale << 1, out factor[1], out factor[0]); |
| | 4160 | 404 | | for (int index = 0; index < querySigns.Length; index++) |
| | | 405 | | { |
| | 1650 | 406 | | Span<ulong> target = query.Slice(index * queryWords, queryWords); |
| | 1650 | 407 | | WideArithmetic.MultiplyMagnitudes(target, factor, product); |
| | 1650 | 408 | | product.CopyTo(target); |
| | | 409 | | } |
| | 430 | 410 | | return 0; |
| | | 411 | | } |
| | | 412 | | |
| | | 413 | | private static void GetNormalFloorBounds(FiniteAxisValueRoot root, |
| | | 414 | | ReadOnlySpan<ulong> componentSquared, ReadOnlySpan<sbyte> componentSigns, |
| | | 415 | | ReadOnlySpan<ulong> squaredLength, ReadOnlySpan<sbyte> lengthSigns, ulong scale, |
| | | 416 | | out ulong lower, out ulong upper) |
| | | 417 | | { |
| | 3332 | 418 | | lower = 0; upper = scale; |
| | 1666 | 419 | | int count = lengthSigns.Length; |
| | 1666 | 420 | | int margin = 2 * (64 - Fixed64.CountLeadingZeroes((ulong)(count - 1))); |
| | | 421 | | // 59 is the largest fixed precision for which 17 normalized |
| | | 422 | | // coefficients plus the error allowance fit unsigned 64-bit endpoints. |
| | 1666 | 423 | | int precision = root.IsRational ? 59 : Math.Min(59, root.DenominatorShift - margin); |
| | 1786 | 424 | | if (precision <= 0) return; |
| | 1546 | 425 | | int bits = Math.Max(WideFiniteAxisIntersection.GetFiniteRootCoefficientBits(componentSquared, count), |
| | 1546 | 426 | | WideFiniteAxisIntersection.GetFiniteRootCoefficientBits(squaredLength, count)); |
| | 1546 | 427 | | WideFiniteAxisIntersection.GetFiniteValueRootNonnegativeBounds(root, squaredLength, lengthSigns, |
| | 1546 | 428 | | precision, bits, out ulong lengthMinimum, out ulong lengthMaximum); |
| | 1993 | 429 | | if (lengthMinimum == 0) return; |
| | 1099 | 430 | | WideFiniteAxisIntersection.GetFiniteValueRootNonnegativeBounds(root, componentSquared, componentSigns, |
| | 1099 | 431 | | precision, bits, out ulong componentMinimum, out ulong componentMaximum); |
| | | 432 | | // C² and L share one normalization. Scale only the bounded numerator: |
| | | 433 | | // <=64-bit endpoints times <=126-bit scale² fit Signed320. Existing |
| | | 434 | | // clipped ratio/square-root division gives outward raw floor bounds; |
| | | 435 | | // exact integer and half-raw threshold signs remain the final authority. |
| | 1099 | 436 | | Fixed64.Multiply64To128(scale, scale, out ulong high, out ulong low); |
| | 1099 | 437 | | Signed192 factor = new(0, high, low); |
| | 1099 | 438 | | Signed320 minimum = WideArithmetic.MultiplySigned192(new Signed192(0, 0, componentMinimum), factor); |
| | 1099 | 439 | | Signed320 maximum = WideArithmetic.MultiplySigned192(new Signed192(0, 0, componentMaximum), factor); |
| | 1099 | 440 | | Span<ulong> bounds = stackalloc ulong[20]; |
| | 1099 | 441 | | bounds.Clear(); |
| | 1099 | 442 | | WideArithmetic.GetMagnitude(minimum, out bounds[4], out bounds[3], out bounds[2], out bounds[1], out bounds[0]); |
| | 1099 | 443 | | WideArithmetic.GetMagnitude(maximum, out bounds[9], out bounds[8], out bounds[7], out bounds[6], out bounds[5]); |
| | 2198 | 444 | | bounds[10] = lengthMinimum; bounds[15] = lengthMaximum; |
| | 1099 | 445 | | lower = WideArithmetic.GetRatioFloorSquareRoot(bounds[..5], bounds[15..], scale); |
| | 1099 | 446 | | upper = WideArithmetic.GetRatioFloorSquareRoot(bounds.Slice(5, 5), bounds.Slice(10, 5), scale); |
| | 1099 | 447 | | } |
| | | 448 | | |
| | | 449 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 450 | | private static void BuildNormalThreshold( |
| | | 451 | | ReadOnlySpan<ulong> squaredLength, ReadOnlySpan<sbyte> squaredLengthSigns, |
| | | 452 | | ulong twiceRaw, ref ulong previousTwiceRaw, ref int removedShift, |
| | | 453 | | Span<ulong> query, Span<sbyte> querySigns) |
| | | 454 | | { |
| | 21978 | 455 | | int squareCount = squaredLengthSigns.Length; |
| | 21978 | 456 | | int squareWords = squaredLength.Length / squareCount; |
| | 21978 | 457 | | int queryWords = query.Length / querySigns.Length; |
| | 21978 | 458 | | Span<ulong> product = stackalloc ulong[queryWords]; |
| | | 459 | | // Q_k=4*scale²*C²-k²*L. Restore the previous normalization and use |
| | | 460 | | // Q_new=Q_old+(old²-new²)*L, retaining C² without a separate buffer. |
| | | 461 | | // Both k values fit 64 unsigned bits, so the signed difference needs |
| | | 462 | | // at most 128 magnitude bits. The raw polynomial has the same height |
| | | 463 | | // bound; the existing two extra coefficient words remain sufficient. |
| | 21978 | 464 | | Fixed64.Multiply64To128(previousTwiceRaw, previousTwiceRaw, out ulong previousHigh, out ulong previousLow); |
| | 21978 | 465 | | Fixed64.Multiply64To128(twiceRaw, twiceRaw, out ulong high, out ulong low); |
| | 21978 | 466 | | Signed192 difference = WideArithmetic.SubtractSigned192( |
| | 21978 | 467 | | new Signed192(0, previousHigh, previousLow), new Signed192(0, high, low)); |
| | 21978 | 468 | | Span<ulong> threshold = stackalloc ulong[2]; |
| | 21978 | 469 | | WideArithmetic.GetMagnitude(difference, out _, out threshold[1], out threshold[0]); |
| | 598104 | 470 | | for (int index = 0; index < squareCount; index++) |
| | | 471 | | { |
| | 277074 | 472 | | Span<ulong> target = query.Slice(index * queryWords, queryWords); |
| | 277074 | 473 | | target.CopyTo(product); |
| | 277074 | 474 | | int shiftedSign = 0; |
| | 277074 | 475 | | target.Clear(); |
| | 277074 | 476 | | WideArithmetic.AddShiftedSignedMagnitude(product, querySigns[index], removedShift, target, ref shiftedSign); |
| | 277074 | 477 | | WideArithmetic.MultiplyMagnitudes(squaredLength.Slice(index * squareWords, squareWords), threshold, product) |
| | 277074 | 478 | | WideArithmetic.AddShiftedSignedMagnitude(product, difference.Sign * squaredLengthSigns[index], |
| | 277074 | 479 | | 0, target, ref shiftedSign); |
| | 277074 | 480 | | querySigns[index] = (sbyte)shiftedSign; |
| | | 481 | | } |
| | 21978 | 482 | | previousTwiceRaw = twiceRaw; |
| | 21978 | 483 | | removedShift = WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(query, querySigns); |
| | 21978 | 484 | | } |
| | | 485 | | } |