| | | 1 | | //======================================================================= |
| | | 2 | | // FiniteSlabRaySelection.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 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | |
| | | 13 | | |
| | | 14 | | /// <summary>Independently retained finite-domain ray maximum and exact witness.</summary> |
| | | 15 | | /// <content>Retains exact active-set roots and materializes their original source charts.</content> |
| | | 16 | | internal ref partial struct FiniteSlabRaySelection |
| | | 17 | | { |
| | | 18 | | private const int RetainedCellWords = 8; |
| | | 19 | | internal const int StorageWords = 13 * FiniteSlabPolynomial.Words + RetainedCellWords; |
| | 10078 | 20 | | private readonly int Words => FieldWords; |
| | 4334 | 21 | | internal static int GetStorageWords(int fieldWords) => 13 * fieldWords + RetainedCellWords; |
| | | 22 | | internal readonly int FieldWords; |
| | | 23 | | internal const int SignCount = 13; |
| | | 24 | | private readonly Span<ulong> _values; |
| | | 25 | | private readonly Span<sbyte> _signs; |
| | | 26 | | private bool _parameterIsDepth; |
| | | 27 | | private int _ordinal, _rootCount, _coefficientCount; |
| | | 28 | | private FixedTriangle _triangle; |
| | | 29 | | private int _orientation; |
| | | 30 | | private int _domainEdgeMask; |
| | | 31 | | private bool _hasCompactCell, _isRational; |
| | | 32 | | private int _denominatorShift; |
| | | 33 | | internal bool HasValue { get; private set; } |
| | 10965 | 34 | | private readonly FiniteSlabPolynomial Polynomial => At(0, 3); |
| | 5875 | 35 | | private readonly FiniteSlabPolynomial Lambda => At(3, 2); |
| | 5858 | 36 | | private readonly FiniteSlabPolynomial Mu => At(5, 2); |
| | 5638 | 37 | | private readonly FiniteSlabPolynomial T => At(7, 2); |
| | 10987 | 38 | | private readonly FiniteSlabPolynomial D => At(9, 2); |
| | 4777 | 39 | | private readonly FiniteSlabPolynomial DepthN => At(11, 1); |
| | 4777 | 40 | | private readonly FiniteSlabPolynomial DepthD => At(12, 1); |
| | 4462 | 41 | | private readonly Span<ulong> RetainedCell => _values[(13 * Words)..]; |
| | | 42 | | private readonly FiniteSlabPolynomial At(int start, int count) => |
| | 48877 | 43 | | FiniteSlabPolynomial.At(_values, _signs, start, count); |
| | | 44 | | |
| | | 45 | | internal FiniteSlabRaySelection(Span<ulong> values, Span<sbyte> signs, int fieldWords = FiniteSlabPolynomial.Words) |
| | | 46 | | { |
| | 1610 | 47 | | if (values.Length < GetStorageWords(fieldWords) || signs.Length < SignCount) |
| | 2 | 48 | | throw new ArgumentException("The finite slab ray selection storage is too small."); |
| | 4824 | 49 | | FieldWords = fieldWords; _values = values[..GetStorageWords(fieldWords)]; _signs = signs[..SignCount]; |
| | 3216 | 50 | | HasValue = _parameterIsDepth = false; _ordinal = _rootCount = _coefficientCount = _orientation = _domainEdgeMask |
| | 1608 | 51 | | _triangle = default; |
| | 3216 | 52 | | _hasCompactCell = _isRational = false; _denominatorShift = 0; |
| | 1608 | 53 | | } |
| | | 54 | | |
| | | 55 | | private readonly FiniteAxisValueRoot Restore(Span<ulong> cell) |
| | | 56 | | { |
| | 1621 | 57 | | if (_hasCompactCell) |
| | | 58 | | { |
| | 3230 | 59 | | RetainedCell.CopyTo(cell); cell[RetainedCellWords..].Clear(); |
| | 1615 | 60 | | return new() { Coefficients = Polynomial.Values[..(_coefficientCount * Words)], |
| | 1615 | 61 | | Signs = Polynomial.Signs[.._coefficientCount], LowerNumerator = cell, |
| | 1615 | 62 | | DenominatorShift = _denominatorShift, Ordinal = _ordinal, IsRational = _isRational }; |
| | | 63 | | } |
| | 6 | 64 | | return WideFiniteAxisIntersection.GetKnownFiniteValueRoot( |
| | 6 | 65 | | Polynomial.Values[..(_coefficientCount * Words)], |
| | 6 | 66 | | Polynomial.Signs[.._coefficientCount], _ordinal, _rootCount, cell); |
| | | 67 | | } |
| | 1621 | 68 | | private readonly int CellWords => Math.Max(RetainedCellWords, |
| | 1621 | 69 | | WideFiniteAxisIntersection.GetFiniteValueRootCellWords(Polynomial.Values, Polynomial.Signs)); |
| | 115 | 70 | | internal readonly FiniteSlabRaySelectionMetadata Metadata => new(HasValue, _parameterIsDepth, |
| | 115 | 71 | | _ordinal, _rootCount, _coefficientCount, _orientation, _triangle, _domainEdgeMask, |
| | 115 | 72 | | _hasCompactCell, _isRational, _denominatorShift); |
| | | 73 | | internal void Restore(FiniteSlabRaySelectionMetadata metadata) |
| | | 74 | | { |
| | 1014 | 75 | | HasValue = metadata.HasValue; _parameterIsDepth = metadata.ParameterIsDepth; _ordinal = metadata.Ordinal; |
| | 676 | 76 | | _rootCount = metadata.RootCount; _coefficientCount = metadata.CoefficientCount; |
| | 676 | 77 | | _orientation = metadata.Orientation; _triangle = metadata.Triangle; |
| | 338 | 78 | | _domainEdgeMask = metadata.DomainEdgeMask; |
| | 676 | 79 | | _hasCompactCell = metadata.HasCompactCell; _isRational = metadata.IsRational; |
| | 338 | 80 | | _denominatorShift = metadata.DenominatorShift; |
| | 338 | 81 | | } |
| | | 82 | | |
| | | 83 | | /// <summary> |
| | | 84 | | /// Compares depths sharing the canonical frame. A generic parameter is |
| | | 85 | | /// u=(1+t)/(2+t); constant-depth families retain a separate rational u. |
| | | 86 | | /// </summary> |
| | | 87 | | internal readonly int CompareDepth(scoped in FiniteSlabRaySelection other) |
| | | 88 | | { |
| | 489 | 89 | | if (!_parameterIsDepth && !other._parameterIsDepth) |
| | 372 | 90 | | return Compare(false, default, DepthN, DepthD, false, default, other.DepthN, other.DepthD); |
| | 117 | 91 | | Span<ulong> firstCell = stackalloc ulong[CellWords]; |
| | 117 | 92 | | Span<ulong> secondCell = stackalloc ulong[other.CellWords]; |
| | 234 | 93 | | FiniteAxisValueRoot first = Restore(firstCell), second = other.Restore(secondCell); |
| | 117 | 94 | | return Compare(_parameterIsDepth, first, DepthN, DepthD, |
| | 117 | 95 | | other._parameterIsDepth, second, other.DepthN, other.DepthD); |
| | | 96 | | } |
| | | 97 | | |
| | | 98 | | /// <summary>Compares physical depths across certified coplanar members in the same slab chart.</summary> |
| | | 99 | | /// <remarks>Positive depths are independent of ray orientation. The caller certifies common physical geometry.</rem |
| | | 100 | | internal readonly int CompareDepthAcrossFrames(scoped in FiniteSlabRaySelection other, |
| | | 101 | | in FiniteSlabRayFrame frame, in FiniteSlabRayFrame otherFrame) |
| | | 102 | | { |
| | 32 | 103 | | if (!HasValue || !other.HasValue) |
| | 2 | 104 | | throw new InvalidOperationException("Both slab-ray selections must have an admitted value."); |
| | 30 | 105 | | if (frame.Plane.NormalSquared.IsZero || !frame.Plane.NormalSquared.Equals(otherFrame.Plane.NormalSquared)) |
| | 2 | 106 | | throw new ArgumentException("The slab-ray frames must share a nonzero reduced normal metric."); |
| | 28 | 107 | | int axis = !frame.Plane.Normal.X.IsZero ? 0 : !frame.Plane.Normal.Y.IsZero ? 1 : 2; |
| | | 108 | | // The common primitive-normal length cancels. Its selected component |
| | | 109 | | // leaves t*|RayNormal[axis]|/(2D), including each independent conditioning shift. |
| | | 110 | | // LP coefficients <1005 bits; PlaneDisk elimination raises the conic's |
| | | 111 | | // <1065-bit coefficients to <2133-bit retained quadratics. The Mobius |
| | | 112 | | // conversion adds <4 bits, the conditioned component <268 and 2D <132. |
| | | 113 | | // Six extra limbs cover these products; quadratic discriminants and |
| | | 114 | | // the shared ratio owner's later products use complete adaptive widths. |
| | 28 | 115 | | int words = Math.Max(DepthCoefficientWords(), other.DepthCoefficientWords()) + 6; |
| | 56 | 116 | | Span<ulong> values = stackalloc ulong[8 * words]; Span<int> signs = stackalloc int[8]; |
| | 56 | 117 | | Span<ulong> firstRoot = stackalloc ulong[2 * words + 1], secondRoot = stackalloc ulong[2 * words + 1]; |
| | 56 | 118 | | ContactQuadratic firstN = ContactQuadratic.At(values, signs, 0, words), firstD = ContactQuadratic.At(values, sig |
| | 56 | 119 | | ContactQuadratic secondN = ContactQuadratic.At(values, signs, 2, words), secondD = ContactQuadratic.At(values, s |
| | 28 | 120 | | GetPhysicalDepthRatio(frame, axis, firstRoot, firstN, firstD); |
| | 28 | 121 | | other.GetPhysicalDepthRatio(otherFrame, axis, secondRoot, secondN, secondD); |
| | 28 | 122 | | return ContactQuadratic.CompareRatios(firstN, firstD, firstRoot, secondN, secondD, secondRoot); |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | private readonly int DepthCoefficientWords() |
| | | 126 | | { |
| | 64 | 127 | | ReadOnlySpan<ulong> values = _parameterIsDepth ? Polynomial.Values |
| | 64 | 128 | | : _values.Slice(11 * Words, 2 * Words); |
| | 64 | 129 | | int words = 1; |
| | 430 | 130 | | for (int index = 0; index < values.Length; index += Words) |
| | 151 | 131 | | words = Math.Max(words, WideArithmetic.GetActiveMagnitudeLength(values.Slice(index, Words))); |
| | 64 | 132 | | return words; |
| | | 133 | | } |
| | | 134 | | |
| | | 135 | | private readonly void GetPhysicalDepthRatio(in FiniteSlabRayFrame frame, int axis, |
| | | 136 | | scoped Span<ulong> radical, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator) |
| | | 137 | | { |
| | 64 | 138 | | int words = numerator.FieldWords; |
| | 128 | 139 | | Span<ulong> values = stackalloc ulong[6 * words]; Span<int> signs = stackalloc int[6]; |
| | 64 | 140 | | ContactQuadratic a = ContactQuadratic.At(values, signs, 0, words); |
| | 128 | 141 | | ContactQuadratic uN = ContactQuadratic.At(values, signs, 1, words), uD = ContactQuadratic.At(values, signs, 2, w |
| | 64 | 142 | | if (_parameterIsDepth) |
| | | 143 | | { |
| | 23 | 144 | | GetRootParameter(radical, uN, uD); |
| | | 145 | | } |
| | | 146 | | else |
| | | 147 | | { |
| | 41 | 148 | | radical.Clear(); |
| | 82 | 149 | | CopyDepthCoefficient(DepthN, 0, uN); CopyDepthCoefficient(DepthD, 0, uD); |
| | | 150 | | } |
| | 192 | 151 | | uN.CopyTo(numerator); numerator.Add(numerator); numerator.Add(uD, -1); |
| | 128 | 152 | | uD.CopyTo(denominator); denominator.Add(uN, -1); |
| | 64 | 153 | | Span<ulong> normal = stackalloc ulong[9]; |
| | 64 | 154 | | WideArithmetic.GetMagnitude(Component(frame.RayNormal, axis), normal); |
| | 128 | 155 | | ContactQuadratic.Scale(numerator, normal, 1, a); a.CopyTo(numerator); |
| | 64 | 156 | | Signed320 scale = WideArithmetic.AddSigned320(frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator, |
| | 64 | 157 | | frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator); |
| | 128 | 158 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(scale), a); a.CopyTo(denominator); |
| | 64 | 159 | | } |
| | | 160 | | |
| | | 161 | | private static void CopyDepthCoefficient(scoped FiniteSlabPolynomial polynomial, int index, scoped ContactQuadratic |
| | | 162 | | { |
| | 304 | 163 | | int Words = polynomial.FieldWords; |
| | 304 | 164 | | value.Clear(); |
| | 333 | 165 | | if (polynomial.Signs[index] == 0) return; |
| | 275 | 166 | | ReadOnlySpan<ulong> coefficient = polynomial.Values.Slice(index * Words, Words); |
| | 275 | 167 | | coefficient[..WideArithmetic.GetActiveMagnitudeLength(coefficient)].CopyTo(value.Rational); |
| | 275 | 168 | | value.Signs[0] = polynomial.Signs[index]; |
| | 275 | 169 | | } |
| | | 170 | | |
| | | 171 | | private static int Compare(bool firstIsDepth, scoped FiniteAxisValueRoot first, |
| | | 172 | | scoped FiniteSlabPolynomial firstN, scoped FiniteSlabPolynomial firstD, |
| | | 173 | | bool secondIsDepth, scoped FiniteAxisValueRoot second, |
| | | 174 | | scoped FiniteSlabPolynomial secondN, scoped FiniteSlabPolynomial secondD) |
| | | 175 | | { |
| | 2823 | 176 | | int Words = Math.Max(firstN.FieldWords, secondN.FieldWords); |
| | 2823 | 177 | | if (firstIsDepth && secondIsDepth) |
| | 258 | 178 | | return WideFiniteAxisIntersection.CompareFiniteValueRoots(first, second); |
| | 2565 | 179 | | Span<ulong> values = stackalloc ulong[3 * Words]; |
| | 2565 | 180 | | Span<sbyte> signs = stackalloc sbyte[3]; |
| | 2565 | 181 | | FiniteSlabPolynomial query = new(values[..(2 * Words)], signs[..2]); |
| | 2565 | 182 | | if (firstIsDepth || secondIsDepth) |
| | | 183 | | { |
| | 407 | 184 | | FiniteSlabPolynomial n = firstIsDepth ? secondN : firstN; |
| | 407 | 185 | | FiniteSlabPolynomial d = firstIsDepth ? secondD : firstD; |
| | 1221 | 186 | | query.Clear(); query.Add(n, -1); query.Add(d, 1, 1); |
| | 407 | 187 | | return firstIsDepth ? WideFiniteAxisIntersection.GetSignAtFiniteValueRoot(first, query.Values, query.Signs) |
| | 407 | 188 | | : -WideFiniteAxisIntersection.GetSignAtFiniteValueRoot(second, query.Values, query.Signs); |
| | | 189 | | } |
| | 4316 | 190 | | FiniteSlabPolynomial left = query.Slice(0, 1), right = query.Slice(1, 1); |
| | 2158 | 191 | | FiniteSlabPolynomial difference = new(values[(2 * Words)..], signs[2..]); |
| | 4316 | 192 | | left.Multiply(firstN, secondD); right.Multiply(secondN, firstD); |
| | 4316 | 193 | | difference.Copy(left); difference.Add(right, -1); |
| | 2158 | 194 | | return difference.Signs[0]; |
| | | 195 | | } |
| | | 196 | | |
| | | 197 | | internal void Keep(scoped FiniteAxisValueRoot root, int rootCount, |
| | | 198 | | scoped FiniteSlabPolynomial lambda, scoped FiniteSlabPolynomial mu, |
| | | 199 | | scoped FiniteSlabPolynomial t, scoped FiniteSlabPolynomial denominator, |
| | | 200 | | bool parameterIsDepth, scoped FiniteSlabPolynomial depthN, scoped FiniteSlabPolynomial depthD, |
| | | 201 | | FixedTriangle triangle, int orientation, int domainEdgeMask) |
| | | 202 | | { |
| | 3586 | 203 | | if (HasValue) |
| | | 204 | | { |
| | 2334 | 205 | | Span<ulong> cell = stackalloc ulong[parameterIsDepth || _parameterIsDepth ? CellWords : 0]; |
| | 2334 | 206 | | FiniteAxisValueRoot previous = parameterIsDepth || _parameterIsDepth ? Restore(cell) : default; |
| | | 207 | | // Equal extrema keep the first admitted witness in the explicitly |
| | | 208 | | // ordered active set and authored triangle traversal. |
| | 2334 | 209 | | if (Compare(parameterIsDepth, root, depthN, depthD, |
| | 4496 | 210 | | _parameterIsDepth, previous, DepthN, DepthD) <= 0) return; |
| | | 211 | | } |
| | 4272 | 212 | | Polynomial.Clear(); root.Coefficients.CopyTo(Polynomial.Values); root.Signs.CopyTo(Polynomial.Signs); |
| | 5696 | 213 | | Lambda.Copy(lambda); Mu.Copy(mu); T.Copy(t); D.Copy(denominator); |
| | 2848 | 214 | | DepthN.Copy(depthN); DepthD.Copy(depthD); |
| | 4272 | 215 | | _parameterIsDepth = parameterIsDepth; _ordinal = root.Ordinal; _rootCount = rootCount; |
| | 5696 | 216 | | _coefficientCount = root.Signs.Length; _triangle = triangle; _orientation = orientation; HasValue = true; |
| | 1424 | 217 | | _domainEdgeMask = domainEdgeMask; |
| | 1424 | 218 | | _hasCompactCell = root.DenominatorShift < RetainedCellWords * 64; |
| | 2848 | 219 | | _isRational = root.IsRational; _denominatorShift = root.DenominatorShift; |
| | 1424 | 220 | | RetainedCell.Clear(); |
| | 1424 | 221 | | if (_hasCompactCell) |
| | 1423 | 222 | | root.LowerNumerator[..WideArithmetic.GetActiveMagnitudeLength(root.LowerNumerator)].CopyTo(RetainedCell); |
| | 1424 | 223 | | } |
| | | 224 | | |
| | | 225 | | private readonly void BuildAuthoredCoordinate(int axis, FiniteSlabPolynomial numerator, FiniteSlabPolynomial term) |
| | | 226 | | { |
| | 2241 | 227 | | numerator.Scale(D, Signed320.ExtendValue(Signed192.Raw(_triangle.A[axis]))); |
| | 2241 | 228 | | term.Scale(Lambda, Signed320.ExtendValue(WideArithmetic.SubtractSigned192( |
| | 4482 | 229 | | Signed192.Raw(_triangle.B[axis]), Signed192.Raw(_triangle.A[axis])))); numerator.Add(term); |
| | 2241 | 230 | | term.Scale(Mu, Signed320.ExtendValue(WideArithmetic.SubtractSigned192( |
| | 4482 | 231 | | Signed192.Raw(_triangle.C[axis]), Signed192.Raw(_triangle.A[axis])))); numerator.Add(term); |
| | 2241 | 232 | | } |
| | | 233 | | |
| | | 234 | | /// <summary>Admits this exact source point against another member of the same physical slab chart.</summary> |
| | | 235 | | internal readonly bool ContainsTrianglePoint(in FiniteSlabRayFrame target, FixedTriangle triangle, |
| | | 236 | | in FiniteSlabRayFrame source) |
| | | 237 | | { |
| | 63 | 238 | | if (!HasValue || target.Plane.NormalSquared.IsZero) return false; |
| | 78 | 239 | | Span<ulong> cell = stackalloc ulong[CellWords]; FiniteAxisValueRoot root = Restore(cell); |
| | 39 | 240 | | Span<ulong> values = stackalloc ulong[12 * Words]; |
| | 39 | 241 | | Span<sbyte> signs = stackalloc sbyte[12]; |
| | 39 | 242 | | FiniteSlabPolynomial x = FiniteSlabPolynomial.At(values, signs, 0, 2), |
| | 78 | 243 | | y = FiniteSlabPolynomial.At(values, signs, 2, 2), z = FiniteSlabPolynomial.At(values, signs, 4, 2), |
| | 39 | 244 | | denominator = FiniteSlabPolynomial.At(values, signs, 6, 2), |
| | 78 | 245 | | query = FiniteSlabPolynomial.At(values, signs, 8, 2), term = FiniteSlabPolynomial.At(values, signs, 10, 2); |
| | 78 | 246 | | WideAxis3 a = source.Section.Transform(_triangle.A), e = Difference(source.Section.Transform(_triangle.B), a), |
| | 39 | 247 | | f = Difference(source.Section.Transform(_triangle.C), a); |
| | 39 | 248 | | Signed320 targetScale = target.Section.RadialFrame.ShapeFrame.CoordinateDenominator; |
| | 312 | 249 | | for (int axis = 0; axis < 3; axis++) |
| | | 250 | | { |
| | 117 | 251 | | FiniteSlabPolynomial coordinate = axis == 0 ? x : axis == 1 ? y : z; |
| | 351 | 252 | | query.Scale(D, Component(a, axis)); term.Scale(Lambda, Component(e, axis)); query.Add(term); |
| | 234 | 253 | | term.Scale(Mu, Component(f, axis)); query.Add(term); |
| | 117 | 254 | | coordinate.Scale(query, targetScale); |
| | | 255 | | } |
| | 39 | 256 | | denominator.Scale(D, source.Section.RadialFrame.ShapeFrame.CoordinateDenominator); |
| | 39 | 257 | | int denominatorSign = FiniteSlabPolynomial.Sign(ref root, denominator); |
| | | 258 | | // Source coordinates carry 2Ds and target walls expect 2Dt. Cross |
| | | 259 | | // scaling preserves the exact point; <3400-bit source products plus |
| | | 260 | | // <132-bit scale and <526-bit walls still fit the 4096-bit bank. |
| | 246 | 261 | | for (int edge = -1; edge < 3; edge++) |
| | | 262 | | { |
| | | 263 | | Signed832 wx, wy, wz, offset; |
| | 108 | 264 | | if (edge < 0) |
| | | 265 | | { |
| | 78 | 266 | | wx = Signed832.ExtendValue(target.Plane.Normal.X); wy = Signed832.ExtendValue(target.Plane.Normal.Y); |
| | 78 | 267 | | wz = Signed832.ExtendValue(target.Plane.Normal.Z); offset = target.Plane.PlaneConstant; |
| | | 268 | | } |
| | 69 | 269 | | else target.Plane.GetTriangleEdgeWall(triangle, edge, out wx, out wy, out wz, out offset); |
| | 540 | 270 | | query.Scale(x, wx); term.Scale(y, wy); query.Add(term); term.Scale(z, wz); query.Add(term); |
| | 216 | 271 | | term.Scale(denominator, offset); query.Add(term, -1); |
| | 108 | 272 | | int sign = FiniteSlabPolynomial.Sign(ref root, query) * denominatorSign; |
| | 132 | 273 | | if (edge < 0 ? sign != 0 : sign < 0) return false; |
| | | 274 | | } |
| | 15 | 275 | | return true; |
| | | 276 | | } |
| | | 277 | | |
| | | 278 | | /// <summary>Rounds the source once on the world lattice, returning its offset from the slab center.</summary> |
| | | 279 | | internal readonly bool TryGetSourcePointInSamplingFrame(in FiniteSlabRayFrame frame, out Vector3d point) |
| | | 280 | | { |
| | 58 | 281 | | point = default; |
| | 60 | 282 | | if (!HasValue) return false; |
| | 112 | 283 | | Span<ulong> cell = stackalloc ulong[CellWords]; FiniteAxisValueRoot root = Restore(cell); |
| | 56 | 284 | | Span<ulong> values = stackalloc ulong[12 * Words]; |
| | 56 | 285 | | Span<sbyte> signs = stackalloc sbyte[12]; |
| | 56 | 286 | | FiniteSlabPolynomial x = FiniteSlabPolynomial.At(values, signs, 0, 2), |
| | 112 | 287 | | y = FiniteSlabPolynomial.At(values, signs, 2, 2), z = FiniteSlabPolynomial.At(values, signs, 4, 2), |
| | 56 | 288 | | denominator = FiniteSlabPolynomial.At(values, signs, 6, 2), |
| | 112 | 289 | | numerator = FiniteSlabPolynomial.At(values, signs, 8, 2), term = FiniteSlabPolynomial.At(values, signs, 10, |
| | 56 | 290 | | bool exact = frame.Section.RadialFrame.ShapeFrame.HasExactScale; |
| | 56 | 291 | | WideRationalBasis3d basis = new(exact ? frame.SlabRotation : frame.Rotation); |
| | 56 | 292 | | if (exact) |
| | | 293 | | { |
| | 2 | 294 | | WideAxis3 a = frame.Section.Transform(_triangle.A), e = Difference(frame.Section.Transform(_triangle.B), a), |
| | 1 | 295 | | f = Difference(frame.Section.Transform(_triangle.C), a); |
| | 8 | 296 | | for (int axis = 0; axis < 3; axis++) |
| | | 297 | | { |
| | 3 | 298 | | FiniteSlabPolynomial coordinate = axis == 0 ? x : axis == 1 ? y : z; |
| | 9 | 299 | | coordinate.Scale(D, Component(a, axis)); term.Scale(Lambda, Component(e, axis)); coordinate.Add(term); |
| | 6 | 300 | | term.Scale(Mu, Component(f, axis)); coordinate.Add(term); |
| | | 301 | | } |
| | 1 | 302 | | Signed320 scale = frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator; |
| | 1 | 303 | | Signed576 divisor = WideArithmetic.MultiplySigned320(WideArithmetic.AddSigned320(scale, scale), basis.Denomi |
| | 1 | 304 | | denominator.Scale(D, divisor); |
| | | 305 | | } |
| | | 306 | | else |
| | | 307 | | { |
| | 165 | 308 | | BuildAuthoredCoordinate(0, x, term); BuildAuthoredCoordinate(1, y, term); BuildAuthoredCoordinate(2, z, term |
| | 55 | 309 | | denominator.Scale(D, Signed320.ExtendValue(basis.Denominator)); |
| | | 310 | | } |
| | | 311 | | // Basis coefficients add <130 bits and the origin difference <65; |
| | | 312 | | // subtraction precedes narrowing, including near scalar-range limits. |
| | 444 | 313 | | for (int axis = 0; axis < 3; axis++) |
| | | 314 | | { |
| | 168 | 315 | | numerator.Scale(x, Signed320.ExtendValue(axis == 0 ? basis.Xx : axis == 1 ? basis.Xy : basis.Xz)); |
| | 336 | 316 | | term.Scale(y, Signed320.ExtendValue(axis == 0 ? basis.Yx : axis == 1 ? basis.Yy : basis.Yz)); numerator.Add( |
| | 336 | 317 | | term.Scale(z, Signed320.ExtendValue(axis == 0 ? basis.Zx : axis == 1 ? basis.Zy : basis.Zz)); numerator.Add( |
| | 168 | 318 | | if (!exact) |
| | | 319 | | { |
| | 165 | 320 | | term.Scale(denominator, Signed320.ExtendValue(WideArithmetic.SubtractSigned192( |
| | 330 | 321 | | Signed192.Raw(frame.Origin[axis]), Signed192.Raw(frame.Center[axis])))); numerator.Add(term); |
| | | 322 | | } |
| | 170 | 323 | | if (!TryRoundRatio(ref root, numerator, denominator, out Fixed64 component, frame.Center[axis].m_rawValue)) |
| | 166 | 324 | | point[axis] = component; |
| | | 325 | | } |
| | 54 | 326 | | return true; |
| | | 327 | | } |
| | | 328 | | |
| | | 329 | | /// <summary> |
| | | 330 | | /// Retains the exit in bounded axial/radial components. A stadium endpoint |
| | | 331 | | /// can exceed the local scalar range even when its translated world anchor |
| | | 332 | | /// is representable; its exact half-core remainder stays in the anchor. |
| | | 333 | | /// </summary> |
| | | 334 | | internal readonly bool TryGetContactAnchor(in FiniteSlabRayFrame frame, out Fixed64 depth, out bool clamped, |
| | | 335 | | out Vector3d source, out FixedPointAnchor anchor) |
| | | 336 | | { |
| | 2780 | 337 | | depth = default; source = default; clamped = false; anchor = default; |
| | 696 | 338 | | if (!HasValue) return false; |
| | 1388 | 339 | | Span<ulong> cell = stackalloc ulong[CellWords]; FiniteAxisValueRoot root = Restore(cell); |
| | 694 | 340 | | Span<ulong> values = stackalloc ulong[25 * Words]; |
| | 694 | 341 | | Span<sbyte> signs = stackalloc sbyte[25]; |
| | 694 | 342 | | FiniteSlabPolynomial numerator = FiniteSlabPolynomial.At(values, signs, 0, 5); |
| | 694 | 343 | | FiniteSlabPolynomial term = FiniteSlabPolynomial.At(values, signs, 5, 5); |
| | 694 | 344 | | FiniteSlabPolynomial divisor = FiniteSlabPolynomial.At(values, signs, 10, 5); |
| | 694 | 345 | | FiniteSlabPolynomial square = FiniteSlabPolynomial.At(values, signs, 15, 9); |
| | 694 | 346 | | FiniteSlabPolynomial scalar = FiniteSlabPolynomial.At(values, signs, 24, 1); |
| | 694 | 347 | | Signed320 scale = WideArithmetic.AddSigned320(frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator, |
| | 694 | 348 | | frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator); |
| | 694 | 349 | | divisor.Scale(D, scale); |
| | 694 | 350 | | WideAxis3 a = frame.Section.Transform(_triangle.A); |
| | 694 | 351 | | WideAxis3 e = Difference(frame.Section.Transform(_triangle.B), a); |
| | 694 | 352 | | WideAxis3 f = Difference(frame.Section.Transform(_triangle.C), a); |
| | 1388 | 353 | | Vector3d axial = default, exit = default; |
| | 694 | 354 | | FixedPointAnchorTerm3d exactTerm = default; |
| | 694 | 355 | | int fullHeightCap = 0; |
| | 5534 | 356 | | for (int axis = 0; axis < 3; axis++) |
| | | 357 | | { |
| | 2076 | 358 | | BuildAuthoredCoordinate(axis, numerator, term); |
| | 2079 | 359 | | if (!TryRoundRatio(ref root, numerator, D, out Fixed64 component)) return false; |
| | 2073 | 360 | | source[axis] = component; |
| | 6219 | 361 | | numerator.Scale(D, Component(a, axis)); term.Scale(Lambda, Component(e, axis)); numerator.Add(term); |
| | 4146 | 362 | | term.Scale(Mu, Component(f, axis)); numerator.Add(term); |
| | 4146 | 363 | | term.Scale(T, Signed(Component(frame.RayNormal, axis), _orientation)); numerator.Add(term); |
| | 2073 | 364 | | if (axis == 1 && (frame.FullHeight.m_rawValue & 1L) != 0) |
| | | 365 | | { |
| | 5 | 366 | | term.Scale(D, frame.Section.Cap); |
| | 5 | 367 | | FiniteSlabPolynomial capQuery = square.Slice(0, 5); |
| | 10 | 368 | | capQuery.Copy(numerator); capQuery.Add(term, -1); |
| | 7 | 369 | | if (FiniteSlabPolynomial.Sign(ref root, capQuery) == 0) fullHeightCap = 1; |
| | | 370 | | else |
| | | 371 | | { |
| | 6 | 372 | | capQuery.Copy(numerator); capQuery.Add(term); |
| | 5 | 373 | | if (FiniteSlabPolynomial.Sign(ref root, capQuery) == 0) fullHeightCap = -1; |
| | | 374 | | } |
| | | 375 | | } |
| | | 376 | | bool bounded; |
| | 2073 | 377 | | if (frame.HasCore && axis == 2) |
| | | 378 | | { |
| | 320 | 379 | | FiniteSlabPolynomial endpointQuery = square.Slice(0, 5); |
| | 960 | 380 | | term.Scale(D, frame.CoreCap); endpointQuery.Copy(numerator); endpointQuery.Add(term, -1); |
| | 320 | 381 | | int denominatorSign = FiniteSlabPolynomial.Sign(ref root, D); |
| | 320 | 382 | | int endpoint = FiniteSlabPolynomial.Sign(ref root, endpointQuery) * denominatorSign >= 0 ? 1 : 0; |
| | 320 | 383 | | if (endpoint == 0) |
| | | 384 | | { |
| | 276 | 385 | | endpointQuery.Copy(numerator); endpointQuery.Add(term); |
| | 256 | 386 | | if (FiniteSlabPolynomial.Sign(ref root, endpointQuery) * denominatorSign <= 0) endpoint = -1; |
| | | 387 | | } |
| | 320 | 388 | | if (endpoint == 0) |
| | | 389 | | { |
| | 20 | 390 | | bounded = TryRoundRatio(ref root, numerator, divisor, out component); |
| | | 391 | | System.Diagnostics.Debug.Assert(bounded); |
| | 20 | 392 | | axial.Z = component; |
| | 20 | 393 | | continue; |
| | | 394 | | } |
| | 300 | 395 | | numerator.Add(term, -endpoint); |
| | 300 | 396 | | axial = TriangleCapsuleSlabEndpointWitnesses.GetCore(Vector2d.Forward, |
| | 300 | 397 | | frame.Section.RadialFrame.Length, -endpoint, out exactTerm); |
| | 300 | 398 | | bounded = TryRoundRatio(ref root, numerator, divisor, out component); |
| | | 399 | | System.Diagnostics.Debug.Assert(bounded); |
| | 300 | 400 | | term.Scale(divisor, Signed320.ExtendValue(Signed192.Raw(component))); |
| | 600 | 401 | | endpointQuery.Copy(numerator); endpointQuery.Add(term, -1); |
| | 300 | 402 | | if (FiniteSlabPolynomial.Sign(ref root, endpointQuery) == 0) |
| | | 403 | | { |
| | | 404 | | // An integral radial term composes exactly with the odd |
| | | 405 | | // half-core remainder retained by the existing anchor. |
| | 279 | 406 | | exit.Z = component; |
| | 279 | 407 | | continue; |
| | | 408 | | } |
| | | 409 | | // Otherwise round the endpoint sum once. Retaining an odd |
| | | 410 | | // half-core after rounding its radial fraction would round a |
| | | 411 | | // second time; parity belongs to the complete integer sum. |
| | 21 | 412 | | exactTerm = default; |
| | 42 | 413 | | term.Scale(D, Signed(frame.CoreCap, endpoint)); numerator.Add(term); |
| | 42 | 414 | | term.Scale(divisor, Signed320.ExtendValue(Signed192.Raw(axial.Z))); numerator.Add(term, -1); |
| | 21 | 415 | | bounded = TryRoundRatio(ref root, numerator, divisor, out component, axial.Z.m_rawValue); |
| | | 416 | | System.Diagnostics.Debug.Assert(bounded); |
| | 21 | 417 | | exit.Z = component; |
| | 21 | 418 | | continue; |
| | | 419 | | } |
| | | 420 | | // Exact shape admission bounds local cap/radial/prism components; |
| | | 421 | | // the authored source above can still exceed its scalar range. |
| | 1753 | 422 | | bounded = TryRoundRatio(ref root, numerator, divisor, out component); |
| | | 423 | | System.Diagnostics.Debug.Assert(bounded); |
| | 1753 | 424 | | exit[axis] = component; |
| | | 425 | | } |
| | 691 | 426 | | if (fullHeightCap != 0) |
| | | 427 | | // The cap's half-raw residual composes with any retained odd core; |
| | | 428 | | // applying it after the Z loop prevents a radial rounding reset. |
| | 4 | 429 | | exactTerm = new(exactTerm.X, FixedPointAnchorTerm3d.GetResidual(Fixed64.One, |
| | 4 | 430 | | fullHeightCap < 0 ? -frame.FullHeight : frame.FullHeight, Fixed64.Zero, Fixed64.Zero, |
| | 4 | 431 | | axial.Y, exit.Y), exactTerm.Z); |
| | 691 | 432 | | anchor = new FixedPointAnchor(frame.Center, frame.SlabRotation, axial, exit, exactTerm); |
| | 691 | 433 | | GetRoundedDepth(frame, ref root, divisor, square, scalar, out depth, out clamped); |
| | 691 | 434 | | return true; |
| | | 435 | | } |
| | | 436 | | |
| | | 437 | | internal readonly void GetDepth(in FiniteSlabRayFrame frame, out Fixed64 depth, out bool clamped) |
| | | 438 | | { |
| | | 439 | | System.Diagnostics.Debug.Assert(HasValue); |
| | 34 | 440 | | Span<ulong> cell = stackalloc ulong[CellWords]; FiniteAxisValueRoot root = Restore(cell); |
| | 34 | 441 | | Span<ulong> values = stackalloc ulong[15 * Words]; Span<sbyte> signs = stackalloc sbyte[15]; |
| | 17 | 442 | | var divisor = FiniteSlabPolynomial.At(values, signs, 0, 5); |
| | 17 | 443 | | var square = FiniteSlabPolynomial.At(values, signs, 5, 9); |
| | 17 | 444 | | var scalar = FiniteSlabPolynomial.At(values, signs, 14, 1); |
| | 17 | 445 | | Signed320 coordinateD = frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator; |
| | 17 | 446 | | divisor.Scale(D, WideArithmetic.AddSigned320(coordinateD, coordinateD)); |
| | 17 | 447 | | GetRoundedDepth(frame, ref root, divisor, square, scalar, out depth, out clamped); |
| | 17 | 448 | | } |
| | | 449 | | |
| | | 450 | | private readonly void GetRoundedDepth(in FiniteSlabRayFrame frame, scoped ref FiniteAxisValueRoot root, |
| | | 451 | | scoped FiniteSlabPolynomial divisor, scoped FiniteSlabPolynomial square, scoped FiniteSlabPolynomial scalar, |
| | | 452 | | out Fixed64 depth, out bool clamped) |
| | | 453 | | { |
| | 708 | 454 | | Signed320 coordinateD = frame.Section.RadialFrame.ShapeFrame.CoordinateDenominator; |
| | 708 | 455 | | Signed320 scale = WideArithmetic.AddSigned320(coordinateD, coordinateD); |
| | 708 | 456 | | if (IsConstant(T) && IsConstant(D)) |
| | | 457 | | { |
| | 516 | 458 | | Span<ulong> rationalValues = stackalloc ulong[4 * Words]; |
| | 516 | 459 | | Span<int> rationalSigns = stackalloc int[4]; |
| | 516 | 460 | | ContactQuadratic numeratorValue = ContactQuadratic.At(rationalValues, rationalSigns, 0, Words); |
| | 516 | 461 | | ContactQuadratic denominatorValue = ContactQuadratic.At(rationalValues, rationalSigns, 1, Words); |
| | 1032 | 462 | | numeratorValue.Clear(); denominatorValue.Clear(); |
| | 516 | 463 | | T.Values[..Words].CopyTo(numeratorValue.Rational); |
| | 516 | 464 | | D.Values[..Words].CopyTo(denominatorValue.Rational); |
| | 1032 | 465 | | numeratorValue.Signs[0] = T.Signs[0] * D.Signs[0]; denominatorValue.Signs[0] = 1; |
| | 1032 | 466 | | Span<ulong> metricValue = stackalloc ulong[13]; WideArithmetic.GetMagnitude(frame.RayNormalSquared, metricVa |
| | 516 | 467 | | Span<ulong> scaleValue = stackalloc ulong[5]; |
| | 516 | 468 | | WideArithmetic.GetMagnitude(scale, out scaleValue[4], out scaleValue[3], out scaleValue[2], out scaleValue[1 |
| | 516 | 469 | | clamped = !ContactQuadratic.TryRoundRootRatio(numeratorValue, denominatorValue, ReadOnlySpan<ulong>.Empty, |
| | 516 | 470 | | metricValue, scaleValue, out depth); |
| | 518 | 471 | | if (clamped) depth = Fixed64.MaxValue; |
| | 516 | 472 | | return; |
| | | 473 | | } |
| | | 474 | | // Depth uses the same conditioned canonical normal as candidate t; |
| | | 475 | | // numerator²*|N|² / ((2D)²*denominator²) stays exact until this point. |
| | 192 | 476 | | square.Multiply(T, T); |
| | 192 | 477 | | Span<ulong> depthValues = stackalloc ulong[27 * Words]; |
| | 192 | 478 | | Span<sbyte> depthSigns = stackalloc sbyte[27]; |
| | 192 | 479 | | FiniteSlabPolynomial metric = FiniteSlabPolynomial.At(depthValues, depthSigns, 0, 9); |
| | 192 | 480 | | FiniteSlabPolynomial squaredDivisor = FiniteSlabPolynomial.At(depthValues, depthSigns, 9, 9); |
| | 192 | 481 | | FiniteSlabPolynomial query = FiniteSlabPolynomial.At(depthValues, depthSigns, 18, 9); |
| | 384 | 482 | | metric.Scale(square, frame.RayNormalSquared); squaredDivisor.Multiply(divisor, divisor); |
| | 384 | 483 | | ulong lower = 0, upper = 1UL << 63; |
| | 192 | 484 | | if (TryGetRatioFloorBounds(ref root, metric, squaredDivisor, 1, 1, |
| | 192 | 485 | | 1UL << 63, true, out ulong minimumDepth, out ulong maximumDepth)) |
| | | 486 | | { |
| | | 487 | | // This search finds the first integer strictly greater than the |
| | | 488 | | // exact depth, rather than the floor used by coordinate rounding. |
| | 190 | 489 | | lower = minimumDepth == 1UL << 63 ? minimumDepth : minimumDepth + 1; |
| | 190 | 490 | | upper = maximumDepth == 1UL << 63 ? maximumDepth : maximumDepth + 1; |
| | | 491 | | } |
| | 711 | 492 | | while (lower < upper) |
| | | 493 | | { |
| | 519 | 494 | | ulong mid = lower + ((upper - lower) >> 1); |
| | 772 | 495 | | if (CompareDepth(ref root, metric, squaredDivisor, query, scalar, mid << 1) >= 0) lower = mid + 1; |
| | 266 | 496 | | else upper = mid; |
| | | 497 | | } |
| | | 498 | | System.Diagnostics.Debug.Assert(lower > 0); |
| | 192 | 499 | | ulong floor = lower - 1; |
| | 192 | 500 | | int comparison = CompareDepth(ref root, metric, squaredDivisor, query, scalar, (floor << 1) | 1); |
| | 192 | 501 | | ulong rounded = floor + (comparison > 0 || comparison == 0 && (floor & 1) != 0 ? 1UL : 0); |
| | 192 | 502 | | clamped = rounded > long.MaxValue; |
| | 192 | 503 | | depth = clamped ? Fixed64.MaxValue : Fixed64.FromRaw((long)rounded); |
| | 192 | 504 | | } |
| | | 505 | | |
| | | 506 | | private static int CompareDepth(scoped ref FiniteAxisValueRoot root, |
| | | 507 | | scoped FiniteSlabPolynomial metric, scoped FiniteSlabPolynomial divisor, scoped FiniteSlabPolynomial query, |
| | | 508 | | scoped FiniteSlabPolynomial scalar, ulong twiceRaw) |
| | | 509 | | { |
| | 711 | 510 | | int Words = metric.FieldWords; |
| | 1422 | 511 | | Span<ulong> factor = stackalloc ulong[Words]; factor.Clear(); |
| | 711 | 512 | | Fixed64.Multiply64To128(twiceRaw, twiceRaw, out factor[1], out factor[0]); |
| | 2133 | 513 | | scalar.Clear(); factor.CopyTo(scalar.Values); scalar.Signs[0] = twiceRaw == 0 ? (sbyte)0 : (sbyte)1; |
| | 711 | 514 | | query.Multiply(divisor, scalar); |
| | 711 | 515 | | query.Add(metric, -4); |
| | 711 | 516 | | return -FiniteSlabPolynomial.Sign(ref root, query); |
| | | 517 | | } |
| | | 518 | | |
| | | 519 | | private static bool TryRoundRatio(scoped ref FiniteAxisValueRoot root, |
| | | 520 | | scoped FiniteSlabPolynomial numerator, scoped FiniteSlabPolynomial denominator, out Fixed64 value, |
| | | 521 | | long parityOffset = 0) |
| | | 522 | | { |
| | 4338 | 523 | | int Words = numerator.FieldWords; |
| | 4338 | 524 | | value = default; |
| | 4338 | 525 | | if (IsConstant(numerator) && IsConstant(denominator)) |
| | | 526 | | { |
| | | 527 | | System.Diagnostics.Debug.Assert(denominator.Signs[0] != 0); |
| | 3235 | 528 | | return Fixed64.TryGetSignedRawRatio(numerator.Values[..Words], |
| | 3235 | 529 | | denominator.Values[..Words], |
| | 3235 | 530 | | numerator.Signs[0] * denominator.Signs[0] < 0, out value, parityOffset); |
| | | 531 | | } |
| | 1103 | 532 | | Span<ulong> values = stackalloc ulong[11 * Words]; |
| | 1103 | 533 | | Span<sbyte> signs = stackalloc sbyte[11]; |
| | 1103 | 534 | | FiniteSlabPolynomial query = FiniteSlabPolynomial.At(values, signs, 0, 5); |
| | 1103 | 535 | | FiniteSlabPolynomial term = FiniteSlabPolynomial.At(values, signs, 5, 5); |
| | 1103 | 536 | | FiniteSlabPolynomial scalar = FiniteSlabPolynomial.At(values, signs, 10, 1); |
| | 2206 | 537 | | int sign = FiniteSlabPolynomial.Sign(ref root, numerator), denominatorSign = FiniteSlabPolynomial.Sign(ref root, |
| | | 538 | | System.Diagnostics.Debug.Assert(denominatorSign != 0); |
| | 1593 | 539 | | if (sign == 0) return true; |
| | 613 | 540 | | sign *= denominatorSign; |
| | 613 | 541 | | ulong limit = sign < 0 ? 1UL << 63 : long.MaxValue; |
| | 1226 | 542 | | ulong lower = 0, upper = limit; |
| | 613 | 543 | | if (TryGetRatioFloorBounds(ref root, numerator, denominator, sign * denominatorSign, |
| | 613 | 544 | | denominatorSign, limit, false, out ulong minimum, out ulong maximum)) |
| | 1202 | 545 | | { lower = minimum; upper = maximum; } |
| | 2851 | 546 | | while (lower < upper) |
| | | 547 | | { |
| | 2238 | 548 | | ulong mid = lower + ((upper - lower) >> 1) + 1; |
| | 6714 | 549 | | scalar.Clear(); scalar.Values[0] = mid; scalar.Signs[0] = (sbyte)denominatorSign; |
| | 8952 | 550 | | term.Multiply(denominator, scalar); query.Clear(); query.Add(numerator, sign * denominatorSign); query.Add(t |
| | 4476 | 551 | | if (FiniteSlabPolynomial.Sign(ref root, query) >= 0) lower = mid; else upper = mid - 1; |
| | | 552 | | } |
| | 1839 | 553 | | scalar.Clear(); scalar.Values[0] = lower; scalar.Signs[0] = (sbyte)denominatorSign; |
| | 2452 | 554 | | term.Multiply(denominator, scalar); query.Clear(); query.Add(numerator, sign * denominatorSign * 2); query.Add(t |
| | 613 | 555 | | query.Add(denominator, -denominatorSign); |
| | 613 | 556 | | int halfway = FiniteSlabPolynomial.Sign(ref root, query); |
| | 613 | 557 | | ulong rounded = lower + (halfway > 0 || halfway == 0 |
| | 613 | 558 | | && ((lower ^ unchecked((ulong)parityOffset)) & 1) != 0 ? 1UL : 0); |
| | 615 | 559 | | if (rounded > limit) return false; |
| | 1222 | 560 | | value = Fixed64.FromRaw(sign < 0 ? unchecked(-(long)rounded) : (long)rounded); return true; |
| | | 561 | | } |
| | | 562 | | |
| | | 563 | | /// <summary> |
| | | 564 | | /// Bounds the final nonnegative ratio using the existing certified Horner |
| | | 565 | | /// intervals. These bounds only narrow the exact nearest-even search; |
| | | 566 | | /// cancellation or a vanishing denominator retains its full-range path. |
| | | 567 | | /// </summary> |
| | | 568 | | private static bool TryGetRatioFloorBounds(scoped ref FiniteAxisValueRoot root, |
| | | 569 | | scoped FiniteSlabPolynomial numerator, scoped FiniteSlabPolynomial denominator, |
| | | 570 | | int numeratorSign, int denominatorSign, ulong limit, bool squareRoot, |
| | | 571 | | out ulong minimum, out ulong maximum) |
| | | 572 | | { |
| | 805 | 573 | | int Words = numerator.FieldWords; |
| | 1610 | 574 | | minimum = 0; maximum = limit; |
| | | 575 | | const int precision = 59; |
| | | 576 | | // At most nine coefficients: 59 + 2*ceil(log2(9)) <= 67. |
| | 805 | 577 | | WideFiniteAxisIntersection.RefineFiniteValueRoot(ref root, 70); |
| | 805 | 578 | | Span<sbyte> signs = stackalloc sbyte[numerator.Count + denominator.Count]; |
| | 1610 | 579 | | int numeratorBits = 0, denominatorBits = 0; |
| | 11178 | 580 | | for (int index = 0; index < numerator.Count; index++) |
| | | 581 | | { |
| | 4784 | 582 | | signs[index] = (sbyte)(numerator.Signs[index] * numeratorSign); |
| | 4784 | 583 | | numeratorBits = Math.Max(numeratorBits, WideArithmetic.GetMagnitudeBitLength( |
| | 4784 | 584 | | numerator.Values.Slice(index * Words, Words))); |
| | | 585 | | } |
| | 9708 | 586 | | for (int index = 0; index < denominator.Count; index++) |
| | | 587 | | { |
| | 4049 | 588 | | signs[numerator.Count + index] = (sbyte)(denominator.Signs[index] * denominatorSign); |
| | 4049 | 589 | | denominatorBits = Math.Max(denominatorBits, WideArithmetic.GetMagnitudeBitLength( |
| | 4049 | 590 | | denominator.Values.Slice(index * Words, Words))); |
| | | 591 | | } |
| | 805 | 592 | | WideFiniteAxisIntersection.GetFiniteValueRootNonnegativeBounds(root, numerator.Values, |
| | 805 | 593 | | signs[..numerator.Count], precision, numeratorBits, out ulong nMin, out ulong nMax); |
| | 805 | 594 | | WideFiniteAxisIntersection.GetFiniteValueRootNonnegativeBounds(root, denominator.Values, |
| | 805 | 595 | | signs[numerator.Count..], precision, denominatorBits, out ulong dMin, out ulong dMax); |
| | 819 | 596 | | if (dMin == 0) return false; |
| | 791 | 597 | | int shift = numeratorBits - denominatorBits; |
| | 791 | 598 | | int words = (Math.Abs(shift) + 127) / 64; |
| | 791 | 599 | | Span<ulong> bank = stackalloc ulong[2 * words]; |
| | 1582 | 600 | | Span<ulong> n = bank[..words], d = bank[words..]; |
| | 791 | 601 | | Span<ulong> endpoints = stackalloc ulong[4] { nMin, nMax, dMin, dMax }; |
| | 4746 | 602 | | for (int bound = 0; bound < 2; bound++) |
| | | 603 | | { |
| | 6328 | 604 | | n.Clear(); d.Clear(); int ns = 0, ds = 0; |
| | 1582 | 605 | | WideArithmetic.AddShiftedSignedMagnitude(endpoints.Slice(bound, 1), 1, Math.Max(0, shift), n, ref ns); |
| | 1582 | 606 | | WideArithmetic.AddShiftedSignedMagnitude(endpoints.Slice(3 - bound, 1), 1, Math.Max(0, -shift), d, ref ds); |
| | | 607 | | ulong value; |
| | 1962 | 608 | | if (squareRoot) value = WideArithmetic.GetRatioFloorSquareRoot(n, d, limit); |
| | | 609 | | else |
| | | 610 | | { |
| | | 611 | | // Negative raw ratios admit the full unsigned 2^63 magnitude. |
| | | 612 | | // Expand nearest-even endpoint rounding by one integer so it |
| | | 613 | | // remains a conservative bound on the exact ratio's floor. |
| | 1202 | 614 | | value = Fixed64.TryGetSignedRawRatio(n, d, true, out Fixed64 rounded) |
| | 1202 | 615 | | ? unchecked((ulong)(-rounded.m_rawValue)) : limit; |
| | 1202 | 616 | | value = Math.Min(value, limit); |
| | 1803 | 617 | | if (bound == 0) value = value == 0 ? 0 : value - 1; |
| | 1192 | 618 | | else if (value < limit) value++; |
| | | 619 | | } |
| | 3164 | 620 | | if (bound == 0) minimum = value; else maximum = value; |
| | | 621 | | } |
| | 791 | 622 | | return true; |
| | | 623 | | } |
| | | 624 | | |
| | | 625 | | private static bool IsConstant(FiniteSlabPolynomial value) |
| | | 626 | | { |
| | 9239 | 627 | | int Words = value.FieldWords; |
| | 90188 | 628 | | for (int index = 1; index < value.Count; index++) if (value.Signs[index] != 0) return false; |
| | 7944 | 629 | | return true; |
| | | 630 | | } |
| | | 631 | | |
| | 6681 | 632 | | private static Signed320 Component(WideAxis3 p, int axis) => axis == 0 ? p.X : axis == 1 ? p.Y : p.Z; |
| | 2188 | 633 | | private static Signed576 Component(WideDirection3d p, int axis) => axis == 0 ? p.X : axis == 1 ? p.Y : p.Z; |
| | 2073 | 634 | | private static Signed576 Signed(Signed576 value, int sign) => sign < 0 ? WideArithmetic.SubtractSigned576(default, v |
| | 21 | 635 | | private static Signed320 Signed(Signed320 value, int sign) => sign < 0 ? WideArithmetic.Negate(value) : value; |
| | 1468 | 636 | | private static WideAxis3 Difference(WideAxis3 p, WideAxis3 q) => new( |
| | 1468 | 637 | | WideArithmetic.SubtractSigned320(p.X, q.X), WideArithmetic.SubtractSigned320(p.Y, q.Y), |
| | 1468 | 638 | | WideArithmetic.SubtractSigned320(p.Z, q.Z)); |
| | | 639 | | } |