| | | 1 | | //======================================================================= |
| | | 2 | | // CapsulePlaneRayEvents.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | |
| | | 10 | | namespace FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | /// <summary>Exact finite capsule-plane admission and boundary-ray critical events.</summary> |
| | | 13 | | internal static class CapsulePlaneRayEvents |
| | | 14 | | { |
| | | 15 | | private const int Words = CapsulePlaneRaySelection.FieldWords; |
| | | 16 | | private static ContactQuadratic Field(Span<ulong> values, Span<int> signs, int index, int words = Words) => |
| | 38139 | 17 | | ContactQuadratic.At(values, signs, index, words); |
| | | 18 | | |
| | | 19 | | internal static bool VisitEvents(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 20 | | int orientation, CapsulePlaneRayEventVisitor visitor) |
| | | 21 | | { |
| | 116 | 22 | | if (visitor == null) throw new ArgumentNullException(nameof(visitor)); |
| | | 23 | | // Visitor dispatch never retains a global maximum; the caller owns |
| | | 24 | | // finite-region ranking, so no unused coefficient banks are needed. |
| | 114 | 25 | | CapsulePlaneRaySelection maximum = default; |
| | 114 | 26 | | return Dispatch(source, frame, orientation, ref maximum, visitor, false, default, default); |
| | | 27 | | } |
| | | 28 | | |
| | | 29 | | internal static bool Accumulate(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 30 | | int orientation, ref CapsulePlaneRaySelection maximum) => |
| | 50 | 31 | | Dispatch(source, frame, orientation, ref maximum, null, false, default, default); |
| | | 32 | | |
| | | 33 | | internal static bool VisitEvents(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 34 | | int orientation, in RationalSegmentInterval interval, CapsulePlaneRayEventVisitor visitor) |
| | | 35 | | { |
| | 14 | 36 | | if (visitor == null) throw new ArgumentNullException(nameof(visitor)); |
| | 12 | 37 | | CapsulePlaneRaySelection maximum = default; |
| | 12 | 38 | | return Dispatch(source, frame, orientation, ref maximum, visitor, false, default, interval); |
| | | 39 | | } |
| | | 40 | | |
| | | 41 | | internal static bool Reconstruct(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 42 | | CapsulePlaneRayEvent descriptor, int orientation, ref CapsulePlaneRaySelection selection) => |
| | 628 | 43 | | Dispatch(source, frame, orientation, ref selection, null, true, descriptor, default); |
| | | 44 | | |
| | | 45 | | internal static bool Reconstruct(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 46 | | CapsulePlaneRayEvent descriptor, int orientation, in RationalSegmentInterval interval, |
| | | 47 | | ref CapsulePlaneRaySelection selection) => |
| | 54 | 48 | | Dispatch(source, frame, orientation, ref selection, null, true, descriptor, interval); |
| | | 49 | | |
| | | 50 | | private static bool Dispatch(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 51 | | int orientation, ref CapsulePlaneRaySelection maximum, CapsulePlaneRayEventVisitor? visitor, |
| | | 52 | | bool targeted, CapsulePlaneRayEvent target, in RationalSegmentInterval interval) |
| | | 53 | | { |
| | 861 | 54 | | if (orientation is not (1 or -1)) throw new ArgumentOutOfRangeException(nameof(orientation)); |
| | 855 | 55 | | if (interval.IsClipped && !source.IsSegment) |
| | 1 | 56 | | throw new ArgumentException("Only finite segment sources can have a clipped interval.", nameof(source)); |
| | 855 | 57 | | if (frame.NormalSquared.IsZero) return false; |
| | | 58 | | // A retained non-projection event has no support/endpoint work to replay. |
| | 853 | 59 | | if (targeted && target.Kind is not (CapsulePlaneRayEventKind.Support or CapsulePlaneRayEventKind.Endpoint)) |
| | 608 | 60 | | return source.IsSegment |
| | 608 | 61 | | ? AccumulateSegment(source, frame, orientation, ref maximum, visitor, true, target, interval) |
| | 608 | 62 | | : AccumulateIntrinsic(source, frame, orientation, ref maximum, visitor, true, target); |
| | 245 | 63 | | int words = GetProducerWords(frame, CapsulePlaneRayEventKind.Support); |
| | 246 | 64 | | if (interval.HasWideBounds) words = Math.Max(words, 19); |
| | 490 | 65 | | Span<ulong> values = stackalloc ulong[12 * words]; Span<int> signs = stackalloc int[12]; |
| | 735 | 66 | | ContactQuadratic x = Field(values, signs, 0, words), y = Field(values, signs, 1, words), z = Field(values, signs |
| | 735 | 67 | | ContactQuadratic d = Field(values, signs, 3, words), gap = Field(values, signs, 4, words), term = Field(values, |
| | 245 | 68 | | bool found = false; |
| | 1418 | 69 | | for (int endpoint = 0; endpoint < (source.IsSegment || frame.Finite.Length != Fixed64.Zero ? 2 : 1); endpoint++) |
| | | 70 | | { |
| | 464 | 71 | | var descriptor = new CapsulePlaneRayEvent(source.IsSegment ? CapsulePlaneRayEventKind.Endpoint |
| | 464 | 72 | | : CapsulePlaneRayEventKind.Support, endpoint); |
| | 464 | 73 | | if (targeted && !target.Matches(descriptor)) continue; |
| | 396 | 74 | | if (source.IsSegment) |
| | | 75 | | { |
| | 168 | 76 | | if (interval.IsClipped) SetClippedEndpoint(source, frame.Finite, interval, endpoint, x, y, z, d); |
| | | 77 | | else |
| | | 78 | | { |
| | 108 | 79 | | WideAxis3 point = frame.Finite.Transform(endpoint == 0 ? source.A : source.B); |
| | 216 | 80 | | x.Set(Signed832.ExtendValue(point.X)); y.Set(Signed832.ExtendValue(point.Y)); |
| | 216 | 81 | | z.Set(Signed832.ExtendValue(point.Z)); d.Set(Integer(1)); |
| | | 82 | | } |
| | | 83 | | } |
| | | 84 | | else |
| | | 85 | | { |
| | | 86 | | // Project the endpoint center, not its rounded radial support. |
| | | 87 | | // The radial normal component cancels under plane projection. |
| | 258 | 88 | | int endpointSign = endpoint == 0 ? -1 : 1; |
| | 258 | 89 | | if (!targeted && frame.Finite.Length != Fixed64.Zero && !frame.Normal.Y.IsZero |
| | 258 | 90 | | && endpointSign != orientation * frame.Normal.Y.Sign) continue; |
| | 177 | 91 | | Signed320 cap = endpointSign < 0 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) : frame.Finite.Sha |
| | 177 | 92 | | gap.Set(frame.PlaneConstant); |
| | 177 | 93 | | term.Set(Signed832.ExtendValue(cap)); |
| | 354 | 94 | | ContactQuadratic.Scale(term, Signed832.ExtendValue(frame.Normal.Y), y); gap.Add(y, -1); |
| | 177 | 95 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.X), x); |
| | 177 | 96 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Y), y); |
| | 531 | 97 | | term.Set(frame.NormalSquared); ContactQuadratic.Scale(term, Signed832.ExtendValue(cap), z); y.Add(z); |
| | 354 | 98 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Z), z); d.Set(frame.NormalSquared); |
| | | 99 | | } |
| | 315 | 100 | | found |= EmitPoint(source, frame, descriptor, orientation, x, y, z, d, ReadOnlySpan<ulong>.Empty, |
| | 315 | 101 | | ref maximum, visitor); |
| | | 102 | | } |
| | 319 | 103 | | if (targeted) return found; |
| | 171 | 104 | | if (source.IsSegment) |
| | 60 | 105 | | found |= AccumulateSegment(source, frame, orientation, ref maximum, visitor, targeted, target, interval); |
| | | 106 | | else |
| | 111 | 107 | | found |= AccumulateIntrinsic(source, frame, orientation, ref maximum, visitor, targeted, target); |
| | 171 | 108 | | return found; |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | private static void SetClippedEndpoint(in CapsulePlaneRayEventSource source, in CapsuleFiniteSectionFrame finite, |
| | | 112 | | in RationalSegmentInterval interval, int endpoint, ContactQuadratic x, ContactQuadratic y, |
| | | 113 | | ContactQuadratic z, ContactQuadratic denominator) |
| | | 114 | | { |
| | 60 | 115 | | WideAxis3 start = finite.Transform(source.A), end = finite.Transform(source.B); |
| | 30 | 116 | | Signed832 n = endpoint == 0 ? interval.WideLowerNumerator : interval.WideUpperNumerator; |
| | 30 | 117 | | Signed832 d = endpoint == 0 ? interval.WideLowerDenominator : interval.WideUpperDenominator; |
| | 30 | 118 | | int words = x.FieldWords; |
| | 60 | 119 | | Span<ulong> values = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 60 | 120 | | ContactQuadratic parameter = Field(values, signs, 0, words), term = Field(values, signs, 1, words); |
| | 60 | 121 | | parameter.Set(n); denominator.Set(d); |
| | 240 | 122 | | for (int axis = 0; axis < 3; axis++) |
| | | 123 | | { |
| | 90 | 124 | | ContactQuadratic coordinate = axis == 0 ? x : axis == 1 ? y : z; |
| | 90 | 125 | | Signed320 first = axis == 0 ? start.X : axis == 1 ? start.Y : start.Z; |
| | 90 | 126 | | Signed320 last = axis == 0 ? end.X : axis == 1 ? end.Y : end.Z; |
| | 90 | 127 | | ContactQuadratic.Scale(denominator, Signed576.ExtendValue(first), coordinate); |
| | 90 | 128 | | ContactQuadratic.Scale(parameter, Signed576.ExtendValue(WideArithmetic.SubtractSigned320(last, first)), term |
| | 90 | 129 | | coordinate.Add(term); |
| | | 130 | | } |
| | 30 | 131 | | } |
| | | 132 | | private static bool AccumulateIntrinsic(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 133 | | int orientation, ref CapsulePlaneRaySelection maximum, CapsulePlaneRayEventVisitor? visitor, |
| | | 134 | | bool targeted, CapsulePlaneRayEvent target) |
| | | 135 | | { |
| | 557 | 136 | | if (frame.Finite.Length == Fixed64.Zero) |
| | 69 | 137 | | return AccumulateCircle(source, frame, 2, default, false, orientation, ref maximum, visitor, targeted, targe |
| | 488 | 138 | | if (targeted && target.Kind == CapsulePlaneRayEventKind.Circle) |
| | | 139 | | { |
| | 260 | 140 | | if (!frame.Normal.X.IsZero || !frame.Normal.Z.IsZero) return false; |
| | | 141 | | // Axial planes in the closed core interval own the body circle, |
| | | 142 | | // including seams. Cap-circle descriptors there remain stale even |
| | | 143 | | // though their endpoint sphere intersects the same geometric circle. |
| | 228 | 144 | | Signed832 extent = Product(frame.Normal.Y, frame.Finite.ShapeFrame.Cap); |
| | 332 | 145 | | if (extent.Sign < 0) extent = WideArithmetic.SubtractSigned832(default, extent); |
| | 228 | 146 | | if (WideArithmetic.AddSigned832(frame.PlaneConstant, extent).Sign >= 0 |
| | 228 | 147 | | && WideArithmetic.SubtractSigned832(frame.PlaneConstant, extent).Sign <= 0) |
| | 147 | 148 | | return AccumulateCircle(source, frame, 0, default, true, orientation, ref maximum, visitor, true, target |
| | 106 | 149 | | if (target.Feature is not (1 or 2)) return false; |
| | 56 | 150 | | Signed320 cap = target.Feature == 1 |
| | 56 | 151 | | ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) : frame.Finite.ShapeFrame.Cap; |
| | 56 | 152 | | return AccumulateCircle(source, frame, target.Feature, cap, false, orientation, ref maximum, visitor, true, |
| | | 153 | | } |
| | 244 | 154 | | int words = GetProducerWords(frame, CapsulePlaneRayEventKind.Sagittal); |
| | 488 | 155 | | Span<ulong> values = stackalloc ulong[28 * words]; Span<int> signs = stackalloc int[28]; |
| | 244 | 156 | | Span<ulong> root = stackalloc ulong[frame.RootWords]; |
| | 976 | 157 | | ContactQuadratic x = Field(values, signs, 0, words), y = Field(values, signs, 1, words), z = Field(values, signs |
| | 732 | 158 | | ContactQuadratic radial = Field(values, signs, 4, words), metric = Field(values, signs, 5, words), r2 = Field(va |
| | 732 | 159 | | ContactQuadratic gap = Field(values, signs, 7, words), radicand = Field(values, signs, 8, words), term = Field(v |
| | 488 | 160 | | ContactQuadratic work = Field(values, signs, 10, words), coefficient = Field(values, signs, 11, words); |
| | 488 | 161 | | ContactQuadratic radx = Field(values, signs, 12, words), rady = Field(values, signs, 13, words); |
| | 244 | 162 | | Signed320 radius = GetRadius(frame); |
| | 244 | 163 | | radial.Set(WideArithmetic.AddSigned832(Product(frame.Normal.X, frame.Normal.X), |
| | 244 | 164 | | Product(frame.Normal.Z, frame.Normal.Z))); |
| | 488 | 165 | | metric.Set(frame.NormalSquared); r2.Set(Product(radius, radius)); |
| | 244 | 166 | | bool found = false; |
| | 244 | 167 | | if (radial.Signs[0] == 0) |
| | | 168 | | { |
| | | 169 | | // A plane perpendicular to the core cuts a full body circle even |
| | | 170 | | // when it is strictly between the two body/cap seams. |
| | 50 | 171 | | gap.Set(frame.PlaneConstant); |
| | 100 | 172 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Y), y); metric.CopyTo(d); |
| | 50 | 173 | | if (ContainsPrimitivePoint(frame, y, d, ReadOnlySpan<ulong>.Empty, 0)) |
| | 33 | 174 | | return AccumulateCircle(source, frame, 0, default, true, orientation, ref maximum, visitor, |
| | 33 | 175 | | targeted, target); |
| | 102 | 176 | | for (int endpoint = 0; endpoint < 2; endpoint++) |
| | | 177 | | { |
| | 34 | 178 | | Signed320 cap = endpoint == 0 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) : frame.Finite.ShapeF |
| | 34 | 179 | | found |= AccumulateCircle(source, frame, endpoint + 1, cap, false, orientation, ref maximum, visitor, |
| | 34 | 180 | | targeted, target); |
| | | 181 | | } |
| | 17 | 182 | | return found; |
| | | 183 | | } |
| | 194 | 184 | | if (!frame.Normal.Y.IsZero) |
| | | 185 | | { |
| | | 186 | | // The sagittal diameter contains the ray extrema. Its transverse |
| | | 187 | | // diameter supplies independent finite coverage on the same ellipse. |
| | 720 | 188 | | for (int kind = 0; kind < 2; kind++) |
| | 1440 | 189 | | for (int branch = -1; branch <= 1; branch += 2) |
| | | 190 | | { |
| | 480 | 191 | | var descriptor = new CapsulePlaneRayEvent(CapsulePlaneRayEventKind.Sagittal, kind == 0 ? 0 : 3, branch); |
| | 480 | 192 | | if (targeted && !target.Matches(descriptor)) continue; |
| | 153 | 193 | | CopyRadicand(radial, root); |
| | 153 | 194 | | ContactQuadratic.Scale(radial, Signed832.ExtendValue(frame.Normal.Y), d); |
| | 153 | 195 | | ContactQuadratic.Scale(radial, frame.PlaneConstant, y); |
| | 153 | 196 | | coefficient.Set(Signed832.ExtendValue(radius)); |
| | 153 | 197 | | ContactQuadratic.Scale(coefficient, Signed832.ExtendValue(kind == 0 ? frame.Normal.X : frame.Normal.Z), |
| | 231 | 198 | | if (kind != 0) term.MultiplySign(-1); |
| | 306 | 199 | | ContactQuadratic.Scale(term, Signed832.ExtendValue(frame.Normal.Y), work); SetRadical(x, work, branch); |
| | 153 | 200 | | ContactQuadratic.Scale(coefficient, Signed832.ExtendValue(kind == 0 ? frame.Normal.Z : frame.Normal.X), |
| | 306 | 201 | | ContactQuadratic.Scale(term, Signed832.ExtendValue(frame.Normal.Y), work); SetRadical(z, work, branch); |
| | 153 | 202 | | if (kind == 0) |
| | | 203 | | { |
| | 75 | 204 | | ContactQuadratic.Scale(radial, Signed832.ExtendValue(radius), work); |
| | 150 | 205 | | SetRadical(term, work, -branch); y.Add(term); |
| | | 206 | | } |
| | 185 | 207 | | if (d.Signs[0] < 0) { x.MultiplySign(-1); y.MultiplySign(-1); z.MultiplySign(-1); d.MultiplySign(-1); } |
| | 153 | 208 | | if (ContainsPrimitivePoint(frame, y, d, root, 0)) |
| | 89 | 209 | | found |= EmitPoint(source, frame, descriptor, orientation, x, y, z, d, root, ref maximum, visitor, a |
| | | 210 | | } |
| | | 211 | | } |
| | 1164 | 212 | | for (int endpoint = 0; endpoint < 2; endpoint++) |
| | | 213 | | { |
| | 388 | 214 | | Signed320 cap = endpoint == 0 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) : frame.Finite.ShapeFrame |
| | 388 | 215 | | int primitive = endpoint + 1; |
| | 388 | 216 | | gap.Set(WideArithmetic.SubtractSigned832(frame.PlaneConstant, Product(frame.Normal.Y, cap))); |
| | 3104 | 217 | | for (int kind = 0; kind < 3; kind++) |
| | | 218 | | { |
| | 1164 | 219 | | var eventKind = kind == 1 ? CapsulePlaneRayEventKind.Seam : CapsulePlaneRayEventKind.Sagittal; |
| | 1164 | 220 | | int feature = kind == 2 ? primitive + 3 : primitive; |
| | 1164 | 221 | | if (targeted && (target.Kind != eventKind || target.Feature != feature)) continue; |
| | 370 | 222 | | ContactQuadratic.Multiply(r2, kind == 1 ? radial : metric, ReadOnlySpan<ulong>.Empty, radicand); |
| | 740 | 223 | | ContactQuadratic.Multiply(gap, gap, ReadOnlySpan<ulong>.Empty, term); radicand.Add(term, -1); |
| | 370 | 224 | | if (radicand.Signs[0] < 0) continue; |
| | 268 | 225 | | if (kind != 1) |
| | | 226 | | { |
| | 208 | 227 | | ContactQuadratic.Multiply(radicand, radial, ReadOnlySpan<ulong>.Empty, work); |
| | 314 | 228 | | if (kind == 2) ContactQuadratic.Multiply(work, metric, ReadOnlySpan<ulong>.Empty, radicand); |
| | 102 | 229 | | else work.CopyTo(radicand); |
| | 208 | 230 | | ContactQuadratic.Multiply(metric, radial, ReadOnlySpan<ulong>.Empty, d); |
| | 208 | 231 | | if (kind == 0) |
| | | 232 | | { |
| | | 233 | | // The cap section's sagittal tangent is (-Ny*Nx,T,-Ny*Nz). |
| | 102 | 234 | | coefficient.Set(Signed832.ExtendValue(frame.Normal.Y)); |
| | 204 | 235 | | ContactQuadratic.Scale(coefficient, Signed832.ExtendValue(frame.Normal.X), radx); radx.MultiplyS |
| | 102 | 236 | | radial.CopyTo(rady); |
| | | 237 | | } |
| | | 238 | | else |
| | | 239 | | { |
| | | 240 | | // Its transverse tangent (-Nz,0,Nx) completes finite |
| | | 241 | | // coverage even when the circle lies wholly in one cap. |
| | 318 | 242 | | radx.Set(Signed832.ExtendValue(frame.Normal.Z)); radx.MultiplySign(-1); rady.Clear(); |
| | | 243 | | } |
| | | 244 | | } |
| | 180 | 245 | | else { radial.CopyTo(d); radx.Set(Signed832.ExtendValue(frame.Normal.Z)); rady.Clear(); } |
| | 268 | 246 | | CopyRadicand(radicand, root); |
| | 1608 | 247 | | for (int branch = -1; branch <= 1; branch += 2) |
| | | 248 | | { |
| | 536 | 249 | | var descriptor = new CapsulePlaneRayEvent(eventKind, feature, branch); |
| | 536 | 250 | | if (targeted && !target.Matches(descriptor)) continue; |
| | 412 | 251 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.X), x); |
| | 412 | 252 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Z), z); |
| | 412 | 253 | | if (kind != 1) |
| | | 254 | | { |
| | 648 | 255 | | ContactQuadratic.Multiply(x, radial, ReadOnlySpan<ulong>.Empty, work); work.CopyTo(x); |
| | 648 | 256 | | ContactQuadratic.Multiply(z, radial, ReadOnlySpan<ulong>.Empty, work); work.CopyTo(z); |
| | 324 | 257 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Y), y); |
| | 648 | 258 | | ContactQuadratic.Scale(metric, Signed832.ExtendValue(cap), term); y.Add(term); |
| | 648 | 259 | | ContactQuadratic.Multiply(y, radial, ReadOnlySpan<ulong>.Empty, work); work.CopyTo(y); |
| | 648 | 260 | | SetRadical(term, rady, branch); y.Add(term); |
| | 324 | 261 | | if (kind == 0) |
| | | 262 | | { |
| | 160 | 263 | | coefficient.Set(Signed832.ExtendValue(frame.Normal.Y)); |
| | 320 | 264 | | ContactQuadratic.Scale(coefficient, Signed832.ExtendValue(frame.Normal.Z), work); work.Multi |
| | | 265 | | } |
| | 164 | 266 | | else work.Set(Signed832.ExtendValue(frame.Normal.X)); |
| | | 267 | | } |
| | | 268 | | else |
| | | 269 | | { |
| | 88 | 270 | | ContactQuadratic.Scale(radial, Signed832.ExtendValue(cap), y); |
| | 176 | 271 | | work.Set(Signed832.ExtendValue(frame.Normal.X)); work.MultiplySign(-1); |
| | | 272 | | } |
| | 1648 | 273 | | SetRadical(term, radx, branch); x.Add(term); SetRadical(term, work, branch); z.Add(term); |
| | 412 | 274 | | if (kind == 1 || ContainsPrimitivePoint(frame, y, d, root, primitive)) |
| | 308 | 275 | | found |= EmitPoint(source, frame, descriptor, orientation, x, y, z, d, root, ref maximum, visito |
| | | 276 | | } |
| | | 277 | | } |
| | | 278 | | } |
| | | 279 | | // N<264, C/gap<465, cap/radius<198 bits. Diameter/seam q fields |
| | | 280 | | // are below 1264 bits, D below 1060, radicands below 1992; the existing |
| | | 281 | | // circular-family 2256-bit bound still owns the widest point root. |
| | 194 | 282 | | return found; |
| | | 283 | | } |
| | | 284 | | |
| | | 285 | | private static bool AccumulateCircle(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 286 | | int primitive, Signed320 cap, bool body, int orientation, ref CapsulePlaneRaySelection maximum, |
| | | 287 | | CapsulePlaneRayEventVisitor? visitor, bool targeted, CapsulePlaneRayEvent target) |
| | | 288 | | { |
| | 388 | 289 | | if (targeted && (target.Kind != CapsulePlaneRayEventKind.Circle || target.Feature != primitive)) return false; |
| | 290 | 290 | | int words = GetProducerWords(frame, CapsulePlaneRayEventKind.Circle); |
| | 580 | 291 | | Span<ulong> values = stackalloc ulong[28 * words]; Span<int> signs = stackalloc int[28]; |
| | 290 | 292 | | Span<ulong> root = stackalloc ulong[frame.RootWords]; |
| | 1160 | 293 | | ContactQuadratic x = Field(values, signs, 0, words), y = Field(values, signs, 1, words), z = Field(values, signs |
| | 870 | 294 | | ContactQuadratic gap = Field(values, signs, 4, words), section = Field(values, signs, 5, words), metric = Field( |
| | 870 | 295 | | ContactQuadratic v2 = Field(values, signs, 7, words), radicand = Field(values, signs, 8, words), term = Field(va |
| | 870 | 296 | | ContactQuadratic vx = Field(values, signs, 10, words), vy = Field(values, signs, 11, words), vz = Field(values, |
| | 290 | 297 | | ContactQuadratic work = Field(values, signs, 13, words); |
| | 290 | 298 | | Signed320 radius = GetRadius(frame); |
| | 580 | 299 | | metric.Set(frame.NormalSquared); section.Set(Product(radius, radius)); |
| | 580 | 300 | | ContactQuadratic.Multiply(section, metric, ReadOnlySpan<ulong>.Empty, work); work.CopyTo(section); |
| | 290 | 301 | | gap.Set(WideArithmetic.SubtractSigned832(frame.PlaneConstant, Product(frame.Normal.Y, cap))); |
| | 582 | 302 | | if (!body) { ContactQuadratic.Multiply(gap, gap, ReadOnlySpan<ulong>.Empty, term); section.Add(term, -1); } |
| | 326 | 303 | | if (section.Signs[0] < 0) return false; |
| | | 304 | | // Author the family basis in the mesh plane. Capsule yaw (and a |
| | | 305 | | // zero-core sphere's arbitrary axis) must not change finite samples. |
| | 254 | 306 | | WideAxis3 seed = frame.AuthoredNormal.X.IsZero && frame.AuthoredNormal.Z.IsZero |
| | 254 | 307 | | ? new(Signed320.ExtendValue(Signed192.Signed(1)), default, default) |
| | 254 | 308 | | : new(default, Signed320.ExtendValue(Signed192.Signed(1)), default); |
| | 254 | 309 | | WideDirection3d first = WideDirection3d.Cross(new WideDirection3d(frame.AuthoredNormal), new WideDirection3d(see |
| | 254 | 310 | | bool found = false; |
| | 1524 | 311 | | for (int tangent = 0; tangent < 2; tangent++) |
| | | 312 | | { |
| | 508 | 313 | | if (targeted && tangent != target.Branch / 2) continue; |
| | 321 | 314 | | WideDirection3d authored = tangent == 0 ? first : WideDirection3d.Cross(new WideDirection3d(frame.AuthoredNo |
| | 321 | 315 | | WideDirection3d transformed = authored.Transform(frame.Finite.ShapeFrame.Basis); |
| | 1284 | 316 | | vx.Set(transformed.X); vy.Set(transformed.Y); vz.Set(transformed.Z); Squared(vx, vy, vz, v2, term); |
| | 321 | 317 | | ContactQuadratic.Multiply(section, metric, ReadOnlySpan<ulong>.Empty, work); |
| | 642 | 318 | | ContactQuadratic.Multiply(work, v2, ReadOnlySpan<ulong>.Empty, radicand); CopyRadicand(radicand, root); |
| | 321 | 319 | | ContactQuadratic.Multiply(metric, v2, ReadOnlySpan<ulong>.Empty, d); |
| | 1926 | 320 | | for (int branch = -1; branch <= 1; branch += 2) |
| | | 321 | | { |
| | 642 | 322 | | var descriptor = new CapsulePlaneRayEvent(CapsulePlaneRayEventKind.Circle, |
| | 642 | 323 | | primitive, 2 * tangent + (branch > 0 ? 1 : 0)); |
| | 642 | 324 | | if (targeted && !target.Matches(descriptor)) continue; |
| | 455 | 325 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.X), work); |
| | 455 | 326 | | ContactQuadratic.Multiply(work, v2, ReadOnlySpan<ulong>.Empty, x); |
| | 455 | 327 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Y), work); |
| | 910 | 328 | | ContactQuadratic.Scale(metric, Signed832.ExtendValue(cap), term); work.Add(term); |
| | 455 | 329 | | ContactQuadratic.Multiply(work, v2, ReadOnlySpan<ulong>.Empty, y); |
| | 455 | 330 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Z), work); |
| | 455 | 331 | | ContactQuadratic.Multiply(work, v2, ReadOnlySpan<ulong>.Empty, z); |
| | 1820 | 332 | | SetRadical(term, vx, branch); x.Add(term); SetRadical(term, vy, branch); y.Add(term); |
| | 910 | 333 | | SetRadical(term, vz, branch); z.Add(term); |
| | 455 | 334 | | if (body || ContainsPrimitivePoint(frame, y, d, root, primitive)) |
| | 447 | 335 | | found |= EmitPoint(source, frame, descriptor, orientation, x, y, z, d, root, ref maximum, visitor, a |
| | | 336 | | } |
| | | 337 | | } |
| | | 338 | | // Authored tangents <262 bits transform to <395-bit components. |
| | | 339 | | // q fields <1523, denominators <1322, radicands <2256 bits; |
| | | 340 | | // the opposite ray norm remains <9444 bits (160 limbs). |
| | 254 | 341 | | return found; |
| | | 342 | | } |
| | | 343 | | |
| | | 344 | | private static void SetRadical(ContactQuadratic target, ContactQuadratic coefficient, int multiplier) |
| | | 345 | | { |
| | 8682 | 346 | | target.Clear(); coefficient.Rational.CopyTo(target.Radical); target.Signs[1] = coefficient.Signs[0] * multiplier |
| | 2894 | 347 | | } |
| | | 348 | | |
| | | 349 | | private static void CopyRadicand(ContactQuadratic value, Span<ulong> root) |
| | | 350 | | { |
| | 1770 | 351 | | root.Clear(); value.Rational[..WideArithmetic.GetActiveMagnitudeLength(value.Rational)].CopyTo(root); |
| | 885 | 352 | | } |
| | | 353 | | |
| | | 354 | | private static bool ContainsPrimitivePoint(in CapsulePlaneRayFrame frame, ContactQuadratic y, |
| | | 355 | | ContactQuadratic d, ReadOnlySpan<ulong> root, int primitive) |
| | | 356 | | { |
| | 1377 | 357 | | if (frame.Finite.Length == Fixed64.Zero) return true; |
| | 2102 | 358 | | Span<ulong> values = stackalloc ulong[4 * Words]; Span<int> signs = stackalloc int[4]; |
| | 2102 | 359 | | ContactQuadratic extent = Field(values, signs, 0), query = Field(values, signs, 1); |
| | 1051 | 360 | | ContactQuadratic.Scale(d, Signed832.ExtendValue(frame.Finite.ShapeFrame.Cap), extent); |
| | 3153 | 361 | | y.CopyTo(query); query.Add(extent, -1); int upper = query.Sign(root); |
| | 3153 | 362 | | y.CopyTo(query); query.Add(extent); int lower = query.Sign(root); |
| | 1051 | 363 | | return primitive == 0 ? upper <= 0 && lower >= 0 : primitive == 1 ? lower <= 0 : upper >= 0; |
| | | 364 | | } |
| | | 365 | | |
| | | 366 | | private static bool AccumulateSegment(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 367 | | int orientation, ref CapsulePlaneRaySelection maximum, CapsulePlaneRayEventVisitor? visitor, |
| | | 368 | | bool targeted, CapsulePlaneRayEvent target, in RationalSegmentInterval interval) |
| | | 369 | | { |
| | 223 | 370 | | if (targeted && target.Kind is not (CapsulePlaneRayEventKind.Section or CapsulePlaneRayEventKind.Stationary)) re |
| | 442 | 371 | | WideAxis3 p = frame.Finite.Transform(source.A), end = frame.Finite.Transform(source.B); |
| | 221 | 372 | | return AccumulateSegment(source, frame, p, end, orientation, ref maximum, visitor, targeted, target, interval); |
| | | 373 | | } |
| | | 374 | | |
| | | 375 | | /// <summary>Visits an exact pretransformed segment without rounding native source coordinates.</summary> |
| | | 376 | | internal static bool VisitLocalSegmentEvents(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 377 | | in WideAxis3 start, in WideAxis3 end, int orientation, in RationalSegmentInterval interval, |
| | | 378 | | CapsulePlaneRayEventVisitor visitor) |
| | | 379 | | { |
| | 19 | 380 | | CapsulePlaneRaySelection maximum = default; |
| | | 381 | | const int words = 32; |
| | 38 | 382 | | Span<ulong> values = stackalloc ulong[12 * words]; Span<int> signs = stackalloc int[12]; |
| | 57 | 383 | | ContactQuadratic x = Field(values, signs, 0, words), y = Field(values, signs, 1, words), z = Field(values, signs |
| | 57 | 384 | | ContactQuadratic d = Field(values, signs, 3, words), parameter = Field(values, signs, 4, words), term = Field(va |
| | 19 | 385 | | bool found = false; |
| | 114 | 386 | | for (int endpoint = 0; endpoint < 2; endpoint++) |
| | | 387 | | { |
| | 38 | 388 | | Signed832 n = interval.IsClipped ? endpoint == 0 ? interval.WideLowerNumerator : interval.WideUpperNumerator |
| | 38 | 389 | | : Signed832.ExtendValue(Signed192.Signed(endpoint)); |
| | 38 | 390 | | Signed832 denominator = interval.IsClipped ? endpoint == 0 ? interval.WideLowerDenominator : interval.WideUp |
| | 38 | 391 | | : Signed832.ExtendValue(Signed192.Signed(1)); |
| | 76 | 392 | | parameter.Set(n); d.Set(denominator); |
| | 304 | 393 | | for (int axis = 0; axis < 3; axis++) |
| | | 394 | | { |
| | 114 | 395 | | Signed320 first = axis == 0 ? start.X : axis == 1 ? start.Y : start.Z; |
| | 114 | 396 | | Signed320 last = axis == 0 ? end.X : axis == 1 ? end.Y : end.Z; |
| | 114 | 397 | | ContactQuadratic coordinate = axis == 0 ? x : axis == 1 ? y : z; |
| | 114 | 398 | | ContactQuadratic.Scale(d, Signed576.ExtendValue(first), coordinate); |
| | 228 | 399 | | ContactQuadratic.Scale(parameter, Signed576.ExtendValue(WideArithmetic.SubtractSigned320(last, first)), |
| | | 400 | | } |
| | 38 | 401 | | found |= EmitPoint(source, frame, new CapsulePlaneRayEvent(CapsulePlaneRayEventKind.Endpoint, endpoint), ori |
| | 38 | 402 | | x, y, z, d, ReadOnlySpan<ulong>.Empty, ref maximum, visitor); |
| | | 403 | | } |
| | 19 | 404 | | return AccumulateSegment(source, frame, start, end, orientation, ref maximum, visitor, false, default, interval) |
| | | 405 | | } |
| | | 406 | | |
| | | 407 | | private static bool AccumulateSegment(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 408 | | in WideAxis3 p, in WideAxis3 end, int orientation, ref CapsulePlaneRaySelection maximum, |
| | | 409 | | CapsulePlaneRayEventVisitor? visitor, bool targeted, CapsulePlaneRayEvent target, in RationalSegmentInterval int |
| | | 410 | | { |
| | 240 | 411 | | WideAxis3 e = new(WideArithmetic.SubtractSigned320(end.X, p.X), WideArithmetic.SubtractSigned320(end.Y, p.Y), |
| | 240 | 412 | | WideArithmetic.SubtractSigned320(end.Z, p.Z)); |
| | | 413 | | // Primitive section roots are already capsule boundary certificates. |
| | | 414 | | // Their plane certificate is shared only by genuinely coplanar sources; |
| | | 415 | | // arbitrary internal segment inputs retain the full point admission. |
| | 240 | 416 | | bool sourceOnPlane = WideArithmetic.SubtractSigned832(WideDirection3d.Dot(frame.Normal, p), frame.PlaneConstant) |
| | 240 | 417 | | && DotVectors(e, frame.Normal).IsZero; |
| | 240 | 418 | | int words = GetSegmentWords(frame, p, end, e); |
| | 480 | 419 | | Span<ulong> values = stackalloc ulong[40 * words]; Span<int> signs = stackalloc int[40]; |
| | 240 | 420 | | Span<ulong> root = stackalloc ulong[frame.RootWords]; |
| | 720 | 421 | | ContactQuadratic a = Field(values, signs, 0, words), b = Field(values, signs, 1, words), c = Field(values, signs |
| | 720 | 422 | | ContactQuadratic m = Field(values, signs, 3, words), n = Field(values, signs, 4, words), A = Field(values, signs |
| | 720 | 423 | | ContactQuadratic qa = Field(values, signs, 6, words), qb = Field(values, signs, 7, words), qc = Field(values, si |
| | 960 | 424 | | ContactQuadratic t = Field(values, signs, 9, words), td = Field(values, signs, 10, words), u = Field(values, sig |
| | 480 | 425 | | ContactQuadratic work = Field(values, signs, 13, words), term = Field(values, signs, 14, words); |
| | 960 | 426 | | ContactQuadratic x = Field(values, signs, 15, words), y = Field(values, signs, 16, words), z = Field(values, sig |
| | 240 | 427 | | ContactQuadratic exitY = Field(values, signs, 19, words); |
| | 240 | 428 | | Span<ulong> candidateValues = stackalloc ulong[frame.SelectionStorageWords]; |
| | 240 | 429 | | Span<int> candidateSigns = stackalloc int[CapsulePlaneRaySelection.SignCount]; |
| | 240 | 430 | | Span<sbyte> candidateValueSigns = stackalloc sbyte[CapsulePlaneRaySelection.ValueSignCount]; |
| | 240 | 431 | | var candidate = new CapsulePlaneRaySelection(candidateValues, candidateSigns, candidateValueSigns, frame.FieldWo |
| | 240 | 432 | | Signed320 radius = GetRadius(frame); |
| | 240 | 433 | | Signed832 radiusSquared = Product(radius, radius); |
| | 240 | 434 | | bool found = false; |
| | 1720 | 435 | | for (int primitive = frame.Finite.Length == Fixed64.Zero ? 2 : 0; primitive < 3; primitive++) |
| | | 436 | | { |
| | 620 | 437 | | if (targeted && primitive != target.Feature) continue; |
| | 370 | 438 | | bool body = primitive == 0; |
| | 370 | 439 | | Signed320 cap = primitive == 1 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) : frame.Finite.ShapeFram |
| | 370 | 440 | | WideAxis3 basePoint = new(p.X, body ? default : WideArithmetic.SubtractSigned320(p.Y, cap), p.Z); |
| | 370 | 441 | | WideAxis3 delta = new(e.X, body ? default : e.Y, e.Z); |
| | 370 | 442 | | WideDirection3d normal = new(frame.Normal.X, body ? default : frame.Normal.Y, frame.Normal.Z); |
| | 740 | 443 | | a.Set(DotVectors(delta, delta)); b.Set(DotVectors(basePoint, delta)); |
| | 370 | 444 | | c.Set(WideArithmetic.SubtractSigned832(DotVectors(basePoint, basePoint), radiusSquared)); |
| | 370 | 445 | | bool wantSection = !targeted || target.Kind == CapsulePlaneRayEventKind.Section; |
| | 370 | 446 | | if (wantSection) |
| | | 447 | | { |
| | 1830 | 448 | | for (int branch = -1; branch <= 1; branch += 2) |
| | | 449 | | { |
| | 610 | 450 | | var descriptor = new CapsulePlaneRayEvent(CapsulePlaneRayEventKind.Section, primitive, branch); |
| | 610 | 451 | | if (targeted && !target.Matches(descriptor)) continue; |
| | 514 | 452 | | if (!ContactQuadratic.TryGetParameter(a, b, c, branch, root, u, ud) |
| | 514 | 453 | | || !IsUnitParameter(u, ud, root, term) |
| | 514 | 454 | | || interval.IsClipped && !interval.Contains(u, ud, root)) continue; |
| | 267 | 455 | | SetAffinePoint(p, e, u, ud, x, y, z, d, term); |
| | 267 | 456 | | if (ContainsPrimitivePoint(frame, y, d, root, primitive)) |
| | 197 | 457 | | found |= EmitPoint(source, frame, descriptor, orientation, x, y, z, d, root, ref maximum, visito |
| | 197 | 458 | | admitted: sourceOnPlane); |
| | | 459 | | } |
| | | 460 | | } |
| | 370 | 461 | | if (a.Signs[0] == 0 || targeted && target.Kind != CapsulePlaneRayEventKind.Stationary) continue; |
| | | 462 | | // Eliminate u from G(u,t)=0 and G_u(u,t)=0. A zero |
| | | 463 | | // edge coefficient is a constant family covered by its endpoints. |
| | 798 | 464 | | m.Set(DotVectors(delta, normal)); n.Set(DotVectors(basePoint, normal)); A.Set(DotVectors(normal, normal)); |
| | 266 | 465 | | ContactQuadratic.Multiply(a, A, ReadOnlySpan<ulong>.Empty, qa); |
| | 532 | 466 | | ContactQuadratic.Multiply(m, m, ReadOnlySpan<ulong>.Empty, term); qa.Add(term, -1); |
| | 266 | 467 | | ContactQuadratic.Multiply(a, n, ReadOnlySpan<ulong>.Empty, qb); |
| | 532 | 468 | | ContactQuadratic.Multiply(b, m, ReadOnlySpan<ulong>.Empty, term); qb.Add(term, -1); |
| | 266 | 469 | | ContactQuadratic.Multiply(a, c, ReadOnlySpan<ulong>.Empty, qc); |
| | 532 | 470 | | ContactQuadratic.Multiply(b, b, ReadOnlySpan<ulong>.Empty, term); qc.Add(term, -1); |
| | 1596 | 471 | | for (int branch = -1; branch <= 1; branch += 2) |
| | | 472 | | { |
| | 532 | 473 | | if (!ContactQuadratic.TryGetParameter(qa, qb, qc, branch, root, t, td)) continue; |
| | 448 | 474 | | int constructionOrientation = t.Sign(root) < 0 ? -1 : 1; |
| | 448 | 475 | | var descriptor = new CapsulePlaneRayEvent(CapsulePlaneRayEventKind.Stationary, primitive, branch, constr |
| | 448 | 476 | | if (targeted && !target.Matches(descriptor)) continue; |
| | | 477 | | // Untargeted visitation preserves the directional event |
| | | 478 | | // inventory; targeted replay can still evaluate the other ray. |
| | 383 | 479 | | if (visitor != null && constructionOrientation != orientation) continue; |
| | 271 | 480 | | if (m.Signs[0] == 0) |
| | | 481 | | { |
| | | 482 | | // An edge perpendicular to the ray has u=-b/a. Keeping |
| | | 483 | | // the unrelated depth denominator here would needlessly |
| | | 484 | | // square its large common factor on opposite-ray replay. |
| | 470 | 485 | | b.CopyTo(u, -1); a.CopyTo(ud); |
| | | 486 | | } |
| | | 487 | | else |
| | | 488 | | { |
| | 36 | 489 | | ContactQuadratic.Multiply(b, td, root, u); |
| | 108 | 490 | | ContactQuadratic.Multiply(m, t, root, term); u.Add(term); u.MultiplySign(-1); |
| | 36 | 491 | | ContactQuadratic.Multiply(a, td, root, ud); |
| | | 492 | | } |
| | 271 | 493 | | if (!IsUnitParameter(u, ud, root, term) |
| | 271 | 494 | | || interval.IsClipped && !interval.Contains(u, ud, root)) continue; |
| | 208 | 495 | | SetAffinePoint(p, e, u, ud, x, y, z, d, term); |
| | 208 | 496 | | ContactQuadratic.Multiply(y, td, root, exitY); |
| | 208 | 497 | | ContactQuadratic.Multiply(d, t, root, term); |
| | 416 | 498 | | ContactQuadratic.Scale(term, Signed832.ExtendValue(frame.Normal.Y), work); exitY.Add(work); |
| | 208 | 499 | | ContactQuadratic.Multiply(d, td, root, work); |
| | 208 | 500 | | if (!ContainsPrimitivePoint(frame, exitY, work, root, primitive)) continue; |
| | 141 | 501 | | if (constructionOrientation == orientation) |
| | | 502 | | { |
| | 95 | 503 | | if (!ContainsPoint(frame, x, y, z, d, root)) continue; |
| | 86 | 504 | | candidate.HasValue = false; |
| | 172 | 505 | | candidate.CurrentSource = source; candidate.CurrentEvent = descriptor; |
| | 86 | 506 | | t.CopyTo(term, orientation); |
| | 86 | 507 | | candidate.Keep(x, y, z, d, term, td, root, primitive, frame); |
| | 133 | 508 | | if (visitor == null) maximum.KeepEvaluated(candidate, frame); |
| | 39 | 509 | | else visitor(source, frame, descriptor, orientation, candidate); |
| | 86 | 510 | | found = true; |
| | | 511 | | } |
| | 46 | 512 | | else if (targeted) |
| | 21 | 513 | | found |= EmitPoint(source, frame, descriptor, orientation, x, y, z, d, root, ref maximum, visitor); |
| | | 514 | | } |
| | | 515 | | } |
| | 240 | 516 | | return found; |
| | | 517 | | } |
| | | 518 | | |
| | | 519 | | private static bool IsUnitParameter(ContactQuadratic numerator, ContactQuadratic denominator, |
| | | 520 | | ReadOnlySpan<ulong> root, ContactQuadratic query) |
| | | 521 | | { |
| | 658 | 522 | | if (numerator.Sign(root) < 0 || denominator.Sign(root) <= 0) return false; |
| | 1752 | 523 | | denominator.CopyTo(query); query.Add(numerator, -1); return query.Sign(root) >= 0; |
| | | 524 | | } |
| | | 525 | | |
| | | 526 | | private static void SetAffinePoint(WideAxis3 start, WideAxis3 delta, ContactQuadratic u, ContactQuadratic ud, |
| | | 527 | | ContactQuadratic x, ContactQuadratic y, ContactQuadratic z, ContactQuadratic d, ContactQuadratic term) |
| | | 528 | | { |
| | 3800 | 529 | | for (int axis = 0; axis < 3; axis++) |
| | | 530 | | { |
| | 1425 | 531 | | ContactQuadratic point = axis == 0 ? x : axis == 1 ? y : z; |
| | 1425 | 532 | | ContactQuadratic.Scale(ud, Signed832.ExtendValue(axis == 0 ? start.X : axis == 1 ? start.Y : start.Z), point |
| | 2850 | 533 | | ContactQuadratic.Scale(u, Signed832.ExtendValue(axis == 0 ? delta.X : axis == 1 ? delta.Y : delta.Z), term); |
| | | 534 | | } |
| | 475 | 535 | | ud.CopyTo(d); |
| | 475 | 536 | | } |
| | | 537 | | |
| | | 538 | | private static bool EmitPoint(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 539 | | CapsulePlaneRayEvent descriptor, int orientation, scoped ContactQuadratic x, scoped ContactQuadratic y, |
| | | 540 | | scoped ContactQuadratic z, scoped ContactQuadratic d, scoped ReadOnlySpan<ulong> root, |
| | | 541 | | ref CapsulePlaneRaySelection maximum, CapsulePlaneRayEventVisitor? visitor, bool admitted = false) |
| | | 542 | | { |
| | | 543 | | // Normalization only shifts right, so copied input coefficients never |
| | | 544 | | // need more than their current active width. |
| | 1415 | 545 | | int words = Math.Max(9, Math.Max(Math.Max(ContactQuadratic.ActiveWords(x), ContactQuadratic.ActiveWords(y)), |
| | 1415 | 546 | | Math.Max(ContactQuadratic.ActiveWords(z), ContactQuadratic.ActiveWords(d)))); |
| | 2830 | 547 | | Span<ulong> pointValues = stackalloc ulong[8 * words]; Span<int> pointSigns = stackalloc int[8]; |
| | 1415 | 548 | | Span<ulong> pointRoot = stackalloc ulong[frame.RootWords]; |
| | 2830 | 549 | | ContactQuadratic nx = Field(pointValues, pointSigns, 0, words), ny = Field(pointValues, pointSigns, 1, words); |
| | 2830 | 550 | | ContactQuadratic nz = Field(pointValues, pointSigns, 2, words), nd = Field(pointValues, pointSigns, 3, words); |
| | 5660 | 551 | | x.CopyTo(nx); y.CopyTo(ny); z.CopyTo(nz); d.CopyTo(nd); |
| | 2830 | 552 | | root.CopyTo(pointRoot); pointRoot[root.Length..].Clear(); |
| | | 553 | | // Keep source branch-loop coefficients and the output selection apart: |
| | | 554 | | // cap admission can retain an exit before testing the opposite cap. |
| | 1415 | 555 | | WideFiniteAxisIntersection.NormalizeQuadraticPointPowerOfTwo(nx, ny, nz, nd, pointRoot); |
| | 1415 | 556 | | Span<ulong> values = stackalloc ulong[frame.SelectionStorageWords]; |
| | 1415 | 557 | | Span<int> signs = stackalloc int[CapsulePlaneRaySelection.SignCount]; |
| | 1415 | 558 | | Span<sbyte> valueSigns = stackalloc sbyte[CapsulePlaneRaySelection.ValueSignCount]; |
| | 1415 | 559 | | var candidate = new CapsulePlaneRaySelection(values, signs, valueSigns, frame.FieldWords, frame.RootWords, frame |
| | 1415 | 560 | | if (descriptor.Kind == CapsulePlaneRayEventKind.Support |
| | 1415 | 561 | | && (frame.Finite.Length == Fixed64.Zero || frame.Normal.Y.IsZero |
| | 1415 | 562 | | || (descriptor.Feature == 0 ? -1 : 1) == orientation * frame.Normal.Y.Sign)) |
| | | 563 | | { |
| | 177 | 564 | | if (!ContainsPoint(frame, nx, ny, nz, nd, pointRoot)) return false; |
| | | 565 | | // This endpoint is the capsule's support hemisphere. Its exact |
| | | 566 | | // support projects to q, so the first exit is |
| | | 567 | | // (r*sqrt(S)-orientation*(C-Ny*cap))/S. A replay toward the other |
| | | 568 | | // hemisphere retains the general finite body/cap solver below. |
| | 143 | 569 | | Signed320 cap = descriptor.Feature == 0 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) |
| | 143 | 570 | | : frame.Finite.ShapeFrame.Cap; |
| | 143 | 571 | | Signed832 gap = WideArithmetic.SubtractSigned832(frame.PlaneConstant, |
| | 143 | 572 | | Product(frame.Normal.Y, cap)); |
| | 143 | 573 | | Signed320 radius = GetRadius(frame); |
| | 286 | 574 | | ContactQuadratic numerator = candidate.DepthNumerator, divisor = candidate.DepthDenominator; |
| | 143 | 575 | | numerator.Set(orientation > 0 ? WideArithmetic.SubtractSigned832(default, gap) : gap); |
| | 143 | 576 | | divisor.Set(Signed832.ExtendValue(radius)); |
| | 286 | 577 | | divisor.Rational.CopyTo(numerator.Radical); numerator.Signs[1] = divisor.Signs[0]; |
| | 286 | 578 | | divisor.Set(frame.NormalSquared); CopyRadicand(divisor, pointRoot); |
| | 143 | 579 | | candidate.Keep(nx, ny, nz, nd, numerator, divisor, pointRoot, |
| | 143 | 580 | | frame.Finite.Length == Fixed64.Zero ? 2 : frame.Normal.Y.IsZero ? 0 : descriptor.Feature + 1, frame); |
| | | 581 | | } |
| | 1255 | 582 | | else if (admitted && descriptor.Kind == CapsulePlaneRayEventKind.Circle |
| | 1255 | 583 | | && (frame.Finite.Length == Fixed64.Zero || descriptor.Feature == 0)) |
| | | 584 | | { |
| | 718 | 585 | | ContactQuadratic numerator = candidate.DepthNumerator, divisor = candidate.DepthDenominator; |
| | | 586 | | Signed832 depthNumerator; |
| | 359 | 587 | | if (frame.Finite.Length == Fixed64.Zero) |
| | | 588 | | { |
| | | 589 | | // A sphere rim has q.N=C. Its outward upper root is the |
| | | 590 | | // nonnegative part of -2*orientation*C/S, for every family q. |
| | 116 | 591 | | depthNumerator = WideArithmetic.AddSigned832(frame.PlaneConstant, frame.PlaneConstant); |
| | 186 | 592 | | if (orientation > 0) depthNumerator = WideArithmetic.SubtractSigned832(default, depthNumerator); |
| | 140 | 593 | | if (depthNumerator.Sign < 0) depthNumerator = default; |
| | | 594 | | } |
| | | 595 | | else |
| | | 596 | | { |
| | | 597 | | // The axial body family lies on a tube generator. Its first |
| | | 598 | | // exit is the forward closed seam: (|Ny|*Cap-orientation*C)/S. |
| | 243 | 599 | | Signed576 normalY = frame.Normal.Y.Sign < 0 ? WideArithmetic.SubtractSigned576(default, frame.Normal.Y) |
| | 243 | 600 | | Signed832 extent = Product(normalY, frame.Finite.ShapeFrame.Cap); |
| | 243 | 601 | | depthNumerator = orientation > 0 ? WideArithmetic.SubtractSigned832(extent, frame.PlaneConstant) |
| | 243 | 602 | | : WideArithmetic.AddSigned832(extent, frame.PlaneConstant); |
| | | 603 | | } |
| | 718 | 604 | | numerator.Set(depthNumerator); divisor.Set(frame.NormalSquared); |
| | 359 | 605 | | candidate.Keep(nx, ny, nz, nd, numerator, divisor, pointRoot, |
| | 359 | 606 | | frame.Finite.Length == Fixed64.Zero ? 2 : 0, frame); |
| | | 607 | | } |
| | 1019 | 608 | | else if (!AccumulateQuadraticPoint(frame, nx, ny, nz, nd, pointRoot, orientation, ref candidate, admitted)) retu |
| | | 609 | | // A visitor owns finite-region ranking. An unconsumed global maximum |
| | | 610 | | // would compare every event again, including outside-region points. |
| | 1962 | 611 | | if (visitor == null) maximum.KeepEvaluated(candidate, frame); |
| | 588 | 612 | | else visitor(source, frame, descriptor, orientation, candidate); |
| | 1275 | 613 | | return true; |
| | | 614 | | } |
| | | 615 | | |
| | | 616 | | private static Signed832 DotVectors(WideAxis3 first, WideAxis3 second) => |
| | 1110 | 617 | | WideDirection3d.Dot(new WideDirection3d(first), second); |
| | | 618 | | private static Signed832 DotVectors(WideAxis3 first, WideDirection3d second) => |
| | 766 | 619 | | WideDirection3d.Dot(second, first); |
| | | 620 | | private static Signed832 DotVectors(WideDirection3d first, WideDirection3d second) => |
| | 266 | 621 | | WideArithmetic.AddSigned832(WideArithmetic.AddSigned832(Product(first.X, second.X), |
| | 266 | 622 | | Product(first.Y, second.Y)), Product(first.Z, second.Z)); |
| | 4778 | 623 | | private static Signed320 GetRadius(in CapsulePlaneRayFrame frame) => Signed320.NarrowValue( |
| | 4778 | 624 | | WideArithmetic.MultiplySigned320(frame.Finite.ShapeFrame.CoordinateDenominator, frame.Finite.ShapeFrame.Radius)) |
| | | 625 | | private static Signed832 Product(Signed320 first, Signed320 second) => |
| | 1955 | 626 | | WideArithmetic.MultiplySigned576ToSigned832(Signed576.ExtendValue(first), second); |
| | | 627 | | private static Signed832 Product(Signed576 first, Signed320 second) => |
| | 2095 | 628 | | WideArithmetic.MultiplySigned576ToSigned832(first, second); |
| | | 629 | | private static Signed832 Product(Signed576 first, Signed576 second) => |
| | 2406 | 630 | | WideArithmetic.MultiplySigned576ToSigned832(first, second); |
| | 688 | 631 | | private static Signed832 Integer(long value) => Signed832.ExtendValue(Signed320.ExtendValue(Signed192.Signed(value)) |
| | | 632 | | |
| | | 633 | | /// <summary>Accumulates the actual first outward ray through one rational admitted plane point.</summary> |
| | | 634 | | /// <remarks>Point coefficients are rational; quadratic finite-edge points use the retained value-root branch.</rema |
| | | 635 | | internal static bool AccumulateRationalPoint(in CapsulePlaneRayFrame frame, |
| | | 636 | | scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic denomin |
| | | 637 | | int orientation, ref CapsulePlaneRaySelection selection) |
| | 27 | 638 | | => AccumulateQuadraticPoint(frame, x, y, z, denominator, ReadOnlySpan<ulong>.Empty, orientation, ref selection); |
| | | 639 | | |
| | | 640 | | private static bool AccumulateQuadraticPoint(in CapsulePlaneRayFrame frame, |
| | | 641 | | scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic denomin |
| | | 642 | | scoped ReadOnlySpan<ulong> pointRoot, int orientation, ref CapsulePlaneRaySelection selection, bool admitted = f |
| | | 643 | | { |
| | 926 | 644 | | if (orientation is not (1 or -1)) throw new ArgumentOutOfRangeException(nameof(orientation)); |
| | 923 | 645 | | if (frame.NormalSquared.IsZero || denominator.Sign(pointRoot) <= 0) return false; |
| | 917 | 646 | | int words = GetPointRayWords(frame, x, y, z, denominator, pointRoot); |
| | 1834 | 647 | | Span<ulong> values = stackalloc ulong[32 * words]; Span<int> signs = stackalloc int[32]; |
| | 917 | 648 | | Span<ulong> root = stackalloc ulong[frame.RootWords]; |
| | 2751 | 649 | | ContactQuadratic a = Field(values, signs, 0, words), b = Field(values, signs, 1, words), c = Field(values, signs |
| | 2751 | 650 | | ContactQuadratic rx = Field(values, signs, 3, words), ry = Field(values, signs, 4, words), rz = Field(values, si |
| | 2751 | 651 | | ContactQuadratic squaredDenominator = Field(values, signs, 6, words), term = Field(values, signs, 7, words), wor |
| | 1834 | 652 | | ContactQuadratic numerator = Field(values, signs, 9, words), divisor = Field(values, signs, 10, words); |
| | 2751 | 653 | | ContactQuadratic px = Field(values, signs, 11, words), py = Field(values, signs, 12, words), pz = Field(values, |
| | 1834 | 654 | | ContactQuadratic pd = Field(values, signs, 14, words), extent = Field(values, signs, 15, words); |
| | 917 | 655 | | ContactQuadratic.Multiply(denominator, denominator, pointRoot, squaredDenominator); |
| | 917 | 656 | | Signed320 radius = GetRadius(frame); |
| | 917 | 657 | | Signed832 radiusSquared = Product(radius, radius); |
| | 917 | 658 | | if (!admitted && !ContainsQuadraticPoint(frame, x, y, z, denominator, radiusSquared, squaredDenominator, |
| | 1041 | 659 | | rx, ry, rz, c, term, work, pointRoot)) return false; |
| | 793 | 660 | | bool rational = x.Signs[1] == 0 && y.Signs[1] == 0 && z.Signs[1] == 0 && denominator.Signs[1] == 0; |
| | 793 | 661 | | Span<ulong> valueStorage = stackalloc ulong[CapsulePlaneRayValue.GetStorageWords(frame.ValueWords)]; |
| | 793 | 662 | | Span<sbyte> valueSigns = stackalloc sbyte[CapsulePlaneRayValue.SignCount]; |
| | 793 | 663 | | Span<sbyte> coefficientSigns = stackalloc sbyte[6]; |
| | 793 | 664 | | var value = new CapsulePlaneRayValue(valueStorage, valueSigns, frame.ValueWords); |
| | 793 | 665 | | bool found = false; |
| | | 666 | | // A ray parallel to the finite core leaves only through its forward |
| | | 667 | | // cap. The opposite cap and zero-quadratic body cannot own that exit. |
| | 793 | 668 | | bool axial = frame.Normal.X.IsZero && frame.Normal.Z.IsZero; |
| | 793 | 669 | | int firstPrimitive = frame.Finite.Length == Fixed64.Zero ? 2 |
| | 793 | 670 | | : axial ? orientation * frame.Normal.Y.Sign < 0 ? 1 : 2 : 0; |
| | 793 | 671 | | int primitiveEnd = axial ? firstPrimitive + 1 : 3; |
| | 3240 | 672 | | for (int primitive = firstPrimitive; primitive < primitiveEnd; primitive++) |
| | | 673 | | { |
| | 3567 | 674 | | x.CopyTo(rx); y.CopyTo(ry); z.CopyTo(rz); |
| | 1189 | 675 | | bool body = primitive == 0; |
| | 1749 | 676 | | if (body) ry.Clear(); |
| | | 677 | | else |
| | | 678 | | { |
| | 629 | 679 | | Signed320 endpoint = primitive == 1 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) : frame.Finite. |
| | 1258 | 680 | | ContactQuadratic.Scale(denominator, Signed832.ExtendValue(endpoint), term); ry.Add(term, -1); |
| | | 681 | | } |
| | 1189 | 682 | | Signed832 metric = body ? WideArithmetic.AddSigned832( |
| | 1189 | 683 | | Product(frame.Normal.X, frame.Normal.X), |
| | 1189 | 684 | | Product(frame.Normal.Z, frame.Normal.Z)) : frame.NormalSquared; |
| | 1189 | 685 | | ContactQuadratic.Scale(squaredDenominator, metric, a); |
| | 1189 | 686 | | Dot(rx, ry, rz, frame.Normal, b, term); |
| | 3567 | 687 | | ContactQuadratic.Multiply(b, denominator, pointRoot, work); work.CopyTo(b); b.MultiplySign(orientation); |
| | 1189 | 688 | | Squared(rx, ry, rz, c, term, pointRoot); |
| | 2378 | 689 | | ContactQuadratic.Scale(squaredDenominator, radiusSquared, term); c.Add(term, -1); |
| | | 690 | | // Remove only a common positive scale. This preserves the original |
| | | 691 | | // quadratic field and every root/hemisphere certificate, while |
| | | 692 | | // avoiding fixed-point powers of two in discriminant products. |
| | 16646 | 693 | | for (int index = 0; index < coefficientSigns.Length; index++) |
| | 7134 | 694 | | coefficientSigns[index] = (sbyte)signs[index]; |
| | 1189 | 695 | | WideFiniteAxisIntersection.NormalizeFiniteAxisPolynomialPowerOfTwo(values[..(6 * words)], coefficientSigns); |
| | 2378 | 696 | | bool zeroConstant = c.Sign(pointRoot) == 0, repeated = false; |
| | 1189 | 697 | | if (!rational && !zeroConstant) |
| | 524 | 698 | | repeated = IsRepeatedRoot(a, b, c, pointRoot); |
| | 1189 | 699 | | bool valueRoot = !rational && !zeroConstant && !repeated; |
| | 1189 | 700 | | if (valueRoot) |
| | | 701 | | { |
| | 480 | 702 | | if (!value.TrySetUpperRoot(a, b, c, pointRoot, GetDepthValueShift(frame))) continue; |
| | | 703 | | // Keep the original section field: the two radicals can be |
| | | 704 | | // independent even when the exit depth itself is quadratic. |
| | 618 | 705 | | y.CopyTo(term); ContactQuadratic.Scale(denominator, Signed832.ExtendValue(frame.Finite.ShapeFrame.Cap), |
| | 309 | 706 | | term.Add(extent, -1); |
| | 618 | 707 | | ContactQuadratic.Scale(denominator, Signed832.ExtendValue(frame.Normal.Y), work); work.MultiplySign(orie |
| | 309 | 708 | | int upper = value.SignLinear(term, work, pointRoot); |
| | 309 | 709 | | if (body) |
| | | 710 | | { |
| | 207 | 711 | | if (upper > 0) continue; |
| | 274 | 712 | | y.CopyTo(term); term.Add(extent); |
| | 210 | 713 | | if (value.SignLinear(term, work, pointRoot) < 0) continue; |
| | | 714 | | } |
| | 102 | 715 | | else if (primitive == 1) |
| | | 716 | | { |
| | 76 | 717 | | y.CopyTo(term); term.Add(extent); |
| | 38 | 718 | | if (value.SignLinear(term, work, pointRoot) > 0) continue; |
| | | 719 | | } |
| | 64 | 720 | | else if (frame.Finite.Length != Fixed64.Zero && upper < 0) continue; |
| | 330 | 721 | | selection.KeepValue(x, y, z, denominator, pointRoot, value, primitive, frame); found = true; |
| | | 722 | | // A positive-quadratic body exit inside the closed core interval |
| | | 723 | | // is the unique outward capsule exit, including the C1 seam. |
| | 229 | 724 | | if (body) return true; |
| | | 725 | | continue; |
| | | 726 | | } |
| | 709 | 727 | | if (rational && !zeroConstant) |
| | | 728 | | { |
| | 131 | 729 | | if (!ContactQuadratic.TryGetParameter(a, b, c, 1, root, numerator, divisor)) continue; |
| | | 730 | | } |
| | | 731 | | else |
| | | 732 | | { |
| | | 733 | | // CopyTo's multiplier changes only signs. Scale the magnitude |
| | | 734 | | // as well for the nonzero exit of a*t²+2*b*t=0. |
| | 1160 | 735 | | ContactQuadratic.Scale(b, Integer(repeated ? -1 : -2), numerator); a.CopyTo(divisor); |
| | 826 | 736 | | if (numerator.Sign(pointRoot) < 0) numerator.Clear(); |
| | 1160 | 737 | | pointRoot.CopyTo(root); root[pointRoot.Length..].Clear(); |
| | | 738 | | } |
| | 707 | 739 | | if (numerator.Sign(root) < 0 || divisor.Sign(root) <= 0) continue; |
| | 706 | 740 | | ContactQuadratic.Multiply(denominator, divisor, root, pd); |
| | 5648 | 741 | | for (int axis = 0; axis < 3; axis++) |
| | | 742 | | { |
| | 2118 | 743 | | ContactQuadratic source = axis == 0 ? x : axis == 1 ? y : z; |
| | 2118 | 744 | | ContactQuadratic target = axis == 0 ? px : axis == 1 ? py : pz; |
| | 2118 | 745 | | ContactQuadratic.Multiply(source, divisor, root, target); |
| | 2118 | 746 | | ContactQuadratic.Multiply(denominator, numerator, root, term); |
| | 2118 | 747 | | Signed576 direction = axis == 0 ? frame.Normal.X : axis == 1 ? frame.Normal.Y : frame.Normal.Z; |
| | 4236 | 748 | | ContactQuadratic.Scale(term, Signed832.ExtendValue(direction), work); target.Add(work, orientation); |
| | | 749 | | } |
| | 706 | 750 | | ContactQuadratic.Scale(pd, Signed832.ExtendValue(frame.Finite.ShapeFrame.Cap), extent); |
| | 1412 | 751 | | py.CopyTo(term); term.Add(extent, -1); |
| | 706 | 752 | | if (body) |
| | | 753 | | { |
| | 353 | 754 | | if (term.Sign(root) > 0) continue; |
| | 630 | 755 | | py.CopyTo(term); term.Add(extent); |
| | 332 | 756 | | if (term.Sign(root) < 0) continue; |
| | | 757 | | } |
| | 353 | 758 | | else if (primitive == 1) |
| | | 759 | | { |
| | 312 | 760 | | py.CopyTo(term); term.Add(extent); |
| | 175 | 761 | | if (term.Sign(root) > 0) continue; |
| | | 762 | | } |
| | 197 | 763 | | else if (frame.Finite.Length != Fixed64.Zero && term.Sign(root) < 0) continue; |
| | 1256 | 764 | | selection.Keep(x, y, z, denominator, numerator, divisor, root, primitive, frame); found = true; |
| | 926 | 765 | | if (body) return true; |
| | | 766 | | } |
| | | 767 | | // Rational support projections have Q<727 and D<528 bits; their ray |
| | | 768 | | // discriminants are <3050 bits. Perpendicular-edge stationary points |
| | | 769 | | // use u=-b/a directly: Q<600 and D<400 give discriminants <2533 bits. |
| | | 770 | | // The existing 4096-bit coefficients and 3072-bit root bank own both. |
| | 431 | 771 | | return found; |
| | | 772 | | } |
| | | 773 | | |
| | | 774 | | private static bool IsRepeatedRoot(scoped ContactQuadratic a, scoped ContactQuadratic b, |
| | | 775 | | scoped ContactQuadratic c, scoped ReadOnlySpan<ulong> root) |
| | | 776 | | { |
| | | 777 | | // Before cancellation a quadratic-field product can contain two |
| | | 778 | | // coefficient magnitudes and the entire point radicand. The compact |
| | | 779 | | // ray banks are not a sufficient discriminant scratch width. |
| | 524 | 780 | | int words = 2 * Math.Max(ContactQuadratic.ActiveWords(a), |
| | 524 | 781 | | Math.Max(ContactQuadratic.ActiveWords(b), ContactQuadratic.ActiveWords(c))) |
| | 524 | 782 | | + WideArithmetic.GetActiveMagnitudeLength(root) + 1; |
| | 1048 | 783 | | Span<ulong> values = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 524 | 784 | | ContactQuadratic discriminant = ContactQuadratic.At(values, signs, 0, words); |
| | 524 | 785 | | ContactQuadratic product = ContactQuadratic.At(values, signs, 1, words); |
| | 524 | 786 | | ContactQuadratic.Multiply(b, b, root, discriminant); |
| | 1048 | 787 | | ContactQuadratic.Multiply(a, c, root, product); discriminant.Add(product, -1); |
| | 524 | 788 | | return discriminant.Sign(root) == 0; |
| | | 789 | | } |
| | | 790 | | |
| | | 791 | | private static bool ContainsQuadraticPoint(in CapsulePlaneRayFrame frame, |
| | | 792 | | scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic denomin |
| | | 793 | | Signed832 radiusSquared, ContactQuadratic squaredDenominator, ContactQuadratic rx, |
| | | 794 | | ContactQuadratic ry, ContactQuadratic rz, ContactQuadratic value, ContactQuadratic term, ContactQuadratic work, |
| | | 795 | | ReadOnlySpan<ulong> root) |
| | | 796 | | { |
| | 498 | 797 | | Dot(x, y, z, frame.Normal, value, term); |
| | 996 | 798 | | ContactQuadratic.Scale(denominator, frame.PlaneConstant, term); value.Add(term, -1); |
| | 512 | 799 | | if (value.Sign(root) != 0) return false; |
| | 1452 | 800 | | x.CopyTo(rx); y.CopyTo(ry); z.CopyTo(rz); |
| | 484 | 801 | | ContactQuadratic.Scale(denominator, Signed832.ExtendValue(frame.Finite.ShapeFrame.Cap), term); |
| | 968 | 802 | | y.CopyTo(value); value.Add(term, -1); |
| | 531 | 803 | | if (value.Sign(root) > 0) ry.Add(term, -1); |
| | | 804 | | else |
| | | 805 | | { |
| | 874 | 806 | | y.CopyTo(value); value.Add(term); |
| | 532 | 807 | | if (value.Sign(root) < 0) ry.Add(term); |
| | 342 | 808 | | else ry.Clear(); |
| | | 809 | | } |
| | 484 | 810 | | Squared(rx, ry, rz, value, work, root); |
| | 968 | 811 | | ContactQuadratic.Scale(squaredDenominator, radiusSquared, term); value.Add(term, -1); |
| | 484 | 812 | | return value.Sign(root) <= 0; |
| | | 813 | | } |
| | | 814 | | |
| | | 815 | | private static bool ContainsPoint(in CapsulePlaneRayFrame frame, scoped ContactQuadratic x, |
| | | 816 | | scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic denominator, |
| | | 817 | | scoped ReadOnlySpan<ulong> root) |
| | | 818 | | { |
| | | 819 | | // Private producers construct positive denominators; segment producers |
| | | 820 | | // additionally certify their finite parameter before reaching this owner. |
| | 255 | 821 | | int words = GetPointRayWords(frame, x, y, z, denominator, root); |
| | 510 | 822 | | Span<ulong> values = stackalloc ulong[14 * words]; Span<int> signs = stackalloc int[14]; |
| | 765 | 823 | | ContactQuadratic d2 = Field(values, signs, 0, words), rx = Field(values, signs, 1, words), ry = Field(values, si |
| | 510 | 824 | | ContactQuadratic rz = Field(values, signs, 3, words), value = Field(values, signs, 4, words); |
| | 510 | 825 | | ContactQuadratic term = Field(values, signs, 5, words), work = Field(values, signs, 6, words); |
| | 255 | 826 | | ContactQuadratic.Multiply(denominator, denominator, root, d2); |
| | 255 | 827 | | Signed320 radius = GetRadius(frame); |
| | 255 | 828 | | return ContainsQuadraticPoint(frame, x, y, z, denominator, Product(radius, radius), |
| | 255 | 829 | | d2, rx, ry, rz, value, term, work, root); |
| | | 830 | | } |
| | | 831 | | |
| | | 832 | | private static int GetSegmentWords(in CapsulePlaneRayFrame frame, WideAxis3 start, WideAxis3 end, WideAxis3 delta) |
| | | 833 | | { |
| | 480 | 834 | | Span<int> scalarBits = stackalloc int[6]; GetGeometryScalarBits(frame, scalarBits); |
| | 240 | 835 | | Span<ulong> magnitude = stackalloc ulong[13]; |
| | 480 | 836 | | int point = 0, edge = 0; |
| | 1920 | 837 | | for (int axis = 0; axis < 3; axis++) |
| | | 838 | | { |
| | 720 | 839 | | Signed320 first = axis == 0 ? start.X : axis == 1 ? start.Y : start.Z; |
| | 720 | 840 | | Signed320 last = axis == 0 ? end.X : axis == 1 ? end.Y : end.Z; |
| | 720 | 841 | | Signed320 difference = axis == 0 ? delta.X : axis == 1 ? delta.Y : delta.Z; |
| | 720 | 842 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(first), magnitude); |
| | 720 | 843 | | point = Math.Max(point, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | 720 | 844 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(last), magnitude); |
| | 720 | 845 | | point = Math.Max(point, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | 720 | 846 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(difference), magnitude); |
| | 720 | 847 | | edge = Math.Max(edge, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | | 848 | | } |
| | 240 | 849 | | int basis = Math.Max(point, scalarBits[0]) + 1; |
| | 240 | 850 | | int normal = Math.Max(scalarBits[2], Math.Max(scalarBits[3], scalarBits[4])); |
| | | 851 | | // B bounds cap-relative points, E the edge, N the normal and R the |
| | | 852 | | // radius. Section/stationary coefficients need 2E+2max(B,R)+6 bits; |
| | | 853 | | // recovered q and its complete exit/cap-incidence products need |
| | | 854 | | // B+6E+4N+16. Their denominators are rational, so these bank products |
| | | 855 | | // never contain two radical factors. Discriminants use separate scratch. |
| | | 856 | | // Zero quadratic coefficients describe constant families: projected |
| | | 857 | | // delta=0 or Cauchy equality also annihilates the linear coefficient. |
| | 240 | 858 | | int bits = Math.Max(2 * edge + 2 * Math.Max(basis, scalarBits[1]) + 6, |
| | 240 | 859 | | basis + 6 * edge + 4 * normal + 16); |
| | 240 | 860 | | return frame.UsesWideBanks ? Math.Max(13, (bits + 4 + 63) / 64) |
| | 240 | 861 | | : Math.Max(9, Math.Min(Words, (bits + 4 + 63) / 64)); |
| | | 862 | | } |
| | | 863 | | |
| | | 864 | | private static int GetProducerWords(in CapsulePlaneRayFrame frame, CapsulePlaneRayEventKind kind) |
| | | 865 | | { |
| | 1558 | 866 | | Span<int> scalarBits = stackalloc int[6]; GetGeometryScalarBits(frame, scalarBits); |
| | 779 | 867 | | int normal = Math.Max(scalarBits[2], Math.Max(scalarBits[3], scalarBits[4])); |
| | 779 | 868 | | Span<ulong> magnitude = stackalloc ulong[13]; |
| | 779 | 869 | | WideArithmetic.GetMagnitude(frame.NormalSquared, magnitude); |
| | 1558 | 870 | | int metric = WideArithmetic.GetMagnitudeBitLength(magnitude), gap = scalarBits[5]; |
| | 779 | 871 | | Signed832 end = Product(frame.Normal.Y, frame.Finite.ShapeFrame.Cap); |
| | 4674 | 872 | | for (int orientation = -1; orientation <= 1; orientation += 2) |
| | | 873 | | { |
| | 1558 | 874 | | Signed832 value = orientation < 0 ? WideArithmetic.SubtractSigned832(frame.PlaneConstant, end) |
| | 1558 | 875 | | : WideArithmetic.AddSigned832(frame.PlaneConstant, end); |
| | 1558 | 876 | | WideArithmetic.GetMagnitude(value, magnitude); |
| | 1558 | 877 | | gap = Math.Max(gap, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | | 878 | | } |
| | | 879 | | int bits; |
| | 779 | 880 | | if (kind == CapsulePlaneRayEventKind.Support) |
| | 245 | 881 | | bits = Math.Max(gap + normal, metric + scalarBits[0]) + 1; |
| | | 882 | | else |
| | | 883 | | { |
| | | 884 | | // H owns the complete r²*S-gap² before cancellation. Since T<=S, |
| | | 885 | | // intrinsic transverse radicands need H+2S; cap q needs P+2S+1. |
| | 534 | 886 | | int section = Math.Max(2 * scalarBits[1] + metric, 2 * gap) + 1; |
| | 534 | 887 | | if (kind == CapsulePlaneRayEventKind.Circle) |
| | | 888 | | { |
| | 290 | 889 | | int authored = 0; |
| | 2320 | 890 | | for (int axis = 0; axis < 3; axis++) |
| | | 891 | | { |
| | 870 | 892 | | Signed320 component = axis == 0 ? frame.AuthoredNormal.X |
| | 870 | 893 | | : axis == 1 ? frame.AuthoredNormal.Y : frame.AuthoredNormal.Z; |
| | 870 | 894 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(component), magnitude); |
| | 870 | 895 | | authored = Math.Max(authored, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | | 896 | | } |
| | 290 | 897 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(Signed320.ExtendValue(frame.Finite.ShapeFrame.Basis.De |
| | | 898 | | // The authored tangents use at most two normal products. A |
| | | 899 | | // rigid numerator is bounded by its positive denominator, so |
| | | 900 | | // transformed V<=2A+B+3 and its three-term square W<=2V+2. |
| | 290 | 901 | | int tangent = 2 * authored + WideArithmetic.GetMagnitudeBitLength(magnitude) + 3; |
| | 290 | 902 | | int tangentSquared = 2 * tangent + 2; |
| | 290 | 903 | | bits = Math.Max(section + metric + tangentSquared, scalarBits[0] + metric + tangentSquared + 1); |
| | | 904 | | } |
| | 244 | 905 | | else bits = Math.Max(section + 2 * metric, scalarBits[0] + 2 * metric + 1); |
| | | 906 | | } |
| | | 907 | | // Four spare carry bits cover all sums; complete Signed832 imports |
| | | 908 | | // still require nine words. Retained point/root banks stay unchanged. |
| | 779 | 909 | | return frame.UsesWideBanks ? Math.Max(13, (bits + 4 + 63) / 64) |
| | 779 | 910 | | : Math.Max(9, Math.Min(Words, (bits + 4 + 63) / 64)); |
| | | 911 | | } |
| | | 912 | | |
| | | 913 | | private static void GetGeometryScalarBits(in CapsulePlaneRayFrame frame, Span<int> bits) |
| | | 914 | | { |
| | 2191 | 915 | | Span<ulong> magnitude = stackalloc ulong[13]; |
| | 2191 | 916 | | Signed320 radius = GetRadius(frame); |
| | 30674 | 917 | | for (int scalar = 0; scalar < bits.Length; scalar++) |
| | | 918 | | { |
| | 13146 | 919 | | Signed832 value = scalar == 0 ? Signed832.ExtendValue(frame.Finite.ShapeFrame.Cap) |
| | 13146 | 920 | | : scalar == 1 ? Signed832.ExtendValue(radius) : scalar == 2 ? Signed832.ExtendValue(frame.Normal.X) |
| | 13146 | 921 | | : scalar == 3 ? Signed832.ExtendValue(frame.Normal.Y) : scalar == 4 ? Signed832.ExtendValue(frame.Normal |
| | 13146 | 922 | | : frame.PlaneConstant; |
| | 13146 | 923 | | WideArithmetic.GetMagnitude(value, magnitude); |
| | 13146 | 924 | | bits[scalar] = WideArithmetic.GetMagnitudeBitLength(magnitude); |
| | | 925 | | } |
| | 2191 | 926 | | } |
| | | 927 | | |
| | | 928 | | private static int GetPointRayWords(in CapsulePlaneRayFrame frame, scoped ContactQuadratic x, |
| | | 929 | | scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic denominator, |
| | | 930 | | scoped ReadOnlySpan<ulong> root) |
| | | 931 | | { |
| | 1172 | 932 | | int coefficientBits = 0; |
| | 11720 | 933 | | for (int coordinate = 0; coordinate < 4; coordinate++) |
| | | 934 | | { |
| | 4688 | 935 | | ContactQuadratic value = coordinate == 0 ? x : coordinate == 1 ? y : coordinate == 2 ? z : denominator; |
| | 4688 | 936 | | coefficientBits = Math.Max(coefficientBits, Math.Max(WideArithmetic.GetMagnitudeBitLength(value.Rational), |
| | 4688 | 937 | | WideArithmetic.GetMagnitudeBitLength(value.Radical))); |
| | | 938 | | } |
| | 2344 | 939 | | Span<int> geometryBits = stackalloc int[6]; GetGeometryScalarBits(frame, geometryBits); |
| | 1172 | 940 | | int scalarBits = 0; |
| | 23440 | 941 | | for (int scalar = 0; scalar < geometryBits.Length; scalar++) scalarBits = Math.Max(scalarBits, geometryBits[scal |
| | | 942 | | // C bounds point coefficients, M unsquared geometry scalars, and K the |
| | | 943 | | // radicand. Complete exit/hemisphere products need 3C+3M+2K+8 bits; |
| | | 944 | | // two spare carry bits cover sums. Signed832 imports require nine words. |
| | 1172 | 945 | | int bits = 3 * coefficientBits + 3 * scalarBits + 2 * WideArithmetic.GetMagnitudeBitLength(root) + 10; |
| | 1172 | 946 | | return frame.UsesWideBanks ? Math.Max(13, (bits + 63) / 64) |
| | 1172 | 947 | | : Math.Max(9, Math.Min(Words, (bits + 63) / 64)); |
| | | 948 | | } |
| | | 949 | | |
| | | 950 | | private static int GetDepthValueShift(in CapsulePlaneRayFrame frame) |
| | | 951 | | { |
| | 480 | 952 | | Signed320 radius = GetRadius(frame); |
| | 480 | 953 | | Signed320 bound = WideArithmetic.AddSigned320(frame.Finite.ShapeFrame.Cap, radius); |
| | 480 | 954 | | Span<ulong> magnitude = stackalloc ulong[13]; |
| | 480 | 955 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(bound), magnitude); |
| | 480 | 956 | | int boundBits = WideArithmetic.GetMagnitudeBitLength(magnitude); |
| | 480 | 957 | | WideArithmetic.GetMagnitude(frame.NormalSquared, magnitude); |
| | 480 | 958 | | int normalPower = (WideArithmetic.GetMagnitudeBitLength(magnitude) - 1) / 2; |
| | | 959 | | // An admitted exit is at most the capsule diameter divided by |N|. |
| | | 960 | | // Infinite-body roots beyond that bound cannot pass finite admission. |
| | 480 | 961 | | return Math.Max(0, boundBits + 2 - normalPower); |
| | | 962 | | } |
| | | 963 | | |
| | | 964 | | private static void Dot(ContactQuadratic x, ContactQuadratic y, ContactQuadratic z, WideDirection3d direction, |
| | | 965 | | ContactQuadratic result, ContactQuadratic term) |
| | | 966 | | { |
| | 1687 | 967 | | ContactQuadratic.Scale(x, Signed832.ExtendValue(direction.X), result); |
| | 3374 | 968 | | ContactQuadratic.Scale(y, Signed832.ExtendValue(direction.Y), term); result.Add(term); |
| | 3374 | 969 | | ContactQuadratic.Scale(z, Signed832.ExtendValue(direction.Z), term); result.Add(term); |
| | 1687 | 970 | | } |
| | | 971 | | |
| | | 972 | | private static void Squared(ContactQuadratic x, ContactQuadratic y, ContactQuadratic z, |
| | | 973 | | ContactQuadratic result, ContactQuadratic term, ReadOnlySpan<ulong> root = default) |
| | | 974 | | { |
| | 1994 | 975 | | ContactQuadratic.Multiply(x, x, root, result); |
| | 3988 | 976 | | ContactQuadratic.Multiply(y, y, root, term); result.Add(term); |
| | 3988 | 977 | | ContactQuadratic.Multiply(z, z, root, term); result.Add(term); |
| | 1994 | 978 | | } |
| | | 979 | | |
| | | 980 | | /// <summary>Tests the closed finite triangle against the capsule's exact plane section.</summary> |
| | | 981 | | internal static bool IntersectsTriangle(in CapsuleFiniteSectionFrame finite, FixedTriangle triangle) |
| | | 982 | | { |
| | 25 | 983 | | if (finite.ContainsPoint(triangle.A) || finite.ContainsPoint(triangle.B) || finite.ContainsPoint(triangle.C)) re |
| | 146 | 984 | | for (int edge = 0; edge < 3; edge++) |
| | 56 | 985 | | if (finite.IntersectsSegment(triangle.GetEdge(edge))) return true; |
| | 18 | 986 | | var frame = finite.ShapeFrame.HasExactScale ? new CapsulePlaneRayFrame(triangle, finite.ShapeFrame.SourceFrame, |
| | 18 | 987 | | : new CapsulePlaneRayFrame(triangle, finite); |
| | 21 | 988 | | if (frame.NormalSquared.IsZero) return false; |
| | | 989 | | // With no admitted boundary crossing, an intersecting capsule section |
| | | 990 | | // is wholly inside the triangle. Its projected nearest core point is |
| | | 991 | | // an interior certificate; endpoint projection alone is not admission. |
| | 15 | 992 | | int words = frame.UsesWideBanks ? 60 : 20; |
| | 15 | 993 | | Span<ulong> values = stackalloc ulong[20 * words]; |
| | 15 | 994 | | Span<int> signs = stackalloc int[20]; |
| | 15 | 995 | | ContactQuadratic x = ContactQuadratic.At(values, signs, 0, words); |
| | 15 | 996 | | ContactQuadratic y = ContactQuadratic.At(values, signs, 1, words); |
| | 15 | 997 | | ContactQuadratic z = ContactQuadratic.At(values, signs, 2, words); |
| | 15 | 998 | | ContactQuadratic denominator = ContactQuadratic.At(values, signs, 3, words); |
| | 15 | 999 | | ContactQuadratic gap = ContactQuadratic.At(values, signs, 4, words); |
| | 15 | 1000 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 5, words); |
| | 15 | 1001 | | ContactQuadratic left = ContactQuadratic.At(values, signs, 6, words); |
| | 15 | 1002 | | ContactQuadratic right = ContactQuadratic.At(values, signs, 7, words); |
| | 15 | 1003 | | ContactQuadratic squaredRadius = ContactQuadratic.At(values, signs, 8, words); |
| | 15 | 1004 | | ContactQuadratic metric = ContactQuadratic.At(values, signs, 9, words); |
| | 15 | 1005 | | Signed832 projectedEnd = Product(frame.Normal.Y, finite.ShapeFrame.Cap); |
| | 15 | 1006 | | Signed832 absoluteEnd = projectedEnd.Sign < 0 ? WideArithmetic.SubtractSigned832(default, projectedEnd) : projec |
| | 45 | 1007 | | x.Clear(); y.Clear(); z.Clear(); |
| | 15 | 1008 | | bool crossesCore = !frame.Normal.Y.IsZero |
| | 15 | 1009 | | && WideArithmetic.AddSigned832(frame.PlaneConstant, absoluteEnd).Sign >= 0 |
| | 15 | 1010 | | && WideArithmetic.SubtractSigned832(frame.PlaneConstant, absoluteEnd).Sign <= 0; |
| | 15 | 1011 | | if (crossesCore) |
| | | 1012 | | { |
| | 12 | 1013 | | y.Set(frame.PlaneConstant); y.MultiplySign(frame.Normal.Y.Sign); |
| | 6 | 1014 | | Signed576 positive = frame.Normal.Y.Sign < 0 ? WideArithmetic.SubtractSigned576(default, frame.Normal.Y) : f |
| | 6 | 1015 | | denominator.Set(Signed832.ExtendValue(positive)); |
| | | 1016 | | } |
| | | 1017 | | else |
| | | 1018 | | { |
| | 9 | 1019 | | int endpointSign = frame.Normal.Y.IsZero ? 0 : frame.PlaneConstant.Sign * frame.Normal.Y.Sign; |
| | 9 | 1020 | | Signed320 endpoint = endpointSign < 0 ? WideArithmetic.Negate(finite.ShapeFrame.Cap) |
| | 9 | 1021 | | : endpointSign > 0 ? finite.ShapeFrame.Cap : default; |
| | 9 | 1022 | | gap.Set(WideArithmetic.SubtractSigned832(frame.PlaneConstant, |
| | 9 | 1023 | | Product(frame.Normal.Y, endpoint))); |
| | 9 | 1024 | | ContactQuadratic.Multiply(gap, gap, ReadOnlySpan<ulong>.Empty, left); |
| | 9 | 1025 | | squaredRadius.Set(Product( |
| | 9 | 1026 | | GetRadius(frame), |
| | 9 | 1027 | | GetRadius(frame))); |
| | 9 | 1028 | | metric.Set(frame.NormalSquared); |
| | 9 | 1029 | | ContactQuadratic.Multiply(squaredRadius, metric, ReadOnlySpan<ulong>.Empty, right); |
| | 9 | 1030 | | left.Add(right, -1); |
| | 12 | 1031 | | if (left.Sign(ReadOnlySpan<ulong>.Empty) > 0) return false; |
| | 6 | 1032 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.X), x); |
| | 6 | 1033 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Y), y); |
| | 6 | 1034 | | ContactQuadratic.Scale(gap, Signed832.ExtendValue(frame.Normal.Z), z); |
| | 6 | 1035 | | denominator.Set(frame.NormalSquared); |
| | 12 | 1036 | | ContactQuadratic.Scale(denominator, Signed832.ExtendValue(endpoint), term); y.Add(term); |
| | | 1037 | | } |
| | | 1038 | | // Rational witness coordinates are below 727 bits; gap squares and |
| | | 1039 | | // radius²*normal² below 930 bits fit the 1280-bit local bank. |
| | 12 | 1040 | | return frame.ContainsTrianglePoint(triangle, x, y, z, denominator, ReadOnlySpan<ulong>.Empty); |
| | | 1041 | | } |
| | | 1042 | | } |