| | | 1 | | //======================================================================= |
| | | 2 | | // WideRationalBasis3d.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.Runtime.CompilerServices; |
| | | 9 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Preserves a quaternion-derived rotation basis as exact rational |
| | | 14 | | /// numerators over one shared denominator. |
| | | 15 | | /// </summary> |
| | | 16 | | internal readonly struct WideRationalBasis3d |
| | | 17 | | { |
| | | 18 | | internal readonly Signed192 Denominator; |
| | | 19 | | internal readonly Signed192 Xx; |
| | | 20 | | internal readonly Signed192 Xy; |
| | | 21 | | internal readonly Signed192 Xz; |
| | | 22 | | internal readonly Signed192 Yx; |
| | | 23 | | internal readonly Signed192 Yy; |
| | | 24 | | internal readonly Signed192 Yz; |
| | | 25 | | internal readonly Signed192 Zx; |
| | | 26 | | internal readonly Signed192 Zy; |
| | | 27 | | internal readonly Signed192 Zz; |
| | | 28 | | |
| | | 29 | | internal WideRationalBasis3d(FixedQuaternion orientation) |
| | | 30 | | { |
| | 36033 | 31 | | Signed192 xx = Fixed64.GetExactRawProduct( |
| | 36033 | 32 | | orientation.X, |
| | 36033 | 33 | | orientation.X); |
| | 36033 | 34 | | Signed192 yy = Fixed64.GetExactRawProduct( |
| | 36033 | 35 | | orientation.Y, |
| | 36033 | 36 | | orientation.Y); |
| | 36033 | 37 | | Signed192 zz = Fixed64.GetExactRawProduct( |
| | 36033 | 38 | | orientation.Z, |
| | 36033 | 39 | | orientation.Z); |
| | 36033 | 40 | | Signed192 ww = Fixed64.GetExactRawProduct( |
| | 36033 | 41 | | orientation.W, |
| | 36033 | 42 | | orientation.W); |
| | 36033 | 43 | | Signed192 xy = Fixed64.GetExactRawProduct( |
| | 36033 | 44 | | orientation.X, |
| | 36033 | 45 | | orientation.Y); |
| | 36033 | 46 | | Signed192 xz = Fixed64.GetExactRawProduct( |
| | 36033 | 47 | | orientation.X, |
| | 36033 | 48 | | orientation.Z); |
| | 36033 | 49 | | Signed192 xw = Fixed64.GetExactRawProduct( |
| | 36033 | 50 | | orientation.X, |
| | 36033 | 51 | | orientation.W); |
| | 36033 | 52 | | Signed192 yz = Fixed64.GetExactRawProduct( |
| | 36033 | 53 | | orientation.Y, |
| | 36033 | 54 | | orientation.Z); |
| | 36033 | 55 | | Signed192 yw = Fixed64.GetExactRawProduct( |
| | 36033 | 56 | | orientation.Y, |
| | 36033 | 57 | | orientation.W); |
| | 36033 | 58 | | Signed192 zw = Fixed64.GetExactRawProduct( |
| | 36033 | 59 | | orientation.Z, |
| | 36033 | 60 | | orientation.W); |
| | 36033 | 61 | | Denominator = WideArithmetic.AddSigned192( |
| | 36033 | 62 | | WideArithmetic.AddSigned192(xx, yy), |
| | 36033 | 63 | | WideArithmetic.AddSigned192(zz, ww)); |
| | | 64 | | |
| | 36033 | 65 | | Xx = WideArithmetic.AddSigned192( |
| | 36033 | 66 | | WideArithmetic.SubtractSigned192( |
| | 36033 | 67 | | WideArithmetic.SubtractSigned192(xx, yy), |
| | 36033 | 68 | | zz), |
| | 36033 | 69 | | ww); |
| | 36033 | 70 | | Xy = Double(WideArithmetic.AddSigned192(xy, zw)); |
| | 36033 | 71 | | Xz = Double(WideArithmetic.SubtractSigned192(xz, yw)); |
| | 36033 | 72 | | Yx = Double(WideArithmetic.SubtractSigned192(xy, zw)); |
| | 36033 | 73 | | Yy = WideArithmetic.AddSigned192( |
| | 36033 | 74 | | WideArithmetic.SubtractSigned192( |
| | 36033 | 75 | | WideArithmetic.SubtractSigned192(yy, xx), |
| | 36033 | 76 | | zz), |
| | 36033 | 77 | | ww); |
| | 36033 | 78 | | Yz = Double(WideArithmetic.AddSigned192(yz, xw)); |
| | 36033 | 79 | | Zx = Double(WideArithmetic.AddSigned192(xz, yw)); |
| | 36033 | 80 | | Zy = Double(WideArithmetic.SubtractSigned192(yz, xw)); |
| | 36033 | 81 | | Zz = WideArithmetic.AddSigned192( |
| | 36033 | 82 | | WideArithmetic.SubtractSigned192( |
| | 36033 | 83 | | WideArithmetic.SubtractSigned192(zz, xx), |
| | 36033 | 84 | | yy), |
| | 36033 | 85 | | ww); |
| | 36033 | 86 | | } |
| | | 87 | | |
| | | 88 | | private WideRationalBasis3d( |
| | | 89 | | Signed192 denominator, |
| | | 90 | | Signed192 xx, |
| | | 91 | | Signed192 xy, |
| | | 92 | | Signed192 xz, |
| | | 93 | | Signed192 yx, |
| | | 94 | | Signed192 yy, |
| | | 95 | | Signed192 yz, |
| | | 96 | | Signed192 zx, |
| | | 97 | | Signed192 zy, |
| | | 98 | | Signed192 zz) |
| | | 99 | | { |
| | 1334 | 100 | | Denominator = denominator; |
| | 1334 | 101 | | Xx = xx; |
| | 1334 | 102 | | Xy = xy; |
| | 1334 | 103 | | Xz = xz; |
| | 1334 | 104 | | Yx = yx; |
| | 1334 | 105 | | Yy = yy; |
| | 1334 | 106 | | Yz = yz; |
| | 1334 | 107 | | Zx = zx; |
| | 1334 | 108 | | Zy = zy; |
| | 1334 | 109 | | Zz = zz; |
| | 1334 | 110 | | } |
| | | 111 | | |
| | | 112 | | internal static WideRationalBasis3d CreateRelative( |
| | | 113 | | in WideRationalBasis3d target, |
| | | 114 | | in WideRationalBasis3d source) => |
| | | 115 | | // Normalized quaternion-basis products and their three-term sums fit |
| | | 116 | | // Signed192, including the unreduced shared denominator. |
| | 1334 | 117 | | new( |
| | 1334 | 118 | | Signed192.NarrowProven( |
| | 1334 | 119 | | WideArithmetic.MultiplySigned192( |
| | 1334 | 120 | | target.Denominator, |
| | 1334 | 121 | | source.Denominator)), |
| | 1334 | 122 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 123 | | target.Xx, target.Xy, target.Xz, |
| | 1334 | 124 | | source.Xx, source.Xy, source.Xz)), |
| | 1334 | 125 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 126 | | target.Yx, target.Yy, target.Yz, |
| | 1334 | 127 | | source.Xx, source.Xy, source.Xz)), |
| | 1334 | 128 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 129 | | target.Zx, target.Zy, target.Zz, |
| | 1334 | 130 | | source.Xx, source.Xy, source.Xz)), |
| | 1334 | 131 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 132 | | target.Xx, target.Xy, target.Xz, |
| | 1334 | 133 | | source.Yx, source.Yy, source.Yz)), |
| | 1334 | 134 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 135 | | target.Yx, target.Yy, target.Yz, |
| | 1334 | 136 | | source.Yx, source.Yy, source.Yz)), |
| | 1334 | 137 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 138 | | target.Zx, target.Zy, target.Zz, |
| | 1334 | 139 | | source.Yx, source.Yy, source.Yz)), |
| | 1334 | 140 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 141 | | target.Xx, target.Xy, target.Xz, |
| | 1334 | 142 | | source.Zx, source.Zy, source.Zz)), |
| | 1334 | 143 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 144 | | target.Yx, target.Yy, target.Yz, |
| | 1334 | 145 | | source.Zx, source.Zy, source.Zz)), |
| | 1334 | 146 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 1334 | 147 | | target.Zx, target.Zy, target.Zz, |
| | 1334 | 148 | | source.Zx, source.Zy, source.Zz))); |
| | | 149 | | |
| | | 150 | | internal WideAxis3 GetAxis(int index) => |
| | 5142 | 151 | | index switch |
| | 5142 | 152 | | { |
| | 2058 | 153 | | 0 => new WideAxis3( |
| | 2058 | 154 | | Signed320.ExtendValue(Xx), |
| | 2058 | 155 | | Signed320.ExtendValue(Xy), |
| | 2058 | 156 | | Signed320.ExtendValue(Xz)), |
| | 2048 | 157 | | 1 => new WideAxis3( |
| | 2048 | 158 | | Signed320.ExtendValue(Yx), |
| | 2048 | 159 | | Signed320.ExtendValue(Yy), |
| | 2048 | 160 | | Signed320.ExtendValue(Yz)), |
| | 1036 | 161 | | _ => new WideAxis3( |
| | 1036 | 162 | | Signed320.ExtendValue(Zx), |
| | 1036 | 163 | | Signed320.ExtendValue(Zy), |
| | 1036 | 164 | | Signed320.ExtendValue(Zz)), |
| | 5142 | 165 | | }; |
| | | 166 | | |
| | | 167 | | internal static Signed576 GetComposedScaledCoordinateNumerator( |
| | | 168 | | Fixed64 outerLocalPoint, |
| | | 169 | | Fixed64 outerScale, |
| | | 170 | | Signed192 axisX, |
| | | 171 | | Signed192 axisY, |
| | | 172 | | Signed192 axisZ, |
| | | 173 | | Signed192 rotationDenominator, |
| | | 174 | | Fixed64 innerFrameOffset, |
| | | 175 | | Fixed64 innerFrameScale, |
| | | 176 | | Vector3d innerLocalDisplacement) |
| | | 177 | | { |
| | 201 | 178 | | Signed576 outerScaled = WideArithmetic.MultiplySigned320( |
| | 201 | 179 | | WideArithmetic.MultiplySigned192( |
| | 201 | 180 | | Signed192.Raw(outerLocalPoint), |
| | 201 | 181 | | Signed192.Raw(outerScale)), |
| | 201 | 182 | | Signed320.ExtendValue(rotationDenominator)); |
| | 201 | 183 | | Signed576 innerScaled = WideArithmetic.MultiplySigned320( |
| | 201 | 184 | | WideArithmetic.MultiplySigned192( |
| | 201 | 185 | | Signed192.Raw(innerFrameOffset), |
| | 201 | 186 | | Signed192.Raw(innerFrameScale)), |
| | 201 | 187 | | Signed320.ExtendValue(rotationDenominator)); |
| | 201 | 188 | | Signed320 displacement = WideArithmetic.AddSigned320( |
| | 201 | 189 | | WideArithmetic.AddSigned320( |
| | 201 | 190 | | WideArithmetic.MultiplySigned192( |
| | 201 | 191 | | Signed192.Raw(innerLocalDisplacement.X), |
| | 201 | 192 | | axisX), |
| | 201 | 193 | | WideArithmetic.MultiplySigned192( |
| | 201 | 194 | | Signed192.Raw(innerLocalDisplacement.Y), |
| | 201 | 195 | | axisY)), |
| | 201 | 196 | | WideArithmetic.MultiplySigned192( |
| | 201 | 197 | | Signed192.Raw(innerLocalDisplacement.Z), |
| | 201 | 198 | | axisZ)); |
| | 201 | 199 | | return WideArithmetic.AddSigned576( |
| | 201 | 200 | | WideArithmetic.AddSigned576( |
| | 201 | 201 | | outerScaled, |
| | 201 | 202 | | innerScaled), |
| | 201 | 203 | | WideArithmetic.MultiplySigned320( |
| | 201 | 204 | | displacement, |
| | 201 | 205 | | Signed320.ExtendValue(Signed192.One))); |
| | | 206 | | } |
| | | 207 | | |
| | | 208 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 209 | | private static Signed192 Double(Signed192 value) => |
| | 216198 | 210 | | WideArithmetic.AddSigned192(value, value); |
| | | 211 | | } |