| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderPairRimFeatures.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 admission and materialization of cylinder rim stationary values. |
| | | 15 | | /// Construction scratch is released before any algebraic-root evaluation. |
| | | 16 | | /// </summary> |
| | | 17 | | internal static class CylinderPairRimFeatures |
| | | 18 | | { |
| | | 19 | | // In the primitive first-rotation frame, weighted coefficient heights |
| | | 20 | | // are <7372 bits (value), <7376 bits (cap-direction derivative), and |
| | | 21 | | // <7212 bits (world-direction derivative), after S=2^ValueShift*t. |
| | | 22 | | internal const int CoefficientWords = 116; |
| | | 23 | | private const int Count = CylinderPairValuePolynomial.CoefficientCount; |
| | | 24 | | internal const int AdmissionCoefficientCount = 4 * Count; |
| | | 25 | | |
| | | 26 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 27 | | internal static int BuildValues(in CylinderPairGeometry geometry, int firstSign, int secondSign, |
| | | 28 | | Span<ulong> coefficients, Span<sbyte> signs) |
| | | 29 | | { |
| | 1269 | 30 | | Span<ulong> invariants = stackalloc ulong[CylinderPairValuePolynomial.InvariantCount * CoefficientWords]; |
| | 1269 | 31 | | Span<sbyte> invariantSigns = stackalloc sbyte[CylinderPairValuePolynomial.InvariantCount]; |
| | 1269 | 32 | | Span<ulong> scratch = stackalloc ulong[CylinderPairValuePolynomial.ScratchCoefficientCount * CoefficientWords]; |
| | 1269 | 33 | | Span<sbyte> scratchSigns = stackalloc sbyte[CylinderPairValuePolynomial.ScratchCoefficientCount]; |
| | 1269 | 34 | | geometry.WriteInvariants(firstSign, secondSign, invariants, invariantSigns, CoefficientWords); |
| | 1269 | 35 | | int degree = CylinderPairValuePolynomial.Build(invariants, invariantSigns, CoefficientWords, |
| | 1269 | 36 | | coefficients, signs, scratch, scratchSigns); |
| | 1269 | 37 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(coefficients, Count, geometry.ValueShift); |
| | 1269 | 38 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(coefficients, signs); |
| | 1269 | 39 | | return degree; |
| | | 40 | | } |
| | | 41 | | |
| | | 42 | | /// <summary> |
| | | 43 | | /// Returns the derivative sign after the caller has excluded repeated roots. |
| | | 44 | | /// Isolation guarantees nonroot cell endpoints, so a simple crossing has |
| | | 45 | | /// the opposite sign to its lower endpoint. Exact dyadic roots use F' directly. |
| | | 46 | | /// </summary> |
| | | 47 | | internal static int GetSimpleValueSlopeSign(FiniteAxisValueRoot root) => |
| | 4382 | 48 | | (root.IsRational ? 1 : -1) * WideFiniteAxisIntersection.EvaluateFiniteRootPolynomial( |
| | 4382 | 49 | | root.Coefficients, root.Signs, root.LowerNumerator, root.DenominatorShift, |
| | 4382 | 50 | | root.IsRational ? 1 : 0); |
| | | 51 | | |
| | | 52 | | /// <summary> |
| | | 53 | | /// Reuses four caller-owned directional polynomials across this cap pair's |
| | | 54 | | /// simple roots. Reset readyMask to zero whenever geometry or cap signs change. |
| | | 55 | | /// Query storage has AdmissionCoefficientCount equal-width slots and signs. |
| | | 56 | | /// </summary> |
| | | 57 | | internal static bool TryAdmitSimple(in CylinderPairGeometry geometry, int firstSign, int secondSign, |
| | | 58 | | ref FiniteAxisValueRoot root, int valueSlopeSign, scoped Span<ulong> derivatives, |
| | | 59 | | scoped Span<sbyte> derivativeSigns, ref int readyMask, out int gapSign) |
| | | 60 | | { |
| | | 61 | | // For a simple value root, v=-W_c/(2W_S), p.v=-rho*W_rho/W_S, |
| | | 62 | | // q.v=-tau*W_tau/W_S. Both radii must choose the same support side. |
| | 4395 | 63 | | int firstRadial = GetDerivativeSign(geometry, firstSign, secondSign, ref root, |
| | 4395 | 64 | | valueSlopeSign, 0, derivatives, derivativeSigns, ref readyMask); |
| | 4395 | 65 | | gapSign = -firstRadial * valueSlopeSign; |
| | | 66 | | // Zero radial support is inadmissible, regardless of the other rim |
| | | 67 | | // or cap signs; do not construct their polynomials for this boundary. |
| | 4395 | 68 | | if (firstRadial == 0) |
| | 6 | 69 | | return false; |
| | 4389 | 70 | | int secondRadial = GetDerivativeSign(geometry, firstSign, secondSign, ref root, |
| | 4389 | 71 | | valueSlopeSign, 1, derivatives, derivativeSigns, ref readyMask); |
| | 4389 | 72 | | if (firstRadial != secondRadial) |
| | 1786 | 73 | | return false; |
| | 2603 | 74 | | int firstCap = GetDerivativeSign(geometry, firstSign, secondSign, ref root, |
| | 2603 | 75 | | valueSlopeSign, 2, derivatives, derivativeSigns, ref readyMask); |
| | 2603 | 76 | | if (firstRadial * firstSign * firstCap < 0) |
| | 1163 | 77 | | return false; |
| | 1440 | 78 | | int secondCap = GetDerivativeSign(geometry, firstSign, secondSign, ref root, |
| | 1440 | 79 | | valueSlopeSign, 3, derivatives, derivativeSigns, ref readyMask); |
| | 1440 | 80 | | return firstRadial * secondSign * secondCap >= 0; |
| | | 81 | | } |
| | | 82 | | |
| | | 83 | | internal static Vector3d GetSimpleNormal(in CylinderPairGeometry geometry, int firstSign, int secondSign, |
| | | 84 | | ref FiniteAxisValueRoot root, int valueSlopeSign, int gapSign) |
| | | 85 | | { |
| | 142 | 86 | | Span<ulong> gradient = stackalloc ulong[3 * Count * CoefficientWords]; |
| | 142 | 87 | | Span<sbyte> gradientSigns = stackalloc sbyte[3 * Count]; |
| | 142 | 88 | | WideRationalBasis3d basis = geometry.WorldBasis; |
| | 1136 | 89 | | for (int axis = 0; axis < 3; axis++) |
| | | 90 | | { |
| | | 91 | | // Rows of the rotation map the first-frame gradient to world |
| | | 92 | | // coordinates. A shared positive denominator cancels on normalization. |
| | 426 | 93 | | WideAxis3 direction = axis == 0 |
| | 426 | 94 | | ? Axis(basis.Xx, basis.Yx, basis.Zx) |
| | 426 | 95 | | : axis == 1 ? Axis(basis.Xy, basis.Yy, basis.Zy) : Axis(basis.Xz, basis.Yz, basis.Zz); |
| | 426 | 96 | | BuildDerivative(geometry, firstSign, secondSign, 0, 0, direction, |
| | 426 | 97 | | gradient.Slice(axis * Count * CoefficientWords, Count * CoefficientWords), |
| | 426 | 98 | | gradientSigns.Slice(axis * Count, Count)); |
| | | 99 | | } |
| | | 100 | | // All three derivatives retain the same raw polynomial factor. Remove |
| | | 101 | | // only common content, never an independent scale per component. |
| | 142 | 102 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(gradient, gradientSigns); |
| | 142 | 103 | | return ConvexContactValueRoot.GetNormalizedDirection(ref root, gradient, gradientSigns, -gapSign * valueSlopeSig |
| | | 104 | | } |
| | | 105 | | |
| | | 106 | | private static int GetDerivativeSign(in CylinderPairGeometry geometry, int firstSign, int secondSign, |
| | | 107 | | ref FiniteAxisValueRoot root, int valueSlopeSign, int query, scoped Span<ulong> derivatives, |
| | | 108 | | scoped Span<sbyte> derivativeSigns, ref int readyMask) |
| | | 109 | | { |
| | 12827 | 110 | | Span<ulong> derivative = derivatives.Slice(query * Count * CoefficientWords, Count * CoefficientWords); |
| | 12827 | 111 | | Span<sbyte> signs = derivativeSigns.Slice(query * Count, Count); |
| | 12827 | 112 | | int bit = 1 << query; |
| | 12827 | 113 | | if ((readyMask & bit) == 0) |
| | | 114 | | { |
| | 4690 | 115 | | WideAxis3 direction = query == 2 ? geometry.FirstHalf : query == 3 ? geometry.SecondHalf : default; |
| | 4690 | 116 | | BuildDerivative(geometry, firstSign, secondSign, query == 0 ? 1 : 0, query == 1 ? 1 : 0, |
| | 4690 | 117 | | direction, derivative, signs); |
| | | 118 | | // These independent positive scales preserve signs. World-gradient |
| | | 119 | | // components still use one common scale in GetSimpleNormal. |
| | 4690 | 120 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(derivative, signs); |
| | 4690 | 121 | | readyMask |= bit; |
| | | 122 | | } |
| | | 123 | | // Isolation exports nonzero endpoints. A simple root has lower sign |
| | | 124 | | // -slope in that cell and every nonrational descendant. Reuse that |
| | | 125 | | // proof instead of reevaluating both defining endpoints; |
| | | 126 | | // the ordinary refinement certificates and exact fallbacks still apply. |
| | 12827 | 127 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, derivative, signs, -valueSlopeSign |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | [MethodImpl(MethodImplOptions.NoInlining)] |
| | | 131 | | private static void BuildDerivative(in CylinderPairGeometry geometry, int firstSign, int secondSign, |
| | | 132 | | int rhoDirection, int tauDirection, WideAxis3 direction, Span<ulong> coefficients, Span<sbyte> signs) |
| | | 133 | | { |
| | 5116 | 134 | | Span<ulong> invariants = stackalloc ulong[CylinderPairValuePolynomial.InvariantCount * CoefficientWords]; |
| | 5116 | 135 | | Span<sbyte> invariantSigns = stackalloc sbyte[CylinderPairValuePolynomial.InvariantCount]; |
| | 5116 | 136 | | Span<ulong> values = stackalloc ulong[Count * CoefficientWords]; |
| | 5116 | 137 | | Span<sbyte> valueSigns = stackalloc sbyte[Count]; |
| | 5116 | 138 | | Span<ulong> scratch = stackalloc ulong[CylinderPairValuePolynomial.ScratchCoefficientCount * CoefficientWords]; |
| | 5116 | 139 | | Span<sbyte> scratchSigns = stackalloc sbyte[CylinderPairValuePolynomial.ScratchCoefficientCount]; |
| | 5116 | 140 | | Span<ulong> derivativeInvariants = stackalloc ulong[CylinderPairValueDerivative.InvariantCount * CoefficientWord |
| | 5116 | 141 | | Span<sbyte> derivativeInvariantSigns = stackalloc sbyte[CylinderPairValueDerivative.InvariantCount]; |
| | 5116 | 142 | | Span<ulong> derivativeScratch = stackalloc ulong[CylinderPairValueDerivative.ScratchCoefficientCount * Coefficie |
| | 5116 | 143 | | Span<sbyte> derivativeScratchSigns = stackalloc sbyte[CylinderPairValueDerivative.ScratchCoefficientCount]; |
| | 5116 | 144 | | geometry.WriteInvariants(firstSign, secondSign, invariants, invariantSigns, CoefficientWords); |
| | 5116 | 145 | | derivativeInvariants.Clear(); |
| | 5116 | 146 | | derivativeInvariantSigns.Clear(); |
| | 5116 | 147 | | derivativeInvariants[0] = (ulong)rhoDirection; |
| | 5116 | 148 | | derivativeInvariantSigns[0] = (sbyte)rhoDirection; |
| | 5116 | 149 | | derivativeInvariants[CoefficientWords] = (ulong)tauDirection; |
| | 5116 | 150 | | derivativeInvariantSigns[1] = (sbyte)tauDirection; |
| | 5116 | 151 | | Signed576 center = WideAxis3.Dot(geometry.GetCapOffset(firstSign, secondSign), direction); |
| | 5116 | 152 | | WriteDerivativeInvariant(WideArithmetic.AddSigned576(center, center), derivativeInvariants, derivativeInvariantS |
| | 5116 | 153 | | WriteDerivativeInvariant(WideAxis3.Dot(geometry.PlaneU, direction), derivativeInvariants, derivativeInvariantSig |
| | 5116 | 154 | | WriteDerivativeInvariant(WideAxis3.Dot(geometry.PlaneW, direction), derivativeInvariants, derivativeInvariantSig |
| | 5116 | 155 | | WriteDerivativeInvariant(WideAxis3.Dot(geometry.SecondAxis, direction), derivativeInvariants, derivativeInvarian |
| | 5116 | 156 | | var derivative = new CylinderPairValueDerivative(derivativeInvariants, derivativeInvariantSigns, |
| | 5116 | 157 | | coefficients, signs, derivativeScratch, derivativeScratchSigns); |
| | 5116 | 158 | | CylinderPairValuePolynomial.Build(invariants, invariantSigns, CoefficientWords, |
| | 5116 | 159 | | values, valueSigns, scratch, scratchSigns, derivative); |
| | 5116 | 160 | | WideFiniteAxisIntersection.ScaleFiniteAxisPolynomialVariable(coefficients, Count, geometry.ValueShift); |
| | 5116 | 161 | | } |
| | | 162 | | |
| | | 163 | | private static void WriteDerivativeInvariant(Signed576 value, Span<ulong> values, Span<sbyte> signs, int index) |
| | | 164 | | { |
| | 20464 | 165 | | WideArithmetic.GetMagnitude(value, values.Slice(index * CoefficientWords, 9)); |
| | 20464 | 166 | | signs[index] = (sbyte)value.Sign; |
| | 20464 | 167 | | } |
| | | 168 | | |
| | 426 | 169 | | private static WideAxis3 Axis(Signed192 x, Signed192 y, Signed192 z) => new( |
| | 426 | 170 | | Signed320.ExtendValue(x), Signed320.ExtendValue(y), Signed320.ExtendValue(z)); |
| | | 171 | | |
| | | 172 | | } |