| | | 1 | | //======================================================================= |
| | | 2 | | // CurvedPrismContactValue.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary>Borrowed native minimum; comparisons never retain or round its wide value.</summary> |
| | | 13 | | internal readonly ref struct CurvedPrismContactValue |
| | | 14 | | { |
| | | 15 | | private readonly ConvexContactCandidate _analytic; |
| | | 16 | | private readonly CurvedPrismRimContacts.Selection _rim; |
| | | 17 | | private readonly bool _hasValue; |
| | 22 | 18 | | internal bool HasValue => _hasValue; |
| | | 19 | | internal CurvedPrismContactValue(ConvexContactCandidate analytic, bool hasAnalytic, in CurvedPrismRimContacts.Select |
| | 256 | 20 | | { _analytic = analytic; _rim = rim; _hasValue = hasAnalytic || rim.HasValue; } |
| | | 21 | | |
| | | 22 | | // Compares N*sqrt(metric)/(D*|scale|) with the native nonnegative gap. |
| | | 23 | | // Squaring preserves order. Keep the CQ denominator instead of multiplying |
| | | 24 | | // by its conjugate: the existing ratio owner already compares two fields. |
| | | 25 | | internal int CompareRootRatio(in CurvedPrismContactGeometry geometry, scoped ContactQuadratic numerator, |
| | | 26 | | scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> root, Signed832 metric, Signed320 scale) |
| | | 27 | | { |
| | 14 | 28 | | if (!HasValue) return -1; |
| | 30 | 29 | | Span<ulong> metricWords = stackalloc ulong[13], scaleWords = stackalloc ulong[5], squaredScale = stackalloc ulon |
| | 10 | 30 | | WideArithmetic.GetMagnitude(metric, metricWords); |
| | 10 | 31 | | WideArithmetic.GetMagnitude(scale, out scaleWords[4], out scaleWords[3], out scaleWords[2], out scaleWords[1], o |
| | 10 | 32 | | WideArithmetic.MultiplyMagnitudes(scaleWords, scaleWords, squaredScale); |
| | 10 | 33 | | int words = 2 * Math.Max(ContactQuadratic.ActiveWords(numerator), ContactQuadratic.ActiveWords(denominator)) |
| | 10 | 34 | | + WideArithmetic.GetActiveMagnitudeLength(root) |
| | 10 | 35 | | + Math.Max(WideArithmetic.GetActiveMagnitudeLength(metricWords), WideArithmetic.GetActiveMagnitudeLength(squ |
| | 20 | 36 | | Span<ulong> fields = stackalloc ulong[6 * words]; Span<int> signs = stackalloc int[6]; |
| | 20 | 37 | | var n = ContactQuadratic.At(fields, signs, 0, words); var d = ContactQuadratic.At(fields, signs, 1, words); |
| | 10 | 38 | | var term = ContactQuadratic.At(fields, signs, 2, words); |
| | 30 | 39 | | ContactQuadratic.Multiply(numerator, numerator, root, n); ContactQuadratic.Scale(n, metricWords, 1, term); term. |
| | 30 | 40 | | ContactQuadratic.Multiply(denominator, denominator, root, d); ContactQuadratic.Scale(d, squaredScale, 1, term); |
| | 12 | 41 | | if (_rim.HasValue) return -CurvedPrismRimContacts.CompareSquaredDepth(geometry, _rim, n, d, root); |
| | 8 | 42 | | int analyticWords = Math.Max(1, Math.Max(WideArithmetic.GetActiveMagnitudeLength(_analytic.GapDenominator), |
| | 8 | 43 | | Math.Max(WideArithmetic.GetActiveMagnitudeLength(_analytic.GapRational), WideArithmetic.GetActiveMagnitudeLe |
| | 16 | 44 | | Span<ulong> analyticValues = stackalloc ulong[4 * analyticWords]; Span<int> analyticSigns = stackalloc int[4]; |
| | 16 | 45 | | var a = ContactQuadratic.At(analyticValues, analyticSigns, 0, analyticWords); var b = ContactQuadratic.At(analyt |
| | 16 | 46 | | a.Clear(); b.Clear(); |
| | 8 | 47 | | _analytic.GapRational[..WideArithmetic.GetActiveMagnitudeLength(_analytic.GapRational)].CopyTo(a.Rational); |
| | 8 | 48 | | _analytic.GapRadical[..WideArithmetic.GetActiveMagnitudeLength(_analytic.GapRadical)].CopyTo(a.Radical); |
| | 16 | 49 | | a.Signs[0] = _analytic.GapRationalSign; a.Signs[1] = _analytic.GapRadicalSign; |
| | 16 | 50 | | _analytic.GapDenominator[..WideArithmetic.GetActiveMagnitudeLength(_analytic.GapDenominator)].CopyTo(b.Rational) |
| | 8 | 51 | | return ContactQuadratic.CompareRatios(n, d, root, a, b, _analytic.GapRadicand); |
| | | 52 | | } |
| | | 53 | | } |