| | | 1 | | //======================================================================= |
| | | 2 | | // CylinderPairGeometry.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 | | /// Exact cylinder-pair geometry in the first authored rotation frame. Positions |
| | | 14 | | /// and radii use one reduced integer scale per raw coordinate; axis directions |
| | | 15 | | /// and the perpendicular plane basis are independently primitive. |
| | | 16 | | /// </summary> |
| | | 17 | | internal readonly struct CylinderPairGeometry |
| | | 18 | | { |
| | | 19 | | internal readonly WideRationalBasis3d WorldBasis; |
| | | 20 | | internal readonly WideAxis3 FirstAxis; |
| | | 21 | | internal readonly WideAxis3 SecondAxis; |
| | | 22 | | internal readonly WideAxis3 PlaneU; |
| | | 23 | | internal readonly WideAxis3 PlaneW; |
| | | 24 | | internal readonly WideAxis3 FirstHalf; |
| | | 25 | | internal readonly WideAxis3 SecondHalf; |
| | | 26 | | internal readonly WideAxis3 CenterDifference; |
| | | 27 | | internal readonly Signed320 FirstRadius; |
| | | 28 | | internal readonly Signed320 SecondRadius; |
| | | 29 | | internal readonly Signed192 RawScale; |
| | | 30 | | internal readonly int ValueShift; |
| | | 31 | | |
| | | 32 | | internal CylinderPairGeometry(Vector3d firstCenter, FixedQuaternion firstRotation, |
| | | 33 | | Vector3d firstLocalAxis, Signed192 firstLength, Fixed64 firstRadius, |
| | | 34 | | Vector3d secondCenter, FixedQuaternion secondRotation, Vector3d secondLocalAxis, |
| | | 35 | | Signed192 secondLength, Fixed64 secondRadius) |
| | | 36 | | { |
| | 489 | 37 | | WorldBasis = new WideRationalBasis3d(firstRotation); |
| | 489 | 38 | | var secondBasis = new WideRationalBasis3d(secondRotation); |
| | 489 | 39 | | WideRationalBasis3d relative = WideRationalBasis3d.CreateRelative(WorldBasis, secondBasis); |
| | 489 | 40 | | var first = new WideAxis3(Signed320.ExtendValue(Signed192.Raw(firstLocalAxis.X)), |
| | 489 | 41 | | Signed320.ExtendValue(Signed192.Raw(firstLocalAxis.Y)), |
| | 489 | 42 | | Signed320.ExtendValue(Signed192.Raw(firstLocalAxis.Z))); |
| | 489 | 43 | | WideAxis3 second = WideRigidProjection.TransformLocalAxis(relative, |
| | 489 | 44 | | Signed192.Raw(secondLocalAxis.X), Signed192.Raw(secondLocalAxis.Y), Signed192.Raw(secondLocalAxis.Z)); |
| | 489 | 45 | | FirstAxis = Primitive(first); |
| | 489 | 46 | | SecondAxis = Primitive(second); |
| | 489 | 47 | | PlaneU = FirstAxis.X.IsZero && FirstAxis.Y.IsZero |
| | 489 | 48 | | ? new WideAxis3(Signed320.ExtendValue(Signed192.Signed(1)), default, default) |
| | 489 | 49 | | : Primitive(new WideAxis3(FirstAxis.Y, WideArithmetic.Negate(FirstAxis.X), default)); |
| | 489 | 50 | | PlaneW = Primitive(WideAxis3.Cross(FirstAxis, PlaneU)); |
| | | 51 | | |
| | 489 | 52 | | Signed192 twiceOne = Signed192.Raw(Fixed64.Two); |
| | 489 | 53 | | Signed320 rawScale = WideArithmetic.MultiplySigned192(relative.Denominator, twiceOne); |
| | 489 | 54 | | WideAxis3 firstHalf = Scale(first, |
| | 489 | 55 | | WideArithmetic.MultiplySigned192(relative.Denominator, firstLength)); |
| | 489 | 56 | | WideAxis3 secondHalf = Scale(second, Signed320.ExtendValue(secondLength)); |
| | 489 | 57 | | WideOrientedBox.GetRelativeLocalPointNumerators(secondCenter, firstCenter, WorldBasis, |
| | 489 | 58 | | out Signed192 dx, out Signed192 dy, out Signed192 dz); |
| | 489 | 59 | | WideAxis3 difference = Scale(new WideAxis3(Signed320.ExtendValue(dx), Signed320.ExtendValue(dy), |
| | 489 | 60 | | Signed320.ExtendValue(dz)), WideArithmetic.MultiplySigned192(secondBasis.Denominator, twiceOne)); |
| | 489 | 61 | | Signed320 radiusA = Signed320.NarrowValue(WideArithmetic.MultiplySigned320(rawScale, Signed192.Raw(firstRadius)) |
| | 489 | 62 | | Signed320 radiusB = Signed320.NarrowValue(WideArithmetic.MultiplySigned320(rawScale, Signed192.Raw(secondRadius) |
| | 489 | 63 | | Span<Signed320> coordinates = stackalloc Signed320[12] |
| | 489 | 64 | | { |
| | 489 | 65 | | rawScale, firstHalf.X, firstHalf.Y, firstHalf.Z, secondHalf.X, secondHalf.Y, secondHalf.Z, |
| | 489 | 66 | | difference.X, difference.Y, difference.Z, radiusA, radiusB |
| | 489 | 67 | | }; |
| | 489 | 68 | | WideArithmetic.ReduceCommonScale(coordinates); |
| | 489 | 69 | | RawScale = Signed192.NarrowProven(coordinates[0]); |
| | 489 | 70 | | FirstHalf = new WideAxis3(coordinates[1], coordinates[2], coordinates[3]); |
| | 489 | 71 | | SecondHalf = new WideAxis3(coordinates[4], coordinates[5], coordinates[6]); |
| | 489 | 72 | | CenterDifference = new WideAxis3(coordinates[7], coordinates[8], coordinates[9]); |
| | 489 | 73 | | FirstRadius = coordinates[10]; |
| | 489 | 74 | | SecondRadius = coordinates[11]; |
| | 489 | 75 | | int bits = 0; |
| | 489 | 76 | | Span<ulong> magnitude = stackalloc ulong[5]; |
| | 11736 | 77 | | for (int index = 1; index < coordinates.Length; index++) |
| | | 78 | | { |
| | 5379 | 79 | | Import(coordinates[index], magnitude); |
| | 5379 | 80 | | bits = Math.Max(bits, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | | 81 | | } |
| | | 82 | | // Each residual is c+p+q. The component-wise bound gives magnitude |
| | | 83 | | // <2^(bits+4); the authored full-domain bound independently gives |
| | | 84 | | // magnitude <2^230. Use the tighter squared-value upper bound. |
| | 489 | 85 | | ValueShift = Math.Min(460, 2 * (bits + 4)); |
| | 489 | 86 | | } |
| | | 87 | | |
| | 13016 | 88 | | internal WideAxis3 GetCapOffset(int firstSign, int secondSign) => new( |
| | 13016 | 89 | | Offset(FirstHalf.X, SecondHalf.X, CenterDifference.X, firstSign, secondSign), |
| | 13016 | 90 | | Offset(FirstHalf.Y, SecondHalf.Y, CenterDifference.Y, firstSign, secondSign), |
| | 13016 | 91 | | Offset(FirstHalf.Z, SecondHalf.Z, CenterDifference.Z, firstSign, secondSign)); |
| | | 92 | | |
| | | 93 | | internal void WriteInvariants(int firstSign, int secondSign, |
| | | 94 | | Span<ulong> values, Span<sbyte> signs, int words) |
| | | 95 | | { |
| | 7684 | 96 | | WideAxis3 c = GetCapOffset(firstSign, secondSign); |
| | 7684 | 97 | | Write(PlaneU.SquaredLength, values, signs, 0, words); |
| | 7684 | 98 | | Write(PlaneW.SquaredLength, values, signs, 1, words); |
| | 7684 | 99 | | Write(SecondAxis.SquaredLength, values, signs, 2, words); |
| | 7684 | 100 | | Write(WideArithmetic.MultiplySigned320(FirstRadius, FirstRadius), values, signs, 3, words); |
| | 7684 | 101 | | Write(WideArithmetic.MultiplySigned320(SecondRadius, SecondRadius), values, signs, 4, words); |
| | 7684 | 102 | | Write(c.SquaredLength, values, signs, 5, words); |
| | 7684 | 103 | | Write(WideAxis3.Dot(c, PlaneU), values, signs, 6, words); |
| | 7684 | 104 | | Write(WideAxis3.Dot(c, PlaneW), values, signs, 7, words); |
| | 7684 | 105 | | Write(WideAxis3.Dot(SecondAxis, PlaneU), values, signs, 8, words); |
| | 7684 | 106 | | Write(WideAxis3.Dot(SecondAxis, PlaneW), values, signs, 9, words); |
| | 7684 | 107 | | Write(WideAxis3.Dot(SecondAxis, c), values, signs, 10, words); |
| | 7684 | 108 | | } |
| | | 109 | | |
| | | 110 | | private static WideAxis3 Primitive(WideAxis3 axis) |
| | | 111 | | { |
| | 1863 | 112 | | Signed192 x = Signed192.NarrowProven(axis.X); |
| | 1863 | 113 | | Signed192 y = Signed192.NarrowProven(axis.Y); |
| | 1863 | 114 | | Signed192 z = Signed192.NarrowProven(axis.Z); |
| | 1863 | 115 | | Signed192 gcd = WideArithmetic.GetGreatestCommonDivisor( |
| | 1863 | 116 | | WideArithmetic.GetGreatestCommonDivisor(x, y), z); |
| | 1863 | 117 | | return new WideAxis3(Signed320.ExtendValue(WideArithmetic.DivideExactSigned192(x, gcd)), |
| | 1863 | 118 | | Signed320.ExtendValue(WideArithmetic.DivideExactSigned192(y, gcd)), |
| | 1863 | 119 | | Signed320.ExtendValue(WideArithmetic.DivideExactSigned192(z, gcd))); |
| | | 120 | | } |
| | | 121 | | |
| | 1467 | 122 | | private static WideAxis3 Scale(WideAxis3 axis, Signed320 scale) => new( |
| | 1467 | 123 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(axis.X, scale)), |
| | 1467 | 124 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(axis.Y, scale)), |
| | 1467 | 125 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(axis.Z, scale))); |
| | | 126 | | |
| | | 127 | | private static Signed320 Offset(Signed320 first, Signed320 second, Signed320 center, |
| | 39048 | 128 | | int firstSign, int secondSign) => WideArithmetic.SubtractSigned320( |
| | 39048 | 129 | | WideArithmetic.AddSigned320(firstSign > 0 ? first : WideArithmetic.Negate(first), |
| | 39048 | 130 | | secondSign > 0 ? second : WideArithmetic.Negate(second)), center); |
| | | 131 | | |
| | | 132 | | private static void Import(Signed320 value, Span<ulong> magnitude) => |
| | 5379 | 133 | | WideArithmetic.GetMagnitude(value, out magnitude[4], out magnitude[3], out magnitude[2], |
| | 5379 | 134 | | out magnitude[1], out magnitude[0]); |
| | | 135 | | |
| | | 136 | | private static void Write(Signed576 value, Span<ulong> values, Span<sbyte> signs, int index, int words) |
| | | 137 | | { |
| | 84524 | 138 | | Span<ulong> destination = values.Slice(index * words, words); |
| | 84524 | 139 | | destination.Clear(); |
| | 84524 | 140 | | WideArithmetic.GetMagnitude(value, destination[..9]); |
| | 84524 | 141 | | signs[index] = (sbyte)value.Sign; |
| | 84524 | 142 | | } |
| | | 143 | | } |