| | | 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 | | { |
| | 178324 | 31 | | if (orientation == FixedQuaternion.Identity) |
| | | 32 | | { |
| | | 33 | | // Identity needs no quaternion-norm scale. Keeping the exact |
| | | 34 | | // unit basis avoids artificial powers in later geometry products. |
| | 38548 | 35 | | this = default; |
| | 38548 | 36 | | Denominator = Xx = Yy = Zz = Signed192.Signed(1); |
| | 38548 | 37 | | return; |
| | | 38 | | } |
| | | 39 | | |
| | 139776 | 40 | | Signed192 xx = Fixed64.GetExactRawProduct( |
| | 139776 | 41 | | orientation.X, |
| | 139776 | 42 | | orientation.X); |
| | 139776 | 43 | | Signed192 yy = Fixed64.GetExactRawProduct( |
| | 139776 | 44 | | orientation.Y, |
| | 139776 | 45 | | orientation.Y); |
| | 139776 | 46 | | Signed192 zz = Fixed64.GetExactRawProduct( |
| | 139776 | 47 | | orientation.Z, |
| | 139776 | 48 | | orientation.Z); |
| | 139776 | 49 | | Signed192 ww = Fixed64.GetExactRawProduct( |
| | 139776 | 50 | | orientation.W, |
| | 139776 | 51 | | orientation.W); |
| | 139776 | 52 | | Signed192 xy = Fixed64.GetExactRawProduct( |
| | 139776 | 53 | | orientation.X, |
| | 139776 | 54 | | orientation.Y); |
| | 139776 | 55 | | Signed192 xz = Fixed64.GetExactRawProduct( |
| | 139776 | 56 | | orientation.X, |
| | 139776 | 57 | | orientation.Z); |
| | 139776 | 58 | | Signed192 xw = Fixed64.GetExactRawProduct( |
| | 139776 | 59 | | orientation.X, |
| | 139776 | 60 | | orientation.W); |
| | 139776 | 61 | | Signed192 yz = Fixed64.GetExactRawProduct( |
| | 139776 | 62 | | orientation.Y, |
| | 139776 | 63 | | orientation.Z); |
| | 139776 | 64 | | Signed192 yw = Fixed64.GetExactRawProduct( |
| | 139776 | 65 | | orientation.Y, |
| | 139776 | 66 | | orientation.W); |
| | 139776 | 67 | | Signed192 zw = Fixed64.GetExactRawProduct( |
| | 139776 | 68 | | orientation.Z, |
| | 139776 | 69 | | orientation.W); |
| | 139776 | 70 | | Denominator = WideArithmetic.AddSigned192( |
| | 139776 | 71 | | WideArithmetic.AddSigned192(xx, yy), |
| | 139776 | 72 | | WideArithmetic.AddSigned192(zz, ww)); |
| | | 73 | | |
| | 139776 | 74 | | Xx = WideArithmetic.AddSigned192( |
| | 139776 | 75 | | WideArithmetic.SubtractSigned192( |
| | 139776 | 76 | | WideArithmetic.SubtractSigned192(xx, yy), |
| | 139776 | 77 | | zz), |
| | 139776 | 78 | | ww); |
| | 139776 | 79 | | Xy = Double(WideArithmetic.AddSigned192(xy, zw)); |
| | 139776 | 80 | | Xz = Double(WideArithmetic.SubtractSigned192(xz, yw)); |
| | 139776 | 81 | | Yx = Double(WideArithmetic.SubtractSigned192(xy, zw)); |
| | 139776 | 82 | | Yy = WideArithmetic.AddSigned192( |
| | 139776 | 83 | | WideArithmetic.SubtractSigned192( |
| | 139776 | 84 | | WideArithmetic.SubtractSigned192(yy, xx), |
| | 139776 | 85 | | zz), |
| | 139776 | 86 | | ww); |
| | 139776 | 87 | | Yz = Double(WideArithmetic.AddSigned192(yz, xw)); |
| | 139776 | 88 | | Zx = Double(WideArithmetic.AddSigned192(xz, yw)); |
| | 139776 | 89 | | Zy = Double(WideArithmetic.SubtractSigned192(yz, xw)); |
| | 139776 | 90 | | Zz = WideArithmetic.AddSigned192( |
| | 139776 | 91 | | WideArithmetic.SubtractSigned192( |
| | 139776 | 92 | | WideArithmetic.SubtractSigned192(zz, xx), |
| | 139776 | 93 | | yy), |
| | 139776 | 94 | | ww); |
| | 139776 | 95 | | } |
| | | 96 | | |
| | | 97 | | private WideRationalBasis3d( |
| | | 98 | | Signed192 denominator, |
| | | 99 | | Signed192 xx, |
| | | 100 | | Signed192 xy, |
| | | 101 | | Signed192 xz, |
| | | 102 | | Signed192 yx, |
| | | 103 | | Signed192 yy, |
| | | 104 | | Signed192 yz, |
| | | 105 | | Signed192 zx, |
| | | 106 | | Signed192 zy, |
| | | 107 | | Signed192 zz) |
| | | 108 | | { |
| | 13710 | 109 | | Denominator = denominator; |
| | 13710 | 110 | | Xx = xx; |
| | 13710 | 111 | | Xy = xy; |
| | 13710 | 112 | | Xz = xz; |
| | 13710 | 113 | | Yx = yx; |
| | 13710 | 114 | | Yy = yy; |
| | 13710 | 115 | | Yz = yz; |
| | 13710 | 116 | | Zx = zx; |
| | 13710 | 117 | | Zy = zy; |
| | 13710 | 118 | | Zz = zz; |
| | 13710 | 119 | | } |
| | | 120 | | |
| | | 121 | | internal static WideRationalBasis3d CreateRelative( |
| | | 122 | | in WideRationalBasis3d target, |
| | | 123 | | in WideRationalBasis3d source) => |
| | | 124 | | // Normalized quaternion-basis products and their three-term sums fit |
| | | 125 | | // Signed192, including the unreduced shared denominator. |
| | 13595 | 126 | | new( |
| | 13595 | 127 | | Signed192.NarrowProven( |
| | 13595 | 128 | | WideArithmetic.MultiplySigned192( |
| | 13595 | 129 | | target.Denominator, |
| | 13595 | 130 | | source.Denominator)), |
| | 13595 | 131 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 132 | | target.Xx, target.Xy, target.Xz, |
| | 13595 | 133 | | source.Xx, source.Xy, source.Xz)), |
| | 13595 | 134 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 135 | | target.Yx, target.Yy, target.Yz, |
| | 13595 | 136 | | source.Xx, source.Xy, source.Xz)), |
| | 13595 | 137 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 138 | | target.Zx, target.Zy, target.Zz, |
| | 13595 | 139 | | source.Xx, source.Xy, source.Xz)), |
| | 13595 | 140 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 141 | | target.Xx, target.Xy, target.Xz, |
| | 13595 | 142 | | source.Yx, source.Yy, source.Yz)), |
| | 13595 | 143 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 144 | | target.Yx, target.Yy, target.Yz, |
| | 13595 | 145 | | source.Yx, source.Yy, source.Yz)), |
| | 13595 | 146 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 147 | | target.Zx, target.Zy, target.Zz, |
| | 13595 | 148 | | source.Yx, source.Yy, source.Yz)), |
| | 13595 | 149 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 150 | | target.Xx, target.Xy, target.Xz, |
| | 13595 | 151 | | source.Zx, source.Zy, source.Zz)), |
| | 13595 | 152 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 153 | | target.Yx, target.Yy, target.Yz, |
| | 13595 | 154 | | source.Zx, source.Zy, source.Zz)), |
| | 13595 | 155 | | Signed192.NarrowProven(WideArithmetic.GetDotProduct3D( |
| | 13595 | 156 | | target.Zx, target.Zy, target.Zz, |
| | 13595 | 157 | | source.Zx, source.Zy, source.Zz))); |
| | | 158 | | |
| | | 159 | | /// <summary>Retains the identity mapping at a caller's exact positive coordinate scale.</summary> |
| | | 160 | | internal static WideRationalBasis3d ScaledIdentity(Signed192 denominator) => |
| | 115 | 161 | | new(denominator, denominator, default, default, default, denominator, default, default, default, denominator); |
| | | 162 | | |
| | | 163 | | internal WideAxis3 GetAxis(int index) => |
| | 354 | 164 | | index switch |
| | 354 | 165 | | { |
| | 128 | 166 | | 0 => new WideAxis3( |
| | 128 | 167 | | Signed320.ExtendValue(Xx), |
| | 128 | 168 | | Signed320.ExtendValue(Xy), |
| | 128 | 169 | | Signed320.ExtendValue(Xz)), |
| | 109 | 170 | | 1 => new WideAxis3( |
| | 109 | 171 | | Signed320.ExtendValue(Yx), |
| | 109 | 172 | | Signed320.ExtendValue(Yy), |
| | 109 | 173 | | Signed320.ExtendValue(Yz)), |
| | 117 | 174 | | _ => new WideAxis3( |
| | 117 | 175 | | Signed320.ExtendValue(Zx), |
| | 117 | 176 | | Signed320.ExtendValue(Zy), |
| | 117 | 177 | | Signed320.ExtendValue(Zz)), |
| | 354 | 178 | | }; |
| | | 179 | | |
| | | 180 | | internal static Signed576 GetComposedScaledCoordinateNumerator( |
| | | 181 | | Fixed64 outerLocalPoint, |
| | | 182 | | Fixed64 outerScale, |
| | | 183 | | Signed192 axisX, |
| | | 184 | | Signed192 axisY, |
| | | 185 | | Signed192 axisZ, |
| | | 186 | | Signed192 rotationDenominator, |
| | | 187 | | Fixed64 innerFrameOffset, |
| | | 188 | | Fixed64 innerFrameScale, |
| | | 189 | | Vector3d innerLocalDisplacement) |
| | | 190 | | { |
| | 201 | 191 | | Signed576 outerScaled = WideArithmetic.MultiplySigned320( |
| | 201 | 192 | | WideArithmetic.MultiplySigned192( |
| | 201 | 193 | | Signed192.Raw(outerLocalPoint), |
| | 201 | 194 | | Signed192.Raw(outerScale)), |
| | 201 | 195 | | Signed320.ExtendValue(rotationDenominator)); |
| | 201 | 196 | | Signed576 innerScaled = WideArithmetic.MultiplySigned320( |
| | 201 | 197 | | WideArithmetic.MultiplySigned192( |
| | 201 | 198 | | Signed192.Raw(innerFrameOffset), |
| | 201 | 199 | | Signed192.Raw(innerFrameScale)), |
| | 201 | 200 | | Signed320.ExtendValue(rotationDenominator)); |
| | 201 | 201 | | Signed320 displacement = WideArithmetic.AddSigned320( |
| | 201 | 202 | | WideArithmetic.AddSigned320( |
| | 201 | 203 | | WideArithmetic.MultiplySigned192( |
| | 201 | 204 | | Signed192.Raw(innerLocalDisplacement.X), |
| | 201 | 205 | | axisX), |
| | 201 | 206 | | WideArithmetic.MultiplySigned192( |
| | 201 | 207 | | Signed192.Raw(innerLocalDisplacement.Y), |
| | 201 | 208 | | axisY)), |
| | 201 | 209 | | WideArithmetic.MultiplySigned192( |
| | 201 | 210 | | Signed192.Raw(innerLocalDisplacement.Z), |
| | 201 | 211 | | axisZ)); |
| | 201 | 212 | | return WideArithmetic.AddSigned576( |
| | 201 | 213 | | WideArithmetic.AddSigned576( |
| | 201 | 214 | | outerScaled, |
| | 201 | 215 | | innerScaled), |
| | 201 | 216 | | WideArithmetic.MultiplySigned320( |
| | 201 | 217 | | displacement, |
| | 201 | 218 | | Signed320.ExtendValue(Signed192.One))); |
| | | 219 | | } |
| | | 220 | | |
| | | 221 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 222 | | private static Signed192 Double(Signed192 value) => |
| | 838656 | 223 | | WideArithmetic.AddSigned192(value, value); |
| | | 224 | | } |