| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderPairSideValuePolynomial.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 squared support values on one cylinder's side-normal plane.</summary> |
| | | 14 | | internal static class CylinderPairSideValuePolynomial |
| | | 15 | | { |
| | | 16 | | internal const int InvariantCount = 8; |
| | | 17 | | internal const int CoefficientCount = 9; |
| | | 18 | | |
| | | 19 | | /// <summary> |
| | | 20 | | /// Builds the unscaled squared-gap polynomial. FirstSide means normals |
| | | 21 | | /// perpendicular to FirstAxis; capSign selects the other cylinder's cap. |
| | | 22 | | /// The caller retains signed-gap/cap admission and the existing ellipse |
| | | 23 | | /// parameter root to identify one value. Output has nine equal-width slots. |
| | | 24 | | /// </summary> |
| | | 25 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 26 | | internal static int Build(in CylinderPairGeometry geometry, bool firstSide, int capSign, |
| | | 27 | | Span<ulong> coefficients, Span<sbyte> signs, |
| | | 28 | | Span<ulong> unshiftedCoefficients = default, Span<sbyte> unshiftedSigns = default) |
| | | 29 | | { |
| | 469 | 30 | | int words = coefficients.Length / CoefficientCount; |
| | 469 | 31 | | Span<ulong> invariants = stackalloc ulong[InvariantCount * words]; |
| | 469 | 32 | | Span<sbyte> invariantSigns = stackalloc sbyte[InvariantCount]; |
| | 469 | 33 | | invariants.Clear(); |
| | 469 | 34 | | WideAxis3 a = firstSide ? geometry.SecondAxis : geometry.FirstAxis; |
| | 469 | 35 | | WideAxis3 b = firstSide ? geometry.FirstAxis : geometry.SecondAxis; |
| | 469 | 36 | | WideAxis3 half = firstSide ? geometry.SecondHalf : geometry.FirstHalf; |
| | 469 | 37 | | WideAxis3 c = new( |
| | 469 | 38 | | WideArithmetic.SubtractSigned320(capSign > 0 ? half.X : WideArithmetic.Negate(half.X), geometry.CenterDiffer |
| | 469 | 39 | | WideArithmetic.SubtractSigned320(capSign > 0 ? half.Y : WideArithmetic.Negate(half.Y), geometry.CenterDiffer |
| | 469 | 40 | | WideArithmetic.SubtractSigned320(capSign > 0 ? half.Z : WideArithmetic.Negate(half.Z), geometry.CenterDiffer |
| | 469 | 41 | | Signed320 ra = firstSide ? geometry.SecondRadius : geometry.FirstRadius; |
| | 469 | 42 | | Signed320 rb = firstSide ? geometry.FirstRadius : geometry.SecondRadius; |
| | 469 | 43 | | Write(a.SquaredLength, 0, invariants, invariantSigns, words); |
| | 469 | 44 | | Write(b.SquaredLength, 1, invariants, invariantSigns, words); |
| | 469 | 45 | | Write(WideAxis3.Dot(a, b), 2, invariants, invariantSigns, words); |
| | 469 | 46 | | Write(c.SquaredLength, 3, invariants, invariantSigns, words); |
| | 469 | 47 | | Write(WideAxis3.Dot(a, c), 4, invariants, invariantSigns, words); |
| | 469 | 48 | | Write(WideAxis3.Dot(b, c), 5, invariants, invariantSigns, words); |
| | 469 | 49 | | Write(WideArithmetic.MultiplySigned320(ra, ra), 6, invariants, invariantSigns, words); |
| | 469 | 50 | | Write(WideArithmetic.MultiplySigned320(rb, rb), 7, invariants, invariantSigns, words); |
| | 469 | 51 | | return Build(invariants, invariantSigns, words, coefficients, signs, unshiftedCoefficients, unshiftedSigns); |
| | | 52 | | } |
| | | 53 | | |
| | | 54 | | /// <summary> |
| | | 55 | | /// Scalar slots: U=a², B=b², C=a.b, q=c², x=a.c, y=b.c, rho=ra², |
| | | 56 | | /// tau=rb², from nonzero nonparallel axes and a positive first radius. |
| | | 57 | | /// Returns degree eight without normalization. |
| | | 58 | | /// Input/output storage is disjoint; every intermediate must fit words. |
| | | 59 | | /// </summary> |
| | | 60 | | /// <remarks> |
| | | 61 | | /// Geometry gives |a|<2^33, |b|<2^163 (or the reverse), |c|<2^229, |
| | | 62 | | /// and radii<2^226. Thus H,C²<2^392, q<2^458, rho,tau<2^452. |
| | | 63 | | /// Weighting coefficient j by 458j gives l,m,v heights 852,853,850. |
| | | 64 | | /// The five quartic summands have heights 3414,3407,3410,3405,3393; |
| | | 65 | | /// their sum has height 3417. Substituting z=2^460*t raises this to 3425. |
| | | 66 | | /// Conjugation has 25 coefficient pairs and binomial absolute sum<=4^8, |
| | | 67 | | /// so the final scaled octic has height<6871, within 116 words. |
| | | 68 | | /// The unshifted phase peaks at 48 coefficient slots; the later conjugation |
| | | 69 | | /// uses 43. Construction scratch is released before algebraic root work. |
| | | 70 | | /// This construction frame must return before a root arena is allocated. |
| | | 71 | | /// </remarks> |
| | | 72 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 73 | | internal static int Build(ReadOnlySpan<ulong> invariants, ReadOnlySpan<sbyte> invariantSigns, |
| | | 74 | | int words, Span<ulong> coefficients, Span<sbyte> signs, |
| | | 75 | | Span<ulong> unshiftedCoefficients = default, Span<sbyte> unshiftedSigns = default) |
| | | 76 | | { |
| | 499 | 77 | | Span<ulong> unshifted = stackalloc ulong[5 * words]; |
| | 499 | 78 | | Span<sbyte> unshiftedSignStorage = stackalloc sbyte[5]; |
| | 499 | 79 | | BuildUnshifted(invariants, invariantSigns, words, unshifted, unshiftedSignStorage); |
| | 499 | 80 | | Span<ulong> scratch = stackalloc ulong[38 * words]; |
| | 499 | 81 | | Span<sbyte> scratchSigns = stackalloc sbyte[38]; |
| | 998 | 82 | | scratch.Clear(); scratchSigns.Clear(); |
| | 499 | 83 | | Polynomial storage = new(scratch, scratchSigns, words); |
| | 998 | 84 | | Polynomial f = storage.Slice(0, 5), tau = storage.Slice(5, 1); |
| | 499 | 85 | | unshifted.CopyTo(f.Magnitudes); |
| | 499 | 86 | | unshiftedSignStorage.CopyTo(f.Signs); |
| | 499 | 87 | | invariants.Slice(7 * words, words).CopyTo(tau.Magnitudes); |
| | 499 | 88 | | tau.Signs[0] = invariantSigns[7]; |
| | 499 | 89 | | if (!unshiftedCoefficients.IsEmpty) |
| | | 90 | | { |
| | 466 | 91 | | f.Magnitudes.CopyTo(unshiftedCoefficients); |
| | 466 | 92 | | f.Signs.CopyTo(unshiftedSigns); |
| | | 93 | | } |
| | 998 | 94 | | Polynomial p0 = storage.Slice(6, 9), p1 = storage.Slice(15, 9); |
| | 998 | 95 | | Polynomial even = storage.Slice(24, 5), odd = storage.Slice(29, 4); |
| | 998 | 96 | | Polynomial shifted = storage.Slice(33, 2), radicalSquared = storage.Slice(35, 2); |
| | 499 | 97 | | Span<ulong> product = scratch.Slice(37 * words, words); |
| | | 98 | | // F(S+tau+w)=even+w*odd, w²=4*tau*S. Four Horner steps |
| | | 99 | | // preserve the radius offset, then conjugation gives even²-w²*odd². |
| | 499 | 100 | | Add(shifted, tau); |
| | 499 | 101 | | shifted.Magnitudes[words] = 1; |
| | 499 | 102 | | shifted.Signs[1] = 1; |
| | 499 | 103 | | Add(radicalSquared, tau, 4, 1); |
| | 499 | 104 | | Add(even, f.Slice(4, 1)); |
| | 4990 | 105 | | for (int index = 3; index >= 0; index--) |
| | | 106 | | { |
| | 1996 | 107 | | Multiply(p0, shifted, even, product); |
| | 1996 | 108 | | Multiply(p1, radicalSquared, odd, product); |
| | 3992 | 109 | | Add(p0, p1); Add(p0, f.Slice(index, 1)); |
| | 3992 | 110 | | Multiply(p1, shifted, odd, product); Add(p1, even); |
| | 3992 | 111 | | Copy(even, p0); Copy(odd, p1); |
| | | 112 | | } |
| | 499 | 113 | | Polynomial output = new(coefficients.Slice(0, CoefficientCount * words), signs.Slice(0, CoefficientCount), words |
| | 499 | 114 | | Multiply(output, even, even, product); |
| | 998 | 115 | | Multiply(p0, odd, odd, product); Multiply(p1, radicalSquared, p0, product); |
| | 499 | 116 | | Add(output, p1, -1); |
| | | 117 | | // F's leading coefficient is H²*(H-C²)², strictly positive for |
| | | 118 | | // nonzero nonparallel axes. Conjugation squares that coefficient. |
| | | 119 | | System.Diagnostics.Debug.Assert(output.Signs[CoefficientCount - 1] > 0); |
| | 499 | 120 | | return CoefficientCount - 1; |
| | | 121 | | } |
| | | 122 | | |
| | | 123 | | |
| | | 124 | | /// <summary> |
| | | 125 | | /// Builds only the degree-four squared distance polynomial for one projected |
| | | 126 | | /// disk and a point in its projection plane. Scalar invariant slots are the |
| | | 127 | | /// same as Build; tau is unused. Output has five equal-width coefficients. |
| | | 128 | | /// </summary> |
| | | 129 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 130 | | internal static int BuildUnshifted(ReadOnlySpan<ulong> invariants, |
| | | 131 | | ReadOnlySpan<sbyte> invariantSigns, int words, |
| | | 132 | | Span<ulong> coefficients, Span<sbyte> signs) |
| | | 133 | | { |
| | 949 | 134 | | Span<ulong> scratch = stackalloc ulong[43 * words]; |
| | 949 | 135 | | Span<sbyte> scratchSigns = stackalloc sbyte[43]; |
| | 1898 | 136 | | scratch.Clear(); scratchSigns.Clear(); |
| | 949 | 137 | | invariants.Slice(0, InvariantCount * words).CopyTo(scratch); |
| | 949 | 138 | | invariantSigns.Slice(0, InvariantCount).CopyTo(scratchSigns); |
| | 949 | 139 | | Polynomial storage = new(scratch, scratchSigns, words); |
| | 2847 | 140 | | Polynomial u = storage.Slice(0, 1), b = storage.Slice(1, 1), c = storage.Slice(2, 1); |
| | 2847 | 141 | | Polynomial q = storage.Slice(3, 1), x = storage.Slice(4, 1), y = storage.Slice(5, 1); |
| | 949 | 142 | | Polynomial rho = storage.Slice(6, 1); |
| | 2847 | 143 | | Polynomial h = storage.Slice(8, 1), cSquared = storage.Slice(9, 1), j = storage.Slice(10, 1); |
| | 1898 | 144 | | Polynomial s0 = storage.Slice(11, 1), s1 = storage.Slice(12, 1); |
| | 2847 | 145 | | Polynomial l = storage.Slice(13, 2), m = storage.Slice(15, 2), v = storage.Slice(17, 2); |
| | 949 | 146 | | Polynomial f = storage.Slice(19, 5); |
| | 1898 | 147 | | Polynomial p0 = storage.Slice(24, 9), p1 = storage.Slice(33, 9); |
| | 949 | 148 | | Span<ulong> product = scratch.Slice(42 * words, words); |
| | 949 | 149 | | Multiply(h, u, b, product); |
| | 949 | 150 | | Multiply(cSquared, c, c, product); |
| | 1898 | 151 | | Add(j, h); Add(j, cSquared); |
| | | 152 | | // l=H*z+rho*(H+C²)-H*q+U*y². |
| | 949 | 153 | | Add(l, h, 1, 1); |
| | 1898 | 154 | | Multiply(s0, rho, j, product); Add(l, s0); |
| | 1898 | 155 | | Multiply(s0, h, q, product); Add(l, s0, -1); |
| | 2847 | 156 | | Multiply(s0, y, y, product); Multiply(s1, u, s0, product); Add(l, s1); |
| | | 157 | | // m=-(H+C²)*z-rho*C²+C²*q+B*x²-2*C*x*y; v=C²*z. |
| | 949 | 158 | | Add(m, j, -1, 1); |
| | 1898 | 159 | | Multiply(s0, rho, cSquared, product); Add(m, s0, -1); |
| | 1898 | 160 | | Multiply(s0, cSquared, q, product); Add(m, s0); |
| | 2847 | 161 | | Multiply(s0, x, x, product); Multiply(s1, b, s0, product); Add(m, s1); |
| | 2847 | 162 | | Multiply(s0, c, x, product); Multiply(s1, s0, y, product); Add(m, s1, -2); |
| | 949 | 163 | | Add(v, cSquared, 1, 1); |
| | | 164 | | |
| | | 165 | | // Disc(rho²*v*lambda³+rho*m*lambda²+l*lambda-H) |
| | | 166 | | // after removing the guaranteed positive rho² factor. The principal |
| | | 167 | | // ellipse denominator E cancels before construction; no division is needed. |
| | 2847 | 168 | | Multiply(p0, m, m, product); Multiply(p1, l, l, product); Multiply(f, p0, p1, product); |
| | 949 | 169 | | Multiply(s0, h, rho, product); |
| | 2847 | 170 | | Multiply(p1, p0, m, product); Multiply(p0, s0, p1, product); Add(f, p0, 4); |
| | 1898 | 171 | | Multiply(p0, l, l, product); Multiply(p1, p0, l, product); |
| | 1898 | 172 | | Multiply(p0, v, p1, product); Add(f, p0, -4); |
| | 1898 | 173 | | Multiply(p0, v, m, product); Multiply(p1, p0, l, product); |
| | 1898 | 174 | | Multiply(p0, s0, p1, product); Add(f, p0, -18); |
| | 1898 | 175 | | Multiply(s1, s0, s0, product); Multiply(p0, v, v, product); |
| | 1898 | 176 | | Multiply(p1, s1, p0, product); Add(f, p1, -27); |
| | | 177 | | |
| | 949 | 178 | | f.Magnitudes.CopyTo(coefficients); |
| | 949 | 179 | | f.Signs.CopyTo(signs); |
| | 949 | 180 | | return 4; |
| | | 181 | | } |
| | | 182 | | |
| | | 183 | | private static void Multiply(Polynomial result, Polynomial left, Polynomial right, Span<ulong> product) => |
| | 33108 | 184 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(left.Magnitudes, left.Signs, |
| | 33108 | 185 | | right.Magnitudes, right.Signs, result.Magnitudes, result.Signs, product); |
| | | 186 | | |
| | | 187 | | private static void Add(Polynomial result, Polynomial source, int multiplier = 1, int offset = 0) => |
| | 23168 | 188 | | WideFiniteAxisIntersection.AddFiniteAxisPolynomial(source.Magnitudes, source.Signs, |
| | 23168 | 189 | | result.Magnitudes, result.Signs, multiplier, offset); |
| | | 190 | | |
| | | 191 | | private static void Write(Signed576 value, int index, Span<ulong> invariants, |
| | | 192 | | Span<sbyte> signs, int words) |
| | | 193 | | { |
| | 3752 | 194 | | WideArithmetic.GetMagnitude(value, invariants.Slice(index * words, 9)); |
| | 3752 | 195 | | signs[index] = (sbyte)value.Sign; |
| | 3752 | 196 | | } |
| | | 197 | | |
| | | 198 | | private static void Copy(Polynomial result, Polynomial source) |
| | | 199 | | { |
| | 3992 | 200 | | source.Magnitudes.Slice(0, result.Magnitudes.Length).CopyTo(result.Magnitudes); |
| | 3992 | 201 | | source.Signs.Slice(0, result.Signs.Length).CopyTo(result.Signs); |
| | 3992 | 202 | | } |
| | | 203 | | |
| | | 204 | | private readonly ref struct Polynomial |
| | | 205 | | { |
| | | 206 | | internal readonly Span<ulong> Magnitudes; |
| | | 207 | | internal readonly Span<sbyte> Signs; |
| | | 208 | | private readonly int words; |
| | | 209 | | internal Polynomial(Span<ulong> magnitudes, Span<sbyte> signs, int words) |
| | | 210 | | { |
| | 25516 | 211 | | Magnitudes = magnitudes; |
| | 25516 | 212 | | Signs = signs; |
| | 25516 | 213 | | this.words = words; |
| | 25516 | 214 | | } |
| | | 215 | | internal Polynomial Slice(int offset, int count) => |
| | 23569 | 216 | | new(Magnitudes.Slice(offset * words, count * words), Signs.Slice(offset, count), words); |
| | | 217 | | } |
| | | 218 | | } |