| | | 1 | | //======================================================================= |
| | | 2 | | // CurvedPrismGeneratorFeatures.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>Exact cone-generator normal-circle extrema against the complete prism.</summary> |
| | | 14 | | internal static class CurvedPrismGeneratorFeatures |
| | | 15 | | { |
| | | 16 | | internal static void KeepAll(in CurvedPrismContactGeometry geometry, |
| | | 17 | | ref CurvedPrismContactSelection selection) |
| | | 18 | | { |
| | | 19 | | // The cone has apex H=T*A and base center -H, radius R. With q=A², |
| | | 20 | | // its generator normals satisfy 2T*A.n=R*sqrt(n²-(A.n)²/q)>=0. |
| | | 21 | | // A vertex patch has two azimuthal extrema; changes of the prism's |
| | | 22 | | // supporting vertex occur only at its real edges. These extrema and |
| | | 23 | | // edge intersections exhaust the piecewise-linear generator circle. |
| | | 24 | | // Accepted raw axes have |A_i|<2^33 and q<2^65. Primitive-axis |
| | | 25 | | // reduction may give T<2^161, but T²*q retains its original <2^321 bound; |
| | | 26 | | // R<2^163, |P_i|<2^165 and |E_i|<2^166. Thus V<2^234, |
| | | 27 | | // d<2^202, k<2^400, U<2^370, C<2^200 and |Δ|<2^730. |
| | | 28 | | // All narrowed vectors/products fit 576 bits; k² and qΔ fit 832. |
| | | 29 | | // Normal coefficients stay below 600 bits, squared-gap coefficients |
| | | 30 | | // below 1600 and the final denominator below 1400, within forty words. |
| | 100 | 31 | | Signed320 q = geometry.SquaredAxis; |
| | 100 | 32 | | Signed576 tSquared = WideArithmetic.MultiplySigned320(Signed320.ExtendValue(geometry.HalfFactor), geometry.HalfF |
| | 100 | 33 | | Signed576 fourTq = Signed576.NarrowValue(WideArithmetic.MultiplySigned576ToSigned832(tSquared, q)); |
| | 100 | 34 | | fourTq = WideArithmetic.AddSigned576(fourTq, fourTq); |
| | 100 | 35 | | fourTq = WideArithmetic.AddSigned576(fourTq, fourTq); |
| | 100 | 36 | | Signed576 radiusSquared = WideArithmetic.MultiplySigned320(geometry.Radius, geometry.Radius); |
| | 100 | 37 | | Signed576 length = WideArithmetic.AddSigned576(fourTq, radiusSquared); |
| | 100 | 38 | | if (selection.HasValue && !selection.Domain.Enabled && |
| | 103 | 39 | | CanExcludeCircle(geometry, selection.Candidate, radiusSquared, length)) return; |
| | | 40 | | |
| | 97 | 41 | | Span<ulong> values = stackalloc ulong[ConvexContactCandidate.Slots * Words]; |
| | 97 | 42 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 97 | 43 | | Span<ulong> work = stackalloc ulong[4 * Words]; |
| | 194 | 44 | | Span<ulong> factor = Slot(work, 0), axisSquared = Slot(work, 1); |
| | 194 | 45 | | Span<ulong> commonMetric = Slot(work, 2), product = Slot(work, 3); |
| | 194 | 46 | | Import(length, factor); Import(q, axisSquared); |
| | 97 | 47 | | WideArithmetic.MultiplyMagnitudes(factor, axisSquared, product); |
| | 97 | 48 | | Import(WideArithmetic.MultiplySigned192(geometry.RawScale, geometry.RawScale), factor); |
| | 97 | 49 | | WideArithmetic.MultiplyMagnitudes(product, factor, commonMetric); |
| | | 50 | | |
| | | 51 | | // If a supporting vertex lies on the axis, its circle value is |
| | | 52 | | // constant. One seed covers a whole-circle patch; otherwise real-edge |
| | | 53 | | // intersections supply the ends of that vertex's admitted arc. |
| | | 54 | | // Complete admission precedes this collector, so the first exact zero |
| | | 55 | | // attains the global lower bound and owns all later ties. |
| | 97 | 56 | | Signed576 one = Signed576.ExtendValue(Signed192.Signed(1)); |
| | 97 | 57 | | WideDirection3d seed = !geometry.Axis.Y.IsZero || !geometry.Axis.Z.IsZero |
| | 97 | 58 | | ? new WideDirection3d(one, default, default) |
| | 97 | 59 | | : new WideDirection3d(default, one, default); |
| | 97 | 60 | | KeepVertex(geometry, seed, commonMetric, values, signs, ref selection); |
| | 1570 | 61 | | for (int vertex = 0; vertex < geometry.VertexCount && (!selection.HasValue || selection.GapSign != 0); vertex++) |
| | 688 | 62 | | KeepVertex(geometry, geometry.Vertex(vertex), commonMetric, values, signs, ref selection, vertex); |
| | | 63 | | |
| | 2258 | 64 | | for (int edge = 0; edge < geometry.EdgeCount && (!selection.HasValue || selection.GapSign != 0); edge++) |
| | | 65 | | { |
| | 1032 | 66 | | geometry.Edge(edge, out int start, out int end); |
| | 1032 | 67 | | WideDirection3d e = CurvedPrismContact.Difference(geometry.Vertex(end), geometry.Vertex(start)); |
| | 1032 | 68 | | Signed576 g = Signed576.NarrowValue(e.SquaredLength); |
| | 1032 | 69 | | Signed576 d = Signed576.NarrowValue(WideDirection3d.Dot(e, geometry.Axis)); |
| | 1032 | 70 | | Signed576 dSquared = Signed576.NarrowValue(WideArithmetic.MultiplySigned576(d, d)); |
| | 1032 | 71 | | Signed576 k = WideArithmetic.SubtractSigned576( |
| | 1032 | 72 | | Signed576.NarrowValue(WideArithmetic.MultiplySigned576ToSigned832(g, q)), |
| | 1032 | 73 | | dSquared); |
| | 1032 | 74 | | if (k.IsZero) |
| | | 75 | | continue; |
| | 798 | 76 | | Signed832 discriminant = WideArithmetic.SubtractSigned832( |
| | 798 | 77 | | WideArithmetic.MultiplySigned576(fourTq, k), |
| | 798 | 78 | | WideArithmetic.MultiplySigned576(radiusSquared, dSquared)); |
| | 798 | 79 | | if (discriminant.Sign < 0) |
| | | 80 | | continue; |
| | | 81 | | |
| | | 82 | | // U=g*A-d*E, C=A×E, k=q*g-d² and Δ=4T²qk-R²d². |
| | | 83 | | // n=Rq*U +/- C*sqrt(qΔ) is perpendicular to E and has |
| | | 84 | | // A.n=Rqk>=0, preserving the unsquared generator condition. |
| | | 85 | | // Its norm²=q*k²*(4T²q+R²), with no remaining radical. |
| | 655 | 86 | | WideDirection3d axis = new(geometry.Axis); |
| | 655 | 87 | | WideDirection3d u = WideDirection3d.DifferenceScaled(axis, g, e, d); |
| | 655 | 88 | | WideDirection3d cross = WideDirection3d.Cross(axis, e); |
| | 655 | 89 | | Signed320 radialScale = Signed320.NarrowValue( |
| | 655 | 90 | | WideArithmetic.MultiplySigned320(geometry.Radius, q)); |
| | 1310 | 91 | | values.Clear(); signs.Clear(); |
| | 5240 | 92 | | for (int component = 0; component < 3; component++) |
| | | 93 | | { |
| | 1965 | 94 | | Write(values, signs, component, WideArithmetic.MultiplySigned576ToSigned832( |
| | 1965 | 95 | | Component(u, component), radialScale)); |
| | 1965 | 96 | | Write(values, signs, component + 3, Signed832.ExtendValue(Component(cross, component)), -1); |
| | | 97 | | } |
| | 655 | 98 | | Import(WideArithmetic.MultiplySigned832(discriminant, q), Slot(values, 6)); |
| | 655 | 99 | | Signed832 metricFactor = WideArithmetic.MultiplySigned576(k, k); |
| | 655 | 100 | | Keep(geometry, values, signs, commonMetric, metricFactor, ref selection, start); |
| | 655 | 101 | | if (discriminant.Sign != 0) |
| | | 102 | | { |
| | 6721 | 103 | | for (int component = 3; component < 6; component++) signs[component] = -signs[component]; |
| | 611 | 104 | | Keep(geometry, values, signs, commonMetric, metricFactor, ref selection, start); |
| | | 105 | | } |
| | | 106 | | } |
| | 97 | 107 | | } |
| | | 108 | | |
| | | 109 | | private static bool CanExcludeCircle(in CurvedPrismContactGeometry geometry, |
| | | 110 | | scoped ConvexContactCandidate incumbent, Signed576 radiusSquared, Signed576 length) |
| | | 111 | | { |
| | 77 | 112 | | WideDirection3d up = geometry.Plane(1); |
| | 77 | 113 | | Signed320 denominator = Signed320.NarrowValue(Signed576.NarrowValue(WideDirection3d.Dot(up, geometry.Axis))); |
| | 79 | 114 | | if (denominator.IsZero) return false; |
| | 81 | 115 | | if (denominator.Sign < 0) { up = -up; denominator = WideArithmetic.Negate(denominator); } |
| | 75 | 116 | | Signed320 numerator = Signed320.NarrowValue(Signed576.NarrowValue(geometry.ProjectionMinimum(up))); |
| | 75 | 117 | | Signed576 lambda = WideArithmetic.SubtractSigned576( |
| | 75 | 118 | | WideArithmetic.MultiplySigned320(Signed320.ExtendValue(geometry.HalfFactor), denominator), |
| | 75 | 119 | | Signed576.ExtendValue(numerator)); |
| | 79 | 120 | | if (lambda.Sign <= 0) return false; |
| | | 121 | | // p=(numerator/denominator)*Axis is the far native-Up slab point. |
| | | 122 | | // It must belong to the complete prism, including degenerate extents. |
| | | 123 | | // Up<65 bits gives numerator<232, denominator<99 and lambda<261. |
| | | 124 | | // Side planes<233 and vertices<165 give plane minima<400 and axial |
| | | 125 | | // dots<267. Both membership products and their difference fit 576 bits. |
| | 380 | 126 | | for (int plane = 0; plane < geometry.PlaneCount; plane++) |
| | | 127 | | { |
| | 174 | 128 | | WideDirection3d face = geometry.Plane(plane); |
| | 1083 | 129 | | for (int orientation = 0; orientation < 2; orientation++, face = -face) |
| | | 130 | | { |
| | 300 | 131 | | Signed320 projection = Signed320.NarrowValue(Signed576.NarrowValue(WideDirection3d.Dot(face, geometry.Ax |
| | 300 | 132 | | Signed576 left = WideArithmetic.MultiplySigned320(numerator, projection); |
| | 300 | 133 | | Signed832 right = WideArithmetic.MultiplySigned832(geometry.ProjectionMinimum(face), denominator); |
| | 355 | 134 | | if (WideArithmetic.SubtractSigned832(Signed832.ExtendValue(left), right).Sign < 0) return false; |
| | | 135 | | } |
| | | 136 | | } |
| | | 137 | | // Every generator normal has Axis.n/|n|=R*sqrt(q)/sqrt(length). |
| | | 138 | | // Since minPrism(n)<=p.n, its gap is at least |
| | | 139 | | // lambda*R*sqrt(q)/(denominator*S*sqrt(length)). Exact analytic ties |
| | | 140 | | // already retain the earlier feature and every witness term. |
| | | 141 | | const int boundWords = 16; |
| | 16 | 142 | | Span<ulong> values = stackalloc ulong[ConvexContactCandidate.Slots * boundWords]; |
| | 16 | 143 | | Span<int> signs = stackalloc int[ConvexContactCandidate.Slots]; |
| | 16 | 144 | | Span<ulong> work = stackalloc ulong[3 * boundWords]; |
| | 32 | 145 | | values.Clear(); signs.Clear(); |
| | 32 | 146 | | values[0] = 1; signs[0] = signs[7] = 1; |
| | 32 | 147 | | Span<ulong> first = Slot(work, 0, boundWords), second = Slot(work, 1, boundWords); |
| | 16 | 148 | | Span<ulong> product = Slot(work, 2, boundWords); |
| | | 149 | | // lambda<261, R<163 and q<65 give numerator<913 bits; |
| | | 150 | | // S<99, denominator<99 and length<327 give denominator<723. |
| | | 151 | | // Sixteen-word fields retain every complete product without rounding. |
| | 16 | 152 | | Import(WideArithmetic.MultiplySigned576ToSigned832(lambda, lambda), first); |
| | 16 | 153 | | Import(radiusSquared, second); |
| | 16 | 154 | | WideArithmetic.MultiplyMagnitudes(first, second, product); |
| | 16 | 155 | | Import(geometry.SquaredAxis, first); |
| | 16 | 156 | | WideArithmetic.MultiplyMagnitudes(product, first, Slot(values, 7, boundWords)); |
| | 16 | 157 | | Import(WideArithmetic.MultiplySigned192(geometry.RawScale, geometry.RawScale), first); |
| | 16 | 158 | | Import(WideArithmetic.MultiplySigned320(denominator, denominator), second); |
| | 16 | 159 | | WideArithmetic.MultiplyMagnitudes(first, second, product); |
| | 16 | 160 | | Import(length, first); |
| | 16 | 161 | | WideArithmetic.MultiplyMagnitudes(product, first, Slot(values, 10, boundWords)); |
| | 16 | 162 | | return WideConvexPrismRelations.CompareConvexContactCandidates( |
| | 16 | 163 | | new ConvexContactCandidate(values, signs, 1, boundWords), incumbent) >= 0; |
| | | 164 | | } |
| | | 165 | | |
| | | 166 | | private static void KeepVertex(in CurvedPrismContactGeometry geometry, in WideDirection3d point, |
| | | 167 | | scoped ReadOnlySpan<ulong> commonMetric, scoped Span<ulong> values, scoped Span<int> signs, |
| | | 168 | | ref CurvedPrismContactSelection selection, int requiredVertex = -1) |
| | | 169 | | { |
| | 785 | 170 | | Signed576 projection = Signed576.NarrowValue(WideDirection3d.Dot(point, geometry.Axis)); |
| | 785 | 171 | | WideDirection3d radial = WideDirection3d.DifferenceScaled(point, Signed576.ExtendValue(geometry.SquaredAxis), |
| | 785 | 172 | | new WideDirection3d(geometry.Axis), projection); |
| | 785 | 173 | | if (radial.IsZero) |
| | 6 | 174 | | return; |
| | 779 | 175 | | Signed832 radialSquared = radial.SquaredLength; |
| | 779 | 176 | | Signed576 twiceTq = WideArithmetic.MultiplySigned320(geometry.SquaredAxis, geometry.HalfFactor); |
| | 779 | 177 | | Signed320 radialScale = Signed320.NarrowValue(WideArithmetic.AddSigned576(twiceTq, twiceTq)); |
| | 1558 | 178 | | values.Clear(); signs.Clear(); |
| | | 179 | | // V=qP-(A.P)A; n=+/-2TqV+R*A*sqrt(V²). |
| | | 180 | | // Both extrema have norm²=q*V²*(4T²q+R²). |
| | 6232 | 181 | | for (int component = 0; component < 3; component++) |
| | | 182 | | { |
| | 2337 | 183 | | Write(values, signs, component, WideArithmetic.MultiplySigned576ToSigned832( |
| | 2337 | 184 | | Component(radial, component), radialScale), -1); |
| | 2337 | 185 | | Write(values, signs, component + 3, Signed832.ExtendValue(WideArithmetic.MultiplySigned320( |
| | 2337 | 186 | | geometry.Radius, Component(geometry.Axis, component)))); |
| | | 187 | | } |
| | 779 | 188 | | Import(radialSquared, Slot(values, 6)); |
| | 779 | 189 | | Keep(geometry, values, signs, commonMetric, radialSquared, ref selection, requiredVertex); |
| | 8569 | 190 | | for (int component = 0; component < 3; component++) signs[component] = -signs[component]; |
| | 779 | 191 | | Keep(geometry, values, signs, commonMetric, radialSquared, ref selection, requiredVertex); |
| | 779 | 192 | | } |
| | | 193 | | |
| | | 194 | | private static void Keep(in CurvedPrismContactGeometry geometry, |
| | | 195 | | scoped Span<ulong> values, scoped Span<int> signs, scoped ReadOnlySpan<ulong> commonMetric, |
| | | 196 | | Signed832 metricFactor, ref CurvedPrismContactSelection selection, int requiredVertex) |
| | | 197 | | { |
| | 2824 | 198 | | Span<ulong> work = stackalloc ulong[6 * Words]; |
| | 2824 | 199 | | Span<int> workSigns = stackalloc int[4]; |
| | 2824 | 200 | | ContactQuadratic minimum = ContactQuadratic.At(work, workSigns, 0); |
| | 2824 | 201 | | ContactQuadratic gap = ContactQuadratic.At(work, workSigns, 1); |
| | 2824 | 202 | | geometry.GetMinimum(values, signs, minimum, out int vertex); |
| | 2824 | 203 | | if (requiredVertex >= 0) |
| | | 204 | | { |
| | | 205 | | // A real vertex extremum or edge intersection belongs to this |
| | | 206 | | // patch only when its generating vertex minimizes the prism dot. |
| | | 207 | | // Equality includes every tied edge endpoint; the artificial |
| | | 208 | | // whole-circle seed remains unrestricted. |
| | 2630 | 209 | | WideDirection3d point = geometry.Vertex(requiredVertex); |
| | 2630 | 210 | | Dot(point, values[..(3 * Words)], signs[..3], gap.Rational, out int pointRationalSign); |
| | 2630 | 211 | | Dot(point, values.Slice(3 * Words, 3 * Words), signs.Slice(3, 3), |
| | 2630 | 212 | | gap.Radical, out int pointRadicalSign); |
| | 5260 | 213 | | gap.Signs[0] = pointRationalSign; gap.Signs[1] = pointRadicalSign; |
| | 2630 | 214 | | gap.Add(minimum, -1); |
| | 4649 | 215 | | if (gap.Sign(Slot(values, 6)) != 0) return; |
| | | 216 | | } |
| | 805 | 217 | | Dot(geometry.Half, values[..(3 * Words)], signs[..3], gap.Rational, out int rationalSign); |
| | 805 | 218 | | Dot(geometry.Half, values.Slice(3 * Words, 3 * Words), signs.Slice(3, 3), |
| | 805 | 219 | | gap.Radical, out int radicalSign); |
| | 1610 | 220 | | gap.Signs[0] = rationalSign; gap.Signs[1] = radicalSign; |
| | 805 | 221 | | gap.Add(minimum, -1); |
| | 805 | 222 | | Import(metricFactor, Slot(work, 4)); |
| | 805 | 223 | | WideArithmetic.MultiplyMagnitudes(commonMetric, Slot(work, 4), Slot(work, 5)); |
| | 805 | 224 | | int gapSign = BuildQuadraticCandidate(gap, Slot(values, 6), Slot(work, 5), values, signs); |
| | 805 | 225 | | selection.Keep(geometry, values, signs, gapSign, vertex, CurvedPrismContactSelection.Free); |
| | 805 | 226 | | } |
| | | 227 | | |
| | | 228 | | private static void Write(Span<ulong> values, Span<int> signs, int slot, Signed832 value, int orientation = 1) |
| | | 229 | | { |
| | 17208 | 230 | | Import(value, Slot(values, slot)); signs[slot] = orientation * value.Sign; |
| | 8604 | 231 | | } |
| | | 232 | | } |