| | | 1 | | //======================================================================= |
| | | 2 | | // RationalSegmentInterval.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 | | /// <summary> |
| | | 13 | | /// An exact closed rational subinterval of a segment's unit parameter. The |
| | | 14 | | /// default value is the complete segment; borrowed calls avoid enlarging events. |
| | | 15 | | /// </summary> |
| | | 16 | | internal readonly struct RationalSegmentInterval |
| | | 17 | | { |
| | | 18 | | internal readonly Signed832 WideLowerNumerator, WideLowerDenominator, WideUpperNumerator, WideUpperDenominator; |
| | | 19 | | internal bool HasWideBounds { get; } |
| | 914 | 20 | | internal Signed320 LowerNumerator => GetLegacyBound(WideLowerNumerator); |
| | 862 | 21 | | internal Signed320 LowerDenominator => GetLegacyBound(WideLowerDenominator); |
| | 739 | 22 | | internal Signed320 UpperNumerator => GetLegacyBound(WideUpperNumerator); |
| | 743 | 23 | | internal Signed320 UpperDenominator => GetLegacyBound(WideUpperDenominator); |
| | 34617 | 24 | | internal bool IsClipped => !WideLowerDenominator.IsZero; |
| | | 25 | | |
| | | 26 | | private Signed320 GetLegacyBound(Signed832 value) |
| | | 27 | | { |
| | 3262 | 28 | | if (HasWideBounds) throw new OverflowException("This interval requires its exact wide bounds."); |
| | 3254 | 29 | | return new Signed320(value.Word4, value.Word3, value.Word2, value.Word1, value.Word0); |
| | | 30 | | } |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// Materializes one endpoint on the original 2D edge, rounding each local |
| | | 34 | | /// coordinate once to nearest-even. The retained interval, not this final |
| | | 35 | | /// point, remains authoritative for incidence and admission. |
| | | 36 | | /// </summary> |
| | | 37 | | internal Vector2d GetEndpoint(FixedSegment2d edge, bool upper) |
| | | 38 | | { |
| | 117 | 39 | | if (!IsClipped) return upper ? edge.End : edge.Start; |
| | 97 | 40 | | if (HasWideBounds) |
| | | 41 | | { |
| | 26 | 42 | | Signed832 wideN = upper ? WideUpperNumerator : WideLowerNumerator; |
| | 26 | 43 | | Signed832 wideD = upper ? WideUpperDenominator : WideLowerDenominator; |
| | 26 | 44 | | return new Vector2d(GetWideCoordinate(edge.Start.X, edge.End.X, wideN, wideD), |
| | 26 | 45 | | GetWideCoordinate(edge.Start.Y, edge.End.Y, wideN, wideD)); |
| | | 46 | | } |
| | 71 | 47 | | Signed320 n = upper ? UpperNumerator : LowerNumerator; |
| | 71 | 48 | | Signed320 d = upper ? UpperDenominator : LowerDenominator; |
| | | 49 | | // Full raw endpoints and a unit Signed320 ratio need fewer than 386 |
| | | 50 | | // coefficient bits; existing Signed576 interpolation preserves them. |
| | 71 | 51 | | return new Vector2d( |
| | 71 | 52 | | WideConvex2dRelations.GetProjectedOffsetCoordinate(edge.Start.X, |
| | 71 | 53 | | WideArithmetic.Difference(edge.End.X, edge.Start.X), n, d), |
| | 71 | 54 | | WideConvex2dRelations.GetProjectedOffsetCoordinate(edge.Start.Y, |
| | 71 | 55 | | WideArithmetic.Difference(edge.End.Y, edge.Start.Y), n, d)); |
| | | 56 | | } |
| | | 57 | | |
| | | 58 | | /// <summary> |
| | | 59 | | /// Maps this source interval onto a collinear target segment in the same |
| | | 60 | | /// local frame, preserving closed endpoints without coordinate rounding. |
| | | 61 | | /// Degenerate, off-line and disjoint segments return false. The endpoint |
| | | 62 | | /// half-plane coefficients retain their full transient products. Reduced |
| | | 63 | | /// retained bounds must fit Signed832; unsupported widths throw rather than truncate. |
| | | 64 | | /// </summary> |
| | | 65 | | internal bool TryMapToSegment(FixedSegment source, FixedSegment target, out RationalSegmentInterval mapped) |
| | | 66 | | { |
| | 62 | 67 | | mapped = default; |
| | 64 | 68 | | if (source.Start == source.End || target.Start == target.End) return false; |
| | 180 | 69 | | Span<Signed192> direction = stackalloc Signed192[3], first = stackalloc Signed192[3], second = stackalloc Signed |
| | 60 | 70 | | int axis = 0; |
| | 480 | 71 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 72 | | { |
| | 180 | 73 | | direction[coordinate] = WideArithmetic.Difference(source.End[coordinate], source.Start[coordinate]); |
| | 180 | 74 | | first[coordinate] = WideArithmetic.Difference(target.Start[coordinate], source.Start[coordinate]); |
| | 180 | 75 | | second[coordinate] = WideArithmetic.Difference(target.End[coordinate], source.Start[coordinate]); |
| | 240 | 76 | | if (!direction[coordinate].IsZero) axis = coordinate; |
| | | 77 | | } |
| | 468 | 78 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | 177 | 79 | | if (!WideArithmetic.MultiplySubtract(direction[axis], first[coordinate], direction[coordinate], first[axis]) |
| | 177 | 80 | | || !WideArithmetic.MultiplySubtract(direction[axis], second[coordinate], direction[coordinate], second[a |
| | 3 | 81 | | return false; |
| | | 82 | | |
| | 86 | 83 | | if (HasWideBounds) return TryMapWide(direction[axis], first[axis], second[axis], out mapped); |
| | | 84 | | |
| | 112 | 85 | | Signed320 lowerN = default, lowerD = Signed320.One, upperN = Signed320.One, upperD = Signed320.One; |
| | 140 | 86 | | for (int bound = 0; bound < 2; bound++) |
| | | 87 | | { |
| | 50 | 88 | | Signed320 n = IsClipped ? (bound == 0 ? LowerNumerator : UpperNumerator) : bound == 0 ? default : Signed320. |
| | 50 | 89 | | Signed320 d = IsClipped ? (bound == 0 ? LowerDenominator : UpperDenominator) : Signed320.One; |
| | | 90 | | // A full raw difference and a Signed320 ratio need <386 bits. |
| | | 91 | | // Compare in Signed576 before enforcing the clipping bank's width. |
| | 50 | 92 | | Signed576 offset = WideArithmetic.MultiplySigned320(n, direction[axis]); |
| | 50 | 93 | | Signed576 a = WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(d, first[axis]), offset); |
| | 50 | 94 | | Signed576 b = WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(d, second[axis]), offset); |
| | 50 | 95 | | if ((direction[axis].Sign < 0) != (bound != 0)) |
| | 44 | 96 | | { a = WideArithmetic.SubtractSigned576(default, a); b = WideArithmetic.SubtractSigned576(default, b); } |
| | 50 | 97 | | Signed576 difference = WideArithmetic.SubtractSigned576(a, b); |
| | 78 | 98 | | if (difference.Sign < 0) difference = WideArithmetic.SubtractSigned576(default, difference); |
| | 50 | 99 | | if (!Signed320.TryNarrowSigned(a, out Signed320 start) || !Signed320.TryNarrowSigned(b, out Signed320 end) |
| | 50 | 100 | | || !Signed320.TryNarrowSigned(difference, out _)) |
| | 4 | 101 | | return TryMapWide(direction[axis], first[axis], second[axis], out mapped); |
| | 50 | 102 | | if (!TryClipHalfPlane(start, end, ref lowerN, ref lowerD, ref upperN, ref upperD)) return false; |
| | | 103 | | } |
| | 20 | 104 | | mapped = new RationalSegmentInterval(lowerN, lowerD, upperN, upperD); |
| | 20 | 105 | | return true; |
| | | 106 | | } |
| | | 107 | | |
| | | 108 | | private bool TryMapWide(Signed192 direction, Signed192 first, Signed192 second, out RationalSegmentInterval mapped) |
| | | 109 | | { |
| | 33 | 110 | | mapped = default; |
| | 132 | 111 | | Signed832 lowerN = default, lowerD = Signed832.ExtendValue(Signed192.One), upperN = lowerD, upperD = lowerD; |
| | | 112 | | const int words = 15; |
| | 66 | 113 | | Span<ulong> values = stackalloc ulong[14 * words]; Span<int> signs = stackalloc int[14]; |
| | 33 | 114 | | var parameter = ContactQuadratic.At(values, signs, 0, words); |
| | 33 | 115 | | var denominator = ContactQuadratic.At(values, signs, 1, words); |
| | 33 | 116 | | var offset = ContactQuadratic.At(values, signs, 2, words); |
| | 33 | 117 | | var a = ContactQuadratic.At(values, signs, 3, words); |
| | 33 | 118 | | var b = ContactQuadratic.At(values, signs, 4, words); |
| | 33 | 119 | | var rootN = ContactQuadratic.At(values, signs, 5, words); |
| | 33 | 120 | | var rootD = ContactQuadratic.At(values, signs, 6, words); |
| | 180 | 121 | | for (int bound = 0; bound < 2; bound++) |
| | | 122 | | { |
| | 63 | 123 | | parameter.Set(bound == 0 ? WideLowerNumerator : WideUpperNumerator); |
| | 63 | 124 | | denominator.Set(bound == 0 ? WideLowerDenominator : WideUpperDenominator); |
| | 63 | 125 | | ContactQuadratic.Scale(parameter, Signed576.ExtendValue(Signed320.ExtendValue(direction)), offset); |
| | 126 | 126 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(Signed320.ExtendValue(first)), a); a.Add(offset, - |
| | 126 | 127 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(Signed320.ExtendValue(second)), b); b.Add(offset, |
| | 127 | 128 | | if ((direction.Sign < 0) != (bound != 0)) { a.MultiplySign(-1); b.MultiplySign(-1); } |
| | 63 | 129 | | if (a.Signs[0] >= 0 && b.Signs[0] >= 0) continue; |
| | 41 | 130 | | if (a.Signs[0] < 0 && b.Signs[0] < 0) return false; |
| | 93 | 131 | | a.CopyTo(rootN); a.CopyTo(rootD); rootD.Add(b, -1); |
| | 63 | 132 | | if (rootD.Signs[0] < 0) { rootN.MultiplySign(-1); rootD.MultiplySign(-1); } |
| | 31 | 133 | | PackReducedParameter(rootN.Rational, rootD.Rational, out Signed832 n, out Signed832 d); |
| | | 134 | | // A collinear nondegenerate target has a monotone source |
| | | 135 | | // parameter, in either direction. The two ordered source bounds |
| | | 136 | | // therefore clip different target endpoints, each at most once. |
| | | 137 | | // A crossing root lies strictly inside its rejected endpoint; |
| | | 138 | | // disjoint targets were rejected above by their two negative signs. |
| | 60 | 139 | | if (a.Signs[0] < 0) { lowerN = n; lowerD = d; } |
| | 30 | 140 | | else { upperN = n; upperD = d; } |
| | | 141 | | } |
| | 27 | 142 | | mapped = new RationalSegmentInterval(lowerN, lowerD, upperN, upperD); |
| | 27 | 143 | | return true; |
| | | 144 | | } |
| | | 145 | | |
| | | 146 | | internal static void PackReducedParameter(ReadOnlySpan<ulong> numerator, ReadOnlySpan<ulong> denominator, |
| | | 147 | | out Signed832 n, out Signed832 d) |
| | | 148 | | { |
| | 95 | 149 | | int words = numerator.Length; |
| | 190 | 150 | | Span<ulong> gcd = stackalloc ulong[words], quotient = stackalloc ulong[words]; |
| | 190 | 151 | | Span<ulong> remainder = stackalloc ulong[words], scratch = stackalloc ulong[2 * words + 1]; |
| | 190 | 152 | | numerator.CopyTo(gcd); denominator.CopyTo(quotient); |
| | 95 | 153 | | WideArithmetic.GetMagnitudeGreatestCommonDivisor(gcd, quotient, scratch); |
| | 190 | 154 | | WideArithmetic.DivideMagnitudes(numerator, gcd, quotient, remainder, scratch); n = Pack(quotient); |
| | 188 | 155 | | WideArithmetic.DivideMagnitudes(denominator, gcd, quotient, remainder, scratch); d = Pack(quotient); |
| | | 156 | | static Signed832 Pack(ReadOnlySpan<ulong> magnitude) |
| | | 157 | | { |
| | | 158 | | if (WideArithmetic.GetActiveMagnitudeLength(magnitude) > 13 || (magnitude[12] & (1UL << 63)) != 0) |
| | | 159 | | throw new OverflowException("Mapped segment bounds must fit Signed832 after exact reduction."); |
| | | 160 | | return new Signed832(magnitude[12], magnitude[11], magnitude[10], magnitude[9], magnitude[8], magnitude[7], |
| | | 161 | | magnitude[6], magnitude[5], magnitude[4], magnitude[3], magnitude[2], magnitude[1], magnitude[0]); |
| | | 162 | | } |
| | 94 | 163 | | } |
| | | 164 | | |
| | | 165 | | /// <summary>Compares nonnegative unit parameters with positive denominators without narrowing products.</summary> |
| | | 166 | | internal static int CompareParameters(Signed320 firstN, Signed320 firstD, Signed320 secondN, Signed320 secondD) => |
| | 1716 | 167 | | WideArithmetic.CompareNonNegativeProducts(Signed832.ExtendValue(Signed576.ExtendValue(firstN)), |
| | 1716 | 168 | | Signed832.ExtendValue(Signed576.ExtendValue(secondD)), Signed832.ExtendValue(Signed576.ExtendValue(secondN)) |
| | 1716 | 169 | | Signed832.ExtendValue(Signed576.ExtendValue(firstD))); |
| | | 170 | | |
| | | 171 | | internal static int CompareParameters(Signed832 firstN, Signed832 firstD, Signed832 secondN, Signed832 secondD) => |
| | 5026 | 172 | | WideArithmetic.CompareNonNegativeProducts(firstN, secondD, secondN, firstD); |
| | | 173 | | |
| | | 174 | | /// <summary> |
| | | 175 | | /// Clips a unit interval by the linear half-plane value at its original |
| | | 176 | | /// endpoints. Bounds have positive denominators; first-second must fit |
| | | 177 | | /// Signed320 (current framed coverage callers require fewer than 268 bits). |
| | | 178 | | /// </summary> |
| | | 179 | | internal static bool TryClipHalfPlane(Signed320 first, Signed320 second, |
| | | 180 | | ref Signed320 lowerN, ref Signed320 lowerD, ref Signed320 upperN, ref Signed320 upperD) |
| | | 181 | | { |
| | 1193 | 182 | | if (first.Sign >= 0 && second.Sign >= 0) return true; |
| | 545 | 183 | | if (first.Sign < 0 && second.Sign < 0) return false; |
| | 946 | 184 | | Signed320 n = first, d = WideArithmetic.SubtractSigned320(first, second); |
| | 959 | 185 | | if (d.Sign < 0) { n = WideArithmetic.Negate(n); d = WideArithmetic.Negate(d); } |
| | 473 | 186 | | if (first.Sign < 0) |
| | | 187 | | { |
| | 717 | 188 | | if (CompareParameters(n, d, lowerN, lowerD) > 0) { lowerN = n; lowerD = d; } |
| | | 189 | | } |
| | 678 | 190 | | else if (CompareParameters(n, d, upperN, upperD) < 0) { upperN = n; upperD = d; } |
| | 473 | 191 | | return CompareParameters(lowerN, lowerD, upperN, upperD) <= 0; |
| | | 192 | | } |
| | | 193 | | |
| | | 194 | | internal RationalSegmentInterval(Signed320 lowerNumerator, Signed320 lowerDenominator, |
| | | 195 | | Signed320 upperNumerator, Signed320 upperDenominator) |
| | 1170 | 196 | | : this(Signed832.ExtendValue(Signed576.ExtendValue(lowerNumerator)), |
| | 1170 | 197 | | Signed832.ExtendValue(Signed576.ExtendValue(lowerDenominator)), |
| | 1170 | 198 | | Signed832.ExtendValue(Signed576.ExtendValue(upperNumerator)), |
| | 1170 | 199 | | Signed832.ExtendValue(Signed576.ExtendValue(upperDenominator))) |
| | | 200 | | { |
| | 1163 | 201 | | } |
| | | 202 | | |
| | | 203 | | internal RationalSegmentInterval(Signed832 lowerNumerator, Signed832 lowerDenominator, |
| | | 204 | | Signed832 upperNumerator, Signed832 upperDenominator) |
| | | 205 | | { |
| | 3386 | 206 | | if (lowerDenominator.Sign <= 0 || upperDenominator.Sign <= 0) |
| | 2 | 207 | | throw new ArgumentOutOfRangeException(nameof(lowerDenominator), "Interval denominators must be positive."); |
| | 3384 | 208 | | if (lowerNumerator.Sign < 0 || upperNumerator.Sign < 0 |
| | 3384 | 209 | | || WideArithmetic.SubtractSigned832(lowerDenominator, lowerNumerator).Sign < 0 |
| | 3384 | 210 | | || WideArithmetic.SubtractSigned832(upperDenominator, upperNumerator).Sign < 0) |
| | 4 | 211 | | throw new ArgumentOutOfRangeException(nameof(lowerNumerator), "Interval bounds must lie in [0,1]."); |
| | 3380 | 212 | | if (CompareParameters(lowerNumerator, lowerDenominator, upperNumerator, upperDenominator) > 0) |
| | 1 | 213 | | throw new ArgumentException("The lower interval bound must not exceed the upper bound."); |
| | 3379 | 214 | | this = default; |
| | 5244 | 215 | | if (lowerNumerator.IsZero && upperNumerator.Equals(upperDenominator)) return; |
| | 3028 | 216 | | WideLowerNumerator = lowerNumerator; WideLowerDenominator = lowerDenominator; |
| | 3028 | 217 | | WideUpperNumerator = upperNumerator; WideUpperDenominator = upperDenominator; |
| | 1514 | 218 | | HasWideBounds = !FitsLegacy(lowerNumerator) || !FitsLegacy(lowerDenominator) |
| | 1514 | 219 | | || !FitsLegacy(upperNumerator) || !FitsLegacy(upperDenominator); |
| | 1514 | 220 | | } |
| | | 221 | | |
| | | 222 | | /// <summary>Restricts an already admitted unit parameter; requires a clipped interval.</summary> |
| | | 223 | | internal bool Contains(scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 224 | | scoped ReadOnlySpan<ulong> root) |
| | | 225 | | { |
| | | 226 | | System.Diagnostics.Debug.Assert(IsClipped); |
| | | 227 | | // Only the retained parameter width changes; its event radicand is unchanged. |
| | 708 | 228 | | int words = Math.Max(ContactQuadratic.ActiveWords(numerator), ContactQuadratic.ActiveWords(denominator)) |
| | 708 | 229 | | + (HasWideBounds ? 14 : 6); |
| | 1416 | 230 | | Span<ulong> values = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 708 | 231 | | var query = ContactQuadratic.At(values, signs, 0, words); |
| | 708 | 232 | | var term = ContactQuadratic.At(values, signs, 1, words); |
| | 708 | 233 | | ScaleParameter(numerator, WideLowerDenominator, query); |
| | 1416 | 234 | | ScaleParameter(denominator, WideLowerNumerator, term); query.Add(term, -1); |
| | 818 | 235 | | if (query.Sign(root) < 0) return false; |
| | 598 | 236 | | ScaleParameter(denominator, WideUpperNumerator, query); |
| | 1196 | 237 | | ScaleParameter(numerator, WideUpperDenominator, term); query.Add(term, -1); |
| | 598 | 238 | | return query.Sign(root) >= 0; |
| | | 239 | | } |
| | | 240 | | |
| | | 241 | | private static bool FitsLegacy(Signed832 value) |
| | | 242 | | { |
| | 5686 | 243 | | ulong extension = (value.Word4 & (1UL << 63)) != 0 ? ulong.MaxValue : 0UL; |
| | 5686 | 244 | | return value.Word5 == extension && value.Word6 == extension && value.Word7 == extension |
| | 5686 | 245 | | && value.Word8 == extension && value.Word9 == extension && value.Word10 == extension |
| | 5686 | 246 | | && value.Word11 == extension && value.Word12 == extension; |
| | | 247 | | } |
| | | 248 | | |
| | | 249 | | private static void ScaleParameter(ContactQuadratic value, Signed832 parameter, ContactQuadratic result) |
| | | 250 | | { |
| | 5224 | 251 | | Span<ulong> magnitude = stackalloc ulong[13]; WideArithmetic.GetMagnitude(parameter, magnitude); |
| | 2612 | 252 | | ContactQuadratic.Scale(value, magnitude, parameter.Sign, result); |
| | 2612 | 253 | | } |
| | | 254 | | |
| | | 255 | | private static Fixed64 GetWideCoordinate(Fixed64 start, Fixed64 end, Signed832 n, Signed832 d) |
| | | 256 | | { |
| | | 257 | | const int words = 15; |
| | 104 | 258 | | Span<ulong> values = stackalloc ulong[8 * words]; Span<int> signs = stackalloc int[8]; |
| | 52 | 259 | | var parameter = ContactQuadratic.At(values, signs, 0, words); |
| | 52 | 260 | | var denominator = ContactQuadratic.At(values, signs, 1, words); |
| | 52 | 261 | | var numerator = ContactQuadratic.At(values, signs, 2, words); |
| | 52 | 262 | | var term = ContactQuadratic.At(values, signs, 3, words); |
| | 104 | 263 | | parameter.Set(n); denominator.Set(d); |
| | 52 | 264 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Raw(start))), numerato |
| | 52 | 265 | | ContactQuadratic.Scale(parameter, Signed576.ExtendValue(Signed320.ExtendValue(WideArithmetic.Difference(end, sta |
| | 52 | 266 | | numerator.Add(term); |
| | 52 | 267 | | return ContactQuadratic.RoundRatio(numerator, denominator, ReadOnlySpan<ulong>.Empty); |
| | | 268 | | } |
| | | 269 | | } |