| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleCapsuleSlabEndpointWitnesses.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 static FixedMathSharp.Geometry.CylinderContactAlgebra; |
| | | 10 | | |
| | | 11 | | namespace FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | /// <summary>Joint radial/core rounding at a selected stadium endpoint.</summary> |
| | | 14 | | internal static class TriangleCapsuleSlabEndpointWitnesses |
| | | 15 | | { |
| | | 16 | | internal static Vector3d GetCore(Vector2d axis, Fixed64 length, int coreSign, |
| | | 17 | | out FixedPointAnchorTerm3d term) |
| | | 18 | | { |
| | 507 | 19 | | Fixed64 signedLength = coreSign > 0 ? -length : length; |
| | 507 | 20 | | Signed576 denominator = Signed576.ExtendValue(Signed320.ExtendValue(FixedPointAnchorTerm3d.Denominator)); |
| | 507 | 21 | | bool representedX = Fixed64.TryGetSignedRawRatio(Signed576.ExtendValue( |
| | 507 | 22 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.X), Signed192.Raw(signedLength))), denominator, out Fixe |
| | 507 | 23 | | bool representedZ = Fixed64.TryGetSignedRawRatio(Signed576.ExtendValue( |
| | 507 | 24 | | WideArithmetic.MultiplySigned192(Signed192.Raw(axis.Y), Signed192.Raw(signedLength))), denominator, out Fixe |
| | | 25 | | System.Diagnostics.Debug.Assert(representedX && representedZ); |
| | 507 | 26 | | var core = new Vector3d(x, Fixed64.Zero, z); |
| | 507 | 27 | | term = CreateTerm(axis, signedLength, core, 5); |
| | 507 | 28 | | return core; |
| | | 29 | | } |
| | | 30 | | |
| | | 31 | | internal static FixedPointAnchorTerm3d CreateTerm(Vector2d axis, Fixed64 signedLength, Vector3d core, int integralMa |
| | 667 | 32 | | FixedPointAnchorTerm3d.CreateCenteredAxisSupport( |
| | 667 | 33 | | new Vector3d((integralMask & 1) != 0 ? axis.X : Fixed64.Zero, Fixed64.Zero, |
| | 667 | 34 | | (integralMask & 4) != 0 ? axis.Y : Fixed64.Zero), signedLength, |
| | 667 | 35 | | Vector3d.Zero, Fixed64.Zero, |
| | 667 | 36 | | new Vector3d((integralMask & 1) != 0 ? core.X : Fixed64.Zero, Fixed64.Zero, |
| | 667 | 37 | | (integralMask & 4) != 0 ? core.Z : Fixed64.Zero), Vector3d.Zero); |
| | | 38 | | |
| | | 39 | | internal static Vector3d AdjustAnalytic(ReadOnlySpan<ulong> direction, ReadOnlySpan<int> signs, |
| | | 40 | | Fixed64 radius, Vector3d core, FixedPointAnchorTerm3d term, Vector3d radial, out int integralMask) |
| | | 41 | | { |
| | 25 | 42 | | Span<ulong> norm = stackalloc ulong[Words]; |
| | 25 | 43 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 25 | 44 | | WideArithmetic.MultiplyMagnitudes(Slot(direction, 0), Slot(direction, 0), norm); |
| | 25 | 45 | | WideArithmetic.MultiplyMagnitudes(Slot(direction, 2), Slot(direction, 2), product); |
| | 25 | 46 | | WideArithmetic.AddMagnitudeInto(product, norm); |
| | 25 | 47 | | Fixed64 x = AdjustAnalyticComponent(Slot(direction, 0), -signs[0], norm, radius, |
| | 25 | 48 | | core.X, term.X, radial.X, out bool integralX); |
| | 25 | 49 | | Fixed64 z = AdjustAnalyticComponent(Slot(direction, 2), -signs[2], norm, radius, |
| | 25 | 50 | | core.Z, term.Z, radial.Z, out bool integralZ); |
| | 25 | 51 | | integralMask = (integralX ? 1 : 0) | (integralZ ? 4 : 0); |
| | 25 | 52 | | return new Vector3d(x, radial.Y, z); |
| | | 53 | | } |
| | | 54 | | |
| | | 55 | | private static Fixed64 AdjustAnalyticComponent(ReadOnlySpan<ulong> component, int sign, |
| | | 56 | | ReadOnlySpan<ulong> norm, Fixed64 radius, Fixed64 core, long residual, Fixed64 rounded, out bool integral) |
| | | 57 | | { |
| | 50 | 58 | | Span<ulong> scaled = stackalloc ulong[Words]; |
| | 50 | 59 | | Span<ulong> factor = stackalloc ulong[Words]; |
| | | 60 | | // Compare radial with T/(2D): sign(2DR*n_i - T*sqrt(K)). |
| | | 61 | | // D=2Q, direction<630 bits: the rational term stays below730 bits; |
| | | 62 | | // K<1262 and the existing quadratic sign owner sizes its own squares. |
| | 50 | 63 | | Signed320 multiplier = WideArithmetic.MultiplySigned192(Signed192.Raw(radius), |
| | 50 | 64 | | WideArithmetic.AddSigned192(FixedPointAnchorTerm3d.Denominator, FixedPointAnchorTerm3d.Denominator)); |
| | 50 | 65 | | Import(multiplier, factor); |
| | 50 | 66 | | WideArithmetic.MultiplyMagnitudes(component, factor, scaled); |
| | 50 | 67 | | integral = CompareAnalytic(scaled, sign, norm, Threshold(rounded, 0, 0)) == 0; |
| | 50 | 68 | | if (integral) |
| | 30 | 69 | | return rounded; |
| | 20 | 70 | | int lower = CompareAnalytic(scaled, sign, norm, Threshold(rounded, -1, residual)); |
| | 20 | 71 | | int upper = CompareAnalytic(scaled, sign, norm, Threshold(rounded, 1, residual)); |
| | 20 | 72 | | return Correct(rounded, core, lower, upper); |
| | | 73 | | } |
| | | 74 | | |
| | | 75 | | private static int CompareAnalytic(ReadOnlySpan<ulong> scaled, int sign, ReadOnlySpan<ulong> norm, Signed192 thresho |
| | | 76 | | { |
| | 90 | 77 | | Span<ulong> magnitude = stackalloc ulong[Words]; |
| | 90 | 78 | | Import(Signed320.ExtendValue(threshold), magnitude); |
| | 90 | 79 | | return WideConvexPrismRelations.GetConvexContactCandidateQuadraticSign(scaled, sign, magnitude, -threshold.Sign, |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | internal static Vector3d AdjustRoot(ref FiniteAxisValueRoot root, scoped ReadOnlySpan<ulong> gradient, |
| | | 83 | | scoped ReadOnlySpan<sbyte> signs, Fixed64 radius, Vector3d core, |
| | | 84 | | FixedPointAnchorTerm3d term, Vector3d radial, out int integralMask) |
| | | 85 | | { |
| | 100 | 86 | | int count = signs.Length / 3, words = gradient.Length / signs.Length; |
| | 50 | 87 | | int squareCount = 2 * count - 1; |
| | 50 | 88 | | Span<ulong> norm = stackalloc ulong[squareCount * 2 * words]; |
| | 50 | 89 | | Span<sbyte> normSigns = stackalloc sbyte[squareCount]; |
| | 50 | 90 | | ConvexContactValueRoot.BuildSquaredLength(gradient, signs, norm, normSigns); |
| | 50 | 91 | | Fixed64 x = AdjustRootComponent(ref root, gradient[..(count * words)], signs[..count], norm, normSigns, |
| | 50 | 92 | | radius, core.X, term.X, radial.X, out bool integralX); |
| | 50 | 93 | | Fixed64 z = AdjustRootComponent(ref root, gradient[(2 * count * words)..], signs[(2 * count)..], norm, normSigns |
| | 50 | 94 | | radius, core.Z, term.Z, radial.Z, out bool integralZ); |
| | 50 | 95 | | integralMask = (integralX ? 1 : 0) | (integralZ ? 4 : 0); |
| | 50 | 96 | | return new Vector3d(x, radial.Y, z); |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | private static Fixed64 AdjustRootComponent(ref FiniteAxisValueRoot root, |
| | | 100 | | scoped ReadOnlySpan<ulong> component, scoped ReadOnlySpan<sbyte> signs, |
| | | 101 | | scoped ReadOnlySpan<ulong> norm, scoped ReadOnlySpan<sbyte> normSigns, |
| | | 102 | | Fixed64 radius, Fixed64 core, long residual, Fixed64 rounded, out bool integral) |
| | | 103 | | { |
| | 100 | 104 | | int sign = -WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, component, signs); |
| | 200 | 105 | | int count = normSigns.Length, words = norm.Length / count + 4; |
| | 100 | 106 | | Span<ulong> squared = stackalloc ulong[count * words]; |
| | 100 | 107 | | Span<sbyte> squaredSigns = stackalloc sbyte[count]; |
| | 100 | 108 | | Span<ulong> product = stackalloc ulong[words]; |
| | 100 | 109 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(component, signs, component, signs, squared, squaredSig |
| | 100 | 110 | | Signed320 multiplier = WideArithmetic.MultiplySigned192(Signed192.Raw(radius), |
| | 100 | 111 | | WideArithmetic.AddSigned192(FixedPointAnchorTerm3d.Denominator, FixedPointAnchorTerm3d.Denominator)); |
| | 100 | 112 | | Span<ulong> factor = stackalloc ulong[9]; |
| | 100 | 113 | | WideArithmetic.GetMagnitude(WideArithmetic.MultiplySigned320(multiplier, multiplier), factor); |
| | 800 | 114 | | for (int index = 0; index < count; index++) |
| | | 115 | | { |
| | 300 | 116 | | Span<ulong> coefficient = squared.Slice(index * words, words); |
| | 300 | 117 | | WideArithmetic.MultiplyMagnitudes(coefficient, factor, product); |
| | 300 | 118 | | product.CopyTo(coefficient); |
| | | 119 | | } |
| | | 120 | | // Sign-gated squaring compares (2DR)^2*C(t)^2 with T^2*K(t). |
| | | 121 | | // T<98 bits and the multiplier<98 bits: four additional coefficient |
| | | 122 | | // words cover both products and subtraction, independent of root width. |
| | 100 | 123 | | integral = CompareRoot(ref root, squared, squaredSigns, norm, normSigns, sign, Threshold(rounded, 0, 0)) == 0; |
| | 100 | 124 | | if (integral) |
| | 64 | 125 | | return rounded; |
| | 36 | 126 | | int lower = CompareRoot(ref root, squared, squaredSigns, norm, normSigns, sign, Threshold(rounded, -1, residual) |
| | 36 | 127 | | int upper = CompareRoot(ref root, squared, squaredSigns, norm, normSigns, sign, Threshold(rounded, 1, residual)) |
| | 36 | 128 | | return Correct(rounded, core, lower, upper); |
| | | 129 | | } |
| | | 130 | | |
| | | 131 | | private static int CompareRoot(ref FiniteAxisValueRoot root, scoped ReadOnlySpan<ulong> squared, |
| | | 132 | | scoped ReadOnlySpan<sbyte> squaredSigns, scoped ReadOnlySpan<ulong> norm, |
| | | 133 | | scoped ReadOnlySpan<sbyte> normSigns, int sign, Signed192 threshold) |
| | | 134 | | { |
| | 172 | 135 | | if (sign != threshold.Sign) |
| | 8 | 136 | | return sign.CompareTo(threshold.Sign); |
| | 164 | 137 | | if (sign == 0) |
| | 1 | 138 | | return 0; |
| | 489 | 139 | | int count = normSigns.Length, words = squared.Length / count, normWords = norm.Length / count; |
| | 163 | 140 | | Span<ulong> query = stackalloc ulong[squared.Length]; |
| | 163 | 141 | | Span<sbyte> querySigns = stackalloc sbyte[count]; |
| | 163 | 142 | | Span<ulong> product = stackalloc ulong[words]; |
| | 163 | 143 | | Span<ulong> factor = stackalloc ulong[5]; |
| | 163 | 144 | | Signed320 square = WideArithmetic.MultiplySigned192(threshold, threshold); |
| | 163 | 145 | | WideArithmetic.GetMagnitude(square, out factor[4], out factor[3], out factor[2], out factor[1], out factor[0]); |
| | 326 | 146 | | squared.CopyTo(query); squaredSigns.CopyTo(querySigns); |
| | 1304 | 147 | | for (int index = 0; index < count; index++) |
| | | 148 | | { |
| | 489 | 149 | | WideArithmetic.MultiplyMagnitudes(norm.Slice(index * normWords, normWords), factor, product); |
| | 489 | 150 | | int coefficientSign = querySigns[index]; |
| | 489 | 151 | | Add(product, -normSigns[index], query.Slice(index * words, words), ref coefficientSign); |
| | 489 | 152 | | querySigns[index] = (sbyte)coefficientSign; |
| | | 153 | | } |
| | 163 | 154 | | return sign * WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, querySigns); |
| | | 155 | | } |
| | | 156 | | |
| | | 157 | | private static Signed192 Threshold(Fixed64 rounded, int halfStep, long residual) |
| | | 158 | | { |
| | 262 | 159 | | Signed192 twice = WideArithmetic.AddSigned192(Signed192.Raw(rounded), Signed192.Raw(rounded)); |
| | 262 | 160 | | twice = WideArithmetic.AddSigned192(twice, Signed192.Signed(halfStep)); |
| | 262 | 161 | | Signed192 numerator = Signed192.NarrowValue(WideArithmetic.MultiplySigned192(twice, FixedPointAnchorTerm3d.Denom |
| | 262 | 162 | | return WideArithmetic.SubtractSigned192(numerator, |
| | 262 | 163 | | WideArithmetic.AddSigned192(Signed192.Signed(residual), Signed192.Signed(residual))); |
| | | 164 | | } |
| | | 165 | | |
| | | 166 | | private static Fixed64 Correct(Fixed64 rounded, Fixed64 core, int lower, int upper) |
| | | 167 | | { |
| | | 168 | | // Core residual is within half a raw unit, so only adjacent rounding |
| | | 169 | | // cells are possible. Parity belongs to the total, not either addend. |
| | | 170 | | // Nonintegral |radial|<R<=Max keeps the corrected displacement scalar |
| | | 171 | | // even when the additive endpoint+radial coordinate exceeds Max. |
| | 56 | 172 | | bool odd = ((rounded.m_rawValue ^ core.m_rawValue) & 1L) != 0; |
| | 56 | 173 | | int adjustment = lower < 0 || lower == 0 && odd ? -1 : upper > 0 || upper == 0 && odd ? 1 : 0; |
| | 56 | 174 | | return Fixed64.FromRaw(rounded.m_rawValue + adjustment); |
| | | 175 | | } |
| | | 176 | | } |