| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderPairRepeatedRim.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> |
| | | 14 | | /// Recovers individual real rim points at repeated cylinder-pair values. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class CylinderPairRepeatedRim |
| | | 17 | | { |
| | | 18 | | private const int Words = 244; |
| | | 19 | | private const int Slots = 64; |
| | | 20 | | private const int Radical = 48; |
| | | 21 | | |
| | | 22 | | internal static bool TryGetZeroNormal(in CylinderPairGeometry geometry, int firstCapSign, |
| | | 23 | | int secondCapSign, int branchOrdinal, out Vector3d normal) |
| | | 24 | | { |
| | 45 | 25 | | normal = default; |
| | | 26 | | System.Diagnostics.Debug.Assert((uint)branchOrdinal < 2); |
| | | 27 | | // E needs <984 bits; the largest radial tangent-cross dot needs <2500. |
| | | 28 | | const int words = 40; |
| | 45 | 29 | | Span<ulong> data = stackalloc ulong[Slots * 2 * words]; |
| | 45 | 30 | | Span<sbyte> signs = stackalloc sbyte[Slots * 2]; |
| | 45 | 31 | | Span<ulong> products = stackalloc ulong[2 * words]; |
| | 45 | 32 | | var r = new Ring(data, signs, products, words); |
| | 45 | 33 | | Prepare(geometry, firstCapSign, secondCapSign, r); |
| | | 34 | | // b.p=-b.c cuts the first circle in at most two points. |
| | 135 | 35 | | r.Copy(8, 28); r.Copy(9, 29); r.Copy(10, 30); |
| | 90 | 36 | | r.Mul(28, 28, 39); r.Mul(1, 39, 40); |
| | 135 | 37 | | r.Mul(29, 29, 39); r.Mul(0, 39, 41); r.Add(40, 41, 31); |
| | | 38 | | // Positive: the pair owner excluded parallel axes before rim recovery. |
| | 180 | 39 | | r.Mul(3, 31, 39); r.Mul(30, 30, 40); r.Mul(11, 40, 41); r.Sub(39, 41, Radical); |
| | 54 | 40 | | if (r[Radical].Signs[0] < 0 || (r[Radical].Signs[0] == 0 && branchOrdinal != 0)) return false; |
| | 36 | 41 | | WideAxis3 c = geometry.GetCapOffset(firstCapSign, secondCapSign); |
| | 288 | 42 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 43 | | { |
| | 108 | 44 | | r.Import(39, Component(geometry.PlaneU, coordinate)); |
| | 108 | 45 | | r.Import(40, Component(geometry.PlaneW, coordinate)); |
| | 216 | 46 | | r.Mul(28, 39, 41); r.Mul(1, 41, 42); |
| | 324 | 47 | | r.Mul(29, 40, 41); r.Mul(0, 41, 43); r.Add(42, 43, 44); |
| | 216 | 48 | | r.Mul(30, 44, 33 + coordinate); r.Negate(33 + coordinate, 33 + coordinate); |
| | 324 | 49 | | r.Mul(29, 39, 41); r.Mul(28, 40, 42); r.Sub(41, 42, 43); |
| | 108 | 50 | | r.CopyOrdinaryToRadical(43, 33 + coordinate, branchOrdinal == 0 ? 1 : -1); |
| | 216 | 51 | | r.Import(39, Component(c, coordinate)); r.Mul(39, 31, 40); |
| | 216 | 52 | | r.Add(33 + coordinate, 40, 36 + coordinate); r.Negate(36 + coordinate, 36 + coordinate); |
| | | 53 | | } |
| | 36 | 54 | | DotBaseVectors(r, 36, 36, 49); |
| | 108 | 55 | | r.Mul(31, 31, 39); r.Mul(4, 39, 40); r.Sub(49, 40, 41); |
| | 41 | 56 | | if (r.Sign(41) != 0) return false; |
| | 31 | 57 | | CrossAxis(r, geometry.FirstAxis, 33, 49); |
| | 31 | 58 | | CrossAxis(r, geometry.SecondAxis, 36, 52); |
| | 31 | 59 | | CrossBaseVectors(r, 49, 52, 55); |
| | 62 | 60 | | DotBaseVectors(r, 33, 55, 58); DotBaseVectors(r, 36, 55, 59); |
| | 31 | 61 | | int orientation = r.Sign(58); |
| | 57 | 62 | | if (orientation == 0 || orientation != r.Sign(59)) return false; |
| | | 63 | | // Parallel rim tangents have zero cross product; any nontrivial |
| | | 64 | | // admitting cone then has a cap/side boundary handled earlier. |
| | 40 | 65 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | 30 | 66 | | { r.Copy(55 + coordinate, 33 + 2 * coordinate); r.Clear(34 + 2 * coordinate); } |
| | 5 | 67 | | r.Clear(32); // Only the first radical is needed at exact touch. |
| | 5 | 68 | | return TryFinishNormal(geometry, firstCapSign, secondCapSign, orientation, r, out normal); |
| | | 69 | | } |
| | | 70 | | |
| | | 71 | | /// <summary> |
| | | 72 | | /// Tests one eigenvalue/value/circle branch, numbered zero through seven. |
| | | 73 | | /// The selected normalized squared value is represented by its primitive |
| | | 74 | | /// repeated-root radical, of degree at most four and height at most 3714 bits. |
| | | 75 | | /// </summary> |
| | | 76 | | internal static bool TryGetRankOneNormal(in CylinderPairGeometry geometry, |
| | | 77 | | int firstCapSign, int secondCapSign, FiniteAxisValueRoot selectedValue, |
| | | 78 | | int branchOrdinal, out Vector3d normal) |
| | | 79 | | { |
| | 1253 | 80 | | normal = default; |
| | | 81 | | System.Diagnostics.Debug.Assert((uint)branchOrdinal < 8); |
| | | 82 | | |
| | | 83 | | // In the pair x+y*sqrt(E), use weighted height |
| | | 84 | | // max(bits(x), bits(y)+ceil(bits(E)/2)). Products add heights plus |
| | | 85 | | // one carry bit; no monic-field leading coefficient is multiplied |
| | | 86 | | // into every operation. From the exact-frame row weights |
| | | 87 | | // (33,66,229), the rational-eigenvalue rank minor has coefficient |
| | | 88 | | // bounds (2573,2115,1657) in physical S. Thus E needs <=4234 bits. |
| | | 89 | | // Rounded stage ceilings are: line 3008, D 6208, Delta 6720, |
| | | 90 | | // U 6528, H numerator 8448, local direction 15352, world direction |
| | | 91 | | // 15424, and cap half-axis dot 15584 <244*64. |
| | | 92 | | // The 64-pair arena is 249856 bytes. Root matching uses only a |
| | | 93 | | // quadratic query below 4400 bits, before rounding. Its caller's |
| | | 94 | | // degree-four repeated-root arena is below 430 KiB, keeping this |
| | | 95 | | // phase below 720 KiB. Rounding has a separate 133280-byte arena and |
| | | 96 | | // below-128-KiB nested radical-sign scratch, not a nested root solver. |
| | 1253 | 97 | | Span<ulong> data = stackalloc ulong[Slots * 2 * Words]; |
| | 1253 | 98 | | Span<sbyte> signs = stackalloc sbyte[Slots * 2]; |
| | 1253 | 99 | | Span<ulong> products = stackalloc ulong[2 * Words]; |
| | 1253 | 100 | | var ring = new Ring(data, signs, products, Words); |
| | 1253 | 101 | | Prepare(geometry, firstCapSign, secondCapSign, ring); |
| | 1253 | 102 | | if (!GetValueAndLine(ring, branchOrdinal) |
| | 1253 | 103 | | || !MatchesValue(ring, selectedValue, geometry.ValueShift)) |
| | 948 | 104 | | return false; |
| | 305 | 105 | | return GetRegularNormal(geometry, firstCapSign, secondCapSign, |
| | 305 | 106 | | branchOrdinal & 1, ring, out normal); |
| | | 107 | | } |
| | | 108 | | |
| | | 109 | | private static void Prepare(in CylinderPairGeometry geometry, int firstSign, |
| | | 110 | | int secondSign, Ring r) |
| | | 111 | | { |
| | 1298 | 112 | | Span<ulong> values = stackalloc ulong[11 * 9]; |
| | 1298 | 113 | | Span<sbyte> signs = stackalloc sbyte[11]; |
| | 1298 | 114 | | geometry.WriteInvariants(firstSign, secondSign, values, signs, 9); |
| | 31152 | 115 | | for (int i = 0; i < 11; i++) |
| | 14278 | 116 | | r.Import(i, values.Slice(i * 9, 9), signs[i]); |
| | 1298 | 117 | | r.Mul(0, 1, 11); // gh |
| | 1298 | 118 | | r.Add(5, 3, 12); // z0=c²+rho |
| | 1298 | 119 | | r.Sub(12, 4, 13); // k0=z0-tau |
| | | 120 | | // Qij=4(delta*ci*cj+tau*bi*bj). |
| | 10384 | 121 | | for (int i = 0; i < 3; i++) |
| | | 122 | | { |
| | 3894 | 123 | | int x = i == 2 ? 7 : 6; |
| | 3894 | 124 | | int y = i == 0 ? 6 : 7; |
| | 7788 | 125 | | r.Mul(x, y, 39); r.Mul(2, 39, 40); |
| | 7788 | 126 | | r.Mul(x + 2, y + 2, 39); r.Mul(4, 39, 41); |
| | 7788 | 127 | | r.Add(40, 41, 14 + i); r.Scale(14 + i, 4, 14 + i); |
| | | 128 | | } |
| | | 129 | | // li=ti0+ti1*S, d=d0+d1*S+d2*S². |
| | 7788 | 130 | | for (int i = 0; i < 2; i++) |
| | | 131 | | { |
| | 5192 | 132 | | r.Mul(2, 6 + i, 39); r.Scale(39, 2, 39); |
| | 2596 | 133 | | r.Negate(39, 18 + 2 * i); |
| | 2596 | 134 | | r.Mul(39, 13, 40); |
| | 7788 | 135 | | r.Mul(4, 10, 39); r.Mul(39, 8 + i, 41); r.Scale(41, 4, 41); |
| | 2596 | 136 | | r.Add(40, 41, 17 + 2 * i); |
| | | 137 | | } |
| | 2596 | 138 | | r.Mul(13, 13, 39); r.Mul(2, 39, 40); |
| | 3894 | 139 | | r.Mul(10, 10, 39); r.Mul(4, 39, 41); r.Scale(41, 4, 41); |
| | 1298 | 140 | | r.Add(40, 41, 21); |
| | 2596 | 141 | | r.Scale(4, 2, 39); r.Add(13, 39, 40); |
| | 3894 | 142 | | r.Mul(2, 40, 22); r.Scale(22, 2, 22); r.Negate(22, 22); |
| | 1298 | 143 | | r.Copy(2, 23); |
| | | 144 | | // C=h*c0²+g*c1², B=h*b0²+g*b1², J=h*c0*b0+g*c1*b1. |
| | 2596 | 145 | | r.Mul(6, 6, 39); r.Mul(1, 39, 40); |
| | 3894 | 146 | | r.Mul(7, 7, 39); r.Mul(0, 39, 41); r.Add(40, 41, 34); |
| | 2596 | 147 | | r.Mul(8, 8, 39); r.Mul(1, 39, 40); |
| | 3894 | 148 | | r.Mul(9, 9, 39); r.Mul(0, 39, 41); r.Add(40, 41, 35); |
| | 2596 | 149 | | r.Mul(6, 8, 39); r.Mul(1, 39, 40); |
| | 3894 | 150 | | r.Mul(7, 9, 39); r.Mul(0, 39, 41); r.Add(40, 41, 36); |
| | 2596 | 151 | | r.Mul(2, 34, 37); r.Mul(4, 35, 38); // A=delta*C, T=tau*B |
| | 1298 | 152 | | } |
| | | 153 | | |
| | | 154 | | private static bool GetValueAndLine(Ring r, int branch) |
| | | 155 | | { |
| | 1253 | 156 | | int eigenSign = (branch & 4) == 0 ? -1 : 1; |
| | 1253 | 157 | | bool valueUpper = (branch & 2) != 0; |
| | 1253 | 158 | | r.Copy(11, 25); // lambda denominator gh >0 |
| | 1253 | 159 | | bool mixedProjection = r.Sign(36) != 0; |
| | 3759 | 160 | | r.Mul(34, 35, 39); r.Mul(36, 36, 40); r.Sub(39, 40, 51); |
| | 1253 | 161 | | bool independentProjection = r.Sign(51) != 0; |
| | 1253 | 162 | | if (mixedProjection && independentProjection) |
| | | 163 | | { |
| | 16 | 164 | | if (valueUpper) |
| | 8 | 165 | | return false; |
| | | 166 | | // E=(A-T)²+4*delta*tau*J², lambda=2(A+T+eps sqrt(E))/gh. |
| | 16 | 167 | | r.Sub(37, 38, 39); r.Mul(39, 39, 40); |
| | 24 | 168 | | r.Mul(36, 36, 39); r.Mul(2, 4, 41); r.Mul(39, 41, 42); |
| | 16 | 169 | | r.Scale(42, 4, 42); r.Add(40, 42, Radical); |
| | 24 | 170 | | r.Add(37, 38, 24); r.Scale(24, 2, 24); r.SetRadical(24, 2 * eigenSign); |
| | | 171 | | // S=k0+j*(T-A+eps sqrt(E))/(delta*J). |
| | 16 | 172 | | r.Mul(2, 36, 27); r.Mul(27, 13, 39); |
| | 24 | 173 | | r.Sub(38, 37, 40); r.Mul(10, 40, 41); r.Add(39, 41, 26); |
| | 8 | 174 | | r.CopyOrdinaryToRadical(10, 26, eigenSign); |
| | 16 | 175 | | if (r.Sign(27) < 0) { r.Negate(27, 27); r.Negate(26, 26); } |
| | 8 | 176 | | BuildSpatial(r); |
| | | 177 | | } |
| | | 178 | | else |
| | | 179 | | { |
| | 1237 | 180 | | if (mixedProjection) |
| | | 181 | | { |
| | | 182 | | // Dependent projections have eigenvalues 0 and 4(A+T)/gh. |
| | | 183 | | // In particular the zero eigenvector is perpendicular to |
| | | 184 | | // both c and b: compatibility cannot be divided by c.v. |
| | 1644 | 185 | | if (eigenSign < 0) r.Clear(24); |
| | 1096 | 186 | | else { r.Add(37, 38, 24); r.Scale(24, 4, 24); } |
| | | 187 | | } |
| | 141 | 188 | | else r.Scale(eigenSign < 0 ? 37 : 38, 4, 24); |
| | 1237 | 189 | | BuildSpatial(r); |
| | 1237 | 190 | | int row = r.Sign(31) != 0 ? 0 : r.Sign(33) != 0 ? 1 : -1; |
| | 1245 | 191 | | if (row < 0) return false; // Rank zero is an earlier cap/side continuum. |
| | 1229 | 192 | | int diagonal = row == 0 ? 31 : 33; |
| | 1229 | 193 | | int own = row == 0 ? 17 : 19; |
| | 1229 | 194 | | int other = row == 0 ? 19 : 17; |
| | 3687 | 195 | | r.Mul(32, own, 39); r.Mul(diagonal, other, 40); r.Sub(39, 40, 41); |
| | 3687 | 196 | | r.Mul(32, own + 1, 39); r.Mul(diagonal, other + 1, 40); r.Sub(39, 40, 42); |
| | 1229 | 197 | | if (r.Sign(42) != 0) |
| | | 198 | | { |
| | 930 | 199 | | if (valueUpper) return false; |
| | 620 | 200 | | r.Negate(41, 26); r.Copy(42, 27); |
| | 630 | 201 | | if (r.Sign(27) < 0) { r.Negate(27, 27); r.Negate(26, 26); } |
| | | 202 | | } |
| | | 203 | | else |
| | | 204 | | { |
| | 613 | 205 | | if (r.Sign(41) != 0) return false; |
| | | 206 | | // Remaining rank-one minor: mii*(lambdaDen*d+lambdaNum*rho) |
| | | 207 | | // -(lambdaDen*li)². Its three coefficients stay rational. |
| | 1210 | 208 | | r.Mul(25, own, 43); r.Mul(25, own + 1, 44); |
| | 4840 | 209 | | for (int power = 0; power < 3; power++) |
| | | 210 | | { |
| | 1815 | 211 | | r.Mul(25, 21 + power, 39); |
| | 3025 | 212 | | if (power == 0) { r.Mul(24, 3, 40); r.Add(39, 40, 39); } |
| | 1815 | 213 | | r.Mul(diagonal, 39, 40); |
| | 1815 | 214 | | r.Mul(power == 2 ? 44 : 43, power == 0 ? 43 : 44, 39); |
| | 2420 | 215 | | if (power == 1) r.Scale(39, 2, 39); |
| | 1815 | 216 | | r.Sub(40, 39, 45 + power); |
| | | 217 | | } |
| | 617 | 218 | | if (!SolveRationalQuadratic(r, 45, 46, 47, valueUpper)) return false; |
| | | 219 | | } |
| | | 220 | | } |
| | 919 | 221 | | if (r.Sign(26) <= 0) return false; |
| | | 222 | | |
| | | 223 | | // Homogeneous M=T-lambda*C at S=N/d. Spatial M has rank one; |
| | | 224 | | // compatibility and the remaining diagonal minor certify full rank one. |
| | | 225 | | // The rational branch already rejected rank zero. In the independent |
| | | 226 | | // mixed branch, (A-T)²+4*delta*tau*J²>0 gives distinct eigenvalues, |
| | | 227 | | // hence this spatial block has rank one and a nonzero diagonal. |
| | 903 | 228 | | int selectedRow = r.Sign(31) != 0 ? 0 : 1; |
| | 5418 | 229 | | for (int i = 0; i < 2; i++) |
| | | 230 | | { |
| | 3612 | 231 | | r.Mul(17 + 2 * i, 27, 39); r.Mul(18 + 2 * i, 26, 40); |
| | 3612 | 232 | | r.Add(39, 40, 41); r.Mul(25, 41, 49 + i); // ld*d*li |
| | | 233 | | } |
| | | 234 | | #if DEBUG |
| | | 235 | | // The selected S solves compatibility. In the rational branches this |
| | | 236 | | // is the linear equation above (or both coefficients vanish); rank |
| | | 237 | | // one makes the other row dependent. The independent mixed branch's |
| | | 238 | | // explicit S solves the same null-eigenvector equation with J!=0. |
| | | 239 | | r.Mul(32, 49, 39); r.Mul(31, 50, 40); r.Sub(39, 40, 41); |
| | | 240 | | System.Diagnostics.Debug.Assert(r.Sign(41) == 0); |
| | | 241 | | r.Mul(32, 50, 39); r.Mul(33, 49, 40); r.Sub(39, 40, 41); |
| | | 242 | | System.Diagnostics.Debug.Assert(r.Sign(41) == 0); |
| | | 243 | | #endif |
| | 2709 | 244 | | r.Mul(27, 27, 43); r.Mul(26, 27, 44); r.Mul(26, 26, 45); |
| | 2709 | 245 | | r.Mul(21, 43, 39); r.Mul(22, 44, 40); r.Add(39, 40, 41); |
| | 2709 | 246 | | r.Mul(23, 45, 39); r.Add(41, 39, 41); r.Mul(25, 41, 42); |
| | 2709 | 247 | | r.Mul(24, 3, 39); r.Mul(39, 43, 40); r.Add(42, 40, 42); |
| | 903 | 248 | | int mii = selectedRow == 0 ? 31 : 33; |
| | 903 | 249 | | int ti = selectedRow == 0 ? 49 : 50; |
| | 2709 | 250 | | r.Mul(mii, 42, 39); r.Mul(ti, ti, 40); r.Sub(39, 40, 41); |
| | 915 | 251 | | if (r.Sign(41) != 0) return false; |
| | 891 | 252 | | r.Mul(selectedRow == 0 ? 31 : 32, 27, 28); |
| | 891 | 253 | | r.Mul(selectedRow == 0 ? 32 : 33, 27, 29); |
| | 891 | 254 | | r.Copy(ti, 30); |
| | 891 | 255 | | return true; |
| | | 256 | | } |
| | | 257 | | |
| | | 258 | | private static void BuildSpatial(Ring r) |
| | | 259 | | { |
| | 3735 | 260 | | r.Mul(25, 14, 39); r.Mul(24, 0, 40); r.Sub(39, 40, 31); |
| | 1245 | 261 | | r.Mul(25, 15, 32); |
| | 3735 | 262 | | r.Mul(25, 16, 39); r.Mul(24, 1, 40); r.Sub(39, 40, 33); |
| | 1245 | 263 | | } |
| | | 264 | | |
| | | 265 | | private static bool SolveRationalQuadratic(Ring r, int c, int b, int a, bool upper) |
| | | 266 | | { |
| | | 267 | | // For rank-one recovery, identically zero |
| | | 268 | | // compatibility puts projected c in range(M). In an orthonormal |
| | | 269 | | // frame M=diag(m,0), c=(u,0), b=(s,t), its off-diagonal gives st=0. |
| | | 270 | | // The quadratic coefficient is proportional to 4*delta*tau*(s²-t²), |
| | | 271 | | // nonzero because nonparallel axes give a nonzero projected b. |
| | | 272 | | System.Diagnostics.Debug.Assert(r.Sign(a) != 0); |
| | 1815 | 273 | | r.Mul(b, b, 39); r.Mul(a, c, 40); r.Scale(40, 4, 40); |
| | 605 | 274 | | r.Sub(39, 40, Radical); |
| | 617 | 275 | | if (r[Radical].Signs[0] < 0 || (r[Radical].Signs[0] == 0 && upper)) return false; |
| | 1779 | 276 | | r.Negate(b, 26); r.SetRadical(26, upper ? 1 : -1); r.Scale(a, 2, 27); |
| | 683 | 277 | | if (r.Sign(27) < 0) { r.Negate(27, 27); r.Negate(26, 26); } |
| | 593 | 278 | | return true; |
| | | 279 | | } |
| | | 280 | | |
| | | 281 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 282 | | private static bool MatchesValue(Ring r, FiniteAxisValueRoot root, int shift) |
| | | 283 | | { |
| | | 284 | | // d*2^shift*t-x = y*sqrt(E). Squaring alone would admit the other |
| | | 285 | | // eigen/value branch, so the unsquared sign is checked as well. |
| | | 286 | | const int words = 70; |
| | 891 | 287 | | Span<ulong> data = stackalloc ulong[6 * words]; |
| | 891 | 288 | | data.Clear(); // A zero signed term may skip the magnitude writer. |
| | 891 | 289 | | Span<sbyte> signs = stackalloc sbyte[6]; |
| | 891 | 290 | | Span<ulong> products = stackalloc ulong[2 * words]; |
| | 891 | 291 | | Span<ulong> e = stackalloc ulong[words]; |
| | 891 | 292 | | CopyMagnitude(r[Radical].Low, e); |
| | 1782 | 293 | | Pair n = r[26], d = r[27]; |
| | 891 | 294 | | Span<ulong> x2 = data[..words]; |
| | 891 | 295 | | Span<ulong> coefficient1 = data.Slice(words, words); |
| | 891 | 296 | | Span<ulong> coefficient2 = data.Slice(2 * words, words); |
| | 891 | 297 | | WideArithmetic.MultiplyMagnitudes(n.Low, n.Low, x2); |
| | 891 | 298 | | int constantSign = MagnitudeSign(x2); |
| | 891 | 299 | | WideArithmetic.MultiplyMagnitudes(n.High, n.High, products[..words]); |
| | 891 | 300 | | WideArithmetic.MultiplyMagnitudes(products[..words], e, products[words..]); |
| | 891 | 301 | | WideArithmetic.AddShiftedSignedMagnitude(products[words..], -MagnitudeSign(products[words..]), |
| | 891 | 302 | | 0, x2, ref constantSign); |
| | 891 | 303 | | WideArithmetic.MultiplyMagnitudes(n.Low, d.Low, products[..words]); |
| | 891 | 304 | | int linearSign = 0; |
| | 891 | 305 | | WideArithmetic.AddShiftedSignedMagnitude(products[..words], -n.Signs[0], shift + 1, |
| | 891 | 306 | | coefficient1, ref linearSign); |
| | 891 | 307 | | WideArithmetic.MultiplyMagnitudes(d.Low, d.Low, products[..words]); |
| | 891 | 308 | | int quadraticSign = 0; |
| | 891 | 309 | | WideArithmetic.AddShiftedSignedMagnitude(products[..words], 1, 2 * shift, |
| | 891 | 310 | | coefficient2, ref quadraticSign); |
| | 2673 | 311 | | signs[0] = (sbyte)constantSign; signs[1] = (sbyte)linearSign; signs[2] = (sbyte)quadraticSign; |
| | 891 | 312 | | if (WideFiniteAxisIntersection.GetSignAtFiniteValueRoot(root, data[..(3 * words)], signs[..3]) != 0) |
| | 328 | 313 | | return false; |
| | 563 | 314 | | Span<ulong> linear = data.Slice(3 * words, 2 * words); |
| | 1126 | 315 | | CopyMagnitude(n.Low, linear[..words]); signs[3] = (sbyte)-n.Signs[0]; |
| | 563 | 316 | | int denominatorSign = 0; |
| | 563 | 317 | | WideArithmetic.AddShiftedSignedMagnitude(d.Low, 1, shift, linear[words..], ref denominatorSign); |
| | 563 | 318 | | signs[4] = (sbyte)denominatorSign; |
| | 563 | 319 | | int actualSign = WideFiniteAxisIntersection.GetSignAtFiniteValueRoot(root, linear, signs.Slice(3, 2)); |
| | 563 | 320 | | return actualSign == (r[Radical].Signs[0] == 0 ? 0 : n.Signs[1]); |
| | | 321 | | } |
| | | 322 | | |
| | | 323 | | private static bool GetRegularNormal(in CylinderPairGeometry geometry, int firstSign, |
| | | 324 | | int secondSign, int circleBranch, Ring r, out Vector3d normal) |
| | | 325 | | { |
| | 305 | 326 | | normal = default; |
| | 610 | 327 | | r.Mul(28, 28, 39); r.Mul(1, 39, 40); |
| | 915 | 328 | | r.Mul(29, 29, 39); r.Mul(0, 39, 41); r.Add(40, 41, 31); // D |
| | 1220 | 329 | | r.Mul(3, 31, 39); r.Mul(30, 30, 40); r.Mul(11, 40, 41); r.Sub(39, 41, 32); // Delta |
| | 305 | 330 | | int deltaSign = r.Sign(32); |
| | 434 | 331 | | if (deltaSign < 0 || (deltaSign == 0 && circleBranch != 0)) return false; |
| | 176 | 332 | | WideAxis3 c = geometry.GetCapOffset(firstSign, secondSign); |
| | 1408 | 333 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 334 | | { |
| | 528 | 335 | | r.Import(39, Component(geometry.PlaneU, coordinate)); |
| | 528 | 336 | | r.Import(40, Component(geometry.PlaneW, coordinate)); |
| | 1056 | 337 | | r.Mul(28, 39, 41); r.Mul(1, 41, 42); |
| | 1584 | 338 | | r.Mul(29, 40, 41); r.Mul(0, 41, 43); r.Add(42, 43, 44); |
| | 1056 | 339 | | r.Mul(30, 44, 33 + 2 * coordinate); r.Negate(33 + 2 * coordinate, 33 + 2 * coordinate); |
| | 1584 | 340 | | r.Mul(29, 39, 41); r.Mul(28, 40, 42); r.Sub(41, 42, 34 + 2 * coordinate); |
| | 642 | 341 | | if (circleBranch != 0) r.Negate(34 + 2 * coordinate, 34 + 2 * coordinate); |
| | 1056 | 342 | | r.Import(39, Component(c, coordinate)); r.Mul(39, 31, 40); |
| | 528 | 343 | | r.Add(33 + 2 * coordinate, 40, 33 + 2 * coordinate); // U=cD+P |
| | | 344 | | } |
| | 176 | 345 | | Dot(r, c, 33, 43, 44); |
| | 528 | 346 | | r.Add(3, 4, 39); r.Sub(39, 5, 39); r.Mul(27, 31, 40); |
| | 528 | 347 | | r.Mul(39, 40, 41); r.Mul(26, 31, 42); r.Sub(41, 42, 28); |
| | 528 | 348 | | r.Mul(27, 43, 39); r.Scale(39, 2, 39); r.Add(28, 39, 28); |
| | 352 | 349 | | r.Mul(27, 44, 29); r.Scale(29, 2, 29); // Hn=d*D*H |
| | 176 | 350 | | int hSign = SignExtension(r[28], r[29], r[32], r[Radical].Low); |
| | 181 | 351 | | if (hSign == 0) return false; // Earlier cap/side candidates dominate H=0. |
| | 513 | 352 | | r.Mul(27, 4, 39); r.Mul(39, 31, 30); r.Scale(30, 2, 30); // 2*d*tau*D |
| | 171 | 353 | | Dot(r, geometry.SecondAxis, 33, 49, 50); |
| | 1368 | 354 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 355 | | { |
| | 1026 | 356 | | int u0 = 33 + 2 * coordinate, u1 = u0 + 1; |
| | 1026 | 357 | | int v0 = coordinate == 0 ? 0 : coordinate == 1 ? 6 : 8, v1 = v0 + 1; |
| | 1539 | 358 | | r.Mul(28, u0, 39); r.Mul(29, u1, 40); r.Mul(40, 32, 41); |
| | 1026 | 359 | | r.Add(39, 41, 42); r.Mul(2, 42, v0); |
| | 2052 | 360 | | r.Mul(28, u1, 39); r.Mul(29, u0, 40); r.Add(39, 40, 41); r.Mul(2, 41, v1); |
| | 513 | 361 | | r.Import(43, Component(geometry.SecondAxis, coordinate)); |
| | 3078 | 362 | | for (int part = 0; part < 2; part++) |
| | | 363 | | { |
| | 3078 | 364 | | r.Mul(2, u0 + part, 39); r.Mul(43, 49 + part, 40); r.Sub(39, 40, 41); |
| | 2052 | 365 | | r.Mul(30, 41, 42); r.Sub(v0 + part, 42, v0 + part); |
| | | 366 | | } |
| | | 367 | | } |
| | | 368 | | // p.v has sign [dD*lambdaNum+2delta*lambdaDen*(Hn-2d*tau*D)]/Hn. |
| | 342 | 369 | | r.Mul(27, 31, 39); r.Mul(39, 24, 40); |
| | 342 | 370 | | r.Mul(2, 25, 41); r.Scale(41, 2, 41); |
| | 513 | 371 | | r.Sub(28, 30, 39); r.Mul(41, 39, 42); r.Add(40, 42, 43); |
| | 171 | 372 | | r.Mul(41, 29, 44); |
| | 171 | 373 | | int firstRadial = SignExtension(r[43], r[44], r[32], r[Radical].Low) * hSign; |
| | 342 | 374 | | r.Sub(30, 28, 43); r.Negate(29, 44); |
| | 171 | 375 | | int secondRadial = SignExtension(r[43], r[44], r[32], r[Radical].Low); |
| | 313 | 376 | | if (firstRadial == 0 || firstRadial != secondRadial) return false; |
| | | 377 | | // Negative radial signs give a unique closest disk pair with Hessian |
| | | 378 | | // Jáµ€J+diag(-p.v,-q.v)>0. A rank-one pencil restricts to a squared |
| | | 379 | | // line on the first circle: two same-level points or fourth-order |
| | | 380 | | // tangency, neither compatible with that unique nondegenerate minimum. |
| | | 381 | | System.Diagnostics.Debug.Assert(firstRadial > 0); |
| | | 382 | | // Pack V contiguously once U is dead; cap vectors include zero half-height. |
| | 174 | 383 | | r.Copy(0, 33); r.Copy(1, 34); r.Copy(6, 35); r.Copy(7, 36); r.Copy(8, 37); r.Copy(9, 38); |
| | 29 | 384 | | return TryFinishNormal(geometry, firstSign, secondSign, hSign, r, out normal); |
| | | 385 | | } |
| | | 386 | | |
| | | 387 | | private static bool TryFinishNormal(in CylinderPairGeometry geometry, int firstSign, |
| | | 388 | | int secondSign, int orientation, Ring r, out Vector3d normal) |
| | | 389 | | { |
| | 34 | 390 | | normal = default; |
| | 34 | 391 | | Dot(r, geometry.FirstHalf, 33, 43, 44); |
| | 34 | 392 | | if (firstSign * orientation * SignExtension(r[43], r[44], r[32], r[Radical].Low) < 0) |
| | 10 | 393 | | return false; |
| | 24 | 394 | | Dot(r, geometry.SecondHalf, 33, 43, 44); |
| | 24 | 395 | | if (secondSign * orientation * SignExtension(r[43], r[44], r[32], r[Radical].Low) < 0) |
| | 7 | 396 | | return false; |
| | 17 | 397 | | Rotate(r, geometry.WorldBasis); |
| | 17 | 398 | | normal = RoundNormal(r.data.Slice(49 * 2 * r.words, 12 * r.words), |
| | 17 | 399 | | r.signs.Slice(49 * 2, 12), r.data.Slice(32 * 2 * r.words, 2 * r.words), |
| | 17 | 400 | | r[32].Signs, r[Radical].Low, orientation); |
| | 17 | 401 | | return true; |
| | | 402 | | } |
| | | 403 | | |
| | | 404 | | |
| | | 405 | | private static void DotBaseVectors(Ring r, int first, int second, int result) |
| | | 406 | | { |
| | 98 | 407 | | r.Clear(result); |
| | 784 | 408 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | 588 | 409 | | { r.Mul(first + coordinate, second + coordinate, 39); r.Add(result, 39, result); } |
| | 98 | 410 | | } |
| | | 411 | | |
| | | 412 | | private static void CrossAxis(Ring r, WideAxis3 axis, int vector, int result) |
| | | 413 | | { |
| | 496 | 414 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 415 | | { |
| | 372 | 416 | | int next = (coordinate + 1) % 3, last = (coordinate + 2) % 3; |
| | 372 | 417 | | r.Import(39, Component(axis, next)); r.Mul(39, vector + last, 40); |
| | 372 | 418 | | r.Import(39, Component(axis, last)); r.Mul(39, vector + next, 41); |
| | 186 | 419 | | r.Sub(40, 41, result + coordinate); |
| | | 420 | | } |
| | 62 | 421 | | } |
| | | 422 | | |
| | | 423 | | private static void CrossBaseVectors(Ring r, int first, int second, int result) |
| | | 424 | | { |
| | 248 | 425 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 426 | | { |
| | 186 | 427 | | int next = (coordinate + 1) % 3, last = (coordinate + 2) % 3; |
| | 186 | 428 | | r.Mul(first + next, second + last, 39); r.Mul(first + last, second + next, 40); |
| | 93 | 429 | | r.Sub(39, 40, result + coordinate); |
| | | 430 | | } |
| | 31 | 431 | | } |
| | | 432 | | |
| | | 433 | | private static void Dot(Ring r, WideAxis3 axis, int vector, int ordinary, int radical) |
| | | 434 | | { |
| | 810 | 435 | | r.Clear(ordinary); r.Clear(radical); |
| | 3240 | 436 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 437 | | { |
| | 1215 | 438 | | r.Import(45, Component(axis, coordinate)); |
| | 2430 | 439 | | r.Mul(45, vector + 2 * coordinate, 46); r.Add(ordinary, 46, ordinary); |
| | 2430 | 440 | | r.Mul(45, vector + 2 * coordinate + 1, 46); r.Add(radical, 46, radical); |
| | | 441 | | } |
| | 405 | 442 | | } |
| | | 443 | | |
| | | 444 | | private static void Rotate(Ring r, WideRationalBasis3d basis) |
| | | 445 | | { |
| | 17 | 446 | | Span<Signed192> rows = stackalloc Signed192[9] |
| | 17 | 447 | | { basis.Xx, basis.Yx, basis.Zx, basis.Xy, basis.Yy, basis.Zy, basis.Xz, basis.Yz, basis.Zz }; |
| | 136 | 448 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 449 | | { |
| | 102 | 450 | | r.Clear(49 + 2 * coordinate); r.Clear(50 + 2 * coordinate); |
| | 408 | 451 | | for (int source = 0; source < 3; source++) |
| | | 452 | | { |
| | 153 | 453 | | r.Import(45, Signed320.ExtendValue(rows[3 * coordinate + source])); |
| | 918 | 454 | | for (int part = 0; part < 2; part++) |
| | | 455 | | { |
| | 306 | 456 | | r.Mul(45, 33 + 2 * source + part, 46); |
| | 306 | 457 | | r.Add(49 + 2 * coordinate + part, 46, 49 + 2 * coordinate + part); |
| | | 458 | | } |
| | | 459 | | } |
| | | 460 | | } |
| | 17 | 461 | | } |
| | | 462 | | |
| | | 463 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 464 | | // Borrow the three world coordinates as (a+b√E)+(c+d√E)√Delta. |
| | | 465 | | // Normalization does not own or mutate the geometric recovery workspace. |
| | | 466 | | // Inputs retain the recovery's weighted-height bounds, not arbitrary |
| | | 467 | | // coefficients filling every word independently of their radicands. |
| | | 468 | | internal static Vector3d RoundNormal(Span<ulong> coordinates, Span<sbyte> coordinateSigns, |
| | | 469 | | Span<ulong> delta, Span<sbyte> deltaSigns, ReadOnlySpan<ulong> e, int orientation) |
| | | 470 | | { |
| | | 471 | | // Squaring and the 66-bit midpoint shift need two input widths plus |
| | | 472 | | // two words. This remains 490 words for the largest rank-one path. |
| | 33 | 473 | | int inputWords = coordinates.Length / 12; |
| | 33 | 474 | | int words = 2 * inputWords + 2; |
| | 33 | 475 | | Span<ulong> data = stackalloc ulong[16 * 2 * words]; |
| | 33 | 476 | | Span<sbyte> signs = stackalloc sbyte[32]; |
| | 33 | 477 | | Span<ulong> products = stackalloc ulong[2 * words]; |
| | 33 | 478 | | var r = new Ring(data, signs, products, words, 15); |
| | 33 | 479 | | r.Import(15, e, MagnitudeSign(e)); |
| | 33 | 480 | | var deltaPair = new Pair(delta, deltaSigns, inputWords); |
| | 33 | 481 | | r.Copy(deltaPair, 14); |
| | 264 | 482 | | for (int component = 0; component < 3; component++) |
| | | 483 | | { |
| | 99 | 484 | | var ordinary = new Pair(coordinates.Slice(4 * component * inputWords, 2 * inputWords), |
| | 99 | 485 | | coordinateSigns.Slice(4 * component, 2), inputWords); |
| | 99 | 486 | | var radical = new Pair(coordinates.Slice((4 * component + 2) * inputWords, 2 * inputWords), |
| | 99 | 487 | | coordinateSigns.Slice(4 * component + 2, 2), inputWords); |
| | 198 | 488 | | r.Copy(ordinary, 10); r.Copy(radical, 11); |
| | 297 | 489 | | r.Mul(10, 10, 12); r.Mul(11, 11, 13); r.Mul(13, 14, 10); |
| | 99 | 490 | | r.Add(12, 10, 2 * component); |
| | 99 | 491 | | r.Copy(ordinary, 10); |
| | 198 | 492 | | r.Mul(10, 11, 2 * component + 1); r.Scale(2 * component + 1, 2, 2 * component + 1); |
| | 198 | 493 | | r.Add(6, 2 * component, 6); r.Add(7, 2 * component + 1, 7); |
| | | 494 | | } |
| | 33 | 495 | | Span<long> result = stackalloc long[3]; |
| | 33 | 496 | | result.Clear(); |
| | 264 | 497 | | for (int component = 0; component < 3; component++) |
| | | 498 | | { |
| | 99 | 499 | | var ordinary = new Pair(coordinates.Slice(4 * component * inputWords, 2 * inputWords), |
| | 99 | 500 | | coordinateSigns.Slice(4 * component, 2), inputWords); |
| | 99 | 501 | | var radical = new Pair(coordinates.Slice((4 * component + 2) * inputWords, 2 * inputWords), |
| | 99 | 502 | | coordinateSigns.Slice(4 * component + 2, 2), inputWords); |
| | 99 | 503 | | int sign = SignExtension(ordinary, radical, deltaPair, e) * orientation; |
| | 99 | 504 | | if (sign == 0) continue; |
| | 132 | 505 | | ulong lower = 0, upper = 1UL << 32; |
| | 2197 | 506 | | while (lower < upper) |
| | | 507 | | { |
| | 2131 | 508 | | ulong middle = lower + (upper - lower + 1) / 2; |
| | 3249 | 509 | | if (CompareNormal(r, component, 2 * middle) >= 0) lower = middle; |
| | 1013 | 510 | | else upper = middle - 1; |
| | | 511 | | } |
| | 66 | 512 | | int midpoint = CompareNormal(r, component, 2 * lower + 1); |
| | 113 | 513 | | if (midpoint > 0 || (midpoint == 0 && (lower & 1) != 0)) lower++; |
| | 66 | 514 | | result[component] = sign * (long)lower; |
| | | 515 | | } |
| | 33 | 516 | | return new Vector3d(Fixed64.FromRaw(result[0]), Fixed64.FromRaw(result[1]), Fixed64.FromRaw(result[2])); |
| | | 517 | | } |
| | | 518 | | |
| | | 519 | | private static int CompareNormal(Ring r, int component, ulong twiceRaw) |
| | | 520 | | { |
| | 13182 | 521 | | for (int part = 0; part < 2; part++) |
| | | 522 | | { |
| | 8788 | 523 | | r.Scale(6 + part, twiceRaw, 10); r.Scale(10, twiceRaw, 10); |
| | 8788 | 524 | | r.Shift(2 * component + part, 66, 11); r.Sub(11, 10, 8 + part); |
| | | 525 | | } |
| | 2197 | 526 | | return SignExtension(r[8], r[9], r[14], r[15].Low); |
| | | 527 | | } |
| | | 528 | | |
| | | 529 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 530 | | private static int SignExtension(Pair ordinary, Pair radical, Pair delta, ReadOnlySpan<ulong> e) |
| | | 531 | | { |
| | 5744 | 532 | | int u = SignPair(ordinary, e), v = SignPair(radical, e); |
| | 5506 | 533 | | if (v == 0 || SignPair(delta, e) == 0) return u; |
| | 354 | 534 | | if (u == 0 || u == v) return v; |
| | 122 | 535 | | int words = 2 * Math.Max(ordinary.Low.Length, radical.Low.Length); |
| | 122 | 536 | | Span<ulong> data = stackalloc ulong[4 * 2 * words]; |
| | 122 | 537 | | Span<sbyte> signs = stackalloc sbyte[8]; |
| | 122 | 538 | | Span<ulong> products = stackalloc ulong[2 * words]; |
| | 122 | 539 | | var r = new Ring(data, signs, products, words, 3); |
| | 122 | 540 | | r.Import(3, e, MagnitudeSign(e)); |
| | 244 | 541 | | r.Mul(ordinary, ordinary, 0); r.Mul(radical, radical, 1); |
| | 244 | 542 | | r.Mul(r[1], delta, 2); r.Sub(0, 2, 1); |
| | 122 | 543 | | return u * r.Sign(1); |
| | | 544 | | } |
| | | 545 | | |
| | | 546 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 547 | | private static int SignPair(Pair value, ReadOnlySpan<ulong> e) => |
| | 16263 | 548 | | WideConvexPrismRelations.GetConvexContactCandidateQuadraticSign( |
| | 16263 | 549 | | value.Low, value.Signs[0], value.High, value.Signs[1], e); |
| | | 550 | | |
| | | 551 | | private static Signed320 Component(WideAxis3 value, int coordinate) => |
| | 4008 | 552 | | coordinate == 0 ? value.X : coordinate == 1 ? value.Y : value.Z; |
| | | 553 | | |
| | | 554 | | private static int MagnitudeSign(ReadOnlySpan<ulong> value) => |
| | 1937 | 555 | | WideArithmetic.GetActiveMagnitudeLength(value) == 0 ? 0 : 1; |
| | | 556 | | |
| | | 557 | | private static void CopyMagnitude(ReadOnlySpan<ulong> source, Span<ulong> destination) |
| | | 558 | | { |
| | 129041 | 559 | | int count = Math.Min(source.Length, destination.Length); |
| | 129041 | 560 | | source[..count].CopyTo(destination); |
| | 129041 | 561 | | destination[count..].Clear(); |
| | 129041 | 562 | | } |
| | | 563 | | |
| | | 564 | | private readonly ref struct Pair |
| | | 565 | | { |
| | | 566 | | internal readonly Span<ulong> Low; |
| | | 567 | | internal readonly Span<ulong> High; |
| | | 568 | | internal readonly Span<sbyte> Signs; |
| | | 569 | | internal Pair(Span<ulong> data, Span<sbyte> signs, int words) |
| | 3436604 | 570 | | { Low = data[..words]; High = data.Slice(words, words); Signs = signs; } |
| | | 571 | | } |
| | | 572 | | |
| | | 573 | | // Focused two-coefficient arithmetic for the two cylinder recovery radicals. |
| | | 574 | | // No inversion, field normalization, polynomial framework, or heap storage. |
| | | 575 | | private readonly ref struct Ring |
| | | 576 | | { |
| | | 577 | | internal readonly Span<ulong> data; |
| | | 578 | | internal readonly Span<sbyte> signs; |
| | | 579 | | private readonly Span<ulong> first; |
| | | 580 | | private readonly Span<ulong> second; |
| | | 581 | | internal readonly int words; |
| | | 582 | | private readonly int e; |
| | | 583 | | |
| | | 584 | | internal Ring(Span<ulong> data, Span<sbyte> signs, Span<ulong> products, int words, int e = Radical) |
| | | 585 | | { |
| | 5812 | 586 | | this.data = data; this.signs = signs; this.words = words; this.e = e; |
| | 2906 | 587 | | first = products[..words]; second = products.Slice(words, words); |
| | 2906 | 588 | | data.Clear(); signs.Clear(); |
| | 1453 | 589 | | } |
| | | 590 | | |
| | 858722 | 591 | | internal Pair this[int index] => new(data.Slice(index * 2 * words, 2 * words), signs.Slice(index * 2, 2), words) |
| | 100860 | 592 | | internal void Clear(int index) { Pair p = this[index]; p.Low.Clear(); p.High.Clear(); p.Signs.Clear(); } |
| | 10278 | 593 | | internal int Sign(int index) => SignPair(this[index], this[e].Low); |
| | 10320 | 594 | | internal void Copy(int source, int target) => Copy(this[source], target); |
| | | 595 | | internal void Copy(Pair source, int target) |
| | | 596 | | { |
| | 31950 | 597 | | Pair p = this[target]; CopyMagnitude(source.Low, p.Low); CopyMagnitude(source.High, p.High); |
| | 10650 | 598 | | source.Signs.CopyTo(p.Signs); |
| | 10650 | 599 | | } |
| | | 600 | | internal void Negate(int source, int target) |
| | 31220 | 601 | | { Copy(source, target); Pair p = this[target]; p.Signs[0] *= -1; p.Signs[1] *= -1; } |
| | | 602 | | internal void Import(int target, ReadOnlySpan<ulong> magnitude, int sign) |
| | 72165 | 603 | | { Clear(target); Pair p = this[target]; CopyMagnitude(magnitude, p.Low); p.Signs[0] = (sbyte)sign; } |
| | | 604 | | internal void Import(int target, Signed320 value) |
| | | 605 | | { |
| | 8322 | 606 | | Clear(target); Pair p = this[target]; |
| | 4161 | 607 | | WideArithmetic.GetMagnitude(value, out p.Low[4], out p.Low[3], out p.Low[2], out p.Low[1], out p.Low[0]); |
| | 4161 | 608 | | p.Signs[0] = (sbyte)value.Sign; |
| | 4161 | 609 | | } |
| | | 610 | | internal void SetRadical(int target, int value) |
| | 3005 | 611 | | { Pair p = this[target]; p.High.Clear(); p.High[0] = (ulong)Math.Abs(value); p.Signs[1] = (sbyte)Math.Sign(value |
| | | 612 | | internal void CopyOrdinaryToRadical(int source, int target, int sign) |
| | 464 | 613 | | { Pair p = this[target]; CopyMagnitude(this[source].Low, p.High); p.Signs[1] = (sbyte)(this[source].Signs[0] * s |
| | 26468 | 614 | | internal void Add(int x, int y, int target) => Sum(x, y, target, 1); |
| | 19401 | 615 | | internal void Sub(int x, int y, int target) => Sum(x, y, target, -1); |
| | | 616 | | private void Sum(int x, int y, int target, int sign) |
| | | 617 | | { |
| | 137607 | 618 | | Pair a = this[x], b = this[y], p = this[target]; |
| | 91738 | 619 | | CopyMagnitude(a.Low, first); int lowSign = a.Signs[0]; |
| | 45869 | 620 | | WideArithmetic.AddShiftedSignedMagnitude(b.Low, sign * b.Signs[0], 0, first, ref lowSign); |
| | 91738 | 621 | | CopyMagnitude(a.High, second); int highSign = a.Signs[1]; |
| | 45869 | 622 | | WideArithmetic.AddShiftedSignedMagnitude(b.High, sign * b.Signs[1], 0, second, ref highSign); |
| | 183476 | 623 | | first.CopyTo(p.Low); second.CopyTo(p.High); p.Signs[0] = (sbyte)lowSign; p.Signs[1] = (sbyte)highSign; |
| | 45869 | 624 | | } |
| | 111566 | 625 | | internal void Mul(int x, int y, int target) => Mul(this[x], this[y], target); |
| | | 626 | | internal void Mul(Pair a, Pair b, int target) |
| | | 627 | | { |
| | 111932 | 628 | | Pair p = this[target]; |
| | 111932 | 629 | | WideArithmetic.MultiplyMagnitudes(a.High, b.High, first); |
| | 111932 | 630 | | WideArithmetic.MultiplyMagnitudes(first, this[e].Low, second); |
| | 111932 | 631 | | WideArithmetic.MultiplyMagnitudes(a.Low, b.Low, p.Low); |
| | 111932 | 632 | | int lowSign = a.Signs[0] * b.Signs[0]; |
| | 111932 | 633 | | WideArithmetic.AddShiftedSignedMagnitude(second, a.Signs[1] * b.Signs[1] * this[e].Signs[0], 0, p.Low, ref l |
| | 111932 | 634 | | WideArithmetic.MultiplyMagnitudes(a.Low, b.High, first); |
| | 111932 | 635 | | int highSign = a.Signs[0] * b.Signs[1]; |
| | 111932 | 636 | | WideArithmetic.MultiplyMagnitudes(a.High, b.Low, second); |
| | 111932 | 637 | | WideArithmetic.AddShiftedSignedMagnitude(second, a.Signs[1] * b.Signs[0], 0, first, ref highSign); |
| | 335796 | 638 | | first.CopyTo(p.High); p.Signs[0] = (sbyte)lowSign; p.Signs[1] = (sbyte)highSign; |
| | 111932 | 639 | | } |
| | | 640 | | internal void Scale(int source, ulong factor, int target) |
| | | 641 | | { |
| | | 642 | | // Callers use positive constants or positive normal midpoints. |
| | | 643 | | System.Diagnostics.Debug.Assert(factor > 0); |
| | 25069 | 644 | | Span<ulong> scalar = stackalloc ulong[1] { factor }; |
| | 50138 | 645 | | Pair a = this[source], p = this[target]; |
| | 25069 | 646 | | WideArithmetic.MultiplyMagnitudes(a.Low, scalar, first); |
| | 25069 | 647 | | WideArithmetic.MultiplyMagnitudes(a.High, scalar, second); |
| | 50138 | 648 | | first.CopyTo(p.Low); second.CopyTo(p.High); |
| | 50138 | 649 | | p.Signs[0] = a.Signs[0]; p.Signs[1] = a.Signs[1]; |
| | 25069 | 650 | | } |
| | | 651 | | internal void Shift(int source, int shift, int target) |
| | | 652 | | { |
| | 17576 | 653 | | Pair a = this[source], p = this[target]; first.Clear(); second.Clear(); |
| | 8788 | 654 | | int lowSign = 0, highSign = 0; |
| | 4394 | 655 | | WideArithmetic.AddShiftedSignedMagnitude(a.Low, a.Signs[0], shift, first, ref lowSign); |
| | 4394 | 656 | | WideArithmetic.AddShiftedSignedMagnitude(a.High, a.Signs[1], shift, second, ref highSign); |
| | 17576 | 657 | | first.CopyTo(p.Low); second.CopyTo(p.High); p.Signs[0] = (sbyte)lowSign; p.Signs[1] = (sbyte)highSign; |
| | 4394 | 658 | | } |
| | | 659 | | } |
| | | 660 | | } |