| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderPairAnalyticFeatures.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> |
| | | 13 | | /// Cap-axis and common-perpendicular cylinder support candidates, represented |
| | | 14 | | /// in the shared exact raw-gap and world-normal contact format. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class CylinderPairAnalyticFeatures |
| | | 17 | | { |
| | | 18 | | private const int Words = ConvexContactCandidate.Words; |
| | | 19 | | |
| | | 20 | | /// <summary> |
| | | 21 | | /// Builds axis 0 (first cap), 1 (second cap), or 2 (axis cross product) |
| | | 22 | | /// for a nonparallel positive-radius pair. Storage is borrowed by |
| | | 23 | | /// ConvexContactCandidate; its shared ranking and rounding need no radius offset. |
| | | 24 | | /// </summary> |
| | | 25 | | internal static void Build(in CylinderPairGeometry geometry, int axisIndex, |
| | | 26 | | Span<ulong> values, Span<int> signs, out int gapSign) |
| | | 27 | | { |
| | | 28 | | System.Diagnostics.Debug.Assert((uint)axisIndex <= 2); |
| | 1160 | 29 | | WideAxis3 direction = axisIndex == 0 ? geometry.FirstAxis |
| | 1160 | 30 | | : axisIndex == 1 ? geometry.SecondAxis : WideAxis3.Cross(geometry.FirstAxis, geometry.SecondAxis); |
| | | 31 | | System.Diagnostics.Debug.Assert(!direction.IsZero); |
| | 1160 | 32 | | values.Clear(); |
| | 1160 | 33 | | signs.Clear(); |
| | 1160 | 34 | | Signed576 center = WideAxis3.Dot(geometry.CenterDifference, direction); |
| | 1160 | 35 | | Signed576 rational = WideArithmetic.SubtractSigned576( |
| | 1160 | 36 | | WideArithmetic.AddSigned576(Abs(WideAxis3.Dot(geometry.FirstHalf, direction)), |
| | 1160 | 37 | | Abs(WideAxis3.Dot(geometry.SecondHalf, direction))), Abs(center)); |
| | 1160 | 38 | | WriteNormal(geometry.WorldBasis, direction, center.Sign < 0 ? -1 : 1, values, signs); |
| | | 39 | | |
| | | 40 | | // |a_i|<2^33, |b_i|<2^163, |(a x b)_i|<2^197. The cap |
| | | 41 | | // rational terms are <2^264/<2^394, and the cross term <2^428. |
| | | 42 | | // Thus A<2^857. Coupled (B,C) bounds are (491,724), (621,464), |
| | | 43 | | // (656,396) bits respectively. D, including RawScale², is <2^724. |
| | | 44 | | // All construction intermediates fit the existing 2560-bit slots. |
| | 1160 | 45 | | Span<ulong> work = stackalloc ulong[9 * Words]; |
| | 1160 | 46 | | Span<ulong> delta = Slot(work, 0); |
| | 1160 | 47 | | Span<ulong> squaredDirection = Slot(work, 1); |
| | 1160 | 48 | | Span<ulong> projection = Slot(work, 2); |
| | 1160 | 49 | | Span<ulong> rationalMagnitude = Slot(work, 3); |
| | 1160 | 50 | | Span<ulong> radius = Slot(work, 4); |
| | 1160 | 51 | | Span<ulong> rationalSquaredDelta = Slot(work, 5); |
| | 1160 | 52 | | Span<ulong> radiusSquaredProjection = Slot(work, 6); |
| | 1160 | 53 | | Span<ulong> temporary = Slot(work, 7); |
| | 1160 | 54 | | Span<ulong> product = Slot(work, 8); |
| | 1160 | 55 | | Import(direction.SquaredLength, squaredDirection); |
| | 1160 | 56 | | if (axisIndex == 2) |
| | | 57 | | { |
| | 353 | 58 | | delta.Clear(); |
| | 353 | 59 | | delta[0] = 1; |
| | 353 | 60 | | squaredDirection.CopyTo(projection); |
| | 353 | 61 | | Import(Signed576.ExtendValue(WideArithmetic.AddSigned320(geometry.FirstRadius, geometry.SecondRadius)), radi |
| | | 62 | | } |
| | | 63 | | else |
| | | 64 | | { |
| | 807 | 65 | | WideAxis3 opposite = axisIndex == 0 ? geometry.SecondAxis : geometry.FirstAxis; |
| | 807 | 66 | | Import(opposite.SquaredLength, delta); |
| | 807 | 67 | | WideArithmetic.MultiplyMagnitudes(delta, squaredDirection, product); |
| | 807 | 68 | | Import(WideAxis3.Dot(opposite, direction), temporary); |
| | 807 | 69 | | WideArithmetic.MultiplyMagnitudes(temporary, temporary, projection); |
| | 807 | 70 | | WideArithmetic.SubtractEqualMagnitudes(product, projection, projection); |
| | 807 | 71 | | Import(Signed576.ExtendValue(axisIndex == 0 ? geometry.SecondRadius : geometry.FirstRadius), radius); |
| | | 72 | | } |
| | | 73 | | |
| | 1160 | 74 | | Import(rational, rationalMagnitude); |
| | 1160 | 75 | | WideArithmetic.MultiplyMagnitudes(rationalMagnitude, rationalMagnitude, temporary); |
| | 1160 | 76 | | WideArithmetic.MultiplyMagnitudes(temporary, delta, rationalSquaredDelta); |
| | 1160 | 77 | | WideArithmetic.MultiplyMagnitudes(radius, radius, temporary); |
| | 1160 | 78 | | WideArithmetic.MultiplyMagnitudes(temporary, projection, radiusSquaredProjection); |
| | 1160 | 79 | | WideArithmetic.AddEqualMagnitudes(rationalSquaredDelta, radiusSquaredProjection, Slot(values, 7)); |
| | 1160 | 80 | | WideArithmetic.MultiplyMagnitudes(rationalMagnitude, radius, temporary); |
| | 1160 | 81 | | WideArithmetic.AddEqualMagnitudes(temporary, temporary, Slot(values, 8)); |
| | 1160 | 82 | | WideArithmetic.MultiplyMagnitudes(projection, delta, Slot(values, 9)); |
| | 1160 | 83 | | Import(Signed576.ExtendValue(Signed320.ExtendValue(geometry.RawScale)), rationalMagnitude); |
| | 1160 | 84 | | WideArithmetic.MultiplyMagnitudes(rationalMagnitude, rationalMagnitude, temporary); |
| | 1160 | 85 | | WideArithmetic.MultiplyMagnitudes(delta, squaredDirection, product); |
| | 1160 | 86 | | WideArithmetic.MultiplyMagnitudes(temporary, product, Slot(values, 10)); |
| | 1160 | 87 | | signs[7] = 1; |
| | 1160 | 88 | | signs[8] = IsZero(Slot(values, 8)) ? 0 : rational.Sign; |
| | 1160 | 89 | | signs[9] = 1; |
| | 1160 | 90 | | signs[10] = 1; |
| | 1160 | 91 | | gapSign = rational.Sign < 0 |
| | 1160 | 92 | | ? WideArithmetic.CompareMagnitudeEqualLength(radiusSquaredProjection, rationalSquaredDelta) |
| | 1160 | 93 | | : 1; // Every family has strictly positive radial support. |
| | 1160 | 94 | | } |
| | | 95 | | |
| | | 96 | | private static void WriteNormal(in WideRationalBasis3d basis, WideAxis3 direction, |
| | | 97 | | int orientation, Span<ulong> values, Span<int> signs) |
| | | 98 | | { |
| | 9280 | 99 | | for (int component = 0; component < 3; component++) |
| | | 100 | | { |
| | 3480 | 101 | | WideAxis3 row = component == 0 ? Axis(basis.Xx, basis.Yx, basis.Zx) |
| | 3480 | 102 | | : component == 1 ? Axis(basis.Xy, basis.Yy, basis.Zy) : Axis(basis.Xz, basis.Yz, basis.Zz); |
| | 3480 | 103 | | Signed576 value = WideAxis3.Dot(direction, row); |
| | 3480 | 104 | | Import(value, Slot(values, component)); |
| | 3480 | 105 | | signs[component] = orientation * value.Sign; |
| | | 106 | | } |
| | 1160 | 107 | | } |
| | | 108 | | |
| | 3480 | 109 | | private static WideAxis3 Axis(Signed192 x, Signed192 y, Signed192 z) => new( |
| | 3480 | 110 | | Signed320.ExtendValue(x), Signed320.ExtendValue(y), Signed320.ExtendValue(z)); |
| | 3480 | 111 | | private static Signed576 Abs(Signed576 value) => value.Sign < 0 ? WideArithmetic.SubtractSigned576(default, value) : |
| | 1160 | 112 | | private static bool IsZero(ReadOnlySpan<ulong> value) => WideArithmetic.GetActiveMagnitudeLength(value) == 0; |
| | 19720 | 113 | | private static Span<ulong> Slot(Span<ulong> values, int slot) => values.Slice(slot * Words, Words); |
| | | 114 | | private static void Import(Signed576 value, Span<ulong> magnitude) |
| | | 115 | | { |
| | 9734 | 116 | | magnitude.Clear(); |
| | 9734 | 117 | | WideArithmetic.GetMagnitude(value, magnitude[..9]); |
| | 9734 | 118 | | } |
| | | 119 | | } |