| | | 1 | | //======================================================================= |
| | | 2 | | // FiniteSectionPoint.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>Borrowed exact homogeneous coordinates and shared radicand of a finite section point.</summary> |
| | | 13 | | internal readonly ref struct FiniteSectionPoint |
| | | 14 | | { |
| | | 15 | | internal const int Words = 10, RootWords = 17, StorageWords = 8 * Words + RootWords; |
| | | 16 | | internal const int AffineWords = 13, AffineRootWords = 23; |
| | | 17 | | internal const int SignCount = 8; |
| | | 18 | | internal readonly Span<ulong> Values; |
| | | 19 | | internal readonly Span<int> Signs; |
| | | 20 | | private readonly int _fieldWords; |
| | 170 | 21 | | internal Span<ulong> Root => Values[(8 * _fieldWords)..]; |
| | 321 | 22 | | internal ContactQuadratic Coordinate(int axis) => ContactQuadratic.At(Values, Signs, axis, _fieldWords); |
| | 179 | 23 | | internal ContactQuadratic Denominator => ContactQuadratic.At(Values, Signs, 3, _fieldWords); |
| | | 24 | | |
| | | 25 | | internal FiniteSectionPoint(Span<ulong> values, Span<int> signs, |
| | | 26 | | int fieldWords = Words, int rootWords = RootWords) |
| | | 27 | | { |
| | 98 | 28 | | Values = values[..(8 * fieldWords + rootWords)]; Signs = signs[..SignCount]; |
| | 49 | 29 | | _fieldWords = fieldWords; |
| | 49 | 30 | | } |
| | | 31 | | |
| | | 32 | | /// <summary>Retains the lexicographically first point of a closed segment/cone section.</summary> |
| | | 33 | | internal static bool TryGetFirstSegmentPoint(FixedSegment segment, |
| | | 34 | | in ConeFiniteSectionFrame frame, FiniteSectionPoint result) |
| | | 35 | | { |
| | 36 | 36 | | result.Values.Clear(); result.Signs.Clear(); |
| | 18 | 37 | | if (ComparePositions(segment.Start, segment.End) > 0) |
| | 3 | 38 | | segment = new FixedSegment(segment.End, segment.Start); |
| | 18 | 39 | | int words = result._fieldWords; |
| | 18 | 40 | | Span<ulong> work = stackalloc ulong[4 * words]; |
| | 18 | 41 | | Span<int> signs = stackalloc int[4]; |
| | 18 | 42 | | ContactQuadratic parameter = ContactQuadratic.At(work, signs, 0, words); |
| | 18 | 43 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 1, words); |
| | 18 | 44 | | ContactQuadratic denominator = result.Denominator; |
| | 18 | 45 | | if (!TryGetSegmentParameter(segment, frame, false, parameter, denominator, result.Root)) |
| | 1 | 46 | | return false; |
| | 17 | 47 | | SetSegmentPoint(segment, parameter, denominator, term, result); |
| | 17 | 48 | | return true; |
| | | 49 | | } |
| | | 50 | | |
| | | 51 | | private static void SetSegmentPoint(FixedSegment segment, ContactQuadratic parameter, |
| | | 52 | | ContactQuadratic denominator, ContactQuadratic term, FiniteSectionPoint result) |
| | | 53 | | { |
| | | 54 | | // The cone frame bounds A/B/C below 528 bits, so a parameter |
| | | 55 | | // numerator/denominator fits 529 bits and its discriminant 1057 bits. |
| | | 56 | | // Authored raw endpoint differences are below 65 bits; the complete |
| | | 57 | | // point coefficients fit 595 bits (ten limbs), root fits seventeen. |
| | 240 | 58 | | for (int axis = 0; axis < 3; axis++) |
| | | 59 | | { |
| | 90 | 60 | | Signed192 first = Signed192.Raw(segment.Start[axis]); |
| | 90 | 61 | | Signed192 difference = WideArithmetic.SubtractSigned192(Signed192.Raw(segment.End[axis]), first); |
| | 90 | 62 | | ContactQuadratic coordinate = result.Coordinate(axis); |
| | 90 | 63 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(Signed320.ExtendValue(first)), coordinate); |
| | 90 | 64 | | ContactQuadratic.Scale(parameter, Signed576.ExtendValue(Signed320.ExtendValue(difference)), term); |
| | 90 | 65 | | coordinate.Add(term); |
| | | 66 | | } |
| | 30 | 67 | | } |
| | | 68 | | |
| | | 69 | | /// <summary>Retains a closed cone-section endpoint in the input segment's exact parameter order.</summary> |
| | | 70 | | /// <remarks> |
| | | 71 | | /// Numerator/denominator coefficients need 529 bits and root needs 1057 |
| | | 72 | | /// bits. A non-dyadic or irrational endpoint remains admitted without ever |
| | | 73 | | /// materializing t; callers may form paired coordinates in this same field. |
| | | 74 | | /// </remarks> |
| | | 75 | | internal static bool TryGetSegmentParameter(FixedSegment segment, in ConeFiniteSectionFrame frame, |
| | | 76 | | bool upper, ContactQuadratic numerator, ContactQuadratic denominator, Span<ulong> root) |
| | | 77 | | { |
| | 12342 | 78 | | numerator.Clear(); denominator.Clear(); root.Clear(); |
| | 8228 | 79 | | WideAxis3 start = frame.Transform(segment.Start), end = frame.Transform(segment.End); |
| | 4117 | 80 | | if (!frame.IntersectsTransformedSegment(start, end)) return false; |
| | 4111 | 81 | | WideAxis3 delta = new(WideArithmetic.SubtractSigned320(end.X, start.X), |
| | 4111 | 82 | | WideArithmetic.SubtractSigned320(end.Y, start.Y), WideArithmetic.SubtractSigned320(end.Z, start.Z)); |
| | 4111 | 83 | | bool axial = frame.TryGetAxialInterval(start.Y, delta.Y, out Signed320 lowerN, out Signed320 lowerD, |
| | 4111 | 84 | | out Signed320 upperN, out Signed320 upperD); |
| | | 85 | | System.Diagnostics.Debug.Assert(axial); |
| | 8222 | 86 | | Signed320 clipN = upper ? upperN : lowerN, clipD = upper ? upperD : lowerD; |
| | 8222 | 87 | | Signed832 a = frame.QuadraticProductWide(delta, delta), b = frame.QuadraticProductWide(start, delta), |
| | 4111 | 88 | | c = frame.QuadraticProductWide(start, start); |
| | 4111 | 89 | | if (WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(a, b, c, clipN, clipD) <= 0) |
| | | 90 | | { |
| | 7816 | 91 | | numerator.Set(Signed576.ExtendValue(clipN)); denominator.Set(Signed576.ExtendValue(clipD)); |
| | 3908 | 92 | | return true; |
| | | 93 | | } |
| | 203 | 94 | | int words = numerator.FieldWords; |
| | 203 | 95 | | Span<ulong> work = stackalloc ulong[6 * words]; |
| | 203 | 96 | | Span<int> signs = stackalloc int[6]; |
| | 406 | 97 | | ContactQuadratic qa = ContactQuadratic.At(work, signs, 0, words), qb = ContactQuadratic.At(work, signs, 1, words |
| | 203 | 98 | | ContactQuadratic qc = ContactQuadratic.At(work, signs, 2, words); |
| | 609 | 99 | | qa.Set(a); qb.Set(b); qc.Set(c); |
| | | 100 | | // For A>0 the admitted interval is between the roots; for A<0 it |
| | | 101 | | // lies outside them. Finite axial clipping excludes the opposite |
| | | 102 | | // nappe. The linear and repeated-root chart has only branch +1. |
| | 203 | 103 | | int branch = a.Sign == 0 ? 1 : (upper ? 1 : -1) * a.Sign; |
| | 203 | 104 | | bool found = ContactQuadratic.TryGetParameter(qa, qb, qc, branch, root, numerator, denominator); |
| | 203 | 105 | | if (!found) |
| | 19 | 106 | | found = ContactQuadratic.TryGetParameter(qa, qb, qc, 1, root, numerator, denominator); |
| | | 107 | | System.Diagnostics.Debug.Assert(found); |
| | 203 | 108 | | return true; |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | internal static int CompareLexicographic(FiniteSectionPoint first, FiniteSectionPoint second) |
| | | 112 | | { |
| | 94 | 113 | | for (int axis = 0; axis < 3; axis++) |
| | | 114 | | { |
| | 37 | 115 | | int comparison = ContactQuadratic.CompareRatios(first.Coordinate(axis), first.Denominator, first.Root, |
| | 37 | 116 | | second.Coordinate(axis), second.Denominator, second.Root); |
| | 37 | 117 | | if (comparison != 0) |
| | 4 | 118 | | return comparison; |
| | | 119 | | } |
| | 10 | 120 | | return 0; |
| | | 121 | | } |
| | | 122 | | |
| | | 123 | | private static int ComparePositions(Vector3d first, Vector3d second) |
| | | 124 | | { |
| | 38 | 125 | | int comparison = first.X.CompareTo(second.X); |
| | 65 | 126 | | if (comparison != 0) return comparison; |
| | 11 | 127 | | comparison = first.Y.CompareTo(second.Y); |
| | 11 | 128 | | return comparison != 0 ? comparison : first.Z.CompareTo(second.Z); |
| | | 129 | | } |
| | | 130 | | |
| | | 131 | | internal void CopyTo(FiniteSectionPoint destination) |
| | | 132 | | { |
| | | 133 | | // The caller proves the complete active fields fit. Different packed |
| | | 134 | | // layouts must not overlap; equal-layout self-copy remains valid. |
| | 50 | 135 | | for (int coordinate = 0; coordinate < 4; coordinate++) |
| | 20 | 136 | | Coordinate(coordinate).CopyTo(destination.Coordinate(coordinate)); |
| | 5 | 137 | | int activeRoot = WideArithmetic.GetActiveMagnitudeLength(Root); |
| | 5 | 138 | | Root[..activeRoot].CopyTo(destination.Root); |
| | 5 | 139 | | destination.Root[activeRoot..].Clear(); |
| | 5 | 140 | | } |
| | | 141 | | |
| | | 142 | | internal static bool TryGetFirstSegmentPoint(FixedSegment segment, |
| | | 143 | | in CapsuleFiniteSectionFrame frame, FiniteSectionPoint result) |
| | | 144 | | { |
| | 32 | 145 | | result.Values.Clear(); result.Signs.Clear(); |
| | 16 | 146 | | if (ComparePositions(segment.Start, segment.End) > 0) |
| | 3 | 147 | | segment = new FixedSegment(segment.End, segment.Start); |
| | 16 | 148 | | int words = result._fieldWords; |
| | 16 | 149 | | Span<ulong> work = stackalloc ulong[4 * words]; |
| | 16 | 150 | | Span<int> signs = stackalloc int[4]; |
| | 16 | 151 | | ContactQuadratic parameter = ContactQuadratic.At(work, signs, 0, words); |
| | 16 | 152 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 1, words); |
| | 22 | 153 | | if (!frame.TryGetFirstSegmentParameter(segment, parameter, result.Denominator, result.Root)) return false; |
| | | 154 | | // Capsule coefficients <400 bits need <466 bits after authored |
| | | 155 | | // endpoint multiplication, within the existing cone-sized bank. |
| | 10 | 156 | | SetSegmentPoint(segment, parameter, result.Denominator, term, result); |
| | 10 | 157 | | return true; |
| | | 158 | | } |
| | | 159 | | |
| | | 160 | | internal static bool TryGetFirstSegmentPoint(FixedSegment segment, |
| | | 161 | | in FiniteSlabSectionFrame frame, FiniteSectionPoint result) |
| | 4 | 162 | | => TryGetFirstSegmentPoint(segment, frame, ReadOnlySpan<Vector2d>.Empty, result); |
| | | 163 | | |
| | | 164 | | internal static bool TryGetFirstSegmentPoint(FixedSegment segment, |
| | | 165 | | in FiniteSlabSectionFrame frame, ReadOnlySpan<Vector2d> prismOffsets, FiniteSectionPoint result) |
| | | 166 | | { |
| | 8 | 167 | | result.Values.Clear(); result.Signs.Clear(); |
| | 4 | 168 | | if (ComparePositions(segment.Start, segment.End) > 0) |
| | 1 | 169 | | segment = new FixedSegment(segment.End, segment.Start); |
| | 4 | 170 | | int words = result._fieldWords; |
| | 8 | 171 | | Span<ulong> work = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 4 | 172 | | ContactQuadratic parameter = ContactQuadratic.At(work, signs, 0, words); |
| | 4 | 173 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 1, words); |
| | 5 | 174 | | if (!frame.TryGetFirstSegmentParameter(segment, prismOffsets, parameter, result.Denominator, result.Root)) retur |
| | 3 | 175 | | SetSegmentPoint(segment, parameter, result.Denominator, term, result); |
| | 3 | 176 | | return true; |
| | | 177 | | } |
| | | 178 | | } |