| | | 1 | | //======================================================================= |
| | | 2 | | // CurvedPrismRimContacts.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>Exactly ranked stationary real-prism-edge/circular-rim support gaps.</summary> |
| | | 14 | | internal static partial class CurvedPrismRimContacts |
| | | 15 | | { |
| | | 16 | | private const int Words = 40; |
| | | 17 | | private const int Slots = 31; |
| | | 18 | | private const int ValueWords = 56; |
| | | 19 | | private const int ValueCellWords = (16 * (ValueWords * 64 + 11) + 255) / 64; |
| | | 20 | | |
| | | 21 | | /// <summary> |
| | | 22 | | /// Retains stationary minima after complete closed intersection admission |
| | | 23 | | /// and the rational, vertex and generator boundaries. Exact ties retain |
| | | 24 | | /// those earlier features; every admitted support gap is nonnegative. |
| | | 25 | | /// </summary> |
| | | 26 | | internal static void GetContact(in CurvedPrismContactGeometry geometry, |
| | | 27 | | scoped ConvexContactCandidate analytic, Fixed64 analyticDepth, bool analyticClamped, |
| | | 28 | | out Selection best, out Fixed64 depth, out bool clamped, PrismCurvedContactDomain domain = default, bool hasAnal |
| | | 29 | | { |
| | 675 | 30 | | best = default; depth = analyticDepth; clamped = analyticClamped; |
| | | 31 | | // A radius-zero primitive is a finite segment, owned by the rational fan. |
| | 229 | 32 | | if (geometry.Radius.IsZero) return; |
| | 221 | 33 | | Span<ulong> bestValues = stackalloc ulong[5 * ValueWords]; |
| | 221 | 34 | | Span<sbyte> bestSigns = stackalloc sbyte[5]; |
| | 221 | 35 | | Span<ulong> bestCell = stackalloc ulong[ValueCellWords]; |
| | 221 | 36 | | WideDirection3d axis = new(geometry.Axis); |
| | 5864 | 37 | | for (int edgeIndex = 0; edgeIndex < geometry.EdgeCount; edgeIndex++) |
| | | 38 | | { |
| | 2719 | 39 | | geometry.Edge(edgeIndex, out int start, out int end); |
| | 2719 | 40 | | WideDirection3d edge = CurvedPrismContact.Difference(geometry.Vertex(end), geometry.Vertex(start)); |
| | | 41 | | // Parallel projection is circular; perpendicular projection is a |
| | | 42 | | // segment. Their complete boundaries and principal directions are |
| | | 43 | | // in the earlier rational/generator fan, not this ellipse quartic. |
| | 2719 | 44 | | if (edge.IsZero || WideDirection3d.Dot(edge, axis).IsZero |
| | 2719 | 45 | | || WideDirection3d.AreParallel(edge, axis)) |
| | | 46 | | continue; |
| | 1177 | 47 | | bool supportCertified = geometry.TryGetEdgeNormalCone(edgeIndex, out WideDirection3d coneFirst, out WideDire |
| | 1235 | 48 | | if (!supportCertified) edge = CurvedPrismContact.ReduceEdgeDirection(edge); |
| | 6248 | 49 | | for (int cap = -1; cap <= (geometry.IsCone ? -1 : 1); cap += 2) |
| | | 50 | | { |
| | 1955 | 51 | | if (supportCertified) |
| | | 52 | | { |
| | 1859 | 53 | | KeepChart(geometry, edgeIndex, edge, coneFirst, CurvedPrismContact.Difference(coneLast, coneFirst), |
| | 1859 | 54 | | analytic, ((ulong)analyticDepth.m_rawValue << 1) | 1UL, hasAnalytic && !analyticClamped, true, |
| | 1859 | 55 | | bestValues, bestSigns, bestCell, ref best, domain, hasAnalytic); |
| | 1877 | 56 | | if (best.IsZero) { depth = Fixed64.Zero; clamped = false; return; } |
| | | 57 | | continue; |
| | | 58 | | } |
| | 96 | 59 | | CircularRimContactAlgebra.GetBasis(edge, out WideDirection3d u, out WideDirection3d v); |
| | 572 | 60 | | for (int firstSign = -1; firstSign <= 1; firstSign += 2) |
| | 1144 | 61 | | for (int secondSign = -1; secondSign <= 1; secondSign += 2) |
| | 2284 | 62 | | for (int chart = 0; chart < 2; chart++) |
| | | 63 | | { |
| | 762 | 64 | | WideDirection3d first = firstSign < 0 ? -u : u; |
| | 762 | 65 | | WideDirection3d second = secondSign < 0 ? -v : v; |
| | 1142 | 66 | | if (chart != 0) (first, second) = (second, first); |
| | 762 | 67 | | KeepChart(geometry, edgeIndex, edge, first, second, cap, analytic, |
| | 762 | 68 | | ((ulong)analyticDepth.m_rawValue << 1) | 1UL, hasAnalytic && !analyticClamped, false, |
| | 762 | 69 | | bestValues, bestSigns, bestCell, ref best, domain, hasAnalytic); |
| | 762 | 70 | | if (best.IsZero) |
| | | 71 | | { |
| | 6 | 72 | | depth = Fixed64.Zero; clamped = false; return; |
| | | 73 | | } |
| | | 74 | | } |
| | | 75 | | } |
| | | 76 | | } |
| | 372 | 77 | | if (!best.HasValue) return; |
| | 54 | 78 | | FiniteAxisValueRoot root = RestoreValue(bestValues, bestSigns, bestCell, best); |
| | 54 | 79 | | ConvexContactValueRoot.GetRoundedDepth(geometry.RawScale, geometry.ValueShift, |
| | 54 | 80 | | ref root, out depth, out clamped); |
| | 54 | 81 | | } |
| | | 82 | | |
| | | 83 | | /// <summary>Replays the selected value polynomial, without rounding or rebuilding its witness.</summary> |
| | | 84 | | internal static int CompareSquaredDepth(in CurvedPrismContactGeometry geometry, in Selection selected, |
| | | 85 | | scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> radical) |
| | | 86 | | { |
| | 11 | 87 | | if (selected.IsZero) return -numerator.Sign(radical); |
| | 18 | 88 | | Span<ulong> values = stackalloc ulong[5 * ValueWords], cell = stackalloc ulong[ValueCellWords]; |
| | 9 | 89 | | Span<sbyte> signs = stackalloc sbyte[5]; |
| | 9 | 90 | | BuildValues(geometry, selected.Edge, selected.CapSign, values, signs); |
| | 9 | 91 | | bool found = WideFiniteAxisIntersection.TryGetFiniteValueRoot(values, signs, selected.ValueOrdinal, cell, out va |
| | | 92 | | System.Diagnostics.Debug.Assert(found); |
| | 9 | 93 | | return ConvexContactValueRoot.CompareRootSquared(geometry.RawScale, geometry.ValueShift, ref root, numerator, de |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 97 | | private static void KeepChart(in CurvedPrismContactGeometry geometry, int edge, |
| | | 98 | | in WideDirection3d edgeDirection, in WideDirection3d first, in WideDirection3d second, int cap, |
| | | 99 | | scoped ConvexContactCandidate analytic, ulong upperTwiceRaw, bool hasUpperBound, bool supportCertified, |
| | | 100 | | scoped Span<ulong> bestValues, scoped Span<sbyte> bestSigns, scoped Span<ulong> bestCell, |
| | | 101 | | ref Selection best, PrismCurvedContactDomain domain, bool hasAnalytic) |
| | | 102 | | { |
| | 5242 | 103 | | int firstAxialSign = 0, lastAxialSign = 0; |
| | 2621 | 104 | | if (!geometry.IsCone) |
| | | 105 | | { |
| | 2102 | 106 | | Signed832 axial = WideDirection3d.Dot(first, geometry.Axis); |
| | 2102 | 107 | | Signed832 slope = WideDirection3d.Dot(second, geometry.Axis); |
| | 2102 | 108 | | firstAxialSign = axial.Sign; |
| | 2102 | 109 | | lastAxialSign = WideArithmetic.AddSigned832(axial, slope).Sign; |
| | | 110 | | // Nonpositive endpoints leave only an analytic side-boundary |
| | | 111 | | // endpoint. Identically zero would mean the excluded parallel edge. |
| | 3045 | 112 | | if (firstAxialSign * cap <= 0 && lastAxialSign * cap <= 0) return; |
| | | 113 | | } |
| | 1678 | 114 | | geometry.Edge(edge, out int start, out _); |
| | 1678 | 115 | | WideDirection3d vertex = geometry.Vertex(start); |
| | | 116 | | // One strictly negative support-wall value at both chart endpoints |
| | | 117 | | // excludes this entire affine chart before constructing its quartic. |
| | 8964 | 118 | | for (int index = 0; !supportCertified && index < geometry.VertexCount; index++) |
| | | 119 | | { |
| | 2900 | 120 | | WideDirection3d delta = CurvedPrismContact.Difference(geometry.Vertex(index), vertex); |
| | 5800 | 121 | | Signed832 a = WideDirection3d.Dot(delta, first), b = WideDirection3d.Dot(delta, second); |
| | 2996 | 122 | | if (a.Sign < 0 && WideArithmetic.AddSigned832(a, b).Sign < 0) return; |
| | | 123 | | } |
| | 1582 | 124 | | WideDirection3d offset = CurvedPrismContact.Difference(cap < 0 ? -geometry.Half : geometry.Half, vertex); |
| | 1582 | 125 | | if (hasAnalytic && !geometry.IsCone && supportCertified && !domain.Enabled && edge % 3 != 2 |
| | 1582 | 126 | | && firstAxialSign * lastAxialSign < 0 |
| | 1658 | 127 | | && IsPhysicalArcConcave(geometry, edgeDirection, offset, first, second, cap, firstAxialSign)) return; |
| | 1506 | 128 | | if (hasAnalytic && !geometry.IsCone && supportCertified && !domain.Enabled |
| | 1810 | 129 | | && HasNoNonnegativeEllipseMinimum(geometry, edgeDirection, offset)) return; |
| | 1202 | 130 | | if (hasAnalytic && IsChartDominated(geometry, edgeDirection, offset, first, second, analytic, |
| | 1524 | 131 | | supportCertified && !domain.Enabled && edge % 3 == 2)) return; |
| | 880 | 132 | | Span<ulong> data = stackalloc ulong[Slots * Words]; |
| | 880 | 133 | | Span<sbyte> signs = stackalloc sbyte[Slots]; |
| | 880 | 134 | | BuildParameter(geometry, edge, first, second, cap, data, signs); |
| | 880 | 135 | | Polynomial storage = new(data, signs, Words); |
| | 2640 | 136 | | Polynomial stationary = storage.Slice(0, 5), k = storage.Slice(8, 2), l = storage.Slice(10, 3); |
| | 2640 | 137 | | Polynomial gap = storage.Slice(13, 3), numerator = storage.Slice(16, 5), denominator = storage.Slice(21, 5); |
| | 885 | 138 | | if (supportCertified && !domain.Enabled && HasNoNewStationaryMinimum(stationary, k)) return; |
| | 875 | 139 | | Span<ulong> batchCells = stackalloc ulong[64]; |
| | 875 | 140 | | Span<int> batchShifts = stackalloc int[8]; |
| | 875 | 141 | | FiniteAxisValueRoots roots = WideFiniteAxisIntersection.GetFiniteValueRoots( |
| | 875 | 142 | | stationary.Values, stationary.Signs, batchCells, batchShifts); |
| | 1271 | 143 | | if (roots.Count == 0) return; |
| | | 144 | | // At K's interior zero, W=L squared is nonnegative. One root, or |
| | | 145 | | // two starting from W(0)<0, can only be maxima; K=0 is analytic. |
| | 479 | 146 | | if (supportCertified && !domain.Enabled && !roots.HasRepeatedRoots && stationary.Signs[0] != 0 |
| | 479 | 147 | | && k.Signs[0] > 0 && k.Signs[1] < 0 |
| | 479 | 148 | | && WideArithmetic.CompareMagnitudeEqualLength(k.Values[..Words], k.Values.Slice(Words, Words)) < 0 |
| | 575 | 149 | | && (roots.Count == 1 || roots.Count == 2 && stationary.Signs[0] < 0)) return; |
| | 383 | 150 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords( |
| | 383 | 151 | | stationary.Values, stationary.Signs)]; |
| | 383 | 152 | | Span<ulong> values = stackalloc ulong[5 * ValueWords]; |
| | 383 | 153 | | Span<sbyte> valueSigns = stackalloc sbyte[5]; |
| | 383 | 154 | | Span<ulong> valueCell = stackalloc ulong[ValueCellWords]; |
| | 383 | 155 | | bool gapReady = false; |
| | 383 | 156 | | bool valuesReady = false; |
| | 383 | 157 | | int firstStationarySign = 0; |
| | 383 | 158 | | if (supportCertified && !domain.Enabled && !roots.HasRepeatedRoots) |
| | 241 | 159 | | foreach (sbyte sign in stationary.Signs) |
| | 239 | 160 | | if (sign != 0) { firstStationarySign = sign; break; } |
| | 2024 | 161 | | for (int ordinal = 0; ordinal < roots.Count; ordinal++) |
| | | 162 | | { |
| | 637 | 163 | | scoped FiniteAxisValueRoot root = FiniteAxisValueRoots.GetRoot( |
| | 637 | 164 | | roots, ordinal, stationary.Values, stationary.Signs, cell); |
| | 637 | 165 | | int kSign = 0; |
| | 637 | 166 | | if (firstStationarySign != 0) |
| | | 167 | | { |
| | 151 | 168 | | kSign = Sign(ref root, k); |
| | | 169 | | // For W=L²-K²Q and L=-K sqrt(Q), sign(g'')=-sign(W')*sign(K). |
| | | 170 | | // Simple roots alternate W's sign from its first nonzero coefficient. |
| | 151 | 171 | | if (firstStationarySign * ((ordinal & 1) == 0 ? 1 : -1) * kSign < 0) continue; |
| | | 172 | | } |
| | 590 | 173 | | int axialSign = Sign(ref root, storage.Slice(26, 2)); |
| | 590 | 174 | | if (geometry.IsCone |
| | 590 | 175 | | ? axialSign > 0 && Sign(ref root, storage.Slice(28, 3)) <= 0 |
| | 590 | 176 | | : axialSign < 0) |
| | | 177 | | continue; |
| | 529 | 178 | | if (!supportCertified && !HasPrismSupport(geometry, vertex, first, second, ref root)) continue; |
| | 935 | 179 | | if (firstStationarySign == 0) kSign = Sign(ref root, k); |
| | | 180 | | // Q is positive on a nondegenerate ellipse. K=0 is its principal |
| | | 181 | | // direction, already present in the rational feature fan. |
| | 515 | 182 | | if (kSign == 0 || Sign(ref root, l) != -kSign) continue; |
| | 294 | 183 | | if (!gapReady) |
| | | 184 | | { |
| | 280 | 185 | | CylinderEdgeContactPolynomial.CompleteGap(data[..(26 * Words)], signs[..26]); |
| | 280 | 186 | | gapReady = true; |
| | | 187 | | } |
| | 294 | 188 | | int gapSign = Sign(ref root, gap) * kSign; |
| | 294 | 189 | | if (domain.Enabled && gapSign < 0) continue; |
| | | 190 | | System.Diagnostics.Debug.Assert(gapSign >= 0); |
| | 292 | 191 | | Selection candidate = new() |
| | 292 | 192 | | { |
| | 292 | 193 | | HasValue = true, |
| | 292 | 194 | | Edge = edge, |
| | 292 | 195 | | CapSign = cap, |
| | 292 | 196 | | First = first, |
| | 292 | 197 | | Second = second, |
| | 292 | 198 | | ParameterOrdinal = ordinal |
| | 292 | 199 | | }; |
| | 292 | 200 | | if (domain.Enabled) |
| | | 201 | | { |
| | 178 | 202 | | if (!TryAdmitDomain(geometry, domain, candidate, data, signs, ref root, out bool physical)) continue; |
| | 47 | 203 | | candidate.Physical = physical; |
| | | 204 | | } |
| | 161 | 205 | | if (gapSign == 0) |
| | | 206 | | { |
| | | 207 | | // Zero gap and zero derivative place the origin on the strictly |
| | | 208 | | // convex projected ellipse: its unique normal needs no tie rank. |
| | 24 | 209 | | candidate.IsZero = true; best = candidate; return; |
| | | 210 | | } |
| | 153 | 211 | | if (hasAnalytic && hasUpperBound && ConvexContactValueRoot.CompareSquaredGapToTwiceRaw(ref root, |
| | 153 | 212 | | numerator.Values, numerator.Signs, denominator.Values, denominator.Signs, upperTwiceRaw) >= 0) |
| | | 213 | | continue; |
| | 94 | 214 | | if (!valuesReady) |
| | | 215 | | { |
| | 184 | 216 | | BuildValues(geometry, edge, cap, values, valueSigns); valuesReady = true; |
| | | 217 | | } |
| | 94 | 218 | | scoped FiniteAxisValueRoot value = ConvexContactValueRoot.MapSquaredValue( |
| | 94 | 219 | | geometry.RawScale, geometry.ValueShift, ref root, numerator.Values, numerator.Signs, |
| | 94 | 220 | | denominator.Values, denominator.Signs, values, valueSigns, valueCell); |
| | 94 | 221 | | int comparison = best.HasValue |
| | 94 | 222 | | ? WideFiniteAxisIntersection.CompareFiniteValueRoots(value, RestoreValue(bestValues, bestSigns, bestCell |
| | 94 | 223 | | : hasAnalytic ? ConvexContactValueRoot.CompareRootSquared(geometry.RawScale, geometry.ValueShift, ref va |
| | 94 | 224 | | if (comparison > 0 || comparison == 0 && (!best.HasValue || best.Edge != edge || best.CapSign != cap)) conti |
| | 63 | 225 | | if (supportCertified) |
| | | 226 | | { |
| | 49 | 227 | | candidate.Order = GetLegacyOrder(edgeDirection, first, second, ordinal, ref root); |
| | 50 | 228 | | if (comparison == 0 && candidate.Order >= best.Order) continue; |
| | | 229 | | } |
| | 14 | 230 | | else if (comparison == 0) continue; |
| | 168 | 231 | | values.CopyTo(bestValues); valueSigns.CopyTo(bestSigns); valueCell.CopyTo(bestCell); |
| | 112 | 232 | | candidate.ValueOrdinal = value.Ordinal; candidate.ValueShift = value.DenominatorShift; |
| | 112 | 233 | | candidate.IsRational = value.IsRational; best = candidate; |
| | | 234 | | } |
| | 375 | 235 | | } |
| | | 236 | | |
| | | 237 | | // At K's positive zero beta, W(beta)=L(beta)^2. Two positive roots |
| | | 238 | | // can only bracket beta (both maxima), or coincide at the analytic K=0. |
| | | 239 | | private static bool HasNoNewStationaryMinimum(Polynomial stationary, Polynomial k) |
| | | 240 | | { |
| | 527 | 241 | | if (stationary.Signs[0] >= 0 || k.Signs[0] <= 0 || k.Signs[1] >= 0) return false; |
| | 282 | 242 | | int previous = 0, variations = 0; |
| | 1692 | 243 | | foreach (int sign in stationary.Signs) |
| | | 244 | | { |
| | 705 | 245 | | if (sign == 0) continue; |
| | 1248 | 246 | | if (previous != 0 && previous != sign) variations++; |
| | 703 | 247 | | previous = sign; |
| | | 248 | | } |
| | 141 | 249 | | return variations == 2; |
| | | 250 | | } |
| | | 251 | | |
| | | 252 | | private static bool TryAdmitDomain(in CurvedPrismContactGeometry geometry, PrismCurvedContactDomain domain, |
| | | 253 | | in Selection candidate, Span<ulong> parameters, Span<sbyte> parameterSigns, |
| | | 254 | | scoped ref FiniteAxisValueRoot root, out bool physical) |
| | | 255 | | { |
| | 178 | 256 | | physical = false; |
| | 356 | 257 | | Span<ulong> fields = stackalloc ulong[36 * Words]; Span<sbyte> signs = stackalloc sbyte[36]; |
| | 356 | 258 | | fields.Clear(); signs.Clear(); |
| | 534 | 259 | | Polynomial storage = new(fields, signs, Words), shape = storage.Slice(0, 9), point = storage.Slice(9, 18); |
| | 356 | 260 | | Polynomial shapeD = storage.Slice(27, 3), pointD = storage.Slice(30, 6); |
| | 178 | 261 | | BuildPointPolynomials(geometry, candidate, parameters, parameterSigns, shape, point, shapeD, pointD); |
| | 178 | 262 | | geometry.Edge(candidate.Edge, out int sourceVertex, out _); |
| | 1994 | 263 | | foreach (var window in domain.Edges) |
| | | 264 | | { |
| | 844 | 265 | | if (NormalDotSign(candidate, CurvedPrismContact.Difference(geometry.Vertex(window.ReferenceVertex), geometry |
| | 209 | 266 | | bool admitted = true; |
| | 428 | 267 | | foreach (var delta in domain.FanCovectors.Slice(window.FanStart, window.FanCount)) |
| | 15 | 268 | | if (NormalDotSign(candidate, delta, ref root) < 0) { admitted = false; break; } |
| | 209 | 269 | | if (!admitted || !PointInEdgeWindow(geometry, window, point, pointD, ref root)) continue; |
| | 50 | 270 | | physical = IsPhysicalPoint(geometry, point, pointD, ref root); |
| | 50 | 271 | | return physical || domain.IncludeFarBoundary; |
| | | 272 | | } |
| | | 273 | | // Every face-normal direction already belongs to the earlier rational |
| | | 274 | | // inventory. Its equal-depth rim representation cannot improve it. |
| | 128 | 275 | | return false; |
| | | 276 | | } |
| | | 277 | | |
| | | 278 | | private static int NormalDotSign(in Selection candidate, in WideDirection3d delta, scoped ref FiniteAxisValueRoot ro |
| | | 279 | | { |
| | 1702 | 280 | | Span<ulong> values = stackalloc ulong[26]; Span<sbyte> signs = stackalloc sbyte[2]; |
| | 851 | 281 | | Polynomial query = new(values, signs, 13); |
| | 851 | 282 | | Write(query, 0, WideDirection3d.Dot(delta, candidate.First)); |
| | 1702 | 283 | | Write(query, 1, WideDirection3d.Dot(delta, candidate.Second)); return Sign(ref root, query); |
| | | 284 | | } |
| | | 285 | | |
| | | 286 | | private static bool PointInEdgeWindow(in CurvedPrismContactGeometry geometry, in PrismCurvedEdgeWindow window, |
| | | 287 | | scoped Polynomial point, scoped Polynomial denominator, scoped ref FiniteAxisValueRoot root) |
| | | 288 | | { |
| | 205 | 289 | | geometry.Edge(window.Edge, out int first, out int last); |
| | 410 | 290 | | var a = geometry.Vertex(first); var edge = CurvedPrismContact.Difference(geometry.Vertex(last), a); |
| | | 291 | | // A retained source coefficient needs <2560 bits. Translating by a |
| | | 292 | | // vertex adds <165, projection <167 and a cut ratio <163: complete |
| | | 293 | | // interval comparisons need <3060 bits, inside 56-word banks. |
| | | 294 | | const int words = 56; |
| | 410 | 295 | | Span<ulong> values = stackalloc ulong[38 * words]; Span<sbyte> signs = stackalloc sbyte[37]; |
| | 615 | 296 | | values.Clear(); signs.Clear(); Polynomial storage = new(values[..(37 * words)], signs, words); |
| | 615 | 297 | | Polynomial q = storage.Slice(0, 18), n = storage.Slice(18, 6), d = storage.Slice(24, 6); |
| | 410 | 298 | | Polynomial work = storage.Slice(30, 6), scalar = storage.Slice(36, 1); |
| | 205 | 299 | | Span<ulong> product = values[(37 * words)..]; |
| | 205 | 300 | | int pivot = !edge.X.IsZero ? 0 : !edge.Y.IsZero ? 1 : 2; |
| | 1640 | 301 | | for (int axis = 0; axis < 3; axis++) |
| | | 302 | | { |
| | 8610 | 303 | | for (int coefficient = 0; coefficient < 6; coefficient++) |
| | 3690 | 304 | | point.Values.Slice((6 * axis + coefficient) * point.Words, point.Words) |
| | 3690 | 305 | | .CopyTo(q.Values.Slice((6 * axis + coefficient) * words, words)); |
| | 615 | 306 | | point.Slice(6 * axis, 6).Signs.CopyTo(q.Slice(6 * axis, 6).Signs); |
| | 615 | 307 | | Write(scalar, 0, Signed832.ExtendValue(Component(a, axis))); |
| | 615 | 308 | | AddProduct(q.Slice(6 * axis, 6), denominator, scalar, -1, product); |
| | | 309 | | } |
| | 1158 | 310 | | for (int axis = 0; axis < 3; axis++) |
| | | 311 | | { |
| | 982 | 312 | | Write(scalar, 0, Signed832.ExtendValue(Component(edge, pivot))); Multiply(work, q.Slice(6 * axis, 6), scalar |
| | 982 | 313 | | Write(scalar, 0, Signed832.ExtendValue(Component(edge, axis))); AddProduct(work, q.Slice(6 * pivot, 6), scal |
| | 608 | 314 | | if (Sign(ref root, work) != 0) return false; |
| | 748 | 315 | | Write(scalar, 0, Signed832.ExtendValue(Component(edge, axis))); AddProduct(n, q.Slice(6 * axis, 6), scalar, |
| | | 316 | | } |
| | 176 | 317 | | Write(scalar, 0, edge.SquaredLength); Multiply(d, denominator, scalar, product); |
| | 88 | 318 | | int orientation = Sign(ref root, d); |
| | 88 | 319 | | var clip = window.Interval; |
| | 88 | 320 | | Write(scalar, 0, Signed832.ExtendValue(Signed576.ExtendValue(clip.IsClipped ? clip.LowerDenominator : Signed320. |
| | 88 | 321 | | Multiply(work, n, scalar, product); |
| | 88 | 322 | | Write(scalar, 0, Signed832.ExtendValue(Signed576.ExtendValue(clip.IsClipped ? clip.LowerNumerator : default))); |
| | 88 | 323 | | AddProduct(work, d, scalar, -1, product); |
| | 177 | 324 | | int lower = Sign(ref root, work) * orientation; if (lower < 0 || !window.Point && lower == 0) return false; |
| | 87 | 325 | | Write(scalar, 0, Signed832.ExtendValue(Signed576.ExtendValue(clip.IsClipped ? clip.UpperDenominator : Signed320. |
| | 87 | 326 | | Multiply(work, n, scalar, product); |
| | 87 | 327 | | Write(scalar, 0, Signed832.ExtendValue(Signed576.ExtendValue(clip.IsClipped ? clip.UpperNumerator : Signed320.On |
| | 87 | 328 | | AddProduct(work, d, scalar, -1, product); |
| | 174 | 329 | | int upper = Sign(ref root, work) * orientation; return upper <= 0 && (window.Point || upper < 0); |
| | | 330 | | } |
| | | 331 | | |
| | | 332 | | private static bool IsPhysicalPoint(in CurvedPrismContactGeometry geometry, scoped Polynomial point, |
| | | 333 | | scoped Polynomial denominator, scoped ref FiniteAxisValueRoot root) |
| | | 334 | | { |
| | | 335 | | // Source coefficients fit the existing 2560-bit banks. Squared cone |
| | | 336 | | // admission adds <832 bits, so complete products fit 96-word banks. |
| | | 337 | | const int words = 96; |
| | 100 | 338 | | Span<ulong> values = stackalloc ulong[53 * words]; Span<sbyte> signs = stackalloc sbyte[52]; |
| | 150 | 339 | | values.Clear(); signs.Clear(); Polynomial fields = new(values[..(52 * words)], signs, words); |
| | 150 | 340 | | Polynomial axial = fields.Slice(0, 6), radial = fields.Slice(6, 11), work = fields.Slice(17, 11); |
| | 150 | 341 | | Polynomial limit = fields.Slice(28, 11), maximum = fields.Slice(39, 6), height = fields.Slice(45, 6); |
| | 100 | 342 | | Polynomial scalar = fields.Slice(51, 1); Span<ulong> product = values[(52 * words)..]; |
| | 400 | 343 | | for (int axis = 0; axis < 3; axis++) |
| | | 344 | | { |
| | 150 | 345 | | Write(scalar, 0, Signed832.ExtendValue(Signed576.ExtendValue(Component(geometry.Axis, axis)))); |
| | 150 | 346 | | AddProduct(axial, point.Slice(6 * axis, 6), scalar, 1, product); |
| | 300 | 347 | | Multiply(work, point.Slice(6 * axis, 6), point.Slice(6 * axis, 6), product); Add(radial, work, 1); |
| | | 348 | | } |
| | 50 | 349 | | Write(scalar, 0, Signed832.ExtendValue(WideArithmetic.MultiplySigned320(geometry.SquaredAxis, geometry.HalfFacto |
| | 50 | 350 | | Multiply(maximum, denominator, scalar, product); |
| | 150 | 351 | | maximum.Values.CopyTo(height.Values); maximum.Signs.CopyTo(height.Signs); Add(height, axial, -1); |
| | 50 | 352 | | int orientation = Sign(ref root, denominator); |
| | 52 | 353 | | if (Sign(ref root, height) * orientation < 0) return false; |
| | 144 | 354 | | maximum.Values.CopyTo(work.Slice(0, 6).Values); maximum.Signs.CopyTo(work.Slice(0, 6).Signs); Add(work.Slice(0, |
| | 52 | 355 | | if (Sign(ref root, work.Slice(0, 6)) * orientation < 0) return false; |
| | 88 | 356 | | Write(scalar, 0, Signed832.ExtendValue(geometry.SquaredAxis)); Multiply(work, radial, scalar, product); |
| | 88 | 357 | | work.Values.CopyTo(radial.Values); work.Signs.CopyTo(radial.Signs); |
| | 88 | 358 | | Multiply(work, axial, axial, product); Add(radial, work, -1); |
| | 44 | 359 | | Signed576 radiusSquared = WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius); |
| | 44 | 360 | | if (geometry.IsCone) |
| | | 361 | | { |
| | 36 | 362 | | Multiply(work, height, height, product); Write(scalar, 0, Signed832.ExtendValue(radiusSquared)); Multiply(li |
| | 12 | 363 | | Signed576 factor = WideArithmetic.MultiplySigned320(Signed320.ExtendValue(geometry.HalfFactor), geometry.Hal |
| | 12 | 364 | | factor = Signed576.NarrowValue(WideArithmetic.MultiplySigned576ToSigned832(factor, geometry.SquaredAxis)); |
| | 24 | 365 | | factor = WideArithmetic.AddSigned576(factor, factor); factor = WideArithmetic.AddSigned576(factor, factor); |
| | 24 | 366 | | Write(scalar, 0, Signed832.ExtendValue(factor)); Multiply(work, radial, scalar, product); |
| | | 367 | | } |
| | | 368 | | else |
| | | 369 | | { |
| | 32 | 370 | | Multiply(work, denominator, denominator, product); |
| | 64 | 371 | | Write(scalar, 0, WideArithmetic.MultiplySigned576ToSigned832(radiusSquared, geometry.SquaredAxis)); Multiply |
| | 64 | 372 | | radial.Values.CopyTo(work.Values); radial.Signs.CopyTo(work.Signs); |
| | | 373 | | } |
| | 88 | 374 | | Add(work, limit, -1); return Sign(ref root, work) <= 0; |
| | | 375 | | } |
| | | 376 | | |
| | | 377 | | private static int GetLegacyOrder(in WideDirection3d edge, in WideDirection3d first, |
| | | 378 | | in WideDirection3d second, int ordinal, scoped ref FiniteAxisValueRoot root) |
| | | 379 | | { |
| | | 380 | | // In the old basis, alpha/beta share a fixed edge pivot. Their signs |
| | | 381 | | // and relative magnitudes identify the signed reciprocal chart; its |
| | | 382 | | // parameter is fractional-linear in t and has a constant derivative sign. |
| | 98 | 383 | | bool xPivot = !edge.X.IsZero, yPivot = !edge.Y.IsZero; |
| | 98 | 384 | | int aCoordinate = xPivot ? 1 : yPivot ? 2 : 1, bCoordinate = xPivot ? 2 : 0; |
| | 49 | 385 | | int pivotSign = (xPivot ? edge.X : yPivot ? edge.Y : edge.Z).Sign; |
| | 98 | 386 | | Signed576 a0 = Component(first, aCoordinate), a1 = Component(second, aCoordinate); |
| | 98 | 387 | | Signed576 b0 = Component(first, bCoordinate), b1 = Component(second, bCoordinate); |
| | 49 | 388 | | if (pivotSign * (xPivot || yPivot ? 1 : -1) < 0) |
| | 86 | 389 | | { a0 = WideArithmetic.Negate(a0); a1 = WideArithmetic.Negate(a1); } |
| | 131 | 390 | | if (pivotSign < 0) { b0 = WideArithmetic.Negate(b0); b1 = WideArithmetic.Negate(b1); } |
| | 49 | 391 | | Span<ulong> data = stackalloc ulong[26]; |
| | 49 | 392 | | Span<sbyte> signs = stackalloc sbyte[2]; |
| | 49 | 393 | | Polynomial query = new(data, signs, 13); |
| | 98 | 394 | | Write(query, 0, Signed832.ExtendValue(a0)); Write(query, 1, Signed832.ExtendValue(a1)); |
| | 49 | 395 | | int aSign = Sign(ref root, query); |
| | 98 | 396 | | Write(query, 0, Signed832.ExtendValue(b0)); Write(query, 1, Signed832.ExtendValue(b1)); |
| | 49 | 397 | | int bSign = Sign(ref root, query); |
| | | 398 | | // Old basis-axis directions already belong to the earlier analytic fan. |
| | | 399 | | System.Diagnostics.Debug.Assert(aSign != 0 && bSign != 0); |
| | 49 | 400 | | Write(query, 0, Signed832.ExtendValue(WideArithmetic.SubtractSigned576( |
| | 49 | 401 | | aSign < 0 ? WideArithmetic.Negate(a0) : a0, bSign < 0 ? WideArithmetic.Negate(b0) : b0))); |
| | 49 | 402 | | Write(query, 1, Signed832.ExtendValue(WideArithmetic.SubtractSigned576( |
| | 49 | 403 | | aSign < 0 ? WideArithmetic.Negate(a1) : a1, bSign < 0 ? WideArithmetic.Negate(b1) : b1))); |
| | 49 | 404 | | bool reciprocal = Sign(ref root, query) < 0; |
| | 49 | 405 | | Signed832 determinant = WideArithmetic.SubtractSigned832( |
| | 49 | 406 | | WideArithmetic.MultiplySigned576ToSigned832(a0, b1), WideArithmetic.MultiplySigned576ToSigned832(a1, b0)); |
| | 49 | 407 | | bool increasing = determinant.Sign * aSign * bSign * (reciprocal ? -1 : 1) > 0; |
| | 49 | 408 | | return (aSign > 0 ? 16 : 0) + (bSign > 0 ? 8 : 0) + (reciprocal ? 4 : 0) |
| | 49 | 409 | | + (increasing ? ordinal : 3 - ordinal); |
| | | 410 | | } |
| | | 411 | | |
| | | 412 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 413 | | private static bool HasNoNonnegativeEllipseMinimum(in CurvedPrismContactGeometry geometry, |
| | | 414 | | in WideDirection3d edge, in WideDirection3d offset) |
| | | 415 | | { |
| | | 416 | | // Nondegenerate ellipse dispatch gives a=R, b=R*|A.E|/sqrt(K), |
| | | 417 | | // K=q*E² and 0<b<a. At a nonnegative stationary minimum, |
| | | 418 | | // g''=rho-g>=0, so 0<=g<=rho. Thus |Proj(Eperp)c|² cannot exceed |
| | | 419 | | // max(a²,((a²-b²)/b)²), the ellipse/evolute maximum squared norms. |
| | | 420 | | // Physical arc endpoints and cap equators remain in the analytic fan. |
| | 505 | 421 | | Signed576 edgeSquared = Signed576.NarrowValue(edge.SquaredLength); |
| | 505 | 422 | | Signed576 offsetSquared = Signed576.NarrowValue(offset.SquaredLength); |
| | 505 | 423 | | Signed576 projection = Signed576.NarrowValue(WideDirection3d.Dot(edge, offset)); |
| | 505 | 424 | | Signed832 projectedSquared = WideArithmetic.SubtractSigned832( |
| | 505 | 425 | | WideArithmetic.MultiplySigned576ToSigned832(edgeSquared, offsetSquared), |
| | 505 | 426 | | WideArithmetic.MultiplySigned576ToSigned832(projection, projection)); |
| | 505 | 427 | | Signed576 radiusSquared = WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius); |
| | 505 | 428 | | Signed832 radiusMetric = WideArithmetic.MultiplySigned576ToSigned832(radiusSquared, edgeSquared); |
| | | 429 | | // C=e²*c²-(e.c)². Strict C>R²*e² first; equality retains fallback. |
| | 654 | 430 | | if (WideArithmetic.SubtractSigned832(projectedSquared, radiusMetric).Sign <= 0) return false; |
| | 356 | 431 | | Signed576 axial = Signed576.NarrowValue(WideDirection3d.Dot(edge, geometry.Axis)); |
| | 356 | 432 | | Signed832 axialSquared = WideArithmetic.MultiplySigned576ToSigned832(axial, axial); |
| | 356 | 433 | | Signed832 metric = WideArithmetic.MultiplySigned832(Signed832.ExtendValue(edgeSquared), geometry.SquaredAxis); |
| | 356 | 434 | | Signed832 anisotropy = WideArithmetic.SubtractSigned832(metric, axialSquared); |
| | | 435 | | // C*K*d² > R²*e²*(K-d²)². Typed factors fit 832; complete |
| | | 436 | | // products need <1471/<1458 bits. Four 24-word fields retain all carries. |
| | | 437 | | const int words = 24; |
| | 356 | 438 | | Span<ulong> work = stackalloc ulong[4 * words]; |
| | 712 | 439 | | Span<ulong> left = work[..words], right = work.Slice(words, words); |
| | 712 | 440 | | Span<ulong> factor = work.Slice(2 * words, words), product = work.Slice(3 * words, words); |
| | 712 | 441 | | CylinderContactAlgebra.Import(projectedSquared, left); CylinderContactAlgebra.Import(metric, factor); |
| | 356 | 442 | | WideArithmetic.MultiplyMagnitudes(left, factor, product); |
| | 356 | 443 | | CylinderContactAlgebra.Import(axialSquared, factor); |
| | 356 | 444 | | WideArithmetic.MultiplyMagnitudes(product, factor, left); |
| | 356 | 445 | | CylinderContactAlgebra.Import(anisotropy, factor); |
| | 356 | 446 | | WideArithmetic.MultiplyMagnitudes(factor, factor, right); |
| | 356 | 447 | | CylinderContactAlgebra.Import(radiusMetric, factor); |
| | 356 | 448 | | WideArithmetic.MultiplyMagnitudes(right, factor, product); |
| | 356 | 449 | | return WideArithmetic.CompareMagnitudeEqualLength(left, product) > 0; |
| | | 450 | | } |
| | | 451 | | |
| | | 452 | | private static bool IsPhysicalArcConcave(in CurvedPrismContactGeometry geometry, |
| | | 453 | | in WideDirection3d edge, in WideDirection3d offset, in WideDirection3d first, |
| | | 454 | | in WideDirection3d second, int cap, int firstAxialSign) |
| | | 455 | | { |
| | | 456 | | // Orthogonal certified horizontal endpoints and a strict axial |
| | | 457 | | // crossing place the equator in their positive span. Orient its |
| | | 458 | | // compact E-cross-A ray by its strictly positive dot with first. |
| | 147 | 459 | | WideDirection3d equator = WideDirection3d.Cross(edge, new WideDirection3d(geometry.Axis)); |
| | 194 | 460 | | if (WideDirection3d.Dot(equator, first).Sign < 0) equator = -equator; |
| | 147 | 461 | | WideDirection3d physical = firstAxialSign * cap > 0 ? first |
| | 147 | 462 | | : new(WideArithmetic.AddSigned576(first.X, second.X), |
| | 147 | 463 | | WideArithmetic.AddSigned576(first.Y, second.Y), WideArithmetic.AddSigned576(first.Z, second.Z)); |
| | 160 | 464 | | if (WideDirection3d.Dot(offset, physical).Sign < 0 || WideDirection3d.Dot(offset, equator).Sign < 0) return fals |
| | | 465 | | // On this arc, |A.n|/|n| is monotone toward the physical endpoint, |
| | | 466 | | // so projected radial support h has its minimum beta there. For an |
| | | 467 | | // ellipse with major radius R and minor radius r, h''=R²r²/h³-h. |
| | | 468 | | // beta^4>=R²r² makes h''<=0 throughout the arc. Since offset.n>=0 |
| | | 469 | | // here, g''=-offset.u+h''<=0; earlier analytic endpoints retain minima. |
| | | 470 | | // With Kp=q*|physical|², rp=Kp-(A.physical)², Ke=q*|E|² and d=A.E, |
| | | 471 | | // beta²=R²rp/Kp and r²=R²d²/Ke cancel R: rp²Ke>=d²Kp². |
| | 134 | 472 | | Signed832 physicalAxial = WideDirection3d.Dot(physical, geometry.Axis); |
| | 134 | 473 | | Signed832 kPhysical = WideArithmetic.MultiplySigned832(physical.SquaredLength, geometry.SquaredAxis); |
| | 134 | 474 | | Signed832 axialSquared = WideArithmetic.MultiplySigned576ToSigned832( |
| | 134 | 475 | | Signed576.NarrowValue(physicalAxial), Signed576.NarrowValue(physicalAxial)); |
| | 134 | 476 | | Signed832 radial = WideArithmetic.SubtractSigned832(kPhysical, axialSquared); |
| | 134 | 477 | | Signed832 kEdge = WideArithmetic.MultiplySigned832(edge.SquaredLength, geometry.SquaredAxis); |
| | 134 | 478 | | Signed832 edgeAxial = WideDirection3d.Dot(edge, geometry.Axis); |
| | 134 | 479 | | Signed832 edgeAxialSquared = WideArithmetic.MultiplySigned576ToSigned832( |
| | 134 | 480 | | Signed576.NarrowValue(edgeAxial), Signed576.NarrowValue(edgeAxial)); |
| | | 481 | | // Native endpoints <165, E <166 and A <33 bits give Kp<397, |
| | | 482 | | // rp<397, Ke<399 and d²<402. Complete products fit 1200 bits. |
| | | 483 | | const int words = 24; |
| | 134 | 484 | | Span<ulong> work = stackalloc ulong[4 * words]; |
| | 268 | 485 | | Span<ulong> left = work[..words], right = work.Slice(words, words); |
| | 268 | 486 | | Span<ulong> factor = work.Slice(2 * words, words), product = work.Slice(3 * words, words); |
| | 268 | 487 | | CylinderContactAlgebra.Import(radial, factor); WideArithmetic.MultiplyMagnitudes(factor, factor, left); |
| | 268 | 488 | | CylinderContactAlgebra.Import(kEdge, factor); WideArithmetic.MultiplyMagnitudes(left, factor, product); |
| | 268 | 489 | | CylinderContactAlgebra.Import(kPhysical, factor); WideArithmetic.MultiplyMagnitudes(factor, factor, left); |
| | 268 | 490 | | CylinderContactAlgebra.Import(edgeAxialSquared, factor); WideArithmetic.MultiplyMagnitudes(left, factor, right); |
| | 134 | 491 | | return WideArithmetic.CompareMagnitudeEqualLength(product, right) >= 0; |
| | | 492 | | } |
| | | 493 | | |
| | | 494 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 495 | | private static bool IsChartDominated(in CurvedPrismContactGeometry geometry, |
| | | 496 | | in WideDirection3d edge, in WideDirection3d offset, in WideDirection3d first, |
| | | 497 | | in WideDirection3d second, scoped ConvexContactCandidate analytic, bool allowConcavity) |
| | | 498 | | { |
| | 877 | 499 | | WideDirection3d last = new(WideArithmetic.AddSigned576(first.X, second.X), |
| | 877 | 500 | | WideArithmetic.AddSigned576(first.Y, second.Y), WideArithmetic.AddSigned576(first.Z, second.Z)); |
| | 1754 | 501 | | Signed832 a = WideDirection3d.Dot(offset, first), b = WideDirection3d.Dot(offset, last); |
| | 1244 | 502 | | if (a.Sign < 0 || b.Sign < 0) return false; |
| | 1020 | 503 | | Signed832 firstMetric = first.SquaredLength, lastMetric = last.SquaredLength; |
| | 510 | 504 | | Signed832 aSquared = WideArithmetic.MultiplySigned576ToSigned832(Signed576.NarrowValue(a), Signed576.NarrowValue |
| | 510 | 505 | | Signed832 bSquared = WideArithmetic.MultiplySigned576ToSigned832(Signed576.NarrowValue(b), Signed576.NarrowValue |
| | 510 | 506 | | bool useLast = WideArithmetic.CompareNonNegativeProducts(aSquared, lastMetric, bSquared, firstMetric) > 0; |
| | 510 | 507 | | Signed832 alphaNumerator = useLast ? b : a; |
| | 510 | 508 | | Signed832 alphaSquared = useLast ? bSquared : aSquared; |
| | 510 | 509 | | Signed832 alphaMetric = useLast ? lastMetric : firstMetric; |
| | | 510 | | |
| | | 511 | | // n(t)=(1-t)*first+t*last. Nonnegative endpoint projections and |
| | | 512 | | // the triangle inequality give offset.n/|n| >= alpha, the smaller |
| | | 513 | | // endpoint projection divided by its exact Euclidean norm. |
| | | 514 | | // On edge-perpendicular normals, the projected disk support is at |
| | | 515 | | // least beta=R*|Axis.edge|/sqrt(q*edge²). Thus (alpha+beta)/S bounds |
| | | 516 | | // every gap from below; beta>0 follows from the outer ellipse dispatch. |
| | 510 | 517 | | Span<ulong> values = stackalloc ulong[ConvexContactCandidate.Slots * Words]; |
| | 510 | 518 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 2040 | 519 | | values.Clear(); signs.Clear(); values[0] = 1; signs[0] = 1; |
| | 510 | 520 | | Span<ulong> work = stackalloc ulong[6 * Words]; |
| | 1020 | 521 | | Span<ulong> projection = work[..Words], radial = work.Slice(Words, Words); |
| | 1020 | 522 | | Span<ulong> metric = work.Slice(2 * Words, Words), k = work.Slice(3 * Words, Words); |
| | 1020 | 523 | | Span<ulong> temporary = work.Slice(4 * Words, Words), product = work.Slice(5 * Words, Words); |
| | 1020 | 524 | | Span<ulong> rational = values.Slice(7 * Words, Words), radical = values.Slice(8 * Words, Words); |
| | 1020 | 525 | | Span<ulong> root = values.Slice(9 * Words, Words), denominator = values.Slice(10 * Words, Words); |
| | 510 | 526 | | CylinderContactAlgebra.Import(alphaNumerator, projection); |
| | 510 | 527 | | CylinderContactAlgebra.Import(alphaMetric, metric); |
| | 510 | 528 | | Signed832 edgeAxial = WideDirection3d.Dot(edge, geometry.Axis); |
| | 510 | 529 | | Signed832 edgeMetric = WideArithmetic.MultiplySigned832(edge.SquaredLength, geometry.SquaredAxis); |
| | 510 | 530 | | CylinderContactAlgebra.Import(WideArithmetic.MultiplySigned832(edgeAxial, geometry.Radius), radial); |
| | 510 | 531 | | CylinderContactAlgebra.Import(edgeMetric, k); |
| | 510 | 532 | | CylinderContactAlgebra.Import(alphaSquared, temporary); |
| | 510 | 533 | | WideArithmetic.MultiplyMagnitudes(temporary, k, rational); |
| | 510 | 534 | | WideArithmetic.MultiplyMagnitudes(radial, radial, temporary); |
| | 510 | 535 | | WideArithmetic.MultiplyMagnitudes(temporary, metric, product); |
| | 510 | 536 | | WideArithmetic.AddMagnitudeInto(product, rational); |
| | 510 | 537 | | WideArithmetic.MultiplyMagnitudes(projection, radial, radical); |
| | 510 | 538 | | WideArithmetic.AddMagnitudeInto(radical, radical); |
| | 510 | 539 | | WideArithmetic.MultiplyMagnitudes(k, metric, root); |
| | 510 | 540 | | CylinderContactAlgebra.Import(Signed576.ExtendValue(geometry.RawScale), projection); |
| | 510 | 541 | | WideArithmetic.MultiplyMagnitudes(projection, projection, temporary); |
| | 510 | 542 | | WideArithmetic.MultiplyMagnitudes(temporary, root, denominator); |
| | 1020 | 543 | | signs[7] = 1; signs[8] = alphaNumerator.Sign; |
| | | 544 | | // With alpha=a/sqrt(M), h=R*|Axis.edge| and K=q*edge², the |
| | | 545 | | // squared bound is (a²K+h²M+2ah*sqrt(KM))/(KM*S²). |
| | | 546 | | // a<2^338, M<2^340, h<2^366 and K<2^400; a² fits Signed832. |
| | | 547 | | // Every squared-candidate field, including sum carries, is <1100 bits. |
| | | 548 | | // The existing forty-word quadratic candidate owner retains them all. |
| | 510 | 549 | | if (WideConvexPrismRelations.CompareConvexContactCandidates( |
| | 824 | 550 | | new ConvexContactCandidate(values, signs, 1), analytic) >= 0) return true; |
| | 356 | 551 | | if (!allowConcavity) return false; |
| | | 552 | | |
| | | 553 | | // Only ordinary certified vertical-edge cones reach this second test. |
| | | 554 | | // Their ellipse has semiaxes R and r=R*|A.E|/sqrt(K), K=q*E.E. |
| | | 555 | | // max(h'')=(R*R-r*r)/r; alpha above this makes the gap concave, |
| | | 556 | | // so its physical subarc endpoints in the earlier fan retain minima. |
| | 36 | 557 | | Signed832 edgeAxialSquared = WideArithmetic.MultiplySigned576ToSigned832( |
| | 36 | 558 | | Signed576.NarrowValue(edgeAxial), Signed576.NarrowValue(edgeAxial)); |
| | 36 | 559 | | Signed832 crossSquared = WideArithmetic.SubtractSigned832(edgeMetric, edgeAxialSquared); |
| | | 560 | | // a²*K*d² >= R²*(K-d²)²*M. Factors fit 832 bits; complete |
| | | 561 | | // products need<1480 bits. Reuse dead bound scratch in24-word fields. |
| | | 562 | | const int curvatureWords = 24; |
| | 72 | 563 | | Span<ulong> left = work[..curvatureWords], right = work.Slice(curvatureWords, curvatureWords); |
| | 36 | 564 | | Span<ulong> factor = work.Slice(2 * curvatureWords, curvatureWords); |
| | 36 | 565 | | Span<ulong> leftProduct = work.Slice(3 * curvatureWords, curvatureWords); |
| | 72 | 566 | | CylinderContactAlgebra.Import(alphaSquared, factor); CylinderContactAlgebra.Import(edgeMetric, right); |
| | 36 | 567 | | WideArithmetic.MultiplyMagnitudes(factor, right, left); |
| | 36 | 568 | | CylinderContactAlgebra.Import(edgeAxialSquared, factor); |
| | 36 | 569 | | WideArithmetic.MultiplyMagnitudes(left, factor, leftProduct); |
| | 36 | 570 | | CylinderContactAlgebra.Import(crossSquared, factor); |
| | 36 | 571 | | WideArithmetic.MultiplyMagnitudes(factor, factor, right); |
| | 36 | 572 | | CylinderContactAlgebra.Import(WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius), factor); |
| | 36 | 573 | | WideArithmetic.MultiplyMagnitudes(factor, right, left); |
| | 36 | 574 | | CylinderContactAlgebra.Import(alphaMetric, factor); |
| | 36 | 575 | | WideArithmetic.MultiplyMagnitudes(left, factor, right); |
| | 36 | 576 | | return WideArithmetic.CompareMagnitudeEqualLength(leftProduct, right) >= 0; |
| | | 577 | | } |
| | | 578 | | |
| | | 579 | | private static bool HasPrismSupport(in CurvedPrismContactGeometry geometry, in WideDirection3d vertex, |
| | | 580 | | in WideDirection3d first, in WideDirection3d second, scoped ref FiniteAxisValueRoot root) |
| | | 581 | | { |
| | 84 | 582 | | Span<ulong> query = stackalloc ulong[26]; |
| | 84 | 583 | | Span<sbyte> signs = stackalloc sbyte[2]; |
| | 1176 | 584 | | for (int index = 0; index < geometry.VertexCount; index++) |
| | | 585 | | { |
| | 518 | 586 | | WideDirection3d delta = CurvedPrismContact.Difference(geometry.Vertex(index), vertex); |
| | 1036 | 587 | | Signed832 a = WideDirection3d.Dot(delta, first), b = WideDirection3d.Dot(delta, second); |
| | | 588 | | // An affine constraint nonnegative at both endpoints holds on the whole chart. |
| | 518 | 589 | | if (a.Sign >= 0 && WideArithmetic.AddSigned832(a, b).Sign >= 0) continue; |
| | 34 | 590 | | WideArithmetic.GetMagnitude(a, query[..13]); |
| | 34 | 591 | | WideArithmetic.GetMagnitude(b, query[13..]); |
| | 68 | 592 | | signs[0] = (sbyte)a.Sign; signs[1] = (sbyte)b.Sign; |
| | 48 | 593 | | if (WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, signs) < 0) return false; |
| | | 594 | | } |
| | 70 | 595 | | return true; |
| | | 596 | | } |
| | | 597 | | |
| | | 598 | | /// <summary> |
| | | 599 | | /// Writes W,Q,K,L,J,N,D into 0..25; 26..27 are the oriented cap axial |
| | | 600 | | /// projection (unoriented for a cone), and 28..30 the cone's strict base |
| | | 601 | | /// switch. All coefficients use forty-word fields. |
| | | 602 | | /// </summary> |
| | | 603 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 604 | | private static void BuildParameter(in CurvedPrismContactGeometry geometry, int edge, |
| | | 605 | | in WideDirection3d first, in WideDirection3d second, int cap, |
| | | 606 | | Span<ulong> data, Span<sbyte> signs) |
| | | 607 | | { |
| | | 608 | | // |u|<2^33, q<2^65, |e|,|first|,|second|<2^166, |
| | | 609 | | // |c|<2^167, R<2^163 and S<2^99 give M<2^335,P<2^401, |
| | | 610 | | // Q<2^792,K<2^740,L<2^1130,W,N<2^2290,D<2^2150. |
| | | 611 | | // Forty words retain all convolution carries without scalar narrowing. |
| | 935 | 612 | | Span<ulong> scratch = stackalloc ulong[22 * Words]; |
| | 935 | 613 | | Span<sbyte> scratchSigns = stackalloc sbyte[21]; |
| | 3740 | 614 | | scratch.Clear(); scratchSigns.Clear(); data.Clear(); signs.Clear(); |
| | 935 | 615 | | Polynomial work = new(scratch[..(21 * Words)], scratchSigns, Words); |
| | 2805 | 616 | | Polynomial p = work.Slice(0, 2), q = work.Slice(2, 1), radiusSquared = work.Slice(3, 1); |
| | 2805 | 617 | | Polynomial factor = work.Slice(4, 1), axial = work.Slice(5, 2), axialSquared = work.Slice(7, 3); |
| | 2805 | 618 | | Polynomial metric = work.Slice(10, 3), radial = work.Slice(13, 3), radical = work.Slice(16, 3); |
| | 1870 | 619 | | Polynomial scale = work.Slice(19, 1), scaledQ = work.Slice(20, 1); |
| | 935 | 620 | | Span<ulong> product = scratch.Slice(21 * Words, Words); |
| | 935 | 621 | | WideDirection3d offset = GetOffset(geometry, edge, cap); |
| | 935 | 622 | | Write(q, 0, Signed832.ExtendValue(geometry.SquaredAxis)); |
| | 935 | 623 | | Write(radiusSquared, 0, Signed832.ExtendValue(WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius) |
| | 935 | 624 | | Write(axial, 0, WideDirection3d.Dot(first, geometry.Axis)); |
| | 935 | 625 | | Write(axial, 1, WideDirection3d.Dot(second, geometry.Axis)); |
| | 935 | 626 | | Write(metric, 0, first.SquaredLength); |
| | 935 | 627 | | Write(metric, 1, Twice(WideDirection3d.Dot(first, second))); |
| | 935 | 628 | | Write(metric, 2, second.SquaredLength); |
| | 935 | 629 | | Multiply(radial, q, metric, product); |
| | 1870 | 630 | | Multiply(axialSquared, axial, axial, product); Add(radial, axialSquared, -1); |
| | 935 | 631 | | Multiply(factor, radiusSquared, q, product); |
| | 935 | 632 | | Multiply(radical, factor, radial, product); |
| | 935 | 633 | | Write(p, 0, WideArithmetic.MultiplySigned832(WideDirection3d.Dot(offset, first), geometry.SquaredAxis)); |
| | 935 | 634 | | Write(p, 1, WideArithmetic.MultiplySigned832(WideDirection3d.Dot(offset, second), geometry.SquaredAxis)); |
| | 935 | 635 | | Write(scale, 0, Signed832.ExtendValue(geometry.RawScale)); |
| | 935 | 636 | | Multiply(scaledQ, q, scale, product); |
| | 935 | 637 | | bool reducedMetric = false; |
| | | 638 | | // Both chart vectors are nonzero; their squared lengths are positive. |
| | | 639 | | System.Diagnostics.Debug.Assert(metric.Signs[0] > 0 && metric.Signs[2] > 0); |
| | 935 | 640 | | if (metric.Signs[1] < 0) |
| | | 641 | | { |
| | 712 | 642 | | metric.Values[..Words].CopyTo(product); |
| | 712 | 643 | | WideArithmetic.AddMagnitudeInto(metric.Values[..Words], product); |
| | 712 | 644 | | reducedMetric = WideArithmetic.CompareMagnitudeEqualLength(product, metric.Values.Slice(Words, Words)) == 0 |
| | 712 | 645 | | && WideArithmetic.CompareMagnitudeEqualLength(product, metric.Values.Slice(2 * Words, Words)) == 0; |
| | 712 | 646 | | if (reducedMetric) |
| | | 647 | | { |
| | | 648 | | // M=m*(1-2t+2t^2). Retain m/2m in dead factor/product |
| | | 649 | | // scratch, then remove m before the derivative products. |
| | 141 | 650 | | metric.Values[..Words].CopyTo(factor.Values); |
| | 141 | 651 | | factor.Signs[0] = 1; |
| | 141 | 652 | | metric.Values.Clear(); |
| | 141 | 653 | | metric.Values[0] = 1; |
| | 141 | 654 | | metric.Values[Words] = metric.Values[2 * Words] = 2; |
| | | 655 | | } |
| | | 656 | | } |
| | 935 | 657 | | CylinderEdgeContactPolynomial.BuildStationary(p.Values, p.Signs, radical.Values, radical.Signs, |
| | 935 | 658 | | metric.Values, metric.Signs, scaledQ.Values, data[..(26 * Words)], signs[..26]); |
| | 935 | 659 | | Polynomial storage = new(data, signs, Words); |
| | 935 | 660 | | if (reducedMetric) |
| | | 661 | | { |
| | | 662 | | // Restore physical M before CompleteGap: K/L/J lost m, so N |
| | | 663 | | // and D both lose m^2. Reduced M here would corrupt the ratio. |
| | 141 | 664 | | factor.Values.CopyTo(storage.Slice(16, 1).Values); |
| | 141 | 665 | | product.CopyTo(storage.Slice(17, 1).Values); |
| | 141 | 666 | | product.CopyTo(storage.Slice(18, 1).Values); |
| | 141 | 667 | | storage.Signs[16] = storage.Signs[18] = 1; |
| | 141 | 668 | | storage.Signs[17] = -1; |
| | | 669 | | } |
| | 935 | 670 | | Add(storage.Slice(26, 2), axial, geometry.IsCone ? 1 : cap); |
| | 1540 | 671 | | if (!geometry.IsCone) return; |
| | 330 | 672 | | Polynomial region = storage.Slice(28, 3); |
| | 330 | 673 | | Multiply(region, radiusSquared, radial, product); |
| | | 674 | | // T may grow when Axis is made primitive; T²*q itself is unchanged. |
| | 330 | 675 | | Write(scale, 0, Signed832.ExtendValue(WideArithmetic.MultiplySigned320( |
| | 330 | 676 | | Signed320.ExtendValue(geometry.HalfFactor), geometry.HalfFactor))); |
| | 330 | 677 | | Multiply(scaledQ, q, scale, product); |
| | 660 | 678 | | Multiply(radical, scaledQ, axialSquared, product); Add(region, radical, -4); |
| | 330 | 679 | | } |
| | | 680 | | |
| | | 681 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 682 | | private static void BuildValues(in CurvedPrismContactGeometry geometry, int edge, int cap, |
| | | 683 | | Span<ulong> values, Span<sbyte> signs) |
| | | 684 | | { |
| | 101 | 685 | | geometry.Edge(edge, out int start, out int end); |
| | 101 | 686 | | WideDirection3d e = CurvedPrismContact.Difference(geometry.Vertex(end), geometry.Vertex(start)); |
| | 101 | 687 | | WideDirection3d c = GetOffset(geometry, edge, cap); |
| | 101 | 688 | | Span<ulong> invariants = stackalloc ulong[8 * ValueWords]; |
| | 101 | 689 | | Span<sbyte> invariantSigns = stackalloc sbyte[8]; |
| | 202 | 690 | | invariants.Clear(); invariantSigns.Clear(); |
| | 101 | 691 | | Polynomial scalars = new(invariants, invariantSigns, ValueWords); |
| | 101 | 692 | | Write(scalars, 0, Signed832.ExtendValue(geometry.SquaredAxis)); |
| | 101 | 693 | | Write(scalars, 1, e.SquaredLength); |
| | 101 | 694 | | Write(scalars, 2, WideDirection3d.Dot(e, geometry.Axis)); |
| | 101 | 695 | | Write(scalars, 3, c.SquaredLength); |
| | 101 | 696 | | Write(scalars, 4, WideDirection3d.Dot(c, geometry.Axis)); |
| | 101 | 697 | | Write(scalars, 5, WideDirection3d.Dot(e, c)); |
| | 101 | 698 | | Write(scalars, 6, Signed832.ExtendValue(WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius))); |
| | 101 | 699 | | CylinderPairSideValuePolynomial.BuildUnshifted(invariants, invariantSigns, ValueWords, values, signs); |
| | | 700 | | // Under the parameter bounds, weighted l,m,v heights are <750 |
| | | 701 | | // after z=2^ValueShift*t (ValueShift<=338). Quartic discriminant |
| | | 702 | | // summands and their carries stay below 3100 bits, within 56 words. |
| | 101 | 703 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(values, 5, geometry.ValueShift); |
| | 101 | 704 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(values, signs); |
| | 101 | 705 | | } |
| | | 706 | | |
| | | 707 | | /// <summary>Restores one retained parameter and rounds its paired physical witnesses once.</summary> |
| | | 708 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 709 | | internal static void GetMaterials(in CurvedPrismContactGeometry geometry, in Selection selection, |
| | | 710 | | Vector3d center, FixedQuaternion rotation, Vector3d prismOrigin, |
| | | 711 | | out Vector3d normal, out FixedPointAnchor shapeAnchor, out FixedPointAnchor prismAnchor) |
| | | 712 | | { |
| | 55 | 713 | | Span<ulong> data = stackalloc ulong[Slots * Words]; |
| | 55 | 714 | | Span<sbyte> signs = stackalloc sbyte[Slots]; |
| | 55 | 715 | | BuildParameter(geometry, selection.Edge, selection.First, selection.Second, selection.CapSign, data, signs); |
| | 55 | 716 | | CylinderEdgeContactPolynomial.CompleteGap(data[..(26 * Words)], signs[..26]); |
| | 55 | 717 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords( |
| | 55 | 718 | | data[..(5 * Words)], signs[..5])]; |
| | 55 | 719 | | bool found = WideFiniteAxisIntersection.TryGetFiniteValueRoot(data[..(5 * Words)], signs[..5], |
| | 55 | 720 | | selection.ParameterOrdinal, cell, out FiniteAxisValueRoot root); |
| | | 721 | | System.Diagnostics.Debug.Assert(found); |
| | 55 | 722 | | Span<ulong> gradient = stackalloc ulong[54]; |
| | 55 | 723 | | Span<sbyte> gradientSigns = stackalloc sbyte[6]; |
| | 55 | 724 | | CircularRimContactAlgebra.WriteGradient(selection.First.Transform(geometry.WorldBasis), |
| | 55 | 725 | | selection.Second.Transform(geometry.WorldBasis), gradient, gradientSigns); |
| | 55 | 726 | | normal = ConvexContactValueRoot.GetNormalizedDirection(ref root, gradient, gradientSigns, 1); |
| | | 727 | | |
| | 55 | 728 | | Span<ulong> points = stackalloc ulong[36 * Words]; |
| | 55 | 729 | | Span<sbyte> pointSigns = stackalloc sbyte[36]; |
| | 110 | 730 | | points.Clear(); pointSigns.Clear(); |
| | 55 | 731 | | Polynomial storage = new(points, pointSigns, Words); |
| | 110 | 732 | | Polynomial shape = storage.Slice(0, 9), prism = storage.Slice(9, 18); |
| | 110 | 733 | | Polynomial shapeDenominator = storage.Slice(27, 3), prismDenominator = storage.Slice(30, 6); |
| | 55 | 734 | | BuildPointPolynomials(geometry, selection, data, signs, shape, prism, shapeDenominator, prismDenominator); |
| | 55 | 735 | | shapeAnchor = CreateRootAnchor(geometry, center, rotation, center, shape, shapeDenominator, ref root); |
| | 55 | 736 | | prismAnchor = CreateRootAnchor(geometry, center, rotation, prismOrigin, prism, prismDenominator, ref root); |
| | 55 | 737 | | } |
| | | 738 | | |
| | | 739 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 740 | | private static void BuildPointPolynomials(in CurvedPrismContactGeometry geometry, in Selection selection, |
| | | 741 | | Span<ulong> data, Span<sbyte> signs, Polynomial shape, Polynomial prism, |
| | | 742 | | Polynomial shapeDenominator, Polynomial prismDenominator) |
| | | 743 | | { |
| | 233 | 744 | | Polynomial parameter = new(data, signs, Words); |
| | 699 | 745 | | Polynomial k = parameter.Slice(8, 2), l = parameter.Slice(10, 3), j = parameter.Slice(13, 3); |
| | 466 | 746 | | l.Values.CopyTo(shapeDenominator.Values); l.Signs.CopyTo(shapeDenominator.Signs); |
| | 233 | 747 | | Span<ulong> work = stackalloc ulong[25 * Words]; |
| | 233 | 748 | | Span<sbyte> workSigns = stackalloc sbyte[24]; |
| | 466 | 749 | | work.Clear(); workSigns.Clear(); |
| | 233 | 750 | | Polynomial scratch = new(work[..(24 * Words)], workSigns, Words); |
| | 699 | 751 | | Polynomial q = scratch.Slice(0, 1), radiusSquared = scratch.Slice(1, 1), metric = scratch.Slice(2, 3); |
| | 699 | 752 | | Polynomial qk = scratch.Slice(5, 2), qkm = scratch.Slice(7, 4), n = scratch.Slice(11, 2); |
| | 699 | 753 | | Polynomial axial = scratch.Slice(13, 2), radial = scratch.Slice(15, 2), scalar = scratch.Slice(17, 1); |
| | 233 | 754 | | Polynomial term = scratch.Slice(18, 6); |
| | 233 | 755 | | Span<ulong> product = work.Slice(24 * Words, Words); |
| | 233 | 756 | | Span<ulong> separation = stackalloc ulong[6 * Words]; |
| | 233 | 757 | | Span<sbyte> separationSigns = stackalloc sbyte[6]; |
| | 233 | 758 | | Polynomial separationPolynomial = new(separation, separationSigns, Words); |
| | 233 | 759 | | Write(q, 0, Signed832.ExtendValue(geometry.SquaredAxis)); |
| | 233 | 760 | | Write(radiusSquared, 0, Signed832.ExtendValue(WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius) |
| | 233 | 761 | | Write(metric, 0, selection.First.SquaredLength); |
| | 233 | 762 | | Write(metric, 1, Twice(WideDirection3d.Dot(selection.First, selection.Second))); |
| | 233 | 763 | | Write(metric, 2, selection.Second.SquaredLength); |
| | 233 | 764 | | Write(axial, 0, WideDirection3d.Dot(selection.First, geometry.Axis)); |
| | 233 | 765 | | Write(axial, 1, WideDirection3d.Dot(selection.Second, geometry.Axis)); |
| | 466 | 766 | | Multiply(qk, q, k, product); Multiply(qkm, qk, metric, product); |
| | 233 | 767 | | Multiply(prismDenominator, l, qkm, product); |
| | 233 | 768 | | WideDirection3d half = selection.CapSign < 0 ? -geometry.Half : geometry.Half; |
| | 1864 | 769 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 770 | | { |
| | 699 | 771 | | Write(n, 0, Signed832.ExtendValue(Component(selection.First, coordinate))); |
| | 699 | 772 | | Write(n, 1, Signed832.ExtendValue(Component(selection.Second, coordinate))); |
| | 699 | 773 | | Multiply(radial, q, n, product); |
| | 699 | 774 | | Write(scalar, 0, Signed832.ExtendValue(Component(geometry.Axis, coordinate))); |
| | 1398 | 775 | | Multiply(term.Slice(0, 2), axial, scalar, product); Add(radial, term.Slice(0, 2), -1); |
| | | 776 | | // sqrt(Q)=-L/K, so the exact rim point is |
| | | 777 | | // capH-R²*K*(q*n-(u.n)*u)/L, with no new radical. |
| | 699 | 778 | | Multiply(term.Slice(0, 3), k, radial, product); |
| | 699 | 779 | | Multiply(shape.Slice(3 * coordinate, 3), term.Slice(0, 3), radiusSquared, product); |
| | 5592 | 780 | | for (int coefficient = 0; coefficient < 3; coefficient++) |
| | 2097 | 781 | | shape.Signs[3 * coordinate + coefficient] *= -1; |
| | 699 | 782 | | Write(scalar, 0, Signed832.ExtendValue(Component(half, coordinate))); |
| | 699 | 783 | | Multiply(term.Slice(0, 3), scalar, l, product); |
| | 699 | 784 | | Add(shape.Slice(3 * coordinate, 3), term.Slice(0, 3), 1); |
| | | 785 | | // Subtract the exact normal gap J*n/(q*K*M) from that point. |
| | 699 | 786 | | Multiply(prism.Slice(6 * coordinate, 6), shape.Slice(3 * coordinate, 3), qkm, product); |
| | 699 | 787 | | Multiply(term.Slice(0, 4), j, n, product); |
| | 699 | 788 | | Multiply(separationPolynomial, term.Slice(0, 4), l, product); |
| | 699 | 789 | | Add(prism.Slice(6 * coordinate, 6), separationPolynomial, -1); |
| | | 790 | | } |
| | 233 | 791 | | } |
| | | 792 | | |
| | | 793 | | private static FixedPointAnchor CreateRootAnchor(in CurvedPrismContactGeometry geometry, |
| | | 794 | | Vector3d center, FixedQuaternion rotation, Vector3d targetOrigin, |
| | | 795 | | scoped Polynomial point, scoped Polynomial denominator, scoped ref FiniteAxisValueRoot root) |
| | | 796 | | { |
| | 110 | 797 | | int count = denominator.Signs.Length; |
| | 110 | 798 | | Span<ulong> work = stackalloc ulong[(3 * count + 2) * Words]; |
| | 110 | 799 | | Span<sbyte> workSigns = stackalloc sbyte[3 * count + 2]; |
| | 220 | 800 | | work.Clear(); workSigns.Clear(); |
| | 110 | 801 | | Polynomial storage = new(work, workSigns, Words); |
| | 220 | 802 | | Polynomial scaledDenominator = storage.Slice(0, count), finalDenominator = storage.Slice(count, count); |
| | 220 | 803 | | Polynomial numerator = storage.Slice(2 * count, count), scalar = storage.Slice(3 * count, 1); |
| | 110 | 804 | | Span<ulong> product = work.Slice((3 * count + 1) * Words, Words); |
| | 110 | 805 | | Write(scalar, 0, Signed832.ExtendValue(geometry.RawScale)); |
| | 110 | 806 | | Multiply(scaledDenominator, denominator, scalar, product); |
| | 110 | 807 | | Write(scalar, 0, Signed832.ExtendValue(geometry.WorldBasis.Denominator)); |
| | 110 | 808 | | Multiply(finalDenominator, scaledDenominator, scalar, product); |
| | 110 | 809 | | int denominatorSign = Sign(ref root, finalDenominator); |
| | | 810 | | System.Diagnostics.Debug.Assert(denominatorSign != 0); |
| | 110 | 811 | | WideOrientedBox.GetRelativeLocalPointNumerators(center, targetOrigin, geometry.WorldBasis, |
| | 110 | 812 | | out Signed192 dx, out Signed192 dy, out Signed192 dz); |
| | 220 | 813 | | Vector3d first = default, second = default; |
| | 880 | 814 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 815 | | { |
| | | 816 | | // Convert P/(S*den) into the target origin's exact primitive-local |
| | | 817 | | // frame: (D*P+delta*den*S)/(D*den*S). Coordinate queries, including |
| | | 818 | | // full raw-domain midpoint products, remain below 2520 bits. |
| | 330 | 819 | | Write(scalar, 0, Signed832.ExtendValue(geometry.WorldBasis.Denominator)); |
| | 330 | 820 | | Multiply(numerator, point.Slice(coordinate * count, count), scalar, product); |
| | 330 | 821 | | Write(scalar, 0, Signed832.ExtendValue(coordinate == 0 ? dx : coordinate == 1 ? dy : dz)); |
| | 330 | 822 | | AddProduct(numerator, scaledDenominator, scalar, 1, product); |
| | 330 | 823 | | long leading = CompareRatio(numerator, finalDenominator, Signed192.Signed(long.MaxValue), denominatorSign, r |
| | 330 | 824 | | ? long.MaxValue |
| | 330 | 825 | | : CompareRatio(numerator, finalDenominator, Signed192.Signed(long.MinValue), denominatorSign, ref root) |
| | 330 | 826 | | ? long.MinValue : 0; |
| | 330 | 827 | | if (leading != 0) |
| | | 828 | | { |
| | 2 | 829 | | Write(scalar, 0, Signed832.ExtendValue(Signed192.Signed(leading))); |
| | 2 | 830 | | AddProduct(numerator, finalDenominator, scalar, -1, product); |
| | | 831 | | } |
| | 330 | 832 | | Fixed64 rounded = RoundRatio(numerator, finalDenominator, denominatorSign, leading, ref root); |
| | 330 | 833 | | first[coordinate] = leading == 0 ? rounded : Fixed64.FromRaw(leading); |
| | 330 | 834 | | second[coordinate] = leading == 0 ? Fixed64.Zero : rounded; |
| | | 835 | | } |
| | | 836 | | // Primitive support coordinates are below 3/2 Max; a prism local |
| | | 837 | | // coordinate is below sqrt(3) Max. Two additive terms preserve both. |
| | 110 | 838 | | return new FixedPointAnchor(targetOrigin, rotation, first, second); |
| | | 839 | | } |
| | | 840 | | |
| | | 841 | | private static Fixed64 RoundRatio(scoped Polynomial numerator, scoped Polynomial denominator, int denominatorSign, |
| | | 842 | | long leading, scoped ref FiniteAxisValueRoot root) |
| | | 843 | | { |
| | 660 | 844 | | long low = long.MinValue, high = long.MaxValue; |
| | 21450 | 845 | | while (low < high) |
| | | 846 | | { |
| | 21120 | 847 | | ulong span = unchecked((ulong)high - (ulong)low); |
| | 21120 | 848 | | long middle = unchecked((long)((ulong)low + (span >> 1) + (span & 1))); |
| | 30086 | 849 | | if (CompareRatio(numerator, denominator, Signed192.Signed(middle), denominatorSign, ref root) >= 0) low = mi |
| | 12154 | 850 | | else high = middle - 1; |
| | | 851 | | } |
| | 330 | 852 | | Signed192 half = WideArithmetic.AddSigned192( |
| | 330 | 853 | | WideArithmetic.AddSigned192(Signed192.Signed(low), Signed192.Signed(low)), Signed192.Signed(1)); |
| | 330 | 854 | | int comparison = CompareRatio(numerator, denominator, half, denominatorSign, ref root, twiceNumerator: true); |
| | | 855 | | // Ties belong to the complete coordinate, including an odd split term. |
| | 330 | 856 | | return Fixed64.FromRaw(low + (comparison > 0 || comparison == 0 && ((low ^ leading) & 1) != 0 ? 1 : 0)); |
| | | 857 | | } |
| | | 858 | | |
| | | 859 | | private static int CompareRatio(scoped Polynomial numerator, scoped Polynomial denominator, Signed192 threshold, |
| | | 860 | | int denominatorSign, scoped ref FiniteAxisValueRoot root, bool twiceNumerator = false) |
| | | 861 | | { |
| | 22109 | 862 | | int count = denominator.Signs.Length; |
| | 22109 | 863 | | Span<ulong> data = stackalloc ulong[(count + 2) * Words]; |
| | 22109 | 864 | | Span<sbyte> signs = stackalloc sbyte[count + 1]; |
| | 44218 | 865 | | data.Clear(); signs.Clear(); |
| | 22109 | 866 | | Polynomial storage = new(data[..((count + 1) * Words)], signs, Words); |
| | 44218 | 867 | | Polynomial query = storage.Slice(0, count), scalar = storage.Slice(count, 1); |
| | 22109 | 868 | | Span<ulong> product = data.Slice((count + 1) * Words, Words); |
| | 22109 | 869 | | Write(scalar, 0, Signed832.ExtendValue(threshold)); |
| | 22109 | 870 | | Multiply(query, denominator, scalar, product); |
| | 342685 | 871 | | for (int index = 0; index < count; index++) query.Signs[index] *= -1; |
| | 22109 | 872 | | Add(query, numerator, twiceNumerator ? 2 : 1); |
| | 22109 | 873 | | return Sign(ref root, query) * denominatorSign; |
| | | 874 | | } |
| | | 875 | | |
| | | 876 | | private static void AddProduct(Polynomial target, Polynomial first, Polynomial second, int multiplier, |
| | | 877 | | Span<ulong> product) |
| | | 878 | | { |
| | 2137 | 879 | | Span<ulong> values = stackalloc ulong[target.Values.Length]; |
| | 2137 | 880 | | Span<sbyte> signs = stackalloc sbyte[target.Signs.Length]; |
| | 2137 | 881 | | Polynomial term = new(values, signs, target.Words); |
| | 4274 | 882 | | Multiply(term, first, second, product); Add(target, term, multiplier); |
| | 2137 | 883 | | } |
| | | 884 | | |
| | | 885 | | private static FiniteAxisValueRoot RestoreValue(ReadOnlySpan<ulong> values, |
| | 93 | 886 | | ReadOnlySpan<sbyte> signs, Span<ulong> cell, in Selection selection) => new() |
| | 93 | 887 | | { |
| | 93 | 888 | | Coefficients = values, |
| | 93 | 889 | | Signs = signs, |
| | 93 | 890 | | LowerNumerator = cell, |
| | 93 | 891 | | DenominatorShift = selection.ValueShift, |
| | 93 | 892 | | Ordinal = selection.ValueOrdinal, |
| | 93 | 893 | | IsRational = selection.IsRational |
| | 93 | 894 | | }; |
| | | 895 | | |
| | | 896 | | private static WideDirection3d GetOffset(in CurvedPrismContactGeometry geometry, int edge, int cap) |
| | | 897 | | { |
| | 1036 | 898 | | geometry.Edge(edge, out int start, out _); |
| | 1036 | 899 | | return CurvedPrismContact.Difference(cap < 0 ? -geometry.Half : geometry.Half, geometry.Vertex(start)); |
| | | 900 | | } |
| | 1168 | 901 | | private static Signed832 Twice(Signed832 value) => WideArithmetic.AddSigned832(value, value); |
| | | 902 | | private static Signed576 Component(in WideDirection3d value, int coordinate) => |
| | 4264 | 903 | | coordinate == 0 ? value.X : coordinate == 1 ? value.Y : value.Z; |
| | | 904 | | private static Signed320 Component(in WideAxis3 value, int coordinate) => |
| | 849 | 905 | | coordinate == 0 ? value.X : coordinate == 1 ? value.Y : value.Z; |
| | | 906 | | private static int Sign(scoped ref FiniteAxisValueRoot root, scoped Polynomial polynomial) => |
| | 26225 | 907 | | WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, polynomial.Values, polynomial.Signs); |
| | | 908 | | private static void Write(Polynomial polynomial, int index, Signed832 value) |
| | | 909 | | { |
| | 42510 | 910 | | Span<ulong> target = polynomial.Values.Slice(index * polynomial.Words, polynomial.Words); |
| | 127530 | 911 | | target.Clear(); WideArithmetic.GetMagnitude(value, target[..13]); polynomial.Signs[index] = (sbyte)value.Sign; |
| | 42510 | 912 | | } |
| | | 913 | | private static void Multiply(Polynomial result, Polynomial first, Polynomial second, Span<ulong> product) => |
| | 37894 | 914 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(first.Values, first.Signs, |
| | 37894 | 915 | | second.Values, second.Signs, result.Values, result.Signs, product); |
| | | 916 | | private static void Add(Polynomial result, Polynomial source, int multiplier) => |
| | 28879 | 917 | | WideFiniteAxisIntersection.AddFiniteAxisPolynomial(source.Values, source.Signs, |
| | 28879 | 918 | | result.Values, result.Signs, multiplier); |
| | | 919 | | private readonly ref struct Polynomial |
| | | 920 | | { |
| | | 921 | | internal readonly Span<ulong> Values; |
| | | 922 | | internal readonly Span<sbyte> Signs; |
| | | 923 | | internal readonly int Words; |
| | | 924 | | internal Polynomial(Span<ulong> values, Span<sbyte> signs, int words) |
| | 442012 | 925 | | { Values = values; Signs = signs; Words = words; } |
| | | 926 | | internal Polynomial Slice(int offset, int count) => |
| | 81209 | 927 | | new(Values.Slice(offset * Words, count * Words), Signs.Slice(offset, count), Words); |
| | | 928 | | } |
| | | 929 | | } |