| | | 1 | | //======================================================================= |
| | | 2 | | // BoxCylinderEdgeContacts.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 | | /// Exact box-edge/cylinder-rim support minima. Signed box-coordinate charts |
| | | 15 | | /// preserve each edge's normal cone instead of reflecting a projected minimum |
| | | 16 | | /// into a different box feature. |
| | | 17 | | /// </summary> |
| | | 18 | | internal static class BoxCylinderEdgeContacts |
| | | 19 | | { |
| | | 20 | | private const int Words = 40; |
| | | 21 | | private const int ValueWords = 56; |
| | | 22 | | private const int ParameterSlots = 28; |
| | | 23 | | private const int ValueCellWords = (16 * (ValueWords * 64 + 11) + 255) / 64; |
| | | 24 | | |
| | | 25 | | private struct Selection |
| | | 26 | | { |
| | | 27 | | internal bool HasValue, IsZero, IsRational; |
| | | 28 | | internal int Edge, First, Second, FirstSign, SecondSign, CapSign, ParameterOrdinal; |
| | | 29 | | internal int ValueOrdinal, ValueShift; |
| | | 30 | | } |
| | | 31 | | |
| | | 32 | | /// <summary> |
| | | 33 | | /// AnalyticBest already has a nonnegative exact gap. Returns false on a |
| | | 34 | | /// certified separating root. Exact ties retain the earlier analytic or |
| | | 35 | | /// edge candidate. Principal and zero-radial directions belong to the caller. |
| | | 36 | | /// </summary> |
| | | 37 | | internal static bool TryGetContact(in BoxCylinderGeometry geometry, |
| | | 38 | | in ConvexContactCandidate analyticBest, Fixed64 analyticDepth, bool analyticClamped, out bool hasBetter, |
| | | 39 | | out Vector3d normal, out Fixed64 depth, out bool depthIsClamped, |
| | | 40 | | out Vector3d localSupportSigns) |
| | | 41 | | { |
| | 1157 | 42 | | hasBetter = false; |
| | 1157 | 43 | | normal = localSupportSigns = default; |
| | 1157 | 44 | | depth = default; |
| | 1157 | 45 | | depthIsClamped = false; |
| | 1157 | 46 | | Span<ulong> bestValues = stackalloc ulong[5 * ValueWords]; |
| | 1157 | 47 | | Span<sbyte> bestSigns = stackalloc sbyte[5]; |
| | 1157 | 48 | | Span<ulong> bestCell = stackalloc ulong[ValueCellWords]; |
| | 1157 | 49 | | Selection best = default; |
| | 9244 | 50 | | for (int edge = 0; edge < 3; edge++) |
| | | 51 | | { |
| | 6934 | 52 | | int first = (edge + 1) % 3, second = (edge + 2) % 3; |
| | | 53 | | // A cylinder parallel to this edge projects to a circle. Its |
| | | 54 | | // principal radial direction is already an analytic candidate. |
| | 3467 | 55 | | if (Component(geometry.Axis, first).IsZero && Component(geometry.Axis, second).IsZero) |
| | | 56 | | continue; |
| | | 57 | | // A perpendicular cylinder projects to a rectangle, not an |
| | | 58 | | // ellipse. Together with the projected box its support is linear |
| | | 59 | | // between box-face, cap-pole and side-cross normals. A positive |
| | | 60 | | // minimum lies at those boundaries; a negative interior value |
| | | 61 | | // implies a negative boundary value. The analytic owner tests all. |
| | 2427 | 62 | | if (Component(geometry.Axis, edge).IsZero) |
| | | 63 | | continue; |
| | 1844 | 64 | | for (int firstSign = -1; firstSign <= 1; firstSign += 2) |
| | 3692 | 65 | | for (int secondSign = -1; secondSign <= 1; secondSign += 2) |
| | 7388 | 66 | | for (int cap = -1; cap <= 1; cap += 2) |
| | | 67 | | { |
| | 2464 | 68 | | if (BoxCylinderAnalyticFeatures.IsEdgeConeDominated(geometry, |
| | 2464 | 69 | | edge, first, second, firstSign, secondSign, cap, analyticBest)) |
| | | 70 | | continue; |
| | 10190 | 71 | | for (int chart = 0; chart < 2; chart++) |
| | | 72 | | { |
| | 3398 | 73 | | int u = chart == 0 ? first : second; |
| | 3398 | 74 | | int v = chart == 0 ? second : first; |
| | 3398 | 75 | | int su = chart == 0 ? firstSign : secondSign; |
| | 3398 | 76 | | int sv = chart == 0 ? secondSign : firstSign; |
| | 3398 | 77 | | if (!TryChart(geometry, edge, u, v, su, sv, cap, analyticBest, |
| | 3398 | 78 | | ((ulong)analyticDepth.m_rawValue << 1) | 1UL, !analyticClamped, |
| | 3398 | 79 | | bestValues, bestSigns, bestCell, ref best)) |
| | 2 | 80 | | return false; |
| | | 81 | | } |
| | | 82 | | } |
| | | 83 | | } |
| | 1155 | 84 | | if (!best.HasValue) |
| | 1121 | 85 | | return true; |
| | 34 | 86 | | hasBetter = true; |
| | 34 | 87 | | normal = GetNormal(geometry, best); |
| | 34 | 88 | | localSupportSigns = new Vector3d( |
| | 34 | 89 | | SupportSign(best, 0), SupportSign(best, 1), SupportSign(best, 2)); |
| | 34 | 90 | | if (!best.IsZero) |
| | | 91 | | { |
| | 30 | 92 | | FiniteAxisValueRoot root = RestoreValueRoot(bestValues, bestSigns, bestCell, best); |
| | 30 | 93 | | ConvexContactValueRoot.GetRoundedDepth(geometry.RawScale, geometry.ValueShift, |
| | 30 | 94 | | ref root, out depth, out depthIsClamped); |
| | | 95 | | } |
| | 34 | 96 | | return true; |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 100 | | private static bool TryChart(in BoxCylinderGeometry geometry, int edge, int first, int second, |
| | | 101 | | int firstSign, int secondSign, int capSign, scoped ConvexContactCandidate analytic, |
| | | 102 | | ulong upperTwiceRaw, bool hasUpperBound, |
| | | 103 | | scoped Span<ulong> bestValues, scoped Span<sbyte> bestSigns, scoped Span<ulong> bestCell, |
| | | 104 | | ref Selection best) |
| | | 105 | | { |
| | 3398 | 106 | | Signed320 au = Signed(Component(geometry.Axis, first), firstSign); |
| | 3398 | 107 | | Signed320 av = Signed(Component(geometry.Axis, second), secondSign); |
| | 3398 | 108 | | if (au.Sign * capSign < 0 |
| | 3398 | 109 | | && WideArithmetic.AddSigned320(au, av).Sign * capSign <= 0) |
| | 1276 | 110 | | return true; |
| | | 111 | | |
| | 2122 | 112 | | Span<ulong> data = stackalloc ulong[ParameterSlots * Words]; |
| | 2122 | 113 | | Span<sbyte> signs = stackalloc sbyte[ParameterSlots]; |
| | 2122 | 114 | | BuildParameter(geometry, edge, first, second, firstSign, secondSign, capSign, data, signs); |
| | 2122 | 115 | | Polynomial storage = new(data, signs, Words); |
| | 2122 | 116 | | Polynomial stationary = storage.Slice(0, 5); |
| | 2122 | 117 | | Polynomial k = storage.Slice(8, 2); |
| | 4244 | 118 | | Polynomial l = storage.Slice(10, 3), signedGap = storage.Slice(13, 3); |
| | 4244 | 119 | | Polynomial numerator = storage.Slice(16, 5), denominator = storage.Slice(21, 5); |
| | 2122 | 120 | | Polynomial cap = storage.Slice(26, 2); |
| | 2122 | 121 | | Span<ulong> batchCells = stackalloc ulong[64]; |
| | 2122 | 122 | | Span<int> batchShifts = stackalloc int[8]; |
| | 2122 | 123 | | FiniteAxisValueRoots roots = WideFiniteAxisIntersection.GetFiniteValueRoots( |
| | 2122 | 124 | | stationary.Values, stationary.Signs, batchCells, batchShifts); |
| | 2122 | 125 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords( |
| | 2122 | 126 | | stationary.Values, stationary.Signs)]; |
| | 2122 | 127 | | Span<ulong> values = stackalloc ulong[5 * ValueWords]; |
| | 2122 | 128 | | Span<sbyte> valueSigns = stackalloc sbyte[5]; |
| | 2122 | 129 | | Span<ulong> valueCell = stackalloc ulong[ValueCellWords]; |
| | 2122 | 130 | | bool valuesReady = false; |
| | 7664 | 131 | | for (int ordinal = 0; ordinal < roots.Count; ordinal++) |
| | | 132 | | { |
| | 1712 | 133 | | scoped FiniteAxisValueRoot root = FiniteAxisValueRoots.GetRoot( |
| | 1712 | 134 | | roots, ordinal, stationary.Values, stationary.Signs, cell); |
| | 1712 | 135 | | if (Sign(ref root, cap) < 0) |
| | | 136 | | continue; |
| | 1333 | 137 | | int kSign = Sign(ref root, k); |
| | | 138 | | // K=0 has the rational direction (p,q), already enumerated by |
| | | 139 | | // the analytic owner. The nonparallel/nonperpendicular dispatch |
| | | 140 | | // above proves this projected ellipse has Q>0 on the whole chart. |
| | 1333 | 141 | | if (kSign == 0 || Sign(ref root, l) != -kSign) |
| | | 142 | | continue; |
| | 732 | 143 | | int gapSign = Sign(ref root, signedGap) * kSign; |
| | 732 | 144 | | if (gapSign < 0) |
| | 2 | 145 | | return false; |
| | 730 | 146 | | if (best.IsZero || analytic.GapSign == 0) |
| | | 147 | | continue; |
| | 728 | 148 | | if (gapSign == 0) |
| | | 149 | | { |
| | 4 | 150 | | best = CreateSelection(edge, first, second, firstSign, secondSign, capSign, ordinal); |
| | 4 | 151 | | best.IsZero = true; |
| | 4 | 152 | | continue; |
| | | 153 | | } |
| | | 154 | | // An unclamped nearest-even depth d bounds the analytic gap by |
| | | 155 | | // d+1/2 raw. A positive edge at or above that bound cannot win; |
| | | 156 | | // ties retain the analytic feature. Admit negative/zero gaps first. |
| | 724 | 157 | | if (hasUpperBound && ConvexContactValueRoot.CompareSquaredGapToTwiceRaw(ref root, |
| | 724 | 158 | | numerator.Values, numerator.Signs, denominator.Values, denominator.Signs, upperTwiceRaw) >= 0) |
| | | 159 | | continue; |
| | 54 | 160 | | if (!valuesReady) |
| | | 161 | | { |
| | 50 | 162 | | BuildValues(geometry, edge, first, second, firstSign, secondSign, capSign, values, valueSigns); |
| | 50 | 163 | | valuesReady = true; |
| | | 164 | | } |
| | 54 | 165 | | scoped FiniteAxisValueRoot value = ConvexContactValueRoot.MapSquaredValue( |
| | 54 | 166 | | geometry.RawScale, geometry.ValueShift, ref root, numerator.Values, numerator.Signs, |
| | 54 | 167 | | denominator.Values, denominator.Signs, values, valueSigns, valueCell); |
| | 54 | 168 | | int comparison = best.HasValue |
| | 54 | 169 | | ? WideFiniteAxisIntersection.CompareFiniteValueRoots(value, |
| | 54 | 170 | | RestoreValueRoot(bestValues, bestSigns, bestCell, best)) |
| | 54 | 171 | | : ConvexContactValueRoot.CompareRootSquared(geometry.RawScale, geometry.ValueShift, ref value, analytic) |
| | 54 | 172 | | if (comparison >= 0) |
| | | 173 | | continue; |
| | 33 | 174 | | values.CopyTo(bestValues); |
| | 33 | 175 | | valueSigns.CopyTo(bestSigns); |
| | 33 | 176 | | valueCell.CopyTo(bestCell); |
| | 33 | 177 | | best = CreateSelection(edge, first, second, firstSign, secondSign, capSign, ordinal); |
| | 33 | 178 | | best.ValueOrdinal = value.Ordinal; |
| | 33 | 179 | | best.ValueShift = value.DenominatorShift; |
| | 33 | 180 | | best.IsRational = value.IsRational; |
| | | 181 | | } |
| | 2120 | 182 | | return true; |
| | | 183 | | } |
| | | 184 | | |
| | | 185 | | private static Selection CreateSelection(int edge, int first, int second, |
| | 37 | 186 | | int firstSign, int secondSign, int capSign, int ordinal) => new() |
| | 37 | 187 | | { |
| | 37 | 188 | | HasValue = true, Edge = edge, First = first, Second = second, |
| | 37 | 189 | | FirstSign = firstSign, SecondSign = secondSign, CapSign = capSign, ParameterOrdinal = ordinal |
| | 37 | 190 | | }; |
| | | 191 | | |
| | | 192 | | private static FiniteAxisValueRoot RestoreValueRoot(ReadOnlySpan<ulong> values, |
| | 34 | 193 | | ReadOnlySpan<sbyte> signs, Span<ulong> cell, Selection selection) => new() |
| | 34 | 194 | | { |
| | 34 | 195 | | Coefficients = values, Signs = signs, LowerNumerator = cell, |
| | 34 | 196 | | DenominatorShift = selection.ValueShift, Ordinal = selection.ValueOrdinal, |
| | 34 | 197 | | IsRational = selection.IsRational |
| | 34 | 198 | | }; |
| | | 199 | | |
| | | 200 | | /// <summary> |
| | | 201 | | /// W=L²-K²Q, with P=p+qt, Q=A+2Bt+Ct², K=q-pt and |
| | | 202 | | /// L=B+(C-A)t-Bt². At an admitted K!=0 root, sqrt(Q)=-L/K, |
| | | 203 | | /// so the signed gap is (PK-L)/(D K sqrt(1+t²)). |
| | | 204 | | /// </summary> |
| | | 205 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 206 | | private static void BuildParameter(in BoxCylinderGeometry geometry, int edge, int first, int second, |
| | | 207 | | int firstSign, int secondSign, int capSign, Span<ulong> data, Span<sbyte> signs) |
| | | 208 | | { |
| | | 209 | | // |a_i|<2^170, |c_i|<2^237, R<2^235, S<2^170 imply |
| | | 210 | | // U<2^342, |p|,|q|<2^579. Q<2^1157 and W,N<2^2340; |
| | | 211 | | // the denominator coefficients are <2^2190. Forty words (2560 bits) |
| | | 212 | | // include every coefficient-sum carry, before positive normalization. |
| | 4312 | 213 | | data.Clear(); signs.Clear(); |
| | 2156 | 214 | | Polynomial storage = new(data, signs, Words); |
| | 2156 | 215 | | Polynomial radial = storage.Slice(5, 3); |
| | 2156 | 216 | | Polynomial cap = storage.Slice(26, 2); |
| | 2156 | 217 | | Span<ulong> scratch = stackalloc ulong[24 * Words]; |
| | 2156 | 218 | | Span<sbyte> scratchSigns = stackalloc sbyte[24]; |
| | 4312 | 219 | | scratch.Clear(); scratchSigns.Clear(); |
| | 2156 | 220 | | Polynomial temp = new(scratch, scratchSigns, Words); |
| | 6468 | 221 | | Polynomial p = temp.Slice(0, 2), u = temp.Slice(2, 1), radius = temp.Slice(3, 1); |
| | 6468 | 222 | | Polynomial factor = temp.Slice(4, 1), axis = temp.Slice(5, 2), axisSquared = temp.Slice(7, 3); |
| | 4312 | 223 | | Polynomial one = temp.Slice(10, 3), temporary = temp.Slice(13, 5); |
| | 4312 | 224 | | Polynomial scalar = temp.Slice(21, 1), scale = temp.Slice(22, 1); |
| | 2156 | 225 | | Span<ulong> product = scratch.Slice(23 * Words, Words); |
| | 2156 | 226 | | Signed320 au = Signed(Component(geometry.Axis, first), firstSign); |
| | 2156 | 227 | | Signed320 av = Signed(Component(geometry.Axis, second), secondSign); |
| | 2156 | 228 | | WideAxis3 offset = GetOffset(geometry, edge, first, second, firstSign, secondSign, capSign); |
| | 2156 | 229 | | Write(u, 0, geometry.Axis.SquaredLength); |
| | 2156 | 230 | | Write(radius, 0, WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius)); |
| | 2156 | 231 | | Multiply(factor, radius, u, product); |
| | 2156 | 232 | | Write(axis, 0, Signed576.ExtendValue(au)); |
| | 2156 | 233 | | Write(axis, 1, Signed576.ExtendValue(av)); |
| | 2156 | 234 | | Add(cap, axis, capSign); |
| | 2156 | 235 | | one.Values[0] = one.Values[2 * Words] = 1; |
| | 2156 | 236 | | one.Signs[0] = one.Signs[2] = 1; |
| | 2156 | 237 | | Multiply(radial, u, one, product); |
| | 2156 | 238 | | Multiply(axisSquared, axis, axis, product); |
| | 2156 | 239 | | Add(radial, axisSquared, -1); |
| | 2156 | 240 | | Multiply(temporary.Slice(0, 3), radial, factor, product); |
| | 2156 | 241 | | Write(axis, 0, Signed576.ExtendValue(Signed(Component(offset, first), firstSign))); |
| | 2156 | 242 | | Write(axis, 1, Signed576.ExtendValue(Signed(Component(offset, second), secondSign))); |
| | 2156 | 243 | | Multiply(p, u, axis, product); |
| | 2156 | 244 | | Write(scale, 0, Signed576.ExtendValue(Signed320.ExtendValue(geometry.RawScale))); |
| | 2156 | 245 | | Multiply(scalar, u, scale, product); |
| | 2156 | 246 | | CylinderEdgeContactPolynomial.Build(p.Values, p.Signs, |
| | 2156 | 247 | | temporary.Slice(0, 3).Values, temporary.Slice(0, 3).Signs, |
| | 2156 | 248 | | one.Values, one.Signs, scalar.Values, data[..(26 * Words)], signs[..26]); |
| | 2156 | 249 | | } |
| | | 250 | | |
| | | 251 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 252 | | private static void BuildValues(in BoxCylinderGeometry geometry, int edge, int first, int second, |
| | | 253 | | int firstSign, int secondSign, int capSign, Span<ulong> values, Span<sbyte> signs) |
| | | 254 | | { |
| | 50 | 255 | | WideAxis3 c = GetOffset(geometry, edge, first, second, firstSign, secondSign, capSign); |
| | 50 | 256 | | Span<ulong> invariants = stackalloc ulong[8 * ValueWords]; |
| | 50 | 257 | | Span<sbyte> invariantSigns = stackalloc sbyte[8]; |
| | 100 | 258 | | invariants.Clear(); invariantSigns.Clear(); |
| | 50 | 259 | | Polynomial scalars = new(invariants, invariantSigns, ValueWords); |
| | 50 | 260 | | Write(scalars, 0, geometry.Axis.SquaredLength); |
| | 50 | 261 | | Write(scalars, 1, Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Signed(1)))); |
| | 50 | 262 | | Write(scalars, 2, Signed576.ExtendValue(Component(geometry.Axis, edge))); |
| | 50 | 263 | | Write(scalars, 3, c.SquaredLength); |
| | 50 | 264 | | Write(scalars, 4, WideAxis3.Dot(geometry.Axis, c)); |
| | | 265 | | // c's edge coordinate is zero: y=b.c=0, and tau=0. |
| | 50 | 266 | | Write(scalars, 6, WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius)); |
| | 50 | 267 | | CylinderPairSideValuePolynomial.BuildUnshifted(invariants, invariantSigns, ValueWords, values, signs); |
| | | 268 | | // Weighted height after z=2^ValueShift*t is <3310 bits, including |
| | | 269 | | // ValueShift<=480 and |c_i|<2^237. Fifty-six words provide 3584 bits. |
| | 50 | 270 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(values, 5, geometry.ValueShift); |
| | 50 | 271 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(values, signs); |
| | 50 | 272 | | } |
| | | 273 | | |
| | | 274 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 275 | | private static Vector3d GetNormal(in BoxCylinderGeometry geometry, Selection selection) |
| | | 276 | | { |
| | 34 | 277 | | Span<ulong> data = stackalloc ulong[ParameterSlots * Words]; |
| | 34 | 278 | | Span<sbyte> signs = stackalloc sbyte[ParameterSlots]; |
| | 34 | 279 | | BuildParameter(geometry, selection.Edge, selection.First, selection.Second, |
| | 34 | 280 | | selection.FirstSign, selection.SecondSign, selection.CapSign, data, signs); |
| | 34 | 281 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords( |
| | 34 | 282 | | data[..(5 * Words)], signs[..5])]; |
| | 34 | 283 | | bool found = WideFiniteAxisIntersection.TryGetFiniteValueRoot(data[..(5 * Words)], signs[..5], |
| | 34 | 284 | | selection.ParameterOrdinal, cell, out FiniteAxisValueRoot root); |
| | | 285 | | System.Diagnostics.Debug.Assert(found); |
| | | 286 | | const int directionWords = 3; |
| | 34 | 287 | | Span<ulong> direction = stackalloc ulong[6 * directionWords]; |
| | 34 | 288 | | Span<sbyte> directionSigns = stackalloc sbyte[6]; |
| | 68 | 289 | | direction.Clear(); directionSigns.Clear(); |
| | 34 | 290 | | WideAxis3 first = geometry.WorldBasis.GetAxis(selection.First); |
| | 34 | 291 | | WideAxis3 second = geometry.WorldBasis.GetAxis(selection.Second); |
| | 272 | 292 | | for (int component = 0; component < 3; component++) |
| | | 293 | | { |
| | 102 | 294 | | WriteDirection(Component(first, component), selection.FirstSign, |
| | 102 | 295 | | direction.Slice(component * 2 * directionWords, directionWords), out directionSigns[2 * component]); |
| | 102 | 296 | | WriteDirection(Component(second, component), selection.SecondSign, |
| | 102 | 297 | | direction.Slice((component * 2 + 1) * directionWords, directionWords), out directionSigns[2 * component |
| | | 298 | | } |
| | 34 | 299 | | return ConvexContactValueRoot.GetNormalizedDirection(ref root, direction, directionSigns, 1); |
| | | 300 | | } |
| | | 301 | | |
| | | 302 | | private static void WriteDirection(Signed320 value, int orientation, Span<ulong> magnitude, out sbyte sign) |
| | | 303 | | { |
| | 204 | 304 | | Signed192 narrow = Signed192.NarrowProven(value); |
| | 204 | 305 | | WideArithmetic.GetMagnitude(narrow, out magnitude[2], out magnitude[1], out magnitude[0]); |
| | 204 | 306 | | sign = (sbyte)(value.Sign * orientation); |
| | 204 | 307 | | } |
| | | 308 | | |
| | | 309 | | private static WideAxis3 GetOffset(in BoxCylinderGeometry geometry, int edge, int first, int second, |
| | | 310 | | int firstSign, int secondSign, int capSign) |
| | | 311 | | { |
| | 2206 | 312 | | int corner = (firstSign > 0 ? 1 << first : 0) | (secondSign > 0 ? 1 << second : 0); |
| | 2206 | 313 | | WideAxis3 offset = geometry.GetCapVertexOffset(corner, capSign); |
| | | 314 | | // Projection onto the edge-normal plane discards this coordinate. |
| | 2206 | 315 | | return new WideAxis3(edge == 0 ? default : offset.X, |
| | 2206 | 316 | | edge == 1 ? default : offset.Y, edge == 2 ? default : offset.Z); |
| | | 317 | | } |
| | | 318 | | |
| | 102 | 319 | | private static Fixed64 SupportSign(Selection selection, int axis) => axis == selection.Edge |
| | 102 | 320 | | ? Fixed64.Zero : (axis == selection.First ? selection.FirstSign : selection.SecondSign) > 0 |
| | 102 | 321 | | ? Fixed64.One : -Fixed64.One; |
| | 23687 | 322 | | private static Signed320 Component(in WideAxis3 value, int axis) => axis == 0 ? value.X : axis == 1 ? value.Y : valu |
| | 15420 | 323 | | private static Signed320 Signed(Signed320 value, int sign) => sign < 0 ? WideArithmetic.Negate(value) : value; |
| | | 324 | | private static int Sign(scoped ref FiniteAxisValueRoot root, scoped Polynomial polynomial) => |
| | 5108 | 325 | | WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, polynomial.Values, polynomial.Signs); |
| | | 326 | | private static void Write(Polynomial polynomial, int index, Signed576 value) |
| | | 327 | | { |
| | 15392 | 328 | | Span<ulong> target = polynomial.Values.Slice(index * polynomial.Words, polynomial.Words); |
| | 15392 | 329 | | target.Clear(); |
| | 15392 | 330 | | WideArithmetic.GetMagnitude(value, target[..9]); |
| | 15392 | 331 | | polynomial.Signs[index] = (sbyte)value.Sign; |
| | 15392 | 332 | | } |
| | | 333 | | private static void Multiply(Polynomial result, Polynomial first, Polynomial second, Span<ulong> product) => |
| | 12936 | 334 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(first.Values, first.Signs, |
| | 12936 | 335 | | second.Values, second.Signs, result.Values, result.Signs, product); |
| | | 336 | | private static void Add(Polynomial result, Polynomial source, int multiplier) => |
| | 4312 | 337 | | WideFiniteAxisIntersection.AddFiniteAxisPolynomial(source.Values, source.Signs, |
| | 4312 | 338 | | result.Values, result.Signs, multiplier); |
| | | 339 | | private readonly ref struct Polynomial |
| | | 340 | | { |
| | | 341 | | internal readonly Span<ulong> Values; |
| | | 342 | | internal readonly Span<sbyte> Signs; |
| | | 343 | | internal readonly int Words; |
| | | 344 | | internal Polynomial(Span<ulong> values, Span<sbyte> signs, int words) |
| | | 345 | | { |
| | 161034 | 346 | | Values = values; Signs = signs; Words = words; |
| | 53678 | 347 | | } |
| | | 348 | | internal Polynomial Slice(int offset, int count) => |
| | 47194 | 349 | | new(Values.Slice(offset * Words, count * Words), Signs.Slice(offset, count), Words); |
| | | 350 | | } |
| | | 351 | | } |