| | | 1 | | //======================================================================= |
| | | 2 | | // SegmentSlabSurfaceContact.Planar.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 | | /// <content>Finite planar strata and exact oblique circular-rim root selection.</content> |
| | | 14 | | internal static partial class SegmentSlabSurfaceContact |
| | | 15 | | { |
| | | 16 | | private static void AccumulatePrism(scoped Input input, scoped ref Selection selection) |
| | | 17 | | { |
| | 58 | 18 | | int winding = FiniteSlabSectionFrame.GetPrismWinding(input.Offsets); |
| | 348 | 19 | | for (int cap = -1; cap <= 1; cap += 2) |
| | 116 | 20 | | AccumulatePlane(input, new(default, cap < 0 ? WideArithmetic.Negate(input.Cap) : input.Cap, default), |
| | 116 | 21 | | Axis(0, cap, 0), false, ref selection); |
| | 58 | 22 | | Span<WideDirection3d> cone = stackalloc WideDirection3d[5]; |
| | 632 | 23 | | for (int index = 0; index < input.Offsets.Length; index++) |
| | | 24 | | { |
| | 516 | 25 | | WideDirection3d a = PrismVertex(input, index), b = PrismVertex(input, (index + 1) % input.Offsets.Length); |
| | 258 | 26 | | WideDirection3d edge = TriangleCircularGeometry.Subtract(b, a); |
| | 258 | 27 | | if (edge.IsZero) continue; |
| | 234 | 28 | | int previous = (index + input.Offsets.Length - 1) % input.Offsets.Length; |
| | 234 | 29 | | WideDirection3d inward = TriangleCircularGeometry.Subtract(PrismVertex(input, previous), a); |
| | 258 | 30 | | while (inward.IsZero) |
| | | 31 | | { |
| | 24 | 32 | | previous = (previous + input.Offsets.Length - 1) % input.Offsets.Length; |
| | 24 | 33 | | inward = TriangleCircularGeometry.Subtract(PrismVertex(input, previous), a); |
| | | 34 | | } |
| | 234 | 35 | | WideDirection3d outward = new(winding > 0 ? edge.Z : WideArithmetic.Negate(edge.Z), default, |
| | 234 | 36 | | winding > 0 ? WideArithmetic.Negate(edge.X) : edge.X); |
| | 234 | 37 | | bool straight = WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576(inward.X, edge.Z), |
| | 234 | 38 | | WideArithmetic.MultiplySigned576(inward.Z, edge.X)).IsZero; |
| | 234 | 39 | | AccumulatePlane(input, a, outward, false, ref selection); |
| | 468 | 40 | | cone[0] = inward; cone[1] = edge; |
| | 468 | 41 | | cone[2] = Axis(0, 1, 0); cone[3] = Axis(0, -1, 0); |
| | | 42 | | // Opposite incident rays at a redundant straight vertex also need the wall's outward orientation. |
| | 238 | 43 | | if (straight) cone[4] = -outward; |
| | 234 | 44 | | WideDirection3d bottom = new(a.X, WideArithmetic.Negate(input.Cap), a.Z); |
| | 234 | 45 | | WideDirection3d top = new(a.X, input.Cap, a.Z); |
| | 234 | 46 | | AccumulateLine(input, bottom, top, cone[..(straight ? 5 : 4)], ref selection); |
| | 1404 | 47 | | for (int cap = -1; cap <= 1; cap += 2) |
| | | 48 | | { |
| | 468 | 49 | | WideDirection3d first = new(a.X, cap < 0 ? WideArithmetic.Negate(input.Cap) : input.Cap, a.Z); |
| | 468 | 50 | | WideDirection3d second = new(b.X, first.Y, b.Z); |
| | 1872 | 51 | | cone[0] = -outward; cone[1] = Axis(0, -cap, 0); cone[2] = edge; cone[3] = -edge; |
| | 468 | 52 | | AccumulateLine(input, first, second, cone[..4], ref selection); |
| | 1404 | 53 | | cone[0] = inward; cone[1] = edge; cone[2] = Axis(0, -cap, 0); |
| | 476 | 54 | | if (straight) cone[3] = -outward; |
| | 468 | 55 | | AccumulateVertex(input, first, cone[..(straight ? 4 : 3)], ref selection); |
| | | 56 | | } |
| | | 57 | | } |
| | 58 | 58 | | } |
| | | 59 | | |
| | | 60 | | private static WideDirection3d PrismVertex(scoped Input input, int index) |
| | | 61 | | { |
| | 774 | 62 | | Vector2d vertex = input.Offsets[index]; |
| | 774 | 63 | | Signed320 scale = input.Scale; |
| | 774 | 64 | | return new(WideArithmetic.MultiplySigned320(scale, Signed192.Raw(vertex.X)), default, |
| | 774 | 65 | | WideArithmetic.MultiplySigned320(scale, Signed192.Raw(vertex.Y))); |
| | | 66 | | } |
| | | 67 | | |
| | 2150 | 68 | | private static WideDirection3d Axis(long x, long y, long z) => new( |
| | 2150 | 69 | | Signed576.ExtendValue(Signed192.Signed(x)), Signed576.ExtendValue(Signed192.Signed(y)), |
| | 2150 | 70 | | Signed576.ExtendValue(Signed192.Signed(z))); |
| | | 71 | | |
| | | 72 | | private static void AccumulateStraight(scoped Input input, scoped ref Selection selection) |
| | | 73 | | { |
| | 30 | 74 | | Span<WideDirection3d> cone = stackalloc WideDirection3d[4]; |
| | 180 | 75 | | for (int cap = -1; cap <= 1; cap += 2) |
| | 60 | 76 | | AccumulatePlane(input, new(default, cap < 0 ? WideArithmetic.Negate(input.Cap) : input.Cap, default), |
| | 60 | 77 | | Axis(0, cap, 0), true, ref selection); |
| | 180 | 78 | | for (int side = -1; side <= 1; side += 2) |
| | | 79 | | { |
| | 60 | 80 | | Signed576 x = side < 0 ? WideArithmetic.Negate(input.Radial) : input.Radial; |
| | 60 | 81 | | AccumulatePlane(input, new(x, default, default), Axis(side, 0, 0), true, ref selection); |
| | 360 | 82 | | for (int cap = -1; cap <= 1; cap += 2) |
| | | 83 | | { |
| | 120 | 84 | | Signed576 y = cap < 0 ? WideArithmetic.Negate(input.Cap) : input.Cap; |
| | 240 | 85 | | cone[0] = Axis(-side, 0, 0); cone[1] = Axis(0, -cap, 0); |
| | 240 | 86 | | cone[2] = Axis(0, 0, 1); cone[3] = Axis(0, 0, -1); |
| | 120 | 87 | | AccumulateLine(input, new(x, y, WideArithmetic.Negate(input.Core)), |
| | 120 | 88 | | new(x, y, input.Core), cone, ref selection); |
| | | 89 | | } |
| | | 90 | | } |
| | 30 | 91 | | } |
| | | 92 | | |
| | | 93 | | private static bool Seed(scoped Input input, scoped Candidate candidate, int seed) |
| | | 94 | | { |
| | 5094 | 95 | | candidate.Clear(); |
| | 5094 | 96 | | if (!input.Clip.IsEmpty && seed < 3) |
| | | 97 | | { |
| | 1170 | 98 | | ref readonly RationalSegmentInterval clip = ref input.Clip[0]; |
| | 1170 | 99 | | if (seed < 2) |
| | | 100 | | { |
| | 780 | 101 | | candidate.Parameter.Set(seed == 0 ? clip.WideLowerNumerator : clip.WideUpperNumerator); |
| | 780 | 102 | | candidate.ParameterDenominator.Set(seed == 0 ? clip.WideLowerDenominator : clip.WideUpperDenominator); |
| | | 103 | | } |
| | | 104 | | else |
| | | 105 | | { |
| | | 106 | | // The mediant lies strictly inside distinct positive-denominator |
| | | 107 | | // bounds without the 640-bit denominator of their arithmetic mean. |
| | 390 | 108 | | candidate.Parameter.Set(WideArithmetic.AddSigned832(clip.WideLowerNumerator, clip.WideUpperNumerator)); |
| | 390 | 109 | | candidate.ParameterDenominator.Set(WideArithmetic.AddSigned832(clip.WideLowerDenominator, clip.WideUpper |
| | | 110 | | } |
| | | 111 | | } |
| | 4490 | 112 | | else if (seed == 1) candidate.Parameter.Set(One); |
| | 3358 | 113 | | else if (seed == 2) |
| | 1132 | 114 | | { candidate.Parameter.Set(One); candidate.ParameterDenominator.Set(Extend(Signed576.ExtendValue(Signed192.Signed |
| | 2792 | 115 | | else if (seed >= 3) |
| | | 116 | | { |
| | 1396 | 117 | | FixedSegment segment = seed == 3 ? input.Segment : new(input.Segment.End, input.Segment.Start); |
| | 1396 | 118 | | bool admitted = input.RequireSourceInside |
| | 1396 | 119 | | ? input.Frame.TryGetFirstSegmentParameter(segment, input.Offsets, |
| | 1396 | 120 | | candidate.Parameter, candidate.ParameterDenominator, candidate.Root) |
| | 1396 | 121 | | : input.Frame.TryGetFirstProjectedSegmentParameter(segment, input.Offsets, |
| | 1396 | 122 | | candidate.Parameter, candidate.ParameterDenominator, candidate.Root); |
| | 1504 | 123 | | if (!admitted) return false; |
| | 1288 | 124 | | if (seed == 4) |
| | 1288 | 125 | | { candidate.Parameter.MultiplySign(-1); candidate.Parameter.Add(candidate.ParameterDenominator); } |
| | | 126 | | } |
| | 9972 | 127 | | SetPoint(input, candidate); return true; |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | private static void AccumulatePlane(scoped Input input, WideDirection3d origin, WideDirection3d outward, |
| | | 131 | | bool central, scoped ref Selection selection) |
| | | 132 | | { |
| | 470 | 133 | | int Words = input.Words; |
| | 470 | 134 | | int StorageWords = Fields * 2 * Words; |
| | 940 | 135 | | Span<ulong> values = stackalloc ulong[StorageWords], root = stackalloc ulong[Words]; |
| | 470 | 136 | | Span<int> signs = stackalloc int[2 * Fields]; |
| | 470 | 137 | | var candidate = new Candidate(values, signs, root); |
| | 940 | 138 | | Span<ulong> work = stackalloc ulong[8 * Words]; Span<int> workSigns = stackalloc int[8]; |
| | 940 | 139 | | ContactQuadratic offset = ContactQuadratic.At(work, workSigns, 0, Words), term = ContactQuadratic.At(work, workS |
| | 940 | 140 | | product = ContactQuadratic.At(work, workSigns, 2, Words), bound = ContactQuadratic.At(work, workSigns, 3, Wo |
| | 470 | 141 | | Signed832 square = outward.SquaredLength; |
| | | 142 | | // Point seeds coincide; keep the first tie and omit segment-only stationary candidates. |
| | 470 | 143 | | int seedCount = input.Edge.IsZero ? 1 : 6; |
| | 4500 | 144 | | for (int seed = 0; seed < seedCount; seed++) |
| | | 145 | | { |
| | 1780 | 146 | | if (seed == 5) |
| | | 147 | | { |
| | 262 | 148 | | Signed832 velocity = WideDirection3d.Dot(input.Edge, outward); |
| | 262 | 149 | | if (velocity.IsZero) continue; |
| | 134 | 150 | | candidate.Clear(); |
| | 134 | 151 | | candidate.Parameter.Set(WideDirection3d.Dot(TriangleCircularGeometry.Subtract(origin, input.First), outw |
| | 134 | 152 | | candidate.ParameterDenominator.Set(velocity); |
| | 134 | 153 | | if (velocity.Sign < 0) |
| | 134 | 154 | | { candidate.Parameter.MultiplySign(-1); candidate.ParameterDenominator.MultiplySign(-1); } |
| | 134 | 155 | | SetPoint(input, candidate); |
| | | 156 | | } |
| | 1518 | 157 | | else if (!Seed(input, candidate, seed)) continue; |
| | 1616 | 158 | | offset.Clear(); |
| | 12928 | 159 | | for (int axis = 0; axis < 3; axis++) |
| | | 160 | | { |
| | 4848 | 161 | | ContactQuadratic.Scale(candidate.Denominator, Extend(Component(origin, axis)), term); |
| | 4848 | 162 | | term.Add(candidate.Point(axis), -1); |
| | 9696 | 163 | | ContactQuadratic.Scale(term, Extend(Component(outward, axis)), product); offset.Add(product); |
| | | 164 | | } |
| | 1616 | 165 | | if (offset.Sign(candidate.Root) < 0) continue; |
| | 3102 | 166 | | term.Set(square); ScaleCoordinates(candidate, term); |
| | 12408 | 167 | | for (int axis = 0; axis < 3; axis++) |
| | | 168 | | { |
| | 4653 | 169 | | ContactQuadratic.Scale(offset, Extend(Component(outward, axis)), term); |
| | 4653 | 170 | | candidate.Boundary(axis).Add(term); |
| | 4653 | 171 | | candidate.Normal(axis).Set(Extend(Component(outward, axis))); |
| | | 172 | | } |
| | 1551 | 173 | | if (central) |
| | | 174 | | { |
| | 532 | 175 | | ContactQuadratic.Scale(candidate.Denominator, Extend(input.Core), bound); |
| | 1064 | 176 | | bound.CopyTo(term); term.Add(candidate.Boundary(2), -1); |
| | 532 | 177 | | if (term.Sign(candidate.Root) < 0) continue; |
| | 670 | 178 | | bound.CopyTo(term); term.Add(candidate.Boundary(2)); |
| | 335 | 179 | | if (term.Sign(candidate.Root) < 0) continue; |
| | | 180 | | } |
| | 1173 | 181 | | Keep(input, candidate, ref selection); |
| | | 182 | | } |
| | 470 | 183 | | } |
| | | 184 | | |
| | | 185 | | private static void AccumulateLine(scoped Input input, WideDirection3d first, WideDirection3d second, |
| | | 186 | | scoped ReadOnlySpan<WideDirection3d> cone, scoped ref Selection selection) |
| | | 187 | | { |
| | 822 | 188 | | int Words = input.Words; |
| | 822 | 189 | | int StorageWords = Fields * 2 * Words; |
| | 822 | 190 | | WideDirection3d axis = TriangleCircularGeometry.Subtract(second, first); |
| | | 191 | | // Positive thickness/core length and distinct prism edges keep every line finite and nonzero. |
| | | 192 | | System.Diagnostics.Debug.Assert(!axis.IsZero); |
| | 822 | 193 | | WideDirection3d offset = TriangleCircularGeometry.Subtract(input.First, first); |
| | 1644 | 194 | | Signed832 ee = input.Edge.SquaredLength, ff = axis.SquaredLength, |
| | 2466 | 195 | | ef = WideDirection3d.Dot(input.Edge, axis), ew = WideDirection3d.Dot(input.Edge, offset), fw = WideDirection |
| | 1644 | 196 | | Span<ulong> work = stackalloc ulong[16 * Words]; Span<int> workSigns = stackalloc int[16]; |
| | 1644 | 197 | | ContactQuadratic a = ContactQuadratic.At(work, workSigns, 0, Words), b = ContactQuadratic.At(work, workSigns, 1, |
| | 1644 | 198 | | determinant = ContactQuadratic.At(work, workSigns, 2, Words), term = ContactQuadratic.At(work, workSigns, 3, |
| | 822 | 199 | | upper = ContactQuadratic.At(work, workSigns, 5, Words), |
| | 1644 | 200 | | projection = ContactQuadratic.At(work, workSigns, 6, Words), divisor = ContactQuadratic.At(work, workSigns, |
| | 2466 | 201 | | a.Set(ee); b.Set(ff); ContactQuadratic.Multiply(a, b, ReadOnlySpan<ulong>.Empty, determinant); |
| | 2466 | 202 | | a.Set(ef); ContactQuadratic.Multiply(a, a, ReadOnlySpan<ulong>.Empty, term); determinant.Add(term, -1); |
| | 1644 | 203 | | Span<ulong> values = stackalloc ulong[StorageWords], root = stackalloc ulong[Words]; |
| | 1644 | 204 | | Span<int> signs = stackalloc int[2 * Fields]; var candidate = new Candidate(values, signs, root); |
| | 822 | 205 | | int seedCount = input.Edge.IsZero ? 1 : 8; |
| | 9392 | 206 | | for (int seed = 0; seed < seedCount; seed++) |
| | | 207 | | { |
| | 3874 | 208 | | if (seed == 5) |
| | | 209 | | { |
| | 436 | 210 | | if (determinant.Signs[0] == 0) continue; |
| | 1296 | 211 | | candidate.Clear(); a.Set(ef); b.Set(fw); ContactQuadratic.Multiply(a, b, root, candidate.Parameter); |
| | 1296 | 212 | | a.Set(ff); b.Set(ew); ContactQuadratic.Multiply(a, b, root, term); candidate.Parameter.Add(term, -1); |
| | 648 | 213 | | determinant.CopyTo(candidate.ParameterDenominator); SetPoint(input, candidate); |
| | | 214 | | } |
| | 3438 | 215 | | else if (seed >= 6) |
| | | 216 | | { |
| | 872 | 217 | | if (ef.IsZero) continue; |
| | 784 | 218 | | candidate.Clear(); candidate.Parameter.Set(WideArithmetic.SubtractSigned832(seed == 6 ? default : ff, fw |
| | 392 | 219 | | candidate.ParameterDenominator.Set(ef); |
| | 752 | 220 | | if (ef.Sign < 0) { candidate.Parameter.MultiplySign(-1); candidate.ParameterDenominator.MultiplySign(-1) |
| | 392 | 221 | | SetPoint(input, candidate); |
| | | 222 | | } |
| | 2566 | 223 | | else if (!Seed(input, candidate, seed)) continue; |
| | 3210 | 224 | | if (seed == 5) |
| | | 225 | | { |
| | 972 | 226 | | a.Set(ee); b.Set(fw); ContactQuadratic.Multiply(a, b, root, projection); |
| | 1296 | 227 | | a.Set(ef); b.Set(ew); ContactQuadratic.Multiply(a, b, root, term); projection.Add(term, -1); |
| | 324 | 228 | | candidate.Denominator.CopyTo(divisor); |
| | | 229 | | } |
| | | 230 | | else |
| | | 231 | | { |
| | 2886 | 232 | | projection.Clear(); |
| | 23088 | 233 | | for (int component = 0; component < 3; component++) |
| | | 234 | | { |
| | 8658 | 235 | | ContactQuadratic.Scale(candidate.Denominator, Extend(Component(first, component)), term); |
| | 17316 | 236 | | candidate.Point(component).CopyTo(a); a.Add(term, -1); |
| | 17316 | 237 | | ContactQuadratic.Scale(a, Extend(Component(axis, component)), term); projection.Add(term); |
| | | 238 | | } |
| | 2886 | 239 | | ContactQuadratic.Scale(candidate.Denominator, ff, divisor); |
| | | 240 | | } |
| | 6420 | 241 | | divisor.CopyTo(upper); upper.Add(projection, -1); |
| | 3210 | 242 | | if (projection.Sign(candidate.Root) < 0 || upper.Sign(candidate.Root) < 0) continue; |
| | 7270 | 243 | | if (seed != 5) { a.Set(ff); ScaleCoordinates(candidate, a); } |
| | 20752 | 244 | | for (int component = 0; component < 3; component++) |
| | | 245 | | { |
| | 7782 | 246 | | ContactQuadratic.Scale(candidate.Denominator, Extend(Component(first, component)), candidate.Boundary(co |
| | 15564 | 247 | | ContactQuadratic.Scale(projection, Extend(Component(axis, component)), term); candidate.Boundary(compone |
| | | 248 | | } |
| | 2594 | 249 | | KeepPlanar(input, candidate, cone, ref selection); |
| | | 250 | | } |
| | 822 | 251 | | } |
| | | 252 | | |
| | | 253 | | private static void AccumulateVertex(scoped Input input, WideDirection3d point, |
| | | 254 | | scoped ReadOnlySpan<WideDirection3d> cone, scoped ref Selection selection) |
| | | 255 | | { |
| | 468 | 256 | | int Words = input.Words; |
| | 468 | 257 | | int StorageWords = Fields * 2 * Words; |
| | 936 | 258 | | Span<ulong> values = stackalloc ulong[StorageWords], root = stackalloc ulong[Words]; |
| | 936 | 259 | | Span<int> signs = stackalloc int[2 * Fields]; var candidate = new Candidate(values, signs, root); |
| | 468 | 260 | | int seedCount = input.Edge.IsZero ? 1 : 4; |
| | 3264 | 261 | | for (int seed = 0; seed < seedCount; seed++) |
| | | 262 | | { |
| | 1164 | 263 | | if (seed == 3) |
| | | 264 | | { |
| | 464 | 265 | | candidate.Clear(); candidate.Parameter.Set(WideDirection3d.Dot( |
| | 232 | 266 | | TriangleCircularGeometry.Subtract(point, input.First), input.Edge)); |
| | 464 | 267 | | candidate.ParameterDenominator.Set(input.Edge.SquaredLength); SetPoint(input, candidate); |
| | | 268 | | } |
| | 932 | 269 | | else Seed(input, candidate, seed); |
| | 9312 | 270 | | for (int axis = 0; axis < 3; axis++) |
| | 3492 | 271 | | ContactQuadratic.Scale(candidate.Denominator, Extend(Component(point, axis)), candidate.Boundary(axis)); |
| | 1164 | 272 | | KeepPlanar(input, candidate, cone, ref selection); |
| | | 273 | | } |
| | 468 | 274 | | } |
| | | 275 | | |
| | | 276 | | private static void KeepPlanar(scoped Input input, scoped Candidate candidate, |
| | | 277 | | scoped ReadOnlySpan<WideDirection3d> cone, scoped ref Selection selection) |
| | | 278 | | { |
| | 3775 | 279 | | int Words = input.Words; |
| | 7550 | 280 | | Span<ulong> values = stackalloc ulong[8 * Words]; Span<int> signs = stackalloc int[8]; |
| | 7550 | 281 | | ContactQuadratic sum = ContactQuadratic.At(values, signs, 0, Words), term = ContactQuadratic.At(values, signs, 1 |
| | 7550 | 282 | | a = ContactQuadratic.At(values, signs, 2, Words), b = ContactQuadratic.At(values, signs, 3, Words); |
| | 3775 | 283 | | bool nonzero = false; |
| | 30200 | 284 | | for (int axis = 0; axis < 3; axis++) |
| | | 285 | | { |
| | 22650 | 286 | | candidate.Boundary(axis).CopyTo(candidate.Normal(axis)); candidate.Normal(axis).Add(candidate.Point(axis), - |
| | 11325 | 287 | | nonzero |= candidate.Normal(axis).Sign(candidate.Root) != 0; |
| | | 288 | | } |
| | 3775 | 289 | | if (nonzero) |
| | | 290 | | { |
| | 33096 | 291 | | foreach (WideDirection3d wall in cone) |
| | 26472 | 292 | | { Dot(candidate, wall, sum, term); if (sum.Sign(candidate.Root) > 0) return; } |
| | 6624 | 293 | | Keep(input, candidate, ref selection); return; |
| | | 294 | | } |
| | 25 | 295 | | Span<WideDirection3d> walls = stackalloc WideDirection3d[input.Fan.Length + cone.Length]; |
| | 50 | 296 | | input.Fan.CopyTo(walls); cone.CopyTo(walls[input.Fan.Length..]); |
| | | 297 | | // A nonempty pointed cone in three dimensions contains an axis or |
| | | 298 | | // an extreme ray at two wall planes. Degenerate lines keep both signs. |
| | 200 | 299 | | for (int axis = 0; axis < 3; axis++) |
| | 450 | 300 | | for (int branch = -1; branch <= 1; branch += 2) |
| | | 301 | | { |
| | 1200 | 302 | | for (int component = 0; component < 3; component++) |
| | 450 | 303 | | candidate.Normal(component).Set(component == axis ? Extend(Signed576.ExtendValue(Signed192.Signed(br |
| | 150 | 304 | | KeepPlanarNormal(input, candidate, cone, ref selection); |
| | | 305 | | } |
| | 432 | 306 | | for (int first = 0; first < walls.Length; first++) |
| | 1670 | 307 | | for (int second = first + 1; second < walls.Length; second++) |
| | | 308 | | { |
| | 5152 | 309 | | for (int axis = 0; axis < 3; axis++) |
| | | 310 | | { |
| | 3864 | 311 | | int next = (axis + 1) % 3, last = (axis + 2) % 3; |
| | 3864 | 312 | | a.Set(Extend(Component(walls[first], next))); b.Set(Extend(Component(walls[second], last))); |
| | 1932 | 313 | | ContactQuadratic.Multiply(a, b, candidate.Root, candidate.Normal(axis)); |
| | 3864 | 314 | | a.Set(Extend(Component(walls[first], last))); b.Set(Extend(Component(walls[second], next))); |
| | 3864 | 315 | | ContactQuadratic.Multiply(a, b, candidate.Root, term); candidate.Normal(axis).Add(term, -1); |
| | | 316 | | } |
| | 644 | 317 | | KeepPlanarNormal(input, candidate, cone, ref selection); |
| | 7084 | 318 | | for (int axis = 0; axis < 3; axis++) candidate.Normal(axis).MultiplySign(-1); |
| | 644 | 319 | | KeepPlanarNormal(input, candidate, cone, ref selection); |
| | | 320 | | } |
| | 25 | 321 | | } |
| | | 322 | | |
| | | 323 | | private static void KeepPlanarNormal(scoped Input input, scoped Candidate candidate, |
| | | 324 | | scoped ReadOnlySpan<WideDirection3d> cone, scoped ref Selection selection) |
| | | 325 | | { |
| | 1438 | 326 | | int Words = input.Words; |
| | 2876 | 327 | | Span<ulong> values = stackalloc ulong[4 * Words]; Span<int> signs = stackalloc int[4]; |
| | 2876 | 328 | | var sum = ContactQuadratic.At(values, signs, 0, Words); var term = ContactQuadratic.At(values, signs, 1, Words); |
| | 10087 | 329 | | foreach (WideDirection3d wall in cone) |
| | 8677 | 330 | | { Dot(candidate, wall, sum, term); if (sum.Sign(candidate.Root) > 0) return; } |
| | 705 | 331 | | Keep(input, candidate, ref selection); |
| | 705 | 332 | | } |
| | | 333 | | private const int ObliqueValueCellWords = (16 * (CircularRimContactAlgebra.ValueWords * 64 + 11) + 255) / 64; |
| | | 334 | | |
| | | 335 | | private struct ObliqueSelection |
| | | 336 | | { |
| | | 337 | | internal bool HasValue, IsZero, IsRational; |
| | | 338 | | internal WideDirection3d First, Second; |
| | | 339 | | internal int Cap, Region, ParameterOrdinal, ValueOrdinal, ValueShift; |
| | | 340 | | } |
| | | 341 | | |
| | | 342 | | private static bool TryImproveObliqueRims(scoped Input input, scoped Selection analytic, out FixedContactAnchors con |
| | | 343 | | { |
| | 153 | 344 | | int ValueWords = input.ValueWords; |
| | 153 | 345 | | int ObliqueValueCellWords = (16 * (ValueWords * 64 + 11) + 255) / 64; |
| | 153 | 346 | | contact = default; |
| | 153 | 347 | | if (input.Edge.Y.IsZero || input.Edge.X.IsZero && input.Edge.Z.IsZero || input.Radius == Fixed64.Zero |
| | 233 | 348 | | || analytic.HasValue && analytic.MetricSigns[10] == 0) return false; |
| | 73 | 349 | | Span<ulong> bestValues = stackalloc ulong[5 * ValueWords]; |
| | 73 | 350 | | Span<sbyte> bestSigns = stackalloc sbyte[5]; |
| | 73 | 351 | | Span<ulong> bestCell = stackalloc ulong[ObliqueValueCellWords]; |
| | 73 | 352 | | ObliqueSelection best = default; |
| | 73 | 353 | | CircularRimContactAlgebra.GetBasis(input.Edge, out WideDirection3d u, out WideDirection3d v); |
| | 73 | 354 | | int firstRegion = input.Length == Fixed64.Zero ? 0 : -1; |
| | 73 | 355 | | Span<Signed576> bounds = stackalloc Signed576[9] |
| | 73 | 356 | | { input.First.X, input.First.Y, input.First.Z, input.Edge.X, input.Edge.Y, input.Edge.Z, input.Cap, input.Core, |
| | 73 | 357 | | int valueShift = TriangleCircularGeometry.GetValueShift(bounds, 5); |
| | 320 | 358 | | for (int region = firstRegion; region <= (input.Length == Fixed64.Zero ? 0 : 1); region += 2) |
| | 522 | 359 | | for (int cap = -1; cap <= 1; cap += 2) |
| | 1044 | 360 | | for (int firstSign = -1; firstSign <= 1; firstSign += 2) |
| | 2088 | 361 | | for (int secondSign = -1; secondSign <= 1; secondSign += 2) |
| | | 362 | | { |
| | 1392 | 363 | | WideDirection3d first = firstSign < 0 ? -u : u, second = secondSign < 0 ? -v : v; |
| | 696 | 364 | | AccumulateObliqueChart(input, analytic, first, second, region, cap, valueShift, |
| | 696 | 365 | | bestValues, bestSigns, bestCell, ref best); |
| | 696 | 366 | | AccumulateObliqueChart(input, analytic, second, first, region, cap, valueShift, |
| | 696 | 367 | | bestValues, bestSigns, bestCell, ref best); |
| | | 368 | | } |
| | 108 | 369 | | if (!best.HasValue) return false; |
| | 38 | 370 | | contact = MaterializeOblique(input, best, valueShift, bestValues, bestSigns, bestCell); |
| | 38 | 371 | | return true; |
| | | 372 | | } |
| | | 373 | | |
| | 1120 | 374 | | private static WideDirection3d RimOffset(scoped Input input, int region, int cap) => new(input.First.X, |
| | 1120 | 375 | | WideArithmetic.SubtractSigned576(input.First.Y, cap < 0 ? WideArithmetic.Negate(input.Cap) : input.Cap), |
| | 1120 | 376 | | WideArithmetic.SubtractSigned576(input.First.Z, region < 0 ? WideArithmetic.Negate(input.Core) : region > 0 ? in |
| | | 377 | | |
| | | 378 | | private static void AccumulateObliqueChart(scoped Input input, scoped Selection analytic, |
| | | 379 | | WideDirection3d first, WideDirection3d second, int region, int cap, int valueShift, |
| | | 380 | | scoped Span<ulong> bestValues, scoped Span<sbyte> bestSigns, scoped Span<ulong> bestCell, ref ObliqueSelection b |
| | | 381 | | { |
| | 1392 | 382 | | int Words = input.Words; |
| | 1392 | 383 | | int ValueWords = input.ValueWords; |
| | 1392 | 384 | | int ObliqueValueCellWords = (16 * (ValueWords * 64 + 11) + 255) / 64; |
| | | 385 | | // The cap generator and stadium hemisphere must agree with the exit |
| | | 386 | | // ray -n. Reject an entirely unpermitted chart before its quartic. |
| | 1740 | 387 | | if (first.Y.Sign * cap > 0 && WideArithmetic.AddSigned576(first.Y, second.Y).Sign * cap > 0) return; |
| | 1044 | 388 | | if (region != 0 && first.Z.Sign * region > 0 |
| | 1156 | 389 | | && WideArithmetic.AddSigned576(first.Z, second.Z).Sign * region > 0) return; |
| | 1864 | 390 | | Span<ulong> data = stackalloc ulong[26 * Words]; Span<sbyte> signs = stackalloc sbyte[26]; |
| | 932 | 391 | | WideDirection3d offset = RimOffset(input, region, cap); |
| | 932 | 392 | | CircularRimContactAlgebra.BuildParameter(offset, input.Radial, input.Scale, |
| | 932 | 393 | | first, second, data, signs); |
| | 1864 | 394 | | Span<ulong> batchCells = stackalloc ulong[64]; Span<int> batchShifts = stackalloc int[8]; |
| | 932 | 395 | | FiniteAxisValueRoots roots = WideFiniteAxisIntersection.GetFiniteValueRoots(data[..(5 * Words)], signs[..5], bat |
| | 932 | 396 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords(data[..(5 * Words)], |
| | 1864 | 397 | | Span<ulong> values = stackalloc ulong[5 * ValueWords]; Span<sbyte> valueSigns = stackalloc sbyte[5]; |
| | 1864 | 398 | | Span<ulong> valueCell = stackalloc ulong[ObliqueValueCellWords]; bool valuesReady = false; |
| | 3100 | 399 | | for (int ordinal = 0; ordinal < roots.Count; ordinal++) |
| | | 400 | | { |
| | 618 | 401 | | FiniteAxisValueRoot root = FiniteAxisValueRoots.GetRoot(roots, ordinal, data[..(5 * Words)], signs[..5], cel |
| | 618 | 402 | | int kSign = CircularRimContactAlgebra.Sign(ref root, data, signs, 8, 2); |
| | 618 | 403 | | if (kSign == 0 || CircularRimContactAlgebra.Sign(ref root, data, signs, 10, 3) != -kSign) continue; |
| | 288 | 404 | | int gapSign = CircularRimContactAlgebra.Sign(ref root, data, signs, 13, 3) * kSign; |
| | 288 | 405 | | if (gapSign < 0 || !AdmitObliqueFoot(input, first, second, region, cap, ref root)) continue; |
| | 90 | 406 | | var candidate = new ObliqueSelection |
| | 90 | 407 | | { HasValue = true, First = first, Second = second, Region = region, Cap = cap, ParameterOrdinal = ordinal }; |
| | 90 | 408 | | if (gapSign == 0) |
| | 74 | 409 | | { if (!best.IsZero) { candidate.IsZero = true; best = candidate; } continue; } |
| | 70 | 410 | | if (best.IsZero) continue; |
| | 64 | 411 | | if (!valuesReady) |
| | 124 | 412 | | { CircularRimContactAlgebra.BuildValues(input.Edge, offset, input.Radial, valueShift, values, valueSigns); v |
| | 64 | 413 | | FiniteAxisValueRoot value = ConvexContactValueRoot.MapSquaredValue(input.Scale, valueShift, |
| | 64 | 414 | | ref root, data.Slice(16 * Words, 5 * Words), signs.Slice(16, 5), data.Slice(21 * Words, 5 * Words), sign |
| | 64 | 415 | | values, valueSigns, valueCell); |
| | 64 | 416 | | int comparison = best.HasValue ? WideFiniteAxisIntersection.CompareFiniteValueRoots(value, |
| | 64 | 417 | | RestoreObliqueValue(bestValues, bestSigns, bestCell, best)) : analytic.HasValue |
| | 64 | 418 | | ? ConvexContactValueRoot.CompareRootSquared(input.Scale, valueShift, ref value, |
| | 64 | 419 | | new ConvexContactCandidate(analytic.Metric, analytic.MetricSigns, analytic.MetricSigns[10], Word |
| | 64 | 420 | | if (comparison >= 0) continue; |
| | 69 | 421 | | values.CopyTo(bestValues); valueSigns.CopyTo(bestSigns); valueCell.CopyTo(bestCell); |
| | 46 | 422 | | candidate.ValueOrdinal = value.Ordinal; candidate.ValueShift = value.DenominatorShift; |
| | 46 | 423 | | candidate.IsRational = value.IsRational; best = candidate; |
| | | 424 | | } |
| | 932 | 425 | | } |
| | | 426 | | |
| | | 427 | | private static FiniteAxisValueRoot RestoreObliqueValue(ReadOnlySpan<ulong> values, ReadOnlySpan<sbyte> signs, |
| | 24 | 428 | | Span<ulong> cell, ObliqueSelection selection) => new() |
| | 24 | 429 | | { Coefficients = values, Signs = signs, LowerNumerator = cell, Ordinal = selection.ValueOrdinal, |
| | 24 | 430 | | DenominatorShift = selection.ValueShift, IsRational = selection.IsRational }; |
| | | 431 | | |
| | | 432 | | private static void RimParameter(scoped Input input, WideDirection3d first, WideDirection3d second, int region, |
| | | 433 | | out Signed832 a0, out Signed832 a1, out Signed832 b0, out Signed832 b1, out Signed832 d0, out Signed832 d1) |
| | | 434 | | { |
| | 150 | 435 | | WideDirection3d offset = RimOffset(input, region, 1); |
| | 150 | 436 | | WideDirection3d end = new(WideArithmetic.AddSigned576(offset.X, input.Edge.X), |
| | 150 | 437 | | WideArithmetic.AddSigned576(offset.Y, input.Edge.Y), WideArithmetic.AddSigned576(offset.Z, input.Edge.Z)); |
| | 150 | 438 | | WideDirection3d tangent0 = new(WideArithmetic.Negate(first.Z), default, first.X), |
| | 150 | 439 | | tangent1 = new(WideArithmetic.Negate(second.Z), default, second.X); |
| | 300 | 440 | | a0 = WideDirection3d.Dot(offset, tangent0); a1 = WideDirection3d.Dot(offset, tangent1); |
| | 300 | 441 | | b0 = WideDirection3d.Dot(end, tangent0); b1 = WideDirection3d.Dot(end, tangent1); |
| | 300 | 442 | | d0 = WideArithmetic.SubtractSigned832(b0, a0); d1 = WideArithmetic.SubtractSigned832(b1, a1); |
| | 150 | 443 | | } |
| | | 444 | | |
| | | 445 | | private static int RootSign(scoped ref FiniteAxisValueRoot root, Signed832 first, Signed832 second) |
| | | 446 | | { |
| | 2596 | 447 | | Span<ulong> values = stackalloc ulong[26]; Span<sbyte> signs = stackalloc sbyte[2]; |
| | 2596 | 448 | | WideArithmetic.GetMagnitude(first, values[..13]); WideArithmetic.GetMagnitude(second, values[13..]); |
| | 2596 | 449 | | signs[0] = (sbyte)first.Sign; signs[1] = (sbyte)second.Sign; |
| | 1298 | 450 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, values, signs); |
| | | 451 | | } |
| | | 452 | | |
| | | 453 | | private static bool AdmitObliqueFoot(scoped Input input, WideDirection3d first, WideDirection3d second, |
| | | 454 | | int region, int cap, scoped ref FiniteAxisValueRoot root) |
| | | 455 | | { |
| | 280 | 456 | | int Words = input.Words; |
| | 280 | 457 | | if (RootSign(ref root, Extend(first.Y), Extend(second.Y)) * cap > 0 |
| | 318 | 458 | | || region != 0 && RootSign(ref root, Extend(first.Z), Extend(second.Z)) * region > 0) return false; |
| | 1622 | 459 | | foreach (WideDirection3d wall in input.Fan) |
| | 764 | 460 | | if (RootSign(ref root, WideDirection3d.Dot(first, wall), WideDirection3d.Dot(second, wall)) < 0) return fals |
| | 112 | 461 | | RimParameter(input, first, second, region, out Signed832 a0, out Signed832 a1, |
| | 112 | 462 | | out Signed832 b0, out Signed832 b1, out Signed832 d0, out Signed832 d1); |
| | 112 | 463 | | int denominator = RootSign(ref root, d0, d1); |
| | | 464 | | // A zero tangent divisor is meridional: L=0 then W=0 forces K=0, rejected by chart admission. |
| | | 465 | | System.Diagnostics.Debug.Assert(denominator != 0); |
| | | 466 | | int start, end; |
| | 112 | 467 | | if (input.Clip.IsEmpty) |
| | | 468 | | { |
| | 100 | 469 | | start = -RootSign(ref root, a0, a1) * denominator; |
| | 100 | 470 | | end = RootSign(ref root, b0, b1) * denominator; |
| | | 471 | | } |
| | | 472 | | else |
| | | 473 | | { |
| | 12 | 474 | | ref readonly RationalSegmentInterval clip = ref input.Clip[0]; |
| | 12 | 475 | | start = CompareObliqueParameter(ref root, a0, a1, d0, d1, |
| | 12 | 476 | | clip.WideLowerNumerator, clip.WideLowerDenominator) * denominator; |
| | 12 | 477 | | end = -CompareObliqueParameter(ref root, a0, a1, d0, d1, |
| | 12 | 478 | | clip.WideUpperNumerator, clip.WideUpperDenominator) * denominator; |
| | | 479 | | } |
| | 120 | 480 | | if (start < 0 || end < 0 || input.InteriorOnly && (start == 0 || end == 0)) return false; |
| | 125 | 481 | | if (!input.RequireSourceInside) return true; |
| | | 482 | | // The tangent-plane ratio is the matched finite foot. Check the full |
| | | 483 | | // slab before ranking its exact gap; outer opposite feet cannot hide |
| | | 484 | | // behind the preflight's unrelated intersecting segment parameter. |
| | 166 | 485 | | Span<ulong> coordinates = stackalloc ulong[8 * Words]; Span<sbyte> coordinateSigns = stackalloc sbyte[8]; |
| | 664 | 486 | | for (int axis = 0; axis < 3; axis++) |
| | | 487 | | { |
| | 249 | 488 | | WriteRootCoordinate(input, axis, a0, a1, d0, d1, coordinates.Slice(axis * 2 * Words, 2 * Words), |
| | 249 | 489 | | coordinateSigns.Slice(axis * 2, 2)); |
| | 249 | 490 | | if (denominator < 0) |
| | 234 | 491 | | { coordinateSigns[2 * axis] = (sbyte)-coordinateSigns[2 * axis]; coordinateSigns[2 * axis + 1] = (sbyte)-coo |
| | | 492 | | } |
| | 83 | 493 | | CircularRimContactAlgebra.Write(coordinates, coordinateSigns, 6, denominator < 0 ? WideArithmetic.SubtractSigned |
| | 83 | 494 | | CircularRimContactAlgebra.Write(coordinates, coordinateSigns, 7, denominator < 0 ? WideArithmetic.SubtractSigned |
| | 83 | 495 | | return ContainsObliqueFoot(input, coordinates, coordinateSigns, ref root); |
| | | 496 | | } |
| | | 497 | | |
| | | 498 | | private static int CompareObliqueParameter(scoped ref FiniteAxisValueRoot root, |
| | | 499 | | Signed832 a0, Signed832 a1, Signed832 d0, Signed832 d1, Signed832 numerator, Signed832 denominator) |
| | | 500 | | { |
| | | 501 | | // The foot is -a(u)/d(u). A retained bound adds at most 319 |
| | | 502 | | // bits to the <398-bit linear coefficients; do not narrow to Signed832. |
| | 48 | 503 | | Span<ulong> values = stackalloc ulong[2 * Words], factor = stackalloc ulong[13], |
| | 48 | 504 | | coefficient = stackalloc ulong[13], product = stackalloc ulong[Words]; |
| | 24 | 505 | | Span<sbyte> signs = stackalloc sbyte[2]; |
| | 144 | 506 | | for (int index = 0; index < 2; index++) |
| | | 507 | | { |
| | 96 | 508 | | Signed832 a = index == 0 ? a0 : a1, d = index == 0 ? d0 : d1; |
| | 96 | 509 | | Import(denominator, factor); Import(a, coefficient); |
| | 48 | 510 | | WideArithmetic.MultiplyMagnitudes(coefficient, factor, Slot(values, index, Words)); |
| | 48 | 511 | | int sign = -a.Sign; |
| | 96 | 512 | | Import(numerator, factor); Import(d, coefficient); |
| | 48 | 513 | | WideArithmetic.MultiplyMagnitudes(coefficient, factor, product); |
| | 96 | 514 | | Add(product, -d.Sign * numerator.Sign, Slot(values, index, Words), ref sign); signs[index] = (sbyte)sign; |
| | | 515 | | } |
| | 24 | 516 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, values, signs); |
| | | 517 | | } |
| | | 518 | | |
| | | 519 | | private static void WriteRootCoordinate(scoped Input input, int axis, Signed832 a0, Signed832 a1, |
| | | 520 | | Signed832 d0, Signed832 d1, Span<ulong> values, Span<sbyte> signs) |
| | | 521 | | { |
| | 249 | 522 | | int Words = input.Words; |
| | | 523 | | // Coordinate numerators need the complete 198+398-bit product; the |
| | | 524 | | // existing forty-word polynomial owner retains it without narrowing. |
| | 996 | 525 | | Span<ulong> factor = stackalloc ulong[13], first = stackalloc ulong[Words], second = stackalloc ulong[Words], pr |
| | 249 | 526 | | Import(Component(input.First, axis), factor); |
| | 1494 | 527 | | for (int index = 0; index < 2; index++) |
| | | 528 | | { |
| | 996 | 529 | | Signed832 d = index == 0 ? d0 : d1, a = index == 0 ? a0 : a1; |
| | 996 | 530 | | Import(d, first); WideArithmetic.MultiplyMagnitudes(first, factor, values.Slice(index * Words, Words)); |
| | 498 | 531 | | int sign = d.Sign * Component(input.First, axis).Sign; |
| | 996 | 532 | | Import(a, second); Import(Component(input.Edge, axis), first); |
| | 498 | 533 | | WideArithmetic.MultiplyMagnitudes(first, second, product); |
| | 498 | 534 | | Add(product, -a.Sign * Component(input.Edge, axis).Sign, values.Slice(index * Words, Words), ref sign); |
| | 498 | 535 | | signs[index] = (sbyte)sign; |
| | | 536 | | } |
| | 249 | 537 | | } |
| | | 538 | | |
| | | 539 | | private static bool ContainsObliqueFoot(scoped Input input, scoped ReadOnlySpan<ulong> coordinates, |
| | | 540 | | scoped ReadOnlySpan<sbyte> coordinateSigns, scoped ref FiniteAxisValueRoot root) |
| | | 541 | | { |
| | 83 | 542 | | int Words = input.Words; |
| | 166 | 543 | | Span<ulong> work = stackalloc ulong[13 * Words]; Span<sbyte> signs = stackalloc sbyte[13]; |
| | 166 | 544 | | Span<ulong> query = work[..(2 * Words)], z = work.Slice(2 * Words, 2 * Words), |
| | 166 | 545 | | denominator = work.Slice(4 * Words, 2 * Words), square = work.Slice(6 * Words, 3 * Words), |
| | 166 | 546 | | term = work.Slice(9 * Words, 3 * Words), product = Slot(work, 12, Words); |
| | 83 | 547 | | Span<ulong> scalar = stackalloc ulong[13]; |
| | 166 | 548 | | ReadOnlySpan<ulong> d = coordinates.Slice(6 * Words, 2 * Words); ReadOnlySpan<sbyte> dSigns = coordinateSigns.Sl |
| | 83 | 549 | | Import(input.Cap, scalar); |
| | 456 | 550 | | for (int cap = -1; cap <= 1; cap += 2) |
| | | 551 | | { |
| | 954 | 552 | | for (int index = 0; index < 2; index++) |
| | | 553 | | { |
| | 636 | 554 | | WideArithmetic.MultiplyMagnitudes(Slot(d, index, Words), scalar, Slot(query, index, Words)); int sign = |
| | 636 | 555 | | Add(Slot(coordinates, index + 2, Words), cap * coordinateSigns[index + 2], Slot(query, index, Words), re |
| | | 556 | | } |
| | 173 | 557 | | if (WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, signs[..2]) < 0) return fa |
| | | 558 | | } |
| | 138 | 559 | | coordinates.Slice(4 * Words, 2 * Words).CopyTo(z); coordinateSigns.Slice(4, 2).CopyTo(signs.Slice(2, 2)); |
| | 69 | 560 | | Import(input.Core, scalar); |
| | 228 | 561 | | for (int region = -1; region <= 1; region += 2) |
| | | 562 | | { |
| | 660 | 563 | | for (int index = 0; index < 2; index++) |
| | | 564 | | { |
| | 440 | 565 | | WideArithmetic.MultiplyMagnitudes(Slot(d, index, Words), scalar, Slot(query, index, Words)); int sign = |
| | 440 | 566 | | Add(Slot(coordinates, index + 4, Words), coordinateSigns[index + 4], Slot(query, index, Words), ref sign |
| | | 567 | | } |
| | 110 | 568 | | int side = WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, query, signs[..2]); |
| | 305 | 569 | | if (side * region >= 0) { query.CopyTo(z); signs[..2].CopyTo(signs.Slice(2, 2)); break; } |
| | 53 | 570 | | if (region == 1) { z.Clear(); signs.Slice(2, 2).Clear(); } |
| | | 571 | | } |
| | 69 | 572 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(coordinates[..(2 * Words)], coordinateSigns[..2], |
| | 69 | 573 | | coordinates[..(2 * Words)], coordinateSigns[..2], square, signs.Slice(6, 3), product); |
| | 69 | 574 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(z, signs.Slice(2, 2), z, signs.Slice(2, 2), term, signs |
| | 552 | 575 | | for (int index = 0; index < 3; index++) |
| | 621 | 576 | | { int sign = signs[index + 6]; Add(Slot(term, index, Words), signs[index + 9], Slot(square, index, Words), ref s |
| | 69 | 577 | | Import(input.Radial, scalar); |
| | 414 | 578 | | for (int index = 0; index < 2; index++) |
| | 276 | 579 | | { WideArithmetic.MultiplyMagnitudes(Slot(d, index, Words), scalar, Slot(denominator, index, Words)); signs[index |
| | 69 | 580 | | WideFiniteAxisIntersection.MultiplyFiniteAxisPolynomials(denominator, signs.Slice(4, 2), denominator, signs.Slic |
| | 552 | 581 | | for (int index = 0; index < 3; index++) |
| | 621 | 582 | | { int sign = signs[index + 9]; Add(Slot(square, index, Words), -signs[index + 6], Slot(term, index, Words), ref |
| | 69 | 583 | | return WideFiniteAxisIntersection.GetSignAtFiniteValueRootAndRefine(ref root, term, signs.Slice(9, 3)) >= 0; |
| | | 584 | | } |
| | | 585 | | |
| | | 586 | | private static FixedContactAnchors MaterializeOblique(scoped Input input, ObliqueSelection best, int valueShift, |
| | | 587 | | scoped ReadOnlySpan<ulong> bestValues, scoped ReadOnlySpan<sbyte> bestSigns, scoped Span<ulong> bestCell) |
| | | 588 | | { |
| | 38 | 589 | | int Words = input.Words; |
| | 76 | 590 | | Span<ulong> data = stackalloc ulong[26 * Words]; Span<sbyte> signs = stackalloc sbyte[26]; |
| | 38 | 591 | | CircularRimContactAlgebra.BuildParameter(RimOffset(input, best.Region, best.Cap), input.Radial, |
| | 38 | 592 | | input.Scale, best.First, best.Second, data, signs); |
| | 38 | 593 | | Span<ulong> cell = stackalloc ulong[WideFiniteAxisIntersection.GetFiniteValueRootCellWords(data[..(5 * Words)], |
| | 38 | 594 | | bool found = WideFiniteAxisIntersection.TryGetFiniteValueRoot(data[..(5 * Words)], signs[..5], best.ParameterOrd |
| | | 595 | | System.Diagnostics.Debug.Assert(found); |
| | 38 | 596 | | RimParameter(input, best.First, best.Second, best.Region, out Signed832 a0, out Signed832 a1, |
| | 38 | 597 | | out Signed832 b0, out Signed832 b1, out Signed832 d0, out Signed832 d1); |
| | 76 | 598 | | int denominator = RootSign(ref root, d0, d1); Vector3d point = default; |
| | 304 | 599 | | for (int axis = 0; axis < 3; axis++) |
| | 114 | 600 | | point[axis] = input.Frame.RadialFrame.ShapeFrame.HasExactScale |
| | 114 | 601 | | ? TriangleCylinderRimWitnesses.RoundRootCoordinate(Component(input.First, axis), |
| | 114 | 602 | | WideArithmetic.AddSigned576(Component(input.First, axis), Component(input.Edge, axis)), input.Scale, |
| | 114 | 603 | | a0, a1, b0, b1, d0, d1, denominator, ref root) |
| | 114 | 604 | | : TriangleCylinderRimWitnesses.RoundRootCoordinate(input.Segment.Start[axis], input.Segment.End[axis], |
| | 114 | 605 | | a0, a1, b0, b1, d0, d1, denominator, ref root); |
| | 76 | 606 | | Span<ulong> gradient = stackalloc ulong[54]; Span<sbyte> gradientSigns = stackalloc sbyte[6]; |
| | 38 | 607 | | CircularRimContactAlgebra.WriteGradient(best.First, best.Second, gradient, gradientSigns); |
| | 76 | 608 | | gradient.Slice(18, 18).Clear(); gradientSigns.Slice(2, 2).Clear(); |
| | 38 | 609 | | Vector3d local = ConvexContactValueRoot.GetScaledNormalizedDirection(ref root, gradient, gradientSigns, input.Ra |
| | 76 | 610 | | Vector3d core = default; FixedPointAnchorTerm3d exactTerm = default; |
| | 38 | 611 | | if (best.Region != 0) |
| | | 612 | | { |
| | 14 | 613 | | core = TriangleCapsuleSlabEndpointWitnesses.GetCore(Vector2d.Forward, input.Length, -best.Region, out exactT |
| | 14 | 614 | | local = TriangleCapsuleSlabEndpointWitnesses.AdjustRoot(ref root, gradient, gradientSigns, |
| | 14 | 615 | | input.Radius, core, exactTerm, local, out int integralMask); |
| | 14 | 616 | | exactTerm = TriangleCapsuleSlabEndpointWitnesses.CreateTerm(Vector2d.Forward, |
| | 14 | 617 | | best.Region < 0 ? -input.Length : input.Length, core, integralMask); |
| | | 618 | | } |
| | 38 | 619 | | Fixed64 halfThickness = input.FullHeight == Fixed64.Zero ? input.HalfThickness : input.FullHeight * Fixed64.Half |
| | 38 | 620 | | local.Y = best.Cap < 0 ? -halfThickness : halfThickness; |
| | 38 | 621 | | if ((input.FullHeight.m_rawValue & 1L) != 0) |
| | 2 | 622 | | exactTerm = new(exactTerm.X, FixedPointAnchorTerm3d.GetResidual(Fixed64.One, |
| | 2 | 623 | | best.Cap < 0 ? -input.FullHeight : input.FullHeight, Fixed64.Zero, Fixed64.Zero, |
| | 2 | 624 | | core.Y, local.Y), exactTerm.Z); |
| | 38 | 625 | | var basis = new WideRationalBasis3d(input.SlabRotation); |
| | 38 | 626 | | CircularRimContactAlgebra.WriteGradient(CircularRimContactAlgebra.Transform(basis, best.First), |
| | 38 | 627 | | CircularRimContactAlgebra.Transform(basis, best.Second), gradient, gradientSigns); |
| | 38 | 628 | | Vector3d normal = ConvexContactValueRoot.GetNormalizedDirection(ref root, gradient, gradientSigns, 1); |
| | 76 | 629 | | Fixed64 depth = default; bool clamped = false; |
| | 38 | 630 | | if (!best.IsZero) |
| | | 631 | | { |
| | 20 | 632 | | FiniteAxisValueRoot value = RestoreObliqueValue(bestValues, bestSigns, bestCell, best); |
| | 20 | 633 | | ConvexContactValueRoot.GetRoundedDepth(input.Scale, valueShift, ref value, out depth, out clamped); |
| | | 634 | | } |
| | 38 | 635 | | return new(new FixedPointAnchor(input.Origin, input.Rotation, point), |
| | 38 | 636 | | new FixedPointAnchor(input.Center, input.SlabRotation, core, local, exactTerm), normal, depth, clamped); |
| | | 637 | | } |
| | | 638 | | } |