| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderEdgeContactPolynomial.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>The shared stationary quartic on a cylinder's edge-normal chart.</summary> |
| | | 13 | | internal static class CylinderEdgeContactPolynomial |
| | | 14 | | { |
| | | 15 | | internal const int Slots = 26; |
| | | 16 | | |
| | | 17 | | /// <summary> |
| | | 18 | | /// For g=(P+sqrt(Q))/(scale*sqrt(M)), writes W=L²-K²Q, |
| | | 19 | | /// Q,K,L,J=PK-L,N=J²,D=scale²K²M into slots 0,5,8,10,13,16,21. |
| | | 20 | | /// K=2P'M-PM' and L=Q'M-QM'. M is a positive quadratic metric. |
| | | 21 | | /// Root admission must retain L=-K sqrt(Q), cap and polytope cones. |
| | | 22 | | /// All buffers are disjoint, and each coefficient must fit its supplied word stride. |
| | | 23 | | /// </summary> |
| | | 24 | | internal static void Build(ReadOnlySpan<ulong> p, ReadOnlySpan<sbyte> ps, |
| | | 25 | | ReadOnlySpan<ulong> q, ReadOnlySpan<sbyte> qs, |
| | | 26 | | ReadOnlySpan<ulong> metric, ReadOnlySpan<sbyte> ms, |
| | | 27 | | ReadOnlySpan<ulong> scale, Span<ulong> data, Span<sbyte> signs) |
| | | 28 | | { |
| | 15166 | 29 | | BuildStationary(p, ps, q, qs, metric, ms, scale, data, signs); |
| | 15166 | 30 | | CompleteGap(data, signs); |
| | 15166 | 31 | | } |
| | | 32 | | |
| | | 33 | | /// <summary> |
| | | 34 | | /// Builds W,Q,K,L for root isolation/admission. Slots 13..14 borrow P, |
| | | 35 | | /// 16..18 borrow M, 19 borrows scale, and 21..23 borrow K squared until |
| | | 36 | | /// completion replaces 13..25 by J,N,D. Call CompleteGap at most once. |
| | | 37 | | /// </summary> |
| | | 38 | | internal static void BuildStationary(ReadOnlySpan<ulong> p, ReadOnlySpan<sbyte> ps, |
| | | 39 | | ReadOnlySpan<ulong> q, ReadOnlySpan<sbyte> qs, |
| | | 40 | | ReadOnlySpan<ulong> metric, ReadOnlySpan<sbyte> ms, |
| | | 41 | | ReadOnlySpan<ulong> scale, Span<ulong> data, Span<sbyte> signs) |
| | | 42 | | { |
| | 16113 | 43 | | int words = data.Length / signs.Length; |
| | 32226 | 44 | | data.Clear(); signs.Clear(); |
| | 32226 | 45 | | q.CopyTo(data.Slice(5 * words, 3 * words)); qs.CopyTo(signs.Slice(5, 3)); |
| | 16113 | 46 | | Span<ulong> scratch = stackalloc ulong[9 * words]; |
| | 16113 | 47 | | Span<sbyte> scratchSigns = stackalloc sbyte[8]; |
| | 32226 | 48 | | scratch.Clear(); scratchSigns.Clear(); |
| | 16113 | 49 | | Span<ulong> product = scratch.Slice(8 * words, words); |
| | 32226 | 50 | | Span<ulong> k = data.Slice(8 * words, 2 * words), l = data.Slice(10 * words, 3 * words); |
| | 32226 | 51 | | Span<sbyte> ks = signs.Slice(8, 2), ls = signs.Slice(10, 3); |
| | 16113 | 52 | | Term(p, ps, 1, metric, ms, 0, k, ks, 0, 2, product); |
| | 16113 | 53 | | Term(p, ps, 0, metric, ms, 1, k, ks, 0, -1, product); |
| | 16113 | 54 | | Term(p, ps, 1, metric, ms, 1, k, ks, 1, 1, product); |
| | 16113 | 55 | | Term(p, ps, 0, metric, ms, 2, k, ks, 1, -2, product); |
| | 16113 | 56 | | Term(q, qs, 1, metric, ms, 0, l, ls, 0, 1, product); |
| | 16113 | 57 | | Term(q, qs, 0, metric, ms, 1, l, ls, 0, -1, product); |
| | 16113 | 58 | | Term(q, qs, 2, metric, ms, 0, l, ls, 1, 2, product); |
| | 16113 | 59 | | Term(q, qs, 0, metric, ms, 2, l, ls, 1, -2, product); |
| | 16113 | 60 | | Term(q, qs, 2, metric, ms, 1, l, ls, 2, 1, product); |
| | 16113 | 61 | | Term(q, qs, 1, metric, ms, 2, l, ls, 2, -1, product); |
| | 32226 | 62 | | Span<ulong> kSquared = scratch[..(3 * words)], temporary = scratch.Slice(3 * words, 5 * words); |
| | 32226 | 63 | | Span<sbyte> kSquaredSigns = scratchSigns[..3], temporarySigns = scratchSigns.Slice(3, 5); |
| | 16113 | 64 | | Multiply(l, ls, l, ls, data[..(5 * words)], signs[..5], product); |
| | 16113 | 65 | | Multiply(k, ks, k, ks, kSquared, kSquaredSigns, product); |
| | 16113 | 66 | | Multiply(kSquared, kSquaredSigns, q, qs, temporary, temporarySigns, product); |
| | 16113 | 67 | | Add(temporary, temporarySigns, data[..(5 * words)], signs[..5], -1); |
| | 16113 | 68 | | p.CopyTo(data.Slice(13 * words, 2 * words)); |
| | 16113 | 69 | | ps.CopyTo(signs.Slice(13, 2)); |
| | 16113 | 70 | | metric.CopyTo(data.Slice(16 * words, 3 * words)); |
| | 16113 | 71 | | ms.CopyTo(signs.Slice(16, 3)); |
| | 16113 | 72 | | scale.CopyTo(data.Slice(19 * words, words)); |
| | 16113 | 73 | | kSquared.CopyTo(data.Slice(21 * words, 3 * words)); |
| | 16113 | 74 | | kSquaredSigns.CopyTo(signs.Slice(21, 3)); |
| | 16113 | 75 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(data[..(5 * words)], signs[..5]); |
| | 16113 | 76 | | } |
| | | 77 | | |
| | | 78 | | /// <summary> |
| | | 79 | | /// Completes J=PK-L, N=J squared and D=scale squared*K squared*M once. |
| | | 80 | | /// Scratch consumes staged P before J overwrites it; D is written before |
| | | 81 | | /// N so staged M/scale can share the future N slots. |
| | | 82 | | /// </summary> |
| | | 83 | | internal static void CompleteGap(Span<ulong> data, Span<sbyte> signs) |
| | | 84 | | { |
| | 15513 | 85 | | int words = data.Length / signs.Length; |
| | 15513 | 86 | | Span<ulong> scratch = stackalloc ulong[7 * words]; |
| | 15513 | 87 | | Span<sbyte> scratchSigns = stackalloc sbyte[6]; |
| | 31026 | 88 | | scratch.Clear(); scratchSigns.Clear(); |
| | 31026 | 89 | | Span<ulong> temporary = scratch[..(5 * words)], scaleSquared = scratch.Slice(5 * words, words); |
| | 31026 | 90 | | Span<sbyte> temporarySigns = scratchSigns[..5], scaleSquaredSigns = scratchSigns.Slice(5, 1); |
| | 15513 | 91 | | Span<ulong> product = scratch.Slice(6 * words, words); |
| | 15513 | 92 | | Multiply(data.Slice(13 * words, 2 * words), signs.Slice(13, 2), |
| | 15513 | 93 | | data.Slice(8 * words, 2 * words), signs.Slice(8, 2), |
| | 15513 | 94 | | temporary[..(3 * words)], temporarySigns[..3], product); |
| | 15513 | 95 | | Add(data.Slice(10 * words, 3 * words), signs.Slice(10, 3), |
| | 15513 | 96 | | temporary[..(3 * words)], temporarySigns[..3], -1); |
| | 15513 | 97 | | temporary[..(3 * words)].CopyTo(data.Slice(13 * words, 3 * words)); |
| | 15513 | 98 | | temporarySigns[..3].CopyTo(signs.Slice(13, 3)); |
| | | 99 | | // Consume staged K squared before D overwrites its slots. |
| | 15513 | 100 | | Multiply(data.Slice(21 * words, 3 * words), signs.Slice(21, 3), |
| | 15513 | 101 | | data.Slice(16 * words, 3 * words), signs.Slice(16, 3), temporary, temporarySigns, product); |
| | 15513 | 102 | | ReadOnlySpan<ulong> scale = data.Slice(19 * words, words); |
| | 15513 | 103 | | WideArithmetic.MultiplyMagnitudes(scale, scale, scaleSquared); |
| | 15513 | 104 | | scaleSquaredSigns[0] = 1; |
| | 15513 | 105 | | Multiply(temporary, temporarySigns, scaleSquared, scaleSquaredSigns, |
| | 15513 | 106 | | data.Slice(21 * words, 5 * words), signs.Slice(21, 5), product); |
| | | 107 | | // D no longer borrows M/scale; N can overwrite those slots. |
| | 15513 | 108 | | Multiply(data.Slice(13 * words, 3 * words), signs.Slice(13, 3), |
| | 15513 | 109 | | data.Slice(13 * words, 3 * words), signs.Slice(13, 3), |
| | 15513 | 110 | | data.Slice(16 * words, 5 * words), signs.Slice(16, 5), product); |
| | 15513 | 111 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(data.Slice(16 * words, 10 * words), signs.Sli |
| | 15513 | 112 | | } |
| | | 113 | | |
| | | 114 | | private static void Term(ReadOnlySpan<ulong> a, ReadOnlySpan<sbyte> aSigns, int ai, |
| | | 115 | | ReadOnlySpan<ulong> b, ReadOnlySpan<sbyte> bSigns, int bi, |
| | | 116 | | Span<ulong> result, Span<sbyte> resultSigns, int ri, int multiplier, Span<ulong> product) |
| | | 117 | | { |
| | 161130 | 118 | | int words = product.Length; |
| | 161130 | 119 | | WideArithmetic.MultiplyMagnitudes(a.Slice(ai * words, words), b.Slice(bi * words, words), product); |
| | 161130 | 120 | | int sign = resultSigns[ri]; |
| | 161130 | 121 | | WideArithmetic.AddShiftedSignedMagnitude(product, |
| | 161130 | 122 | | aSigns[ai] * bSigns[bi] * Math.Sign(multiplier), Math.Abs(multiplier) == 2 ? 1 : 0, |
| | 161130 | 123 | | result.Slice(ri * words, words), ref sign); |
| | 161130 | 124 | | resultSigns[ri] = (sbyte)sign; |
| | 161130 | 125 | | } |
| | | 126 | | private static void Multiply(ReadOnlySpan<ulong> a, ReadOnlySpan<sbyte> aSigns, |
| | | 127 | | ReadOnlySpan<ulong> b, ReadOnlySpan<sbyte> bSigns, Span<ulong> result, |
| | | 128 | | Span<sbyte> resultSigns, Span<ulong> product) => |
| | 110391 | 129 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(a, aSigns, b, bSigns, result, resultSigns, product); |
| | | 130 | | private static void Add(ReadOnlySpan<ulong> source, ReadOnlySpan<sbyte> sourceSigns, |
| | | 131 | | Span<ulong> result, Span<sbyte> resultSigns, int multiplier) => |
| | 31626 | 132 | | WideFiniteAxisIntersection.AddFiniteAxisPolynomial(source, sourceSigns, result, resultSigns, multiplier); |
| | | 133 | | } |