| | | 1 | | //======================================================================= |
| | | 2 | | // ConePlaneRayPointMaterialization.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 world and local anchors from admitted exact plane-section points.</summary> |
| | | 13 | | internal static class ConePlaneRayPointMaterialization |
| | | 14 | | { |
| | | 15 | | /// <summary>Conservatively certifies the world coordinate range of every point in a validated finite cone.</summary |
| | | 16 | | /// <remarks>False is inconclusive: individual points may still be representable.</remarks> |
| | | 17 | | internal static bool IsWorldRangeRepresentable(in ConePlaneRayFrame frame) |
| | 78 | 18 | | => IsRangeRepresentable(frame, Vector3d.Zero); |
| | | 19 | | |
| | | 20 | | /// <summary>Conservatively certifies rounded world coordinates relative to a common sampling origin.</summary> |
| | | 21 | | internal static bool IsRangeRepresentable(in ConePlaneRayFrame frame, Vector3d samplingOrigin) |
| | | 22 | | { |
| | | 23 | | // The frame's exact rational rotation is orthogonal. Each centered |
| | | 24 | | // world component is bounded by Radius+Height/2, so doubled raw bounds |
| | | 25 | | // avoid rounding odd heights. All sums are <68 bits in Signed192. |
| | | 26 | | // Closed [MinRaw,MaxRaw] bounds also avoid nearest-even half-tie edges. |
| | 90 | 27 | | Signed192 radius = Signed192.Raw(frame.Radius); |
| | 90 | 28 | | Signed192 bound = WideArithmetic.AddSigned192(Signed192.Raw(frame.Height), WideArithmetic.AddSigned192(radius, r |
| | 90 | 29 | | Signed192 minimum = WideArithmetic.AddSigned192(Signed192.Signed(long.MinValue), Signed192.Signed(long.MinValue) |
| | 90 | 30 | | Signed192 maximum = WideArithmetic.AddSigned192(Signed192.Signed(long.MaxValue), Signed192.Signed(long.MaxValue) |
| | 540 | 31 | | for (int axis = 0; axis < 3; axis++) |
| | | 32 | | { |
| | 216 | 33 | | Signed192 center = WideArithmetic.SubtractSigned192(Signed192.Raw(frame.ConeCenter[axis]), Signed192.Raw(sam |
| | 216 | 34 | | center = WideArithmetic.AddSigned192(center, center); |
| | 216 | 35 | | if (WideArithmetic.SubtractSigned192(WideArithmetic.SubtractSigned192(center, bound), minimum).Sign < 0 |
| | 216 | 36 | | || WideArithmetic.SubtractSigned192(WideArithmetic.AddSigned192(center, bound), maximum).Sign > 0) |
| | 36 | 37 | | return false; |
| | | 38 | | } |
| | 54 | 39 | | return true; |
| | | 40 | | } |
| | | 41 | | |
| | | 42 | | internal static bool TryGetWorldPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 43 | | scoped ReadOnlySpan<ulong> root, out Vector3d worldPoint) |
| | 119 | 44 | | => TryGetPointInFrame(frame, point, root, Vector3d.Zero, out worldPoint); |
| | | 45 | | |
| | | 46 | | /// <summary>Rounds once on the world lattice, then expresses that point relative to the sampling origin.</summary> |
| | | 47 | | internal static bool TryGetPointInFrame(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 48 | | scoped ReadOnlySpan<ulong> root, Vector3d samplingOrigin, out Vector3d pointInFrame) |
| | 1737 | 49 | | => TryGetPoint(frame, point.X, point.Y, point.Z, point.Denominator, root, false, true, samplingOrigin, out point |
| | | 50 | | |
| | | 51 | | internal static bool TryGetExitWorldPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 52 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 53 | | int orientation, out Vector3d worldPoint) |
| | 36 | 54 | | => TryGetExitPointInFrame(frame, point, root, numerator, denominator, orientation, Vector3d.Zero, out worldPoint |
| | | 55 | | |
| | | 56 | | /// <summary>Rounds an exit once on the world lattice, retaining a representable common-frame coordinate.</summary> |
| | | 57 | | internal static bool TryGetExitPointInFrame(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 58 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 59 | | int orientation, Vector3d samplingOrigin, out Vector3d pointInFrame) |
| | 1605 | 60 | | => TryGetExitPoint(frame, point, root, numerator, denominator, orientation, false, true, samplingOrigin, out poi |
| | | 61 | | |
| | | 62 | | /// <summary>Independently rounds a common-frame exit and its cone-local anchor from one exact construction.</summar |
| | | 63 | | internal static bool TryGetExitPointsInFrame(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 64 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 65 | | int orientation, Vector3d samplingOrigin, out Vector3d pointInFrame, out Vector3d coneLocalPoint) |
| | 39 | 66 | | => TryGetExitPoint(frame, point, root, numerator, denominator, orientation, |
| | 39 | 67 | | coneLocal: false, materialize: true, samplingOrigin, out pointInFrame, |
| | 39 | 68 | | includeConeLocal: true, out coneLocalPoint); |
| | | 69 | | |
| | | 70 | | /// <summary>Rounds a selected exit directly in the cone's centered local frame, independently of world rounding.</s |
| | | 71 | | internal static bool TryGetExitConeLocalPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 72 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 73 | | int orientation, out Vector3d localPoint) |
| | 45 | 74 | | => TryGetExitPoint(frame, point, root, numerator, denominator, orientation, true, true, Vector3d.Zero, out local |
| | | 75 | | |
| | | 76 | | /// <summary>Rounds an admitted point directly in its authored local frame, independently of world rounding.</summar |
| | | 77 | | internal static bool TryGetAuthoredPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 78 | | scoped ReadOnlySpan<ulong> root, out Vector3d localPoint) |
| | | 79 | | { |
| | 47 | 80 | | if (frame.Finite.ShapeFrame.HasExactScale) |
| | | 81 | | { |
| | 17 | 82 | | int fieldWords = Math.Max(ContactQuadratic.ActiveWords(point.Y), |
| | 17 | 83 | | ContactQuadratic.ActiveWords(point.Denominator)) + 5; |
| | 34 | 84 | | Span<ulong> fields = stackalloc ulong[4 * fieldWords]; Span<int> fieldSigns = stackalloc int[4]; |
| | 17 | 85 | | ContactQuadratic centered = ContactQuadratic.At(fields, fieldSigns, 0, fieldWords), |
| | 17 | 86 | | denominator = ContactQuadratic.At(fields, fieldSigns, 1, fieldWords); |
| | 17 | 87 | | ContactQuadratic.Scale(point.Denominator, Signed576.ExtendValue(frame.Finite.ShapeFrame.Cap), centered); |
| | 17 | 88 | | centered.Add(point.Y, -1); |
| | 17 | 89 | | ContactQuadratic.Scale(point.Denominator, Signed576.ExtendValue(WideArithmetic.AddSigned320( |
| | 17 | 90 | | frame.Finite.ShapeFrame.CoordinateDenominator, frame.Finite.ShapeFrame.CoordinateDenominator)), denomina |
| | 17 | 91 | | return QuadraticPointMaterialization.TryGetPointInScaledFrame(frame.ConeCenter, frame.ConeRotation, |
| | 17 | 92 | | frame.SourceFrame, point.X, centered, point.Z, denominator, root, out localPoint); |
| | | 93 | | } |
| | | 94 | | // The retained centered transform is P=2*(B*a+T), with B/D exactly |
| | | 95 | | // orthogonal. Its inverse is a=B^T*(P-2*T)/(2*D^2). Relative basis |
| | | 96 | | // entries <130 bits and translation <198 add <333 bits to any retained |
| | | 97 | | // field; six extra limbs cover every complete sum without narrowing. |
| | 30 | 98 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(point.X), ContactQuadratic.ActiveWords(point.Y)), |
| | 30 | 99 | | Math.Max(ContactQuadratic.ActiveWords(point.Z), ContactQuadratic.ActiveWords(point.Denominator))) + 6; |
| | 60 | 100 | | Span<ulong> work = stackalloc ulong[16 * words]; Span<int> signs = stackalloc int[16]; |
| | 30 | 101 | | ContactQuadratic x = ContactQuadratic.At(work, signs, 0, words); |
| | 30 | 102 | | ContactQuadratic y = ContactQuadratic.At(work, signs, 1, words); |
| | 30 | 103 | | ContactQuadratic z = ContactQuadratic.At(work, signs, 2, words); |
| | 30 | 104 | | ContactQuadratic d = ContactQuadratic.At(work, signs, 3, words); |
| | 30 | 105 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 4, words); |
| | 30 | 106 | | Signed576 one = Extend(Signed192.Signed(1)); |
| | 60 | 107 | | ContactQuadratic.Scale(point.X, one, x); ContactQuadratic.Scale(point.Z, one, z); |
| | 60 | 108 | | ContactQuadratic.Scale(point.Denominator, Signed576.ExtendValue(frame.Finite.ShapeFrame.Cap), y); y.Add(point.Y, |
| | 240 | 109 | | for (int axis = 0; axis < 3; axis++) |
| | | 110 | | { |
| | 90 | 111 | | Signed320 offset = axis == 0 ? frame.Finite.ShapeFrame.Translation.X |
| | 90 | 112 | | : axis == 1 ? frame.Finite.ShapeFrame.Translation.Y : frame.Finite.ShapeFrame.Translation.Z; |
| | 180 | 113 | | ContactQuadratic.Scale(point.Denominator, Signed576.ExtendValue(offset), term); term.Add(term); |
| | 90 | 114 | | (axis == 0 ? x : axis == 1 ? y : z).Add(term, -1); |
| | | 115 | | } |
| | 30 | 116 | | WideRationalBasis3d basis = frame.Finite.ShapeFrame.Basis; |
| | 30 | 117 | | Signed192 tripleDenominator = WideArithmetic.AddSigned192( |
| | 30 | 118 | | WideArithmetic.AddSigned192(basis.Denominator, basis.Denominator), basis.Denominator); |
| | 30 | 119 | | if (WideArithmetic.AddSigned192(WideArithmetic.AddSigned192(basis.Xx, basis.Yy), basis.Zz).Equals(tripleDenomina |
| | | 120 | | { |
| | | 121 | | // An exact proper orthogonal rotation has trace 3 only at identity. |
| | | 122 | | // Then B=D*I and B^T*(P-2T)/(2D²) cancels to (P-2T)/(2D), |
| | | 123 | | // including equal noncardinal authored/cone rotations. Keep this |
| | | 124 | | // cancellation local; other geometry owners retain their basis ABI. |
| | 24 | 125 | | ContactQuadratic.Scale(point.Denominator, |
| | 24 | 126 | | Extend(WideArithmetic.AddSigned192(basis.Denominator, basis.Denominator)), d); |
| | 24 | 127 | | return QuadraticPointMaterialization.TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Identity, x, y, z, d, r |
| | | 128 | | } |
| | 6 | 129 | | Signed320 square = WideArithmetic.MultiplySigned192(basis.Denominator, basis.Denominator); |
| | 6 | 130 | | ContactQuadratic.Scale(point.Denominator, Signed576.ExtendValue(WideArithmetic.AddSigned320(square, square)), d) |
| | 48 | 131 | | for (int axis = 0; axis < 3; axis++) |
| | | 132 | | { |
| | 18 | 133 | | ContactQuadratic target = ContactQuadratic.At(work, signs, 5 + axis, words); |
| | 18 | 134 | | ContactQuadratic.Scale(x, Extend(axis == 0 ? basis.Xx : axis == 1 ? basis.Yx : basis.Zx), target); |
| | 36 | 135 | | ContactQuadratic.Scale(y, Extend(axis == 0 ? basis.Xy : axis == 1 ? basis.Yy : basis.Zy), term); target.Add( |
| | 36 | 136 | | ContactQuadratic.Scale(z, Extend(axis == 0 ? basis.Xz : axis == 1 ? basis.Yz : basis.Zz), term); target.Add( |
| | | 137 | | } |
| | 6 | 138 | | return QuadraticPointMaterialization.TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Identity, |
| | 6 | 139 | | ContactQuadratic.At(work, signs, 5, words), ContactQuadratic.At(work, signs, 6, words), |
| | 6 | 140 | | ContactQuadratic.At(work, signs, 7, words), d, root, out localPoint); |
| | | 141 | | } |
| | | 142 | | |
| | | 143 | | /// <summary>Rounds a source-frame point directly in another authored frame of the same cone.</summary> |
| | | 144 | | internal static bool TryGetAuthoredPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 145 | | scoped ReadOnlySpan<ulong> root, in ConePlaneRayFrame sourceFrame, out Vector3d localPoint) |
| | | 146 | | { |
| | 8 | 147 | | Signed320 sourceScale = sourceFrame.Finite.ShapeFrame.CoordinateDenominator; |
| | 8 | 148 | | Signed320 targetScale = frame.Finite.ShapeFrame.CoordinateDenominator; |
| | 12 | 149 | | if (sourceScale.Equals(targetScale)) return TryGetAuthoredPoint(frame, point, root, out localPoint); |
| | 4 | 150 | | Span<ulong> values = stackalloc ulong[frame.PointStorageWords]; |
| | 4 | 151 | | Span<int> signs = stackalloc int[ConePlaneRayPoint.SignCount]; |
| | 4 | 152 | | ConePlaneRayPoint transformed = new(values, signs, frame.FieldWords); |
| | 4 | 153 | | point.CopyTo(transformed, sourceScale, targetScale); |
| | 4 | 154 | | return TryGetAuthoredPoint(frame, transformed, root, out localPoint); |
| | | 155 | | } |
| | | 156 | | |
| | | 157 | | /// <summary>Tests the final nearest-even exit coordinate range without materializing a world point.</summary> |
| | | 158 | | internal static bool IsExitWorldPointRepresentable(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 159 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 160 | | int orientation) |
| | 32 | 161 | | => IsExitPointRepresentable(frame, point, root, numerator, denominator, orientation, Vector3d.Zero); |
| | | 162 | | |
| | | 163 | | /// <summary>Tests final common-frame coordinate range using the world's nearest-even parity.</summary> |
| | | 164 | | internal static bool IsExitPointRepresentable(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 165 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 166 | | int orientation, Vector3d samplingOrigin) |
| | 44 | 167 | | => TryGetExitPoint(frame, point, root, numerator, denominator, orientation, false, false, samplingOrigin, out _) |
| | | 168 | | |
| | | 169 | | private static bool TryGetExitPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 170 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 171 | | int orientation, bool coneLocal, bool materialize, Vector3d samplingOrigin, out Vector3d worldPoint) |
| | 1694 | 172 | | => TryGetExitPoint(frame, point, root, numerator, denominator, orientation, coneLocal, materialize, |
| | 1694 | 173 | | samplingOrigin, out worldPoint, includeConeLocal: false, out _); |
| | | 174 | | |
| | | 175 | | private static bool TryGetExitPoint(in ConePlaneRayFrame frame, scoped ConePlaneRayPoint point, |
| | | 176 | | scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic numerator, scoped ContactQuadratic denominator, |
| | | 177 | | int orientation, bool coneLocal, bool materialize, Vector3d samplingOrigin, out Vector3d worldPoint, |
| | | 178 | | bool includeConeLocal, out Vector3d coneLocalPoint) |
| | | 179 | | { |
| | 1733 | 180 | | worldPoint = coneLocalPoint = default; |
| | 1733 | 181 | | int numeratorSign = numerator.Sign(root); |
| | 1733 | 182 | | if (numeratorSign < 0 || denominator.Sign(root) <= 0 || point.Denominator.Sign(root) <= 0) |
| | 18 | 183 | | return false; |
| | 1715 | 184 | | if (numeratorSign == 0) |
| | 518 | 185 | | return TryGetPoint(frame, point.X, point.Y, point.Z, point.Denominator, root, coneLocal, materialize, sampli |
| | 518 | 186 | | && (!includeConeLocal || TryGetPoint(frame, point.X, point.Y, point.Z, point.Denominator, |
| | 518 | 187 | | root, true, true, Vector3d.Zero, out coneLocalPoint)); |
| | | 188 | | // q=(P*d+orientation*N*n*D)/(D*d). Complete quadratic products |
| | | 189 | | // need p+e+r limbs and one addition carry. The Signed320 normal |
| | | 190 | | // adds at most five limbs, and the final sum adds another carry. |
| | | 191 | | // Count actual coefficients, including the root product; retained |
| | | 192 | | // padded tails never justify narrowing a complete active product. |
| | 1197 | 193 | | int pointWords = Math.Max(Math.Max(ContactQuadratic.ActiveWords(point.X), ContactQuadratic.ActiveWords(point.Y)) |
| | 1197 | 194 | | Math.Max(ContactQuadratic.ActiveWords(point.Z), ContactQuadratic.ActiveWords(point.Denominator))); |
| | 1197 | 195 | | int exitWords = pointWords + Math.Max(ContactQuadratic.ActiveWords(numerator), ContactQuadratic.ActiveWords(deno |
| | 1197 | 196 | | + WideArithmetic.GetActiveMagnitudeLength(root) + 7; |
| | 2394 | 197 | | Span<ulong> work = stackalloc ulong[10 * exitWords]; Span<int> signs = stackalloc int[10]; |
| | 1197 | 198 | | ContactQuadratic x = ContactQuadratic.At(work, signs, 0, exitWords); |
| | 1197 | 199 | | ContactQuadratic y = ContactQuadratic.At(work, signs, 1, exitWords); |
| | 1197 | 200 | | ContactQuadratic z = ContactQuadratic.At(work, signs, 2, exitWords); |
| | 1197 | 201 | | ContactQuadratic d = ContactQuadratic.At(work, signs, 3, exitWords); |
| | 1197 | 202 | | ContactQuadratic term = ContactQuadratic.At(work, signs, 4, exitWords); |
| | 1197 | 203 | | ContactQuadratic.Multiply(point.Denominator, numerator, root, d); |
| | 9576 | 204 | | for (int axis = 0; axis < 3; axis++) |
| | | 205 | | { |
| | 3591 | 206 | | ContactQuadratic coordinate = axis == 0 ? point.X : axis == 1 ? point.Y : point.Z; |
| | 3591 | 207 | | ContactQuadratic target = axis == 0 ? x : axis == 1 ? y : z; |
| | 3591 | 208 | | Signed576 normal = axis == 0 ? frame.Normal.X : axis == 1 ? frame.Normal.Y : frame.Normal.Z; |
| | 3591 | 209 | | ContactQuadratic.Multiply(coordinate, denominator, root, target); |
| | 7182 | 210 | | ContactQuadratic.Scale(d, normal, term); target.Add(term, orientation); |
| | | 211 | | } |
| | 1197 | 212 | | ContactQuadratic.Multiply(point.Denominator, denominator, root, d); |
| | | 213 | | // Reuse the exact exit, then round each output on its own lattice. |
| | | 214 | | // Inverse-transforming the rounded world output would change local ties. |
| | 1197 | 215 | | return TryGetPoint(frame, x, y, z, d, root, coneLocal, materialize, samplingOrigin, out worldPoint) |
| | 1197 | 216 | | && (!includeConeLocal || TryGetPoint(frame, x, y, z, d, root, true, true, Vector3d.Zero, out coneLocalPoint) |
| | | 217 | | } |
| | | 218 | | |
| | | 219 | | private static bool TryGetPoint(in ConePlaneRayFrame frame, scoped ContactQuadratic px, |
| | | 220 | | scoped ContactQuadratic py, scoped ContactQuadratic pz, scoped ContactQuadratic pd, |
| | | 221 | | scoped ReadOnlySpan<ulong> root, bool coneLocal, bool materialize, Vector3d samplingOrigin, out Vector3d worldPo |
| | | 222 | | { |
| | | 223 | | // Convert apex coordinates to centered raw cone coordinates before |
| | | 224 | | // the shared exact rigid transform. This changes no retained field. |
| | 3482 | 225 | | int words = Math.Max(ContactQuadratic.ActiveWords(py), ContactQuadratic.ActiveWords(pd)) + 5; |
| | 6964 | 226 | | Span<ulong> work = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 3482 | 227 | | ContactQuadratic centeredY = ContactQuadratic.At(work, signs, 0, words); |
| | 3482 | 228 | | ContactQuadratic denominator = ContactQuadratic.At(work, signs, 1, words); |
| | 3482 | 229 | | ContactQuadratic.Scale(pd, Signed576.ExtendValue(frame.Finite.ShapeFrame.Cap), centeredY); |
| | 3482 | 230 | | centeredY.Add(py, -1); |
| | 3482 | 231 | | Signed320 scale = WideArithmetic.AddSigned320(frame.Finite.ShapeFrame.CoordinateDenominator, |
| | 3482 | 232 | | frame.Finite.ShapeFrame.CoordinateDenominator); |
| | 3482 | 233 | | ContactQuadratic.Scale(pd, Signed576.ExtendValue(scale), denominator); |
| | 3482 | 234 | | return QuadraticPointMaterialization.TryGetWorldPoint(coneLocal ? Vector3d.Zero : frame.ConeCenter, |
| | 3482 | 235 | | coneLocal ? FixedQuaternion.Identity : frame.ConeRotation, |
| | 3482 | 236 | | px, centeredY, pz, denominator, root, materialize, samplingOrigin, out worldPoint); |
| | | 237 | | } |
| | | 238 | | |
| | 108 | 239 | | private static Signed576 Extend(Signed192 value) => Signed576.ExtendValue(Signed320.ExtendValue(value)); |
| | | 240 | | } |