| | | 1 | | //======================================================================= |
| | | 2 | | // TriangleCylinderRimWitnesses.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 | | using static FixedMathSharp.Geometry.CylinderContactAlgebra; |
| | | 10 | | |
| | | 11 | | namespace FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | /// <summary>Selected rim witnesses before either support point is rounded.</summary> |
| | | 14 | | internal static class TriangleCylinderRimWitnesses |
| | | 15 | | { |
| | | 16 | | internal static Vector3d GetAnalyticPoint(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 17 | | int mask, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> normalSigns, int cap, Signed576 radialRadius) |
| | | 18 | | { |
| | 102 | 19 | | bool admitted = TryGetAnalyticPoint(geometry, triangle, mask, normal, normalSigns, cap, radialRadius, |
| | 102 | 20 | | out Vector3d point, false); |
| | | 21 | | System.Diagnostics.Debug.Assert(admitted); |
| | 102 | 22 | | return point; |
| | | 23 | | } |
| | | 24 | | |
| | | 25 | | internal static bool TryGetAnalyticPoint(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 26 | | int mask, ReadOnlySpan<ulong> normal, ReadOnlySpan<int> normalSigns, int cap, Signed576 radialRadius, |
| | | 27 | | out Vector3d point, bool requireAdmission = true) |
| | | 28 | | { |
| | 212 | 29 | | point = default; |
| | 212 | 30 | | int first = (mask & 1) != 0 ? 0 : 1; |
| | 212 | 31 | | if ((mask & (mask - 1)) == 0) |
| | | 32 | | { |
| | 27 | 33 | | int vertex = (mask & 4) != 0 ? 2 : first; |
| | 27 | 34 | | if (requireAdmission) |
| | | 35 | | { |
| | 36 | 36 | | Span<ulong> vertexWeights = stackalloc ulong[6 * Words]; Span<int> vertexSigns = stackalloc int[4]; |
| | 54 | 37 | | vertexWeights.Clear(); vertexSigns.Clear(); vertexWeights[0] = 1; |
| | 18 | 38 | | WideArithmetic.MultiplyMagnitudes(Slot(normal, 0), Slot(normal, 0), Slot(vertexWeights, 5)); |
| | 18 | 39 | | WideArithmetic.MultiplyMagnitudes(Slot(normal, 2), Slot(normal, 2), Slot(vertexWeights, 1)); |
| | 18 | 40 | | WideArithmetic.AddMagnitudeInto(Slot(vertexWeights, 1), Slot(vertexWeights, 5)); |
| | 18 | 41 | | Slot(vertexWeights, 1).Clear(); |
| | 18 | 42 | | if (!HasMatchingPoint(geometry, vertex, default, default, vertexWeights, vertexSigns, |
| | 27 | 43 | | normal, normalSigns, cap, radialRadius)) return false; |
| | | 44 | | } |
| | 18 | 45 | | point = geometry.WitnessVertex(triangle, vertex); |
| | 18 | 46 | | return true; |
| | | 47 | | } |
| | 185 | 48 | | bool face = mask == 7; |
| | 185 | 49 | | int second = face ? 1 : (mask & 4) != 0 ? 2 : 1; |
| | 185 | 50 | | WideDirection3d e = geometry.EdgeFromTo(first, second); |
| | 185 | 51 | | WideDirection3d f = face ? -geometry.Edge(2) : default; |
| | 185 | 52 | | WideDirection3d p0 = new(default, cap > 0 ? WideArithmetic.Negate(geometry.HalfHeight) : geometry.HalfHeight, de |
| | 185 | 53 | | WideDirection3d offset = TriangleCircularGeometry.Subtract(p0, geometry.Vertex(first)); |
| | 555 | 54 | | Signed832 ee = e.SquaredLength, ff = f.SquaredLength, ef = WideDirection3d.Dot(e, f); |
| | 370 | 55 | | Signed832 a = WideDirection3d.Dot(offset, e), b = WideDirection3d.Dot(offset, f); |
| | 185 | 56 | | Span<ulong> radial = stackalloc ulong[3 * Words]; |
| | 185 | 57 | | Span<int> radialSigns = stackalloc int[3]; |
| | 370 | 58 | | normal.CopyTo(radial); normalSigns.CopyTo(radialSigns); |
| | 370 | 59 | | Slot(radial, 1).Clear(); radialSigns[1] = 0; |
| | 185 | 60 | | Span<ulong> weights = stackalloc ulong[6 * Words]; |
| | 185 | 61 | | Span<int> signs = stackalloc int[4]; |
| | 370 | 62 | | weights.Clear(); signs.Clear(); |
| | | 63 | | // Slots: denominator, edge rational/radical, face rational/radical, |
| | | 64 | | // radial norm squared. Coordinates are (A+B/sqrt(K))/D. |
| | 185 | 65 | | SumSquares(radial, Slot(weights, 5)); |
| | 185 | 66 | | if (radialRadius.IsZero) |
| | | 67 | | { |
| | 46 | 68 | | Slot(weights, 5).Clear(); Slot(weights, 5)[0] = 1; |
| | | 69 | | } |
| | 185 | 70 | | Span<ulong> edgeRadical = stackalloc ulong[Words]; |
| | 185 | 71 | | Span<ulong> faceRadical = stackalloc ulong[Words]; |
| | 185 | 72 | | GetRadialProjection(radialRadius, e, radial, radialSigns, edgeRadical, out int edgeSign); |
| | 185 | 73 | | if (face) |
| | | 74 | | { |
| | 128 | 75 | | GetRadialProjection(radialRadius, f, radial, radialSigns, faceRadical, out int faceSign); |
| | 256 | 76 | | Span<ulong> determinant = stackalloc ulong[Words], factor = stackalloc ulong[Words]; |
| | 128 | 77 | | CylinderContactAlgebra.DifferenceOfProducts(ee, ff, ef, ef, determinant, out _); |
| | 128 | 78 | | CylinderContactAlgebra.Import(Signed576.ExtendValue(geometry.EdgeScale), factor); |
| | 128 | 79 | | WideArithmetic.MultiplyMagnitudes(determinant, factor, Slot(weights, 0)); |
| | 128 | 80 | | CylinderContactAlgebra.DifferenceOfProducts(ff, a, ef, b, Slot(weights, 1), out signs[0]); |
| | 128 | 81 | | CylinderContactAlgebra.DifferenceOfProducts(ee, b, ef, a, Slot(weights, 3), out signs[2]); |
| | 128 | 82 | | Combine(ff, edgeRadical, edgeSign, ef, faceRadical, faceSign, Slot(weights, 2), out signs[1]); |
| | 128 | 83 | | Combine(ee, faceRadical, faceSign, ef, edgeRadical, edgeSign, Slot(weights, 4), out signs[3]); |
| | | 84 | | } |
| | | 85 | | else |
| | | 86 | | { |
| | 57 | 87 | | CylinderContactAlgebra.Import(WideArithmetic.MultiplySigned832(ee, geometry.EdgeScale), Slot(weights, 0)); |
| | 114 | 88 | | CylinderContactAlgebra.Import(a, Slot(weights, 1)); signs[0] = a.Sign; |
| | 114 | 89 | | edgeRadical.CopyTo(Slot(weights, 2)); signs[1] = edgeSign; |
| | | 90 | | } |
| | 185 | 91 | | if (requireAdmission && !HasMatchingPoint(geometry, first, e, f, weights, signs, |
| | 225 | 92 | | normal, normalSigns, cap, radialRadius)) return false; |
| | 435 | 93 | | Vector3d va = Vertex(triangle, first), vb = Vertex(triangle, second), vc = face ? triangle.C : va; |
| | | 94 | | // Exact feature admission proves this affine projection is on the |
| | | 95 | | // selected edge/face. No clamp of an already-rounded foot is needed. |
| | 145 | 96 | | if (geometry.IsAffine) |
| | 30 | 97 | | point = new Vector3d( |
| | 30 | 98 | | RoundAnalyticCoordinate(geometry.WitnessCoordinate(triangle, first, 0), geometry.WitnessCoordinate(trian |
| | 30 | 99 | | RoundAnalyticCoordinate(geometry.WitnessCoordinate(triangle, first, 1), geometry.WitnessCoordinate(trian |
| | 30 | 100 | | RoundAnalyticCoordinate(geometry.WitnessCoordinate(triangle, first, 2), geometry.WitnessCoordinate(trian |
| | 115 | 101 | | else point = new Vector3d(RoundAnalyticCoordinate(va.X, vb.X, vc.X, weights, signs), |
| | 115 | 102 | | RoundAnalyticCoordinate(va.Y, vb.Y, vc.Y, weights, signs), |
| | 115 | 103 | | RoundAnalyticCoordinate(va.Z, vb.Z, vc.Z, weights, signs)); |
| | 145 | 104 | | return true; |
| | | 105 | | } |
| | | 106 | | |
| | | 107 | | private static bool HasMatchingPoint(in TriangleCircularGeometry geometry, int first, |
| | | 108 | | in WideDirection3d edge, in WideDirection3d faceEdge, ReadOnlySpan<ulong> weights, ReadOnlySpan<int> signs, |
| | | 109 | | ReadOnlySpan<ulong> normal, ReadOnlySpan<int> normalSigns, int cap, Signed576 radius) |
| | | 110 | | { |
| | 110 | 111 | | int words = geometry.IsAffine ? 80 : Words; |
| | 220 | 112 | | Span<ulong> values = stackalloc ulong[16 * words]; Span<int> valueSigns = stackalloc int[16]; |
| | 110 | 113 | | ContactQuadratic beta = ContactQuadratic.At(values, valueSigns, 0, words), |
| | 220 | 114 | | gamma = ContactQuadratic.At(values, valueSigns, 1, words), denominator = ContactQuadratic.At(values, valueSi |
| | 220 | 115 | | alpha = ContactQuadratic.At(values, valueSigns, 3, words), term = ContactQuadratic.At(values, valueSigns, 4, |
| | 220 | 116 | | query = ContactQuadratic.At(values, valueSigns, 5, words), product = ContactQuadratic.At(values, valueSigns, |
| | 110 | 117 | | difference = ContactQuadratic.At(values, valueSigns, 7, words); |
| | 220 | 118 | | values.Clear(); valueSigns.Clear(); |
| | 220 | 119 | | Slot(weights, 2).CopyTo(beta.Rational); Slot(weights, 1).CopyTo(beta.Radical); |
| | 220 | 120 | | beta.Signs[0] = signs[1]; beta.Signs[1] = signs[0]; |
| | 220 | 121 | | Slot(weights, 4).CopyTo(gamma.Rational); Slot(weights, 3).CopyTo(gamma.Radical); |
| | 220 | 122 | | gamma.Signs[0] = signs[3]; gamma.Signs[1] = signs[2]; |
| | 220 | 123 | | Slot(weights, 0).CopyTo(denominator.Radical); denominator.Signs[1] = 1; |
| | 110 | 124 | | ReadOnlySpan<ulong> root = Slot(weights, 5); |
| | 330 | 125 | | denominator.CopyTo(alpha); alpha.Add(beta, -1); alpha.Add(gamma, -1); |
| | 146 | 126 | | if (alpha.Sign(root) < 0 || beta.Sign(root) < 0 || gamma.Sign(root) < 0) return false; |
| | 148 | 127 | | Span<ulong> residualValues = stackalloc ulong[6 * words]; Span<int> residualSigns = stackalloc int[6]; |
| | 592 | 128 | | for (int component = 0; component < 3; component++) |
| | | 129 | | { |
| | 222 | 130 | | ContactQuadratic residual = ContactQuadratic.At(residualValues, residualSigns, component, words); |
| | 222 | 131 | | Signed576 offset = Component(geometry.Vertex(first), component); |
| | 296 | 132 | | if (component == 1) offset = WideArithmetic.AddSigned576(offset, cap > 0 ? geometry.HalfHeight : WideArithme |
| | 222 | 133 | | ContactQuadratic.Scale(denominator, offset, residual); |
| | 444 | 134 | | ContactQuadratic.Scale(beta, Component(edge, component), term); residual.Add(term); |
| | 444 | 135 | | ContactQuadratic.Scale(gamma, Component(faceEdge, component), term); residual.Add(term); |
| | 222 | 136 | | if (component != 1) |
| | | 137 | | { |
| | 444 | 138 | | query.Clear(); Slot(weights, 0).CopyTo(query.Rational); query.Signs[0] = 1; |
| | 148 | 139 | | ContactQuadratic.Scale(query, radius, product); |
| | 148 | 140 | | ContactQuadratic.Scale(product, Slot(normal, component), normalSigns[component], difference); |
| | 148 | 141 | | residual.Add(difference); |
| | | 142 | | } |
| | | 143 | | } |
| | 74 | 144 | | int axis = normalSigns[0] != 0 ? 0 : normalSigns[1] != 0 ? 1 : 2; |
| | 550 | 145 | | for (int component = 0; component < 3; component++) |
| | | 146 | | { |
| | 214 | 147 | | ContactQuadratic.Scale(ContactQuadratic.At(residualValues, residualSigns, component, words), |
| | 214 | 148 | | Slot(normal, axis), normalSigns[axis], query); |
| | 214 | 149 | | ContactQuadratic.Scale(ContactQuadratic.At(residualValues, residualSigns, axis, words), |
| | 214 | 150 | | Slot(normal, component), normalSigns[component], product); |
| | 214 | 151 | | query.Add(product, -1); |
| | 227 | 152 | | if (query.Sign(root) != 0) return false; |
| | | 153 | | } |
| | 61 | 154 | | return true; |
| | | 155 | | } |
| | | 156 | | |
| | | 157 | | internal static bool IsRootPointOnEdge(in TriangleCircularGeometry geometry, int edge, |
| | | 158 | | in WideDirection3d first, in WideDirection3d second, ref FiniteAxisValueRoot root) |
| | | 159 | | { |
| | 1832 | 160 | | WideDirection3d t0 = new(WideArithmetic.Negate(first.Z), default, first.X); |
| | 1832 | 161 | | WideDirection3d t1 = new(WideArithmetic.Negate(second.Z), default, second.X); |
| | 3664 | 162 | | Signed832 a0 = WideDirection3d.Dot(geometry.Vertex(edge), t0), a1 = WideDirection3d.Dot(geometry.Vertex(edge), t |
| | 3664 | 163 | | Signed832 b0 = WideDirection3d.Dot(geometry.Vertex((edge + 1) % 3), t0), b1 = WideDirection3d.Dot(geometry.Verte |
| | 3664 | 164 | | Span<ulong> query = stackalloc ulong[26]; Span<sbyte> signs = stackalloc sbyte[2]; |
| | 1832 | 165 | | int denominator = Sign(ref root, WideArithmetic.SubtractSigned832(b0, a0), WideArithmetic.SubtractSigned832(b1, |
| | 1832 | 166 | | return denominator != 0 && Sign(ref root, a0, a1, query, signs) * denominator <= 0 |
| | 1832 | 167 | | && Sign(ref root, b0, b1, query, signs) * denominator >= 0; |
| | | 168 | | } |
| | | 169 | | |
| | | 170 | | internal static Vector3d GetRootPoint(in TriangleCircularGeometry geometry, FixedTriangle triangle, |
| | | 171 | | int edge, in WideDirection3d first, in WideDirection3d second, ref FiniteAxisValueRoot root) |
| | | 172 | | { |
| | 104 | 173 | | int next = (edge + 1) % 3; |
| | 104 | 174 | | WideDirection3d tangent0 = new(WideArithmetic.Negate(first.Z), default, first.X); |
| | 104 | 175 | | WideDirection3d tangent1 = new(WideArithmetic.Negate(second.Z), default, second.X); |
| | 208 | 176 | | Signed832 a0 = WideDirection3d.Dot(geometry.Vertex(edge), tangent0), a1 = WideDirection3d.Dot(geometry.Vertex(ed |
| | 208 | 177 | | Signed832 b0 = WideDirection3d.Dot(geometry.Vertex(next), tangent0), b1 = WideDirection3d.Dot(geometry.Vertex(ne |
| | 208 | 178 | | Signed832 d0 = WideArithmetic.SubtractSigned832(b0, a0), d1 = WideArithmetic.SubtractSigned832(b1, a1); |
| | 104 | 179 | | Span<ulong> query = stackalloc ulong[26]; |
| | 104 | 180 | | Span<sbyte> signs = stackalloc sbyte[2]; |
| | 104 | 181 | | int denominatorSign = Sign(ref root, d0, d1, query, signs); |
| | | 182 | | // q-p is parallel n, so q lies in the exact radial tangent plane. |
| | | 183 | | // A zero denominator would make this edge meridional: its stationary |
| | | 184 | | // normal is the earlier projected-Up principal direction and K=0. |
| | | 185 | | // Such a root cannot be an admitted K!=0 winner. |
| | | 186 | | System.Diagnostics.Debug.Assert(denominatorSign != 0); |
| | 208 | 187 | | Vector3d aVertex = Vertex(triangle, edge), bVertex = Vertex(triangle, next); |
| | 104 | 188 | | if (geometry.IsAffine) |
| | 13 | 189 | | return new Vector3d( |
| | 13 | 190 | | RoundRootCoordinate(geometry.WitnessCoordinate(triangle, edge, 0), geometry.WitnessCoordinate(triangle, |
| | 13 | 191 | | RoundRootCoordinate(geometry.WitnessCoordinate(triangle, edge, 1), geometry.WitnessCoordinate(triangle, |
| | 13 | 192 | | RoundRootCoordinate(geometry.WitnessCoordinate(triangle, edge, 2), geometry.WitnessCoordinate(triangle, |
| | 91 | 193 | | return new Vector3d( |
| | 91 | 194 | | RoundRootCoordinate(aVertex.X, bVertex.X, a0, a1, b0, b1, d0, d1, denominatorSign, ref root), |
| | 91 | 195 | | RoundRootCoordinate(aVertex.Y, bVertex.Y, a0, a1, b0, b1, d0, d1, denominatorSign, ref root), |
| | 91 | 196 | | RoundRootCoordinate(aVertex.Z, bVertex.Z, a0, a1, b0, b1, d0, d1, denominatorSign, ref root)); |
| | | 197 | | } |
| | | 198 | | |
| | | 199 | | internal static Fixed64 RoundRootCoordinate(Signed576 a, Signed576 b, Signed320 scale, |
| | | 200 | | Signed832 a0, Signed832 a1, Signed832 b0, Signed832 b1, Signed832 d0, Signed832 d1, |
| | | 201 | | int denominatorSign, ref FiniteAxisValueRoot root) |
| | | 202 | | { |
| | | 203 | | // Complete linear-ratio coefficients stay borrowed: physical coordinate |
| | | 204 | | // numerators need not fit a scalar or a thirteen-word product. |
| | 108 | 205 | | Span<ulong> data = stackalloc ulong[4 * Words], temporary = stackalloc ulong[Words]; |
| | 162 | 206 | | Span<int> dataSigns = stackalloc int[4]; data.Clear(); dataSigns.Clear(); |
| | 108 | 207 | | Span<ulong> query = stackalloc ulong[2 * Words]; Span<sbyte> querySigns = stackalloc sbyte[2]; |
| | 324 | 208 | | for (int coefficient = 0; coefficient < 2; coefficient++) |
| | | 209 | | { |
| | 108 | 210 | | Import(coefficient == 0 ? b0 : b1, temporary); |
| | 108 | 211 | | Accumulate(a, temporary, (coefficient == 0 ? b0 : b1).Sign, Slot(data, coefficient), ref dataSigns[coefficie |
| | 108 | 212 | | Import(coefficient == 0 ? a0 : a1, temporary); |
| | 108 | 213 | | Accumulate(b, temporary, -(coefficient == 0 ? a0 : a1).Sign, Slot(data, coefficient), ref dataSigns[coeffici |
| | 108 | 214 | | WideArithmetic.AddMagnitudeInto(Slot(data, coefficient), Slot(data, coefficient)); |
| | 108 | 215 | | Import(coefficient == 0 ? d0 : d1, temporary); |
| | 108 | 216 | | Accumulate(Signed576.ExtendValue(scale), temporary, (coefficient == 0 ? d0 : d1).Sign, |
| | 108 | 217 | | Slot(data, coefficient + 2), ref dataSigns[coefficient + 2]); |
| | | 218 | | } |
| | 108 | 219 | | long ar = RoundEndpoint(a, scale), br = RoundEndpoint(b, scale); |
| | | 220 | | // This overload materializes admitted affine witnesses in the finite |
| | | 221 | | // shape's translated chart (reach <=16384), far from scalar endpoints. |
| | | 222 | | // The full-domain authored-coordinate overload retains its own bounds. |
| | 108 | 223 | | long low = Math.Min(ar, br) - 1, high = Math.Max(ar, br) + 1; |
| | 1617 | 224 | | while (low < high) |
| | | 225 | | { |
| | 1563 | 226 | | ulong span = unchecked((ulong)high - (ulong)low); |
| | 1563 | 227 | | long midpoint = unchecked((long)((ulong)low + (span >> 1) + (span & 1))); |
| | 1563 | 228 | | Signed192 threshold = WideArithmetic.AddSigned192(Signed192.Signed(midpoint), Signed192.Signed(midpoint)); |
| | 1563 | 229 | | int comparison = CompareRootRatio(data, dataSigns, threshold, query, querySigns, ref root) * denominatorSign |
| | 3126 | 230 | | if (comparison >= 0) low = midpoint; else high = midpoint - 1; |
| | | 231 | | } |
| | 54 | 232 | | Signed192 half = WideArithmetic.AddSigned192(WideArithmetic.AddSigned192(Signed192.Signed(low), Signed192.Signed |
| | 54 | 233 | | int halfSign = CompareRootRatio(data, dataSigns, half, query, querySigns, ref root) * denominatorSign; |
| | 54 | 234 | | return Fixed64.FromRaw(low + (halfSign > 0 || halfSign == 0 && (low & 1) != 0 ? 1 : 0)); |
| | | 235 | | } |
| | | 236 | | |
| | | 237 | | private static int CompareRootRatio(scoped ReadOnlySpan<ulong> data, scoped ReadOnlySpan<int> signs, Signed192 thres |
| | | 238 | | scoped Span<ulong> query, scoped Span<sbyte> querySigns, ref FiniteAxisValueRoot root) |
| | | 239 | | { |
| | 9702 | 240 | | for (int coefficient = 0; coefficient < 2; coefficient++) |
| | | 241 | | { |
| | 3234 | 242 | | Span<ulong> target = Slot(query, coefficient); |
| | 6468 | 243 | | Slot(data, coefficient).CopyTo(target); int sign = signs[coefficient]; |
| | 3234 | 244 | | Accumulate(Signed576.ExtendValue(threshold), Slot(data, coefficient + 2), -signs[coefficient + 2], target, r |
| | 3234 | 245 | | querySigns[coefficient] = (sbyte)sign; |
| | | 246 | | } |
| | 1617 | 247 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, querySigns); |
| | | 248 | | } |
| | | 249 | | |
| | | 250 | | internal static Fixed64 RoundRootCoordinate(Fixed64 a, Fixed64 b, Signed832 a0, Signed832 a1, |
| | | 251 | | Signed832 b0, Signed832 b1, Signed832 d0, Signed832 d1, int denominatorSign, ref FiniteAxisValueRoot root) |
| | | 252 | | { |
| | | 253 | | // Shifted tangent-distance coefficients <432 bits; authored-coordinate |
| | | 254 | | // numerators and midpoint queries <500, within Signed832. The |
| | | 255 | | // retained root sign authority rounds the linear ratio directly. |
| | 471 | 256 | | Signed832 n0 = WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned832(b0, Signed192.Raw(a)), |
| | 471 | 257 | | WideArithmetic.MultiplySigned832(a0, Signed192.Raw(b))); |
| | 471 | 258 | | Signed832 n1 = WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned832(b1, Signed192.Raw(a)), |
| | 471 | 259 | | WideArithmetic.MultiplySigned832(a1, Signed192.Raw(b))); |
| | 942 | 260 | | n0 = WideArithmetic.AddSigned832(n0, n0); n1 = WideArithmetic.AddSigned832(n1, n1); |
| | 471 | 261 | | Span<ulong> query = stackalloc ulong[26]; |
| | 471 | 262 | | Span<sbyte> signs = stackalloc sbyte[2]; |
| | 942 | 263 | | long low = Math.Min(a.m_rawValue, b.m_rawValue), high = Math.Max(a.m_rawValue, b.m_rawValue); |
| | 14848 | 264 | | while (low < high) |
| | | 265 | | { |
| | 14377 | 266 | | ulong span = unchecked((ulong)high - (ulong)low); |
| | 14377 | 267 | | long midpoint = unchecked((long)((ulong)low + (span >> 1) + (span & 1))); |
| | 14377 | 268 | | Signed192 threshold = WideArithmetic.AddSigned192(Signed192.Signed(midpoint), Signed192.Signed(midpoint)); |
| | 14377 | 269 | | int comparison = Sign(ref root, WideArithmetic.SubtractSigned832(n0, WideArithmetic.MultiplySigned832(d0, th |
| | 14377 | 270 | | WideArithmetic.SubtractSigned832(n1, WideArithmetic.MultiplySigned832(d1, threshold)), query, signs) * d |
| | 16940 | 271 | | if (comparison >= 0) low = midpoint; |
| | 11814 | 272 | | else high = midpoint - 1; |
| | | 273 | | } |
| | 471 | 274 | | Signed192 half = WideArithmetic.AddSigned192(WideArithmetic.AddSigned192(Signed192.Signed(low), Signed192.Signed |
| | 471 | 275 | | int halfSign = Sign(ref root, WideArithmetic.SubtractSigned832(n0, WideArithmetic.MultiplySigned832(d0, half)), |
| | 471 | 276 | | WideArithmetic.SubtractSigned832(n1, WideArithmetic.MultiplySigned832(d1, half)), query, signs) * denominato |
| | 471 | 277 | | return Fixed64.FromRaw(low + (halfSign > 0 || halfSign == 0 && (low & 1) != 0 ? 1 : 0)); |
| | | 278 | | } |
| | | 279 | | |
| | | 280 | | private static int Sign(ref FiniteAxisValueRoot root, Signed832 constant, Signed832 linear, |
| | | 281 | | scoped Span<ulong> query, scoped Span<sbyte> signs) |
| | | 282 | | { |
| | 40722 | 283 | | WideArithmetic.GetMagnitude(constant, query[..13]); signs[0] = (sbyte)constant.Sign; |
| | 40722 | 284 | | WideArithmetic.GetMagnitude(linear, query[13..]); signs[1] = (sbyte)linear.Sign; |
| | 20361 | 285 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, signs); |
| | | 286 | | } |
| | | 287 | | |
| | | 288 | | private static Fixed64 RoundAnalyticCoordinate(Fixed64 a, Fixed64 b, Fixed64 c, |
| | 345 | 289 | | ReadOnlySpan<ulong> weights, ReadOnlySpan<int> signs) => RoundAnalyticCoordinate( |
| | 345 | 290 | | Signed576.ExtendValue(Signed192.Raw(a)), Signed576.ExtendValue(Signed192.Raw(b)), |
| | 345 | 291 | | Signed576.ExtendValue(Signed192.Raw(c)), Signed320.ExtendValue(Signed192.Signed(1)), weights, signs, |
| | 345 | 292 | | Math.Min(a.m_rawValue, Math.Min(b.m_rawValue, c.m_rawValue)), |
| | 345 | 293 | | Math.Max(a.m_rawValue, Math.Max(b.m_rawValue, c.m_rawValue)), hasScalarBounds: true); |
| | | 294 | | |
| | | 295 | | private static Fixed64 RoundAnalyticCoordinate(Signed576 a, Signed576 b, Signed576 c, Signed320 scale, |
| | | 296 | | ReadOnlySpan<ulong> weights, ReadOnlySpan<int> signs, |
| | | 297 | | long low = 0, long high = 0, bool hasScalarBounds = false) |
| | | 298 | | { |
| | 435 | 299 | | Span<ulong> rational = stackalloc ulong[Words]; |
| | 435 | 300 | | Span<ulong> radical = stackalloc ulong[Words]; |
| | 1740 | 301 | | rational.Clear(); radical.Clear(); int rationalSign = 0, radicalSign = 0; |
| | 435 | 302 | | Accumulate(a, Slot(weights, 0), 1, rational, ref rationalSign); |
| | 435 | 303 | | Signed576 e = WideArithmetic.SubtractSigned576(b, a); |
| | 435 | 304 | | Signed576 f = WideArithmetic.SubtractSigned576(c, a); |
| | 435 | 305 | | Accumulate(e, Slot(weights, 1), signs[0], rational, ref rationalSign); |
| | 435 | 306 | | Accumulate(f, Slot(weights, 3), signs[2], rational, ref rationalSign); |
| | 435 | 307 | | Accumulate(e, Slot(weights, 2), signs[1], radical, ref radicalSign); |
| | 435 | 308 | | Accumulate(f, Slot(weights, 4), signs[3], radical, ref radicalSign); |
| | 435 | 309 | | ReadOnlySpan<ulong> k = Slot(weights, 5); |
| | | 310 | | // The scalar bridge already owns exact raw endpoints and unit scale. |
| | | 311 | | // Borrow its existing denominator; dividing every endpoint by one or |
| | | 312 | | // multiplying this denominator by one only repeats known information. |
| | 435 | 313 | | Span<ulong> scaledDenominator = stackalloc ulong[hasScalarBounds ? 0 : Words]; |
| | 435 | 314 | | scoped ReadOnlySpan<ulong> denominator = Slot(weights, 0); |
| | 435 | 315 | | if (!hasScalarBounds) |
| | | 316 | | { |
| | 90 | 317 | | Span<ulong> scaleMagnitude = stackalloc ulong[5]; |
| | 90 | 318 | | WideArithmetic.GetMagnitude(scale, out scaleMagnitude[4], out scaleMagnitude[3], out scaleMagnitude[2], out |
| | 90 | 319 | | WideArithmetic.MultiplyMagnitudes(denominator, scaleMagnitude, scaledDenominator); |
| | 90 | 320 | | denominator = scaledDenominator; |
| | | 321 | | } |
| | 435 | 322 | | if (radicalSign == 0) |
| | | 323 | | { |
| | | 324 | | // Apex projections and exact cancellation are rational A/D. |
| | | 325 | | // Feature admission bounds A/D within the authored coordinates; |
| | | 326 | | // the existing raw-ratio owner performs the same nearest-even rounding. |
| | 132 | 327 | | bool represented = Fixed64.TryGetSignedRawRatio(rational, denominator, rationalSign < 0, out Fixed64 result) |
| | | 328 | | System.Diagnostics.Debug.Assert(represented); |
| | 132 | 329 | | return result; |
| | | 330 | | } |
| | 303 | 331 | | WideArithmetic.AddMagnitudeInto(radical, radical); |
| | | 332 | | // Compare x=(A+B/sqrt(K))/D with t/2 by the sign of |
| | | 333 | | // 2B+(2A-tD)*sqrt(K). Shifted directions <630 bits, K<1262 and Gram |
| | | 334 | | // minors<827. A/B remain <895/<1535 bits, including raw midpoint |
| | | 335 | | // products. Forty words suffice; the shared sign owner sizes squares. |
| | 303 | 336 | | if (!hasScalarBounds) |
| | | 337 | | { |
| | 237 | 338 | | long ar = RoundEndpoint(a, scale), br = RoundEndpoint(b, scale), cr = RoundEndpoint(c, scale); |
| | 158 | 339 | | low = Math.Min(ar, Math.Min(br, cr)); high = Math.Max(ar, Math.Max(br, cr)); |
| | | 340 | | } |
| | 605 | 341 | | if (low != long.MinValue) low--; |
| | 605 | 342 | | if (high != long.MaxValue) high++; |
| | 10497 | 343 | | while (low < high) |
| | | 344 | | { |
| | 10194 | 345 | | ulong span = unchecked((ulong)high - (ulong)low); |
| | 10194 | 346 | | long midpoint = unchecked((long)((ulong)low + (span >> 1) + (span & 1))); |
| | 10194 | 347 | | Signed192 threshold = WideArithmetic.AddSigned192(Signed192.Signed(midpoint), Signed192.Signed(midpoint)); |
| | 13262 | 348 | | if (Compare(rational, rationalSign, radical, radicalSign, denominator, k, threshold) >= 0) low = midpoint; |
| | 7126 | 349 | | else high = midpoint - 1; |
| | | 350 | | } |
| | 303 | 351 | | Signed192 half = WideArithmetic.AddSigned192(WideArithmetic.AddSigned192(Signed192.Signed(low), Signed192.Signed |
| | 303 | 352 | | int comparison = Compare(rational, rationalSign, radical, radicalSign, denominator, k, half); |
| | 303 | 353 | | return Fixed64.FromRaw(low + (comparison > 0 || comparison == 0 && (low & 1) != 0 ? 1 : 0)); |
| | | 354 | | } |
| | | 355 | | |
| | | 356 | | private static int Compare(ReadOnlySpan<ulong> rational, int rationalSign, ReadOnlySpan<ulong> radical, |
| | | 357 | | int radicalSign, ReadOnlySpan<ulong> denominator, ReadOnlySpan<ulong> k, Signed192 threshold) |
| | | 358 | | { |
| | 10497 | 359 | | Span<ulong> query = stackalloc ulong[Words]; |
| | 10497 | 360 | | Span<ulong> value = stackalloc ulong[Words]; |
| | 10497 | 361 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 10497 | 362 | | WideArithmetic.AddEqualMagnitudes(rational, rational, query); |
| | 10497 | 363 | | CylinderContactAlgebra.Import(Signed576.ExtendValue(threshold), value); |
| | 10497 | 364 | | WideArithmetic.MultiplyMagnitudes(value, denominator, product); |
| | 10497 | 365 | | Add(product, -threshold.Sign, query, ref rationalSign); |
| | 10497 | 366 | | return WideConvexPrismRelations.GetConvexContactCandidateQuadraticSign(radical, radicalSign, query, rationalSign |
| | | 367 | | } |
| | | 368 | | |
| | | 369 | | private static void GetRadialProjection(Signed576 radius, in WideDirection3d edge, |
| | | 370 | | ReadOnlySpan<ulong> radial, ReadOnlySpan<int> signs, Span<ulong> result, out int sign) |
| | | 371 | | { |
| | 313 | 372 | | Span<ulong> dot = stackalloc ulong[Words]; |
| | 313 | 373 | | Span<ulong> magnitude = stackalloc ulong[Words]; |
| | 313 | 374 | | Dot(edge, radial, signs, dot, out sign); |
| | 313 | 375 | | CylinderContactAlgebra.Import(radius, magnitude); |
| | | 376 | | // Apex witnesses use zero radius: its product must also have zero sign |
| | | 377 | | // before combining weights, even when the unscaled projection is nonzero. |
| | 626 | 378 | | WideArithmetic.MultiplyMagnitudes(dot, magnitude, result); sign = -sign * radius.Sign; |
| | 313 | 379 | | } |
| | | 380 | | |
| | | 381 | | private static void Combine(Signed832 a, ReadOnlySpan<ulong> first, int firstSign, |
| | | 382 | | Signed832 b, ReadOnlySpan<ulong> second, int secondSign, Span<ulong> result, out int sign) |
| | | 383 | | { |
| | 256 | 384 | | Span<ulong> factor = stackalloc ulong[Words]; |
| | 256 | 385 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 256 | 386 | | CylinderContactAlgebra.Import(a, factor); |
| | 512 | 387 | | WideArithmetic.MultiplyMagnitudes(factor, first, result); sign = a.Sign * firstSign; |
| | 256 | 388 | | CylinderContactAlgebra.Import(b, factor); |
| | 256 | 389 | | WideArithmetic.MultiplyMagnitudes(factor, second, product); |
| | 256 | 390 | | Add(product, -b.Sign * secondSign, result, ref sign); |
| | 256 | 391 | | } |
| | | 392 | | |
| | | 393 | | private static void Accumulate(Signed576 scalar, ReadOnlySpan<ulong> value, int sign, |
| | | 394 | | Span<ulong> sum, ref int sumSign) |
| | | 395 | | { |
| | 5733 | 396 | | Span<ulong> factor = stackalloc ulong[Words]; |
| | 5733 | 397 | | Span<ulong> product = stackalloc ulong[Words]; |
| | 5733 | 398 | | CylinderContactAlgebra.Import(scalar, factor); |
| | 5733 | 399 | | WideArithmetic.MultiplyMagnitudes(factor, value, product); |
| | 5733 | 400 | | Add(product, sign * scalar.Sign, sum, ref sumSign); |
| | 5733 | 401 | | } |
| | | 402 | | private static long RoundEndpoint(Signed576 numerator, Signed320 denominator) |
| | | 403 | | { |
| | 690 | 404 | | Span<ulong> n = stackalloc ulong[9], d = stackalloc ulong[5]; |
| | 345 | 405 | | WideArithmetic.GetMagnitude(numerator, n); |
| | 345 | 406 | | WideArithmetic.GetMagnitude(denominator, out d[4], out d[3], out d[2], out d[1], out d[0]); |
| | 345 | 407 | | bool represented = Fixed64.TryGetSignedRawRatio(n, d, numerator.Sign < 0, out Fixed64 value); |
| | | 408 | | System.Diagnostics.Debug.Assert(represented); |
| | 345 | 409 | | return value.m_rawValue; |
| | | 410 | | } |
| | | 411 | | |
| | 498 | 412 | | private static Vector3d Vertex(FixedTriangle triangle, int index) => index == 0 ? triangle.A : index == 1 ? triangle |
| | | 413 | | } |