| | | 1 | | //======================================================================= |
| | | 2 | | // QuadraticPointMaterialization.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>Final nearest-even rigid-frame points from retained exact quadratic coordinates.</summary> |
| | | 13 | | internal static class QuadraticPointMaterialization |
| | | 14 | | { |
| | | 15 | | /// <summary>Composes an exact rigid point with an affine inverse before final source-lattice rounding.</summary> |
| | | 16 | | internal static bool TryGetPointInScaledFrame(Vector3d sourceOrigin, FixedQuaternion sourceRotation, |
| | | 17 | | in ScaledRigidFrame3d target, scoped ContactQuadratic px, scoped ContactQuadratic py, |
| | | 18 | | scoped ContactQuadratic pz, scoped ContactQuadratic pd, scoped ReadOnlySpan<ulong> root, |
| | | 19 | | out Vector3d sourcePoint) |
| | | 20 | | { |
| | 21 | 21 | | sourcePoint = default; |
| | 22 | 22 | | if (pd.Sign(root) <= 0) return false; |
| | | 23 | | // The rigid composition adds <132 bits, and affine inverse |
| | | 24 | | // coefficients/offsets add <453. Ten extra limbs retain every sum. |
| | 20 | 25 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(px), ContactQuadratic.ActiveWords(py)), |
| | 20 | 26 | | Math.Max(ContactQuadratic.ActiveWords(pz), ContactQuadratic.ActiveWords(pd))) + 10; |
| | 40 | 27 | | Span<ulong> work = stackalloc ulong[16 * words]; Span<int> signs = stackalloc int[16]; |
| | 20 | 28 | | ContactQuadratic x = ContactQuadratic.At(work, signs, 0, words), |
| | 40 | 29 | | y = ContactQuadratic.At(work, signs, 1, words), z = ContactQuadratic.At(work, signs, 2, words), |
| | 40 | 30 | | denominator = ContactQuadratic.At(work, signs, 3, words), term = ContactQuadratic.At(work, signs, 4, words), |
| | 40 | 31 | | coordinate = ContactQuadratic.At(work, signs, 5, words), divisor = ContactQuadratic.At(work, signs, 6, words |
| | 20 | 32 | | zero = ContactQuadratic.At(work, signs, 7, words); |
| | 20 | 33 | | zero.Clear(); |
| | 20 | 34 | | TransformPointInRigidFrame(sourceOrigin, sourceRotation, Vector3d.Zero, FixedQuaternion.Identity, |
| | 20 | 35 | | px, py, pz, pd, x, y, z, denominator, term); |
| | 20 | 36 | | Span<Fixed64> coordinates = stackalloc Fixed64[3]; |
| | 148 | 37 | | for (int axis = 0; axis < 3; axis++) |
| | | 38 | | { |
| | 56 | 39 | | target.GetSourceCoordinateTransform(axis, out Signed576 cx, out Signed576 cy, out Signed576 cz, |
| | 56 | 40 | | out Signed576 offset, out Signed576 scale); |
| | 57 | 41 | | if (scale.IsZero) return false; |
| | 55 | 42 | | ContactQuadratic.Scale(x, cx, coordinate); |
| | 110 | 43 | | ContactQuadratic.Scale(y, cy, term); coordinate.Add(term); |
| | 110 | 44 | | ContactQuadratic.Scale(z, cz, term); coordinate.Add(term); |
| | 110 | 45 | | ContactQuadratic.Scale(denominator, offset, term); coordinate.Add(term); |
| | 55 | 46 | | ContactQuadratic.Scale(denominator, scale, divisor); |
| | 57 | 47 | | if (scale.Sign < 0) { coordinate.MultiplySign(-1); divisor.MultiplySign(-1); } |
| | | 48 | | // Reuse the authoritative scalar range/tie policy; the two zero |
| | | 49 | | // coordinates add no geometry or intermediate materialization. |
| | 55 | 50 | | if (!TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Identity, |
| | 55 | 51 | | coordinate, zero, zero, divisor, root, out Vector3d rounded)) |
| | 1 | 52 | | return false; |
| | 54 | 53 | | coordinates[axis] = rounded.X; |
| | | 54 | | } |
| | 18 | 55 | | sourcePoint = new Vector3d(coordinates[0], coordinates[1], coordinates[2]); |
| | 18 | 56 | | return true; |
| | | 57 | | } |
| | | 58 | | |
| | | 59 | | /// <summary>Rounds an exact offset after composing its source and target rigid bases.</summary> |
| | | 60 | | internal static bool TryGetOffsetInFrame(FixedQuaternion sourceRotation, FixedQuaternion targetRotation, |
| | | 61 | | scoped ContactQuadratic px, scoped ContactQuadratic py, scoped ContactQuadratic pz, |
| | | 62 | | scoped ContactQuadratic pd, scoped ReadOnlySpan<ulong> root, out Vector3d offset) |
| | 28 | 63 | | => TryGetPointInRigidFrame(Vector3d.Zero, sourceRotation, Vector3d.Zero, targetRotation, |
| | 28 | 64 | | px, py, pz, pd, root, out offset); |
| | | 65 | | |
| | | 66 | | /// <summary>Composes both rigid frames before final nearest-even point materialization.</summary> |
| | | 67 | | internal static bool TryGetPointInRigidFrame(Vector3d sourceOrigin, FixedQuaternion sourceRotation, |
| | | 68 | | Vector3d targetOrigin, FixedQuaternion targetRotation, |
| | | 69 | | scoped ContactQuadratic px, scoped ContactQuadratic py, scoped ContactQuadratic pz, |
| | | 70 | | scoped ContactQuadratic pd, scoped ReadOnlySpan<ulong> root, out Vector3d point) |
| | | 71 | | { |
| | 666 | 72 | | if (sourceRotation == targetRotation && sourceOrigin == targetOrigin) |
| | 174 | 73 | | return TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Identity, px, py, pz, pd, root, out point); |
| | 492 | 74 | | if (targetRotation == FixedQuaternion.Identity) |
| | 228 | 75 | | return TryGetPointInFrame(sourceOrigin, sourceRotation, px, py, pz, pd, root, targetOrigin, out point); |
| | | 76 | | |
| | | 77 | | // Each quaternion basis coefficient is <130 bits. Composing two bases |
| | | 78 | | // and summing three terms adds <262 bits. An origin difference adds |
| | | 79 | | // <65 bits, so its complete translated coefficient stays <327 bits. |
| | 264 | 80 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(px), ContactQuadratic.ActiveWords(py)), |
| | 264 | 81 | | Math.Max(ContactQuadratic.ActiveWords(pz), ContactQuadratic.ActiveWords(pd))) + 6; |
| | 528 | 82 | | Span<ulong> work = stackalloc ulong[10 * words]; Span<int> signs = stackalloc int[10]; |
| | 264 | 83 | | ContactQuadratic x = ContactQuadratic.At(work, signs, 0, words); |
| | 264 | 84 | | ContactQuadratic y = ContactQuadratic.At(work, signs, 1, words); |
| | 264 | 85 | | ContactQuadratic z = ContactQuadratic.At(work, signs, 2, words); |
| | 264 | 86 | | ContactQuadratic denominator = ContactQuadratic.At(work, signs, 3, words); |
| | 264 | 87 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 4, words); |
| | 264 | 88 | | TransformPointInRigidFrame(sourceOrigin, sourceRotation, targetOrigin, targetRotation, |
| | 264 | 89 | | px, py, pz, pd, x, y, z, denominator, term); |
| | 264 | 90 | | return TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Identity, x, y, z, denominator, root, out point); |
| | | 91 | | } |
| | | 92 | | |
| | | 93 | | /// <summary> |
| | | 94 | | /// Retains exact target-frame coordinates for subsequent algebraic queries. |
| | | 95 | | /// All output banks and the disjoint scratch bank need input active words+6. |
| | | 96 | | /// </summary> |
| | | 97 | | private static void TransformPointInRigidFrame(Vector3d sourceOrigin, FixedQuaternion sourceRotation, |
| | | 98 | | Vector3d targetOrigin, FixedQuaternion targetRotation, |
| | | 99 | | scoped ContactQuadratic px, scoped ContactQuadratic py, scoped ContactQuadratic pz, scoped ContactQuadratic pd, |
| | | 100 | | scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, |
| | | 101 | | scoped ContactQuadratic denominator, scoped ContactQuadratic term) |
| | | 102 | | { |
| | 568 | 103 | | WideRationalBasis3d source = new(sourceRotation), target = new(targetRotation); |
| | 284 | 104 | | bool translated = sourceOrigin != targetOrigin; |
| | 284 | 105 | | ContactQuadratic.Scale(pd, Signed576.ExtendValue(WideArithmetic.MultiplySigned192( |
| | 284 | 106 | | source.Denominator, target.Denominator)), denominator); |
| | 2272 | 107 | | for (int axis = 0; axis < 3; axis++) |
| | | 108 | | { |
| | 852 | 109 | | Signed192 tx = axis == 0 ? target.Xx : axis == 1 ? target.Yx : target.Zx; |
| | 852 | 110 | | Signed192 ty = axis == 0 ? target.Xy : axis == 1 ? target.Yy : target.Zy; |
| | 852 | 111 | | Signed192 tz = axis == 0 ? target.Xz : axis == 1 ? target.Yz : target.Zz; |
| | 852 | 112 | | ContactQuadratic coordinate = axis == 0 ? x : axis == 1 ? y : z; |
| | 852 | 113 | | ContactQuadratic.Scale(px, Signed576.ExtendValue(WideArithmetic.GetDotProduct3D( |
| | 852 | 114 | | source.Xx, source.Xy, source.Xz, tx, ty, tz)), coordinate); |
| | 852 | 115 | | ContactQuadratic.Scale(py, Signed576.ExtendValue(WideArithmetic.GetDotProduct3D( |
| | 1704 | 116 | | source.Yx, source.Yy, source.Yz, tx, ty, tz)), term); coordinate.Add(term); |
| | 852 | 117 | | ContactQuadratic.Scale(pz, Signed576.ExtendValue(WideArithmetic.GetDotProduct3D( |
| | 1704 | 118 | | source.Zx, source.Zy, source.Zz, tx, ty, tz)), term); coordinate.Add(term); |
| | 852 | 119 | | if (translated) |
| | | 120 | | { |
| | 678 | 121 | | Signed320 translation = WideArithmetic.GetDotProduct3D( |
| | 678 | 122 | | WideArithmetic.SubtractSigned192(Signed192.Raw(sourceOrigin.X), Signed192.Raw(targetOrigin.X)), |
| | 678 | 123 | | WideArithmetic.SubtractSigned192(Signed192.Raw(sourceOrigin.Y), Signed192.Raw(targetOrigin.Y)), |
| | 678 | 124 | | WideArithmetic.SubtractSigned192(Signed192.Raw(sourceOrigin.Z), Signed192.Raw(targetOrigin.Z)), tx, |
| | 678 | 125 | | ContactQuadratic.Scale(pd, WideArithmetic.MultiplySigned576(Signed576.ExtendValue(translation), source.D |
| | 678 | 126 | | coordinate.Add(term); |
| | | 127 | | } |
| | | 128 | | } |
| | 284 | 129 | | } |
| | | 130 | | |
| | | 131 | | /// <summary>Rounds exact raw local coordinates after one rational rigid transform.</summary> |
| | | 132 | | internal static bool TryGetWorldPoint(Vector3d origin, FixedQuaternion rotation, |
| | | 133 | | scoped ContactQuadratic px, scoped ContactQuadratic py, scoped ContactQuadratic pz, |
| | | 134 | | scoped ContactQuadratic pd, scoped ReadOnlySpan<ulong> root, out Vector3d worldPoint) |
| | 1273 | 135 | | => TryGetPointInFrame(origin, rotation, px, py, pz, pd, root, Vector3d.Zero, out worldPoint); |
| | | 136 | | |
| | | 137 | | internal static bool TryGetPointInFrame(Vector3d origin, FixedQuaternion rotation, |
| | | 138 | | scoped ContactQuadratic px, scoped ContactQuadratic py, scoped ContactQuadratic pz, |
| | | 139 | | scoped ContactQuadratic pd, scoped ReadOnlySpan<ulong> root, Vector3d samplingOrigin, out Vector3d pointInFrame) |
| | 1589 | 140 | | => TryGetWorldPoint(origin, rotation, px, py, pz, pd, root, true, samplingOrigin, out pointInFrame); |
| | | 141 | | |
| | | 142 | | internal static bool TryGetWorldPoint(Vector3d origin, FixedQuaternion rotation, |
| | | 143 | | scoped ContactQuadratic px, scoped ContactQuadratic py, scoped ContactQuadratic pz, |
| | | 144 | | scoped ContactQuadratic pd, scoped ReadOnlySpan<ulong> root, bool materialize, Vector3d samplingOrigin, out Vect |
| | | 145 | | { |
| | 5071 | 146 | | worldPoint = default; |
| | 5071 | 147 | | if (pd.Sign(root) <= 0) |
| | 3 | 148 | | return false; |
| | | 149 | | // Representable quaternion products add <130 bits, translation <64, |
| | | 150 | | // and the final doubled representability threshold <65. Four extra |
| | | 151 | | // limbs cover all complete products and sums for any supplied fields. |
| | 5068 | 152 | | bool identity = rotation == FixedQuaternion.Identity; |
| | 5068 | 153 | | WideRationalBasis3d basis = identity ? default : new(rotation); |
| | 5068 | 154 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(px), ContactQuadratic.ActiveWords(py)), |
| | 5068 | 155 | | Math.Max(ContactQuadratic.ActiveWords(pz), ContactQuadratic.ActiveWords(pd))) + 4; |
| | 10136 | 156 | | Span<ulong> work = stackalloc ulong[8 * words]; Span<int> signs = stackalloc int[8]; |
| | 5068 | 157 | | ContactQuadratic denominator = ContactQuadratic.At(work, signs, 0, words); |
| | 5068 | 158 | | ContactQuadratic numerator = ContactQuadratic.At(work, signs, 1, words); |
| | 5068 | 159 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 2, words); |
| | 5068 | 160 | | ContactQuadratic query = ContactQuadratic.At(work, signs, 3, words); |
| | 9443 | 161 | | if (identity) ContactQuadratic.Scale(pd, Extend(Signed192.Signed(1)), denominator); |
| | 693 | 162 | | else ContactQuadratic.Scale(pd, Extend(basis.Denominator), denominator); |
| | 5068 | 163 | | Signed192 upperHalf = WideArithmetic.AddSigned192(WideArithmetic.AddSigned192( |
| | 5068 | 164 | | Signed192.Signed(long.MaxValue), Signed192.Signed(long.MaxValue)), Signed192.Signed(1)); |
| | 5068 | 165 | | Signed192 lowerHalf = WideArithmetic.SubtractSigned192(WideArithmetic.AddSigned192( |
| | 5068 | 166 | | Signed192.Signed(long.MinValue), Signed192.Signed(long.MinValue)), Signed192.Signed(1)); |
| | 5068 | 167 | | Span<Fixed64> coordinates = stackalloc Fixed64[3]; |
| | 40128 | 168 | | for (int axis = 0; axis < 3; axis++) |
| | | 169 | | { |
| | 15068 | 170 | | Signed192 x = axis == 0 ? basis.Xx : axis == 1 ? basis.Xy : basis.Xz; |
| | 15068 | 171 | | Signed192 y = axis == 0 ? basis.Yx : axis == 1 ? basis.Yy : basis.Yz; |
| | 15068 | 172 | | Signed192 z = axis == 0 ? basis.Zx : axis == 1 ? basis.Zy : basis.Zz; |
| | | 173 | | // Identity is also the direct local-output route. Preserve the |
| | | 174 | | // original ratio instead of multiplying it by a redundant S^2 |
| | | 175 | | // quaternion denominator and performing six zero products. |
| | 28101 | 176 | | if (identity) ContactQuadratic.Scale(axis == 0 ? px : axis == 1 ? py : pz, Extend(Signed192.Signed(1)), nume |
| | | 177 | | else |
| | | 178 | | { |
| | 2035 | 179 | | ContactQuadratic.Scale(px, Extend(x), numerator); |
| | 4070 | 180 | | ContactQuadratic.Scale(py, Extend(y), term); numerator.Add(term); |
| | 4070 | 181 | | ContactQuadratic.Scale(pz, Extend(z), term); numerator.Add(term); |
| | | 182 | | } |
| | | 183 | | // Subtract origins before narrowing. A valid point offset can exist |
| | | 184 | | // even when its conceptual absolute world coordinate is outside Q32.32. |
| | 15068 | 185 | | Signed192 center = WideArithmetic.SubtractSigned192(Signed192.Raw(origin[axis]), Signed192.Raw(samplingOrigi |
| | 30136 | 186 | | ContactQuadratic.Scale(denominator, Extend(center), term); numerator.Add(term); |
| | 15068 | 187 | | long parityOffset = samplingOrigin[axis].m_rawValue; |
| | | 188 | | // Rational coordinates share the raw ratio owner's exact signed |
| | | 189 | | // range and nearest-even policy, combining admission and rounding. |
| | 15068 | 190 | | if (numerator.Signs[1] == 0 && denominator.Signs[1] == 0) |
| | | 191 | | { |
| | 10184 | 192 | | if (!Fixed64.TryGetSignedRawRatio(numerator.Rational, denominator.Rational, numerator.Signs[0] < 0, out |
| | 39 | 193 | | return false; |
| | | 194 | | continue; |
| | | 195 | | } |
| | | 196 | | // Ties belong to the global lattice: an odd sampling origin flips |
| | | 197 | | // the common-frame parity. Preserve this at both range thresholds |
| | | 198 | | // and final rounding, so translating cannot change a selected row. |
| | 9768 | 199 | | numerator.CopyTo(term); term.Add(numerator); |
| | 4884 | 200 | | ContactQuadratic.Scale(denominator, Extend(upperHalf), query); |
| | 4884 | 201 | | query.Add(term, -1); |
| | 4884 | 202 | | int upperComparison = query.Sign(root); |
| | 4884 | 203 | | if (upperComparison < 0 || upperComparison == 0 && (parityOffset & 1) == 0) |
| | 8 | 204 | | return false; |
| | 4876 | 205 | | ContactQuadratic.Scale(denominator, Extend(lowerHalf), query); |
| | 4876 | 206 | | query.Add(term, -1); |
| | 4876 | 207 | | int lowerComparison = query.Sign(root); |
| | 4876 | 208 | | if (lowerComparison > 0 || lowerComparison == 0 && (parityOffset & 1) != 0) |
| | 25 | 209 | | return false; |
| | 9660 | 210 | | if (materialize) coordinates[axis] = ContactQuadratic.RoundRatio(numerator, denominator, root, parityOffset: |
| | | 211 | | } |
| | 9962 | 212 | | if (materialize) worldPoint = new Vector3d(coordinates[0], coordinates[1], coordinates[2]); |
| | 4996 | 213 | | return true; |
| | | 214 | | } |
| | | 215 | | |
| | 49034 | 216 | | private static Signed576 Extend(Signed192 value) => Signed576.ExtendValue(Signed320.ExtendValue(value)); |
| | | 217 | | } |