| | | 1 | | //======================================================================= |
| | | 2 | | // CapsuleFiniteSectionFrame.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 closed finite-capsule admission in an authored rigid frame.</summary> |
| | | 13 | | internal readonly struct CapsuleFiniteSectionFrame |
| | | 14 | | { |
| | | 15 | | internal readonly CylinderPolytopeFrame ShapeFrame; |
| | | 16 | | internal readonly Fixed64 Length; |
| | | 17 | | private readonly Signed576 _radiusSquared; |
| | | 18 | | private readonly Signed832 _wideRadiusSquared; |
| | | 19 | | |
| | | 20 | | /// <summary>Exact capsule-local coordinates supplied by a retained rational source chart.</summary> |
| | | 21 | | internal CapsuleFiniteSectionFrame(Fixed64 length, Fixed64 radius, Signed320 coordinateDenominator) |
| | | 22 | | { |
| | 87 | 23 | | Length = length; |
| | 87 | 24 | | ShapeFrame = new CylinderPolytopeFrame(length, radius, coordinateDenominator); |
| | 87 | 25 | | Signed320 scaledRadius = Signed320.NarrowValue(WideArithmetic.MultiplySigned320(coordinateDenominator, Signed320 |
| | 87 | 26 | | _wideRadiusSquared = ShapeFrame.HasExactScale ? Product(scaledRadius, scaledRadius) : default; |
| | 87 | 27 | | _radiusSquared = ShapeFrame.HasExactScale ? default : WideArithmetic.MultiplySigned320(scaledRadius, scaledRadiu |
| | 87 | 28 | | } |
| | | 29 | | |
| | | 30 | | internal CapsuleFiniteSectionFrame(Vector3d origin, FixedQuaternion rotation, |
| | | 31 | | Vector3d center, FixedQuaternion capsuleRotation, Fixed64 length, Fixed64 radius) |
| | | 32 | | { |
| | 733 | 33 | | _wideRadiusSquared = default; |
| | 734 | 34 | | if (length < Fixed64.Zero) throw new ArgumentOutOfRangeException(nameof(length)); |
| | 733 | 35 | | if (radius < Fixed64.Zero) throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 732 | 36 | | if (!rotation.IsNormalized()) throw new ArgumentException("The authored rotation must be normalized.", nameof(ro |
| | 731 | 37 | | if (!capsuleRotation.IsNormalized()) throw new ArgumentException("The capsule rotation must be normalized.", nam |
| | 729 | 38 | | Length = length; |
| | 729 | 39 | | ShapeFrame = new CylinderPolytopeFrame(center, capsuleRotation, length, radius, origin, rotation); |
| | 729 | 40 | | Signed320 scaledRadius = WideArithmetic.MultiplySigned192(ShapeFrame.Radius, ShapeFrame.Denominator); |
| | 729 | 41 | | _radiusSquared = WideArithmetic.MultiplySigned320(scaledRadius, scaledRadius); |
| | 729 | 42 | | } |
| | | 43 | | |
| | | 44 | | internal CapsuleFiniteSectionFrame(in ScaledRigidFrame3d sourceFrame, |
| | | 45 | | Vector3d center, FixedQuaternion capsuleRotation, Fixed64 length, Fixed64 radius) |
| | | 46 | | { |
| | 186 | 47 | | if (sourceFrame.IsRigid) |
| | | 48 | | { |
| | 6 | 49 | | this = new CapsuleFiniteSectionFrame(sourceFrame.RigidOrigin, sourceFrame.Rotation, |
| | 6 | 50 | | center, capsuleRotation, length, radius); |
| | 6 | 51 | | return; |
| | | 52 | | } |
| | 180 | 53 | | if (length < Fixed64.Zero || length > (Fixed64)8192) |
| | 2 | 54 | | throw new ArgumentOutOfRangeException(nameof(length)); |
| | 178 | 55 | | if (radius < Fixed64.Zero || radius > (Fixed64)4096) |
| | 2 | 56 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 176 | 57 | | if (!capsuleRotation.IsNormalized()) |
| | 1 | 58 | | throw new ArgumentException("The capsule rotation must be normalized.", nameof(capsuleRotation)); |
| | 175 | 59 | | Length = length; |
| | 175 | 60 | | ShapeFrame = new CylinderPolytopeFrame(sourceFrame, center, capsuleRotation, Signed192.Raw(length), radius); |
| | 175 | 61 | | Signed320 scaledRadius = Signed320.NarrowValue( |
| | 175 | 62 | | WideArithmetic.MultiplySigned320(ShapeFrame.CoordinateDenominator, ShapeFrame.Radius)); |
| | 175 | 63 | | _wideRadiusSquared = Product(scaledRadius, scaledRadius); |
| | 175 | 64 | | _radiusSquared = default; |
| | 175 | 65 | | } |
| | | 66 | | |
| | 21530 | 67 | | internal WideAxis3 Transform(Vector3d point) => ShapeFrame.Transform(point); |
| | 97 | 68 | | internal bool ContainsPoint(Vector3d point) => ContainsTransformedPoint(Transform(point)); |
| | | 69 | | |
| | | 70 | | internal bool ContainsTransformedPoint(WideAxis3 point) |
| | | 71 | | { |
| | 122 | 72 | | Signed320 y = point.Y.Sign < 0 ? WideArithmetic.Negate(point.Y) : point.Y; |
| | 122 | 73 | | y = WideArithmetic.SubtractSigned320(y, ShapeFrame.Cap); |
| | 175 | 74 | | if (y.Sign < 0) y = default; |
| | 122 | 75 | | var offset = new WideAxis3(point.X, y, point.Z); |
| | 122 | 76 | | return ShapeFrame.HasExactScale |
| | 122 | 77 | | ? WideArithmetic.SubtractSigned832(DotWide(offset, offset), _wideRadiusSquared).Sign <= 0 |
| | 122 | 78 | | : WideArithmetic.SubtractSigned576(Dot(offset, offset), _radiusSquared).Sign <= 0; |
| | | 79 | | } |
| | | 80 | | |
| | | 81 | | internal bool IntersectsSegment(FixedSegment segment) |
| | | 82 | | { |
| | 182 | 83 | | WideAxis3 start = Transform(segment.Start), delta = Difference(Transform(segment.End), start); |
| | 91 | 84 | | if (ShapeFrame.HasExactScale) |
| | 13 | 85 | | return IntersectsWideSegment(start, delta, default); |
| | 522 | 86 | | for (int primitive = 0; primitive < (Length == Fixed64.Zero ? 1 : 3); primitive++) |
| | 204 | 87 | | if (TryGetPrimitive(start, delta, primitive, out Signed576 a, out Signed576 b, out Signed576 c, |
| | 204 | 88 | | out Signed320 lowerN, out Signed320 lowerD, out Signed320 upperN, out Signed320 upperD) |
| | 225 | 89 | | && IntersectsQuadratic(a, b, c, lowerN, lowerD, upperN, upperD)) return true; |
| | 57 | 90 | | return false; |
| | | 91 | | } |
| | | 92 | | |
| | | 93 | | /// <summary>Tests the retained exact subsegment, without rounding its endpoints.</summary> |
| | | 94 | | internal bool IntersectsSegment(FixedSegment segment, in RationalSegmentInterval clip) |
| | | 95 | | { |
| | 200 | 96 | | if (!clip.IsClipped) return IntersectsSegment(segment); |
| | 368 | 97 | | WideAxis3 start = Transform(segment.Start), delta = Difference(Transform(segment.End), start); |
| | 184 | 98 | | if (ShapeFrame.HasExactScale || clip.HasWideBounds) |
| | 12 | 99 | | return IntersectsWideSegment(start, delta, clip); |
| | 502 | 100 | | for (int primitive = 0; primitive < (Length == Fixed64.Zero ? 1 : 3); primitive++) |
| | | 101 | | { |
| | 213 | 102 | | if (!TryGetPrimitive(start, delta, primitive, out Signed576 a, out Signed576 b, out Signed576 c, |
| | 213 | 103 | | out Signed320 ln, out Signed320 ld, out Signed320 un, out Signed320 ud)) continue; |
| | 213 | 104 | | if (RationalSegmentInterval.CompareParameters(ln, ld, clip.LowerNumerator, clip.LowerDenominator) < 0) |
| | 298 | 105 | | { ln = clip.LowerNumerator; ld = clip.LowerDenominator; } |
| | 213 | 106 | | if (RationalSegmentInterval.CompareParameters(un, ud, clip.UpperNumerator, clip.UpperDenominator) > 0) |
| | 312 | 107 | | { un = clip.UpperNumerator; ud = clip.UpperDenominator; } |
| | 213 | 108 | | if (RationalSegmentInterval.CompareParameters(ln, ld, un, ud) <= 0 |
| | 347 | 109 | | && IntersectsQuadratic(a, b, c, ln, ld, un, ud)) return true; |
| | | 110 | | } |
| | 38 | 111 | | return false; |
| | | 112 | | } |
| | | 113 | | |
| | | 114 | | private bool IntersectsWideSegment(WideAxis3 start, WideAxis3 delta, in RationalSegmentInterval clip) |
| | | 115 | | { |
| | 154 | 116 | | for (int primitive = 0; primitive < (Length == Fixed64.Zero ? 1 : 3); primitive++) |
| | | 117 | | { |
| | 57 | 118 | | if (!TryGetPrimitiveWide(start, delta, primitive, out Signed832 a, out Signed832 b, out Signed832 c, |
| | 57 | 119 | | out Signed320 ln, out Signed320 ld, out Signed320 un, out Signed320 ud)) |
| | | 120 | | continue; |
| | 51 | 121 | | Signed832 lowerN = Signed832.ExtendValue(Signed576.ExtendValue(ln)), |
| | 51 | 122 | | lowerD = Signed832.ExtendValue(Signed576.ExtendValue(ld)), |
| | 51 | 123 | | upperN = Signed832.ExtendValue(Signed576.ExtendValue(un)), |
| | 51 | 124 | | upperD = Signed832.ExtendValue(Signed576.ExtendValue(ud)); |
| | 51 | 125 | | if (clip.IsClipped) |
| | | 126 | | { |
| | 20 | 127 | | if (RationalSegmentInterval.CompareParameters(lowerN, lowerD, clip.WideLowerNumerator, clip.WideLowerDen |
| | 24 | 128 | | { lowerN = clip.WideLowerNumerator; lowerD = clip.WideLowerDenominator; } |
| | 20 | 129 | | if (RationalSegmentInterval.CompareParameters(upperN, upperD, clip.WideUpperNumerator, clip.WideUpperDen |
| | 24 | 130 | | { upperN = clip.WideUpperNumerator; upperD = clip.WideUpperDenominator; } |
| | | 131 | | } |
| | 51 | 132 | | if (RationalSegmentInterval.CompareParameters(lowerN, lowerD, upperN, upperD) <= 0 |
| | 51 | 133 | | && IntersectsQuadratic(a, b, c, lowerN, lowerD, upperN, upperD)) |
| | 5 | 134 | | return true; |
| | | 135 | | } |
| | 20 | 136 | | return false; |
| | | 137 | | } |
| | | 138 | | |
| | | 139 | | internal bool TryGetPrimitiveWide(WideAxis3 start, WideAxis3 delta, int primitive, |
| | | 140 | | out Signed832 a, out Signed832 b, out Signed832 c, |
| | | 141 | | out Signed320 lowerN, out Signed320 lowerD, out Signed320 upperN, out Signed320 upperD) |
| | | 142 | | { |
| | 11733 | 143 | | if (!ShapeFrame.HasExactScale) |
| | | 144 | | { |
| | 1702 | 145 | | bool found = TryGetPrimitive(start, delta, primitive, out Signed576 sa, out Signed576 sb, out Signed576 sc, |
| | 1702 | 146 | | out lowerN, out lowerD, out upperN, out upperD); |
| | 5106 | 147 | | a = Signed832.ExtendValue(sa); b = Signed832.ExtendValue(sb); c = Signed832.ExtendValue(sc); |
| | 1702 | 148 | | return found; |
| | | 149 | | } |
| | 10031 | 150 | | lowerN = default; |
| | 10031 | 151 | | lowerD = upperN = upperD = Signed320.One; |
| | 10031 | 152 | | if (primitive == 0 && Length != Fixed64.Zero) |
| | | 153 | | { |
| | 3412 | 154 | | a = b = c = default; |
| | 3412 | 155 | | if (!TryGetCenteredAxisInterval(start.Y, delta.Y, ShapeFrame.Cap, |
| | 3412 | 156 | | out lowerN, out lowerD, out upperN, out upperD)) |
| | 1002 | 157 | | return false; |
| | 2410 | 158 | | start = new WideAxis3(start.X, default, start.Z); |
| | 2410 | 159 | | delta = new WideAxis3(delta.X, default, delta.Z); |
| | | 160 | | } |
| | | 161 | | else |
| | | 162 | | { |
| | 6619 | 163 | | Signed320 cap = primitive == 1 ? WideArithmetic.Negate(ShapeFrame.Cap) : ShapeFrame.Cap; |
| | 6619 | 164 | | start = new WideAxis3(start.X, WideArithmetic.SubtractSigned320(start.Y, cap), start.Z); |
| | | 165 | | } |
| | 18058 | 166 | | a = DotWide(delta, delta); b = DotWide(start, delta); |
| | 9029 | 167 | | c = WideArithmetic.SubtractSigned832(DotWide(start, start), _wideRadiusSquared); |
| | 9029 | 168 | | return true; |
| | | 169 | | } |
| | | 170 | | |
| | | 171 | | /// <summary>Retains the first closed section parameter in the input segment's order.</summary> |
| | | 172 | | internal bool TryGetFirstSegmentParameter(FixedSegment segment, |
| | | 173 | | ContactQuadratic numerator, ContactQuadratic denominator, Span<ulong> root) |
| | | 174 | | { |
| | 30 | 175 | | if (ShapeFrame.HasExactScale) |
| | 7 | 176 | | return TryGetFirstWideSegmentParameter(segment, numerator, denominator, root); |
| | 69 | 177 | | numerator.Clear(); denominator.Clear(); root.Clear(); |
| | 46 | 178 | | WideAxis3 start = Transform(segment.Start), delta = Difference(Transform(segment.End), start); |
| | 23 | 179 | | Span<ulong> values = stackalloc ulong[10 * FiniteSectionPoint.Words]; |
| | 23 | 180 | | Span<int> signs = stackalloc int[10]; |
| | 23 | 181 | | Span<ulong> candidateRoot = stackalloc ulong[FiniteSectionPoint.RootWords]; |
| | 23 | 182 | | ContactQuadratic qa = ContactQuadratic.At(values, signs, 0, FiniteSectionPoint.Words); |
| | 23 | 183 | | ContactQuadratic qb = ContactQuadratic.At(values, signs, 1, FiniteSectionPoint.Words); |
| | 23 | 184 | | ContactQuadratic qc = ContactQuadratic.At(values, signs, 2, FiniteSectionPoint.Words); |
| | 23 | 185 | | ContactQuadratic n = ContactQuadratic.At(values, signs, 3, FiniteSectionPoint.Words); |
| | 23 | 186 | | ContactQuadratic d = ContactQuadratic.At(values, signs, 4, FiniteSectionPoint.Words); |
| | 23 | 187 | | bool found = false; |
| | 168 | 188 | | for (int primitive = 0; primitive < (Length == Fixed64.Zero ? 1 : 3); primitive++) |
| | | 189 | | { |
| | 61 | 190 | | if (!TryGetPrimitive(start, delta, primitive, out Signed576 a, out Signed576 b, out Signed576 c, |
| | 61 | 191 | | out Signed320 lowerN, out Signed320 lowerD, out Signed320 upperN, out Signed320 upperD) |
| | 61 | 192 | | || !IntersectsQuadratic(a, b, c, lowerN, lowerD, upperN, upperD)) continue; |
| | 63 | 193 | | n.Clear(); d.Clear(); candidateRoot.Clear(); |
| | 21 | 194 | | GetFirstQuadraticParameter(a, b, c, lowerN, lowerD, qa, qb, qc, n, d, candidateRoot); |
| | 21 | 195 | | if (!found || ContactQuadratic.CompareRatios(n, d, candidateRoot, numerator, denominator, root) < 0) |
| | | 196 | | { |
| | 60 | 197 | | n.CopyTo(numerator); d.CopyTo(denominator); candidateRoot.CopyTo(root); found = true; |
| | | 198 | | } |
| | | 199 | | } |
| | 23 | 200 | | return found; |
| | | 201 | | } |
| | | 202 | | |
| | | 203 | | private bool TryGetFirstWideSegmentParameter(FixedSegment segment, |
| | | 204 | | ContactQuadratic numerator, ContactQuadratic denominator, Span<ulong> root) |
| | | 205 | | { |
| | 21 | 206 | | numerator.Clear(); denominator.Clear(); root.Clear(); |
| | 14 | 207 | | WideAxis3 start = Transform(segment.Start), delta = Difference(Transform(segment.End), start); |
| | 7 | 208 | | int words = numerator.FieldWords; |
| | 7 | 209 | | Span<ulong> values = stackalloc ulong[10 * words]; |
| | 7 | 210 | | Span<int> signs = stackalloc int[10]; |
| | 7 | 211 | | Span<ulong> candidateRoot = stackalloc ulong[root.Length]; |
| | 7 | 212 | | ContactQuadratic qa = ContactQuadratic.At(values, signs, 0, words); |
| | 7 | 213 | | ContactQuadratic qb = ContactQuadratic.At(values, signs, 1, words); |
| | 7 | 214 | | ContactQuadratic qc = ContactQuadratic.At(values, signs, 2, words); |
| | 7 | 215 | | ContactQuadratic n = ContactQuadratic.At(values, signs, 3, words); |
| | 7 | 216 | | ContactQuadratic d = ContactQuadratic.At(values, signs, 4, words); |
| | 7 | 217 | | bool found = false; |
| | 44 | 218 | | for (int primitive = 0; primitive < (Length == Fixed64.Zero ? 1 : 3); primitive++) |
| | | 219 | | { |
| | 15 | 220 | | if (!TryGetPrimitiveWide(start, delta, primitive, out Signed832 a, out Signed832 b, out Signed832 c, |
| | 15 | 221 | | out Signed320 ln, out Signed320 ld, out Signed320 un, out Signed320 ud) |
| | 15 | 222 | | || !IntersectsQuadratic(a, b, c, ln, ld, un, ud)) |
| | | 223 | | continue; |
| | 21 | 224 | | n.Clear(); d.Clear(); candidateRoot.Clear(); |
| | 7 | 225 | | if (WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(a, b, c, ln, ld) <= 0) |
| | | 226 | | { |
| | 8 | 227 | | n.Set(Signed576.ExtendValue(ln)); d.Set(Signed576.ExtendValue(ld)); |
| | | 228 | | } |
| | | 229 | | else |
| | | 230 | | { |
| | 9 | 231 | | qa.Set(a); qb.Set(b); qc.Set(c); |
| | 3 | 232 | | bool retained = ContactQuadratic.TryGetParameter(qa, qb, qc, -1, candidateRoot, n, d); |
| | 5 | 233 | | if (!retained) retained = ContactQuadratic.TryGetParameter(qa, qb, qc, 1, candidateRoot, n, d); |
| | | 234 | | System.Diagnostics.Debug.Assert(retained); |
| | | 235 | | } |
| | 7 | 236 | | if (!found || ContactQuadratic.CompareRatios(n, d, candidateRoot, numerator, denominator, root) < 0) |
| | | 237 | | { |
| | 12 | 238 | | n.CopyTo(numerator); d.CopyTo(denominator); candidateRoot.CopyTo(root); found = true; |
| | | 239 | | } |
| | | 240 | | } |
| | 7 | 241 | | return found; |
| | | 242 | | } |
| | | 243 | | |
| | | 244 | | internal bool TryGetPrimitive(WideAxis3 start, WideAxis3 delta, int primitive, |
| | | 245 | | out Signed576 a, out Signed576 b, out Signed576 c, |
| | | 246 | | out Signed320 lowerN, out Signed320 lowerD, out Signed320 upperN, out Signed320 upperD) |
| | | 247 | | { |
| | 2180 | 248 | | lowerN = default; |
| | 2180 | 249 | | lowerD = upperN = upperD = Signed320.ExtendValue(Signed192.Signed(1)); |
| | | 250 | | // The capsule is exactly a finite cylinder plus both full endpoint |
| | | 251 | | // balls. Full balls avoid hemisphere clipping without changing the |
| | | 252 | | // union, and the zero-core capsule needs only one ball. |
| | 2180 | 253 | | bool body = primitive == 0 && Length != Fixed64.Zero; |
| | 2180 | 254 | | if (body) |
| | | 255 | | { |
| | 667 | 256 | | a = b = c = default; |
| | 1042 | 257 | | if (!TryGetCenteredAxisInterval(start.Y, delta.Y, ShapeFrame.Cap, out lowerN, out lowerD, out upperN, out up |
| | 292 | 258 | | start = new WideAxis3(start.X, default, start.Z); |
| | 292 | 259 | | delta = new WideAxis3(delta.X, default, delta.Z); |
| | | 260 | | } |
| | | 261 | | else |
| | | 262 | | { |
| | 1513 | 263 | | Signed320 cap = primitive == 1 ? WideArithmetic.Negate(ShapeFrame.Cap) : ShapeFrame.Cap; |
| | 1513 | 264 | | start = new WideAxis3(start.X, WideArithmetic.SubtractSigned320(start.Y, cap), start.Z); |
| | | 265 | | } |
| | | 266 | | // Doubled rigid coordinates and differences are below 198 bits; |
| | | 267 | | // squared coefficients fit 400 bits and discriminants 801 bits. |
| | 3610 | 268 | | a = Dot(delta, delta); b = Dot(start, delta); |
| | 1805 | 269 | | c = WideArithmetic.SubtractSigned576(Dot(start, start), _radiusSquared); |
| | 1805 | 270 | | return true; |
| | | 271 | | } |
| | | 272 | | |
| | | 273 | | internal static bool TryGetCenteredAxisInterval(Signed320 start, Signed320 velocity, Signed320 cap, |
| | | 274 | | out Signed320 lowerN, out Signed320 lowerD, out Signed320 upperN, out Signed320 upperD) |
| | | 275 | | { |
| | 22586 | 276 | | lowerN = default; |
| | 22586 | 277 | | lowerD = upperN = upperD = Signed320.ExtendValue(Signed192.Signed(1)); |
| | 22586 | 278 | | if (velocity.IsZero) |
| | 9693 | 279 | | return WideArithmetic.AddSigned320(start, cap).Sign >= 0 |
| | 9693 | 280 | | && WideArithmetic.SubtractSigned320(start, cap).Sign <= 0; |
| | 12893 | 281 | | Signed320 first = WideArithmetic.SubtractSigned320(WideArithmetic.Negate(cap), start); |
| | 12893 | 282 | | Signed320 second = WideArithmetic.SubtractSigned320(cap, start); |
| | 12893 | 283 | | Signed320 divisor = velocity; |
| | 12893 | 284 | | if (velocity.Sign < 0) |
| | | 285 | | { |
| | 4833 | 286 | | (first, second) = (WideArithmetic.Negate(second), WideArithmetic.Negate(first)); |
| | 4833 | 287 | | divisor = WideArithmetic.Negate(divisor); |
| | | 288 | | } |
| | 17985 | 289 | | if (second.Sign < 0 || WideArithmetic.SubtractSigned320(first, divisor).Sign > 0) return false; |
| | 14863 | 290 | | if (first.Sign > 0) { lowerN = first; lowerD = divisor; } |
| | 16473 | 291 | | if (WideArithmetic.SubtractSigned320(second, divisor).Sign < 0) { upperN = second; upperD = divisor; } |
| | 7801 | 292 | | return true; |
| | | 293 | | } |
| | | 294 | | |
| | | 295 | | internal static bool IntersectsQuadratic(Signed576 a, Signed576 b, Signed576 c, |
| | | 296 | | Signed320 lowerN, Signed320 lowerD, Signed320 upperN, Signed320 upperD) |
| | | 297 | | { |
| | 2286 | 298 | | if (PolynomialSign(a, b, c, lowerN, lowerD) <= 0 || PolynomialSign(a, b, c, upperN, upperD) <= 0) return true; |
| | 2323 | 299 | | if (a.Sign == 0 || b.Sign >= 0) return false; |
| | 1605 | 300 | | Signed576 vertex = WideArithmetic.SubtractSigned576(default, b); |
| | 2859 | 301 | | if (CompareProducts(vertex, lowerD, a, lowerN) < 0 || CompareProducts(vertex, upperD, a, upperN) > 0) return fal |
| | | 302 | | System.Diagnostics.Debug.Assert(c.Sign > 0); |
| | 351 | 303 | | return WideArithmetic.CompareNonNegativeProducts(Signed832.ExtendValue(b), Signed832.ExtendValue(b), |
| | 351 | 304 | | Signed832.ExtendValue(a), Signed832.ExtendValue(c)) >= 0; |
| | | 305 | | } |
| | | 306 | | |
| | | 307 | | internal static bool IntersectsQuadratic(Signed832 a, Signed832 b, Signed832 c, |
| | | 308 | | Signed320 lowerN, Signed320 lowerD, Signed320 upperN, Signed320 upperD) |
| | | 309 | | { |
| | 3843 | 310 | | if (WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(a, b, c, lowerN, lowerD) <= 0 |
| | 3843 | 311 | | || WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(a, b, c, upperN, upperD) <= 0) |
| | 836 | 312 | | return true; |
| | 3007 | 313 | | if (a.Sign <= 0 || b.Sign >= 0) |
| | 1252 | 314 | | return false; |
| | 1755 | 315 | | Signed832 numerator = WideArithmetic.SubtractSigned832(default, b); |
| | 1755 | 316 | | if (WideArithmetic.CompareNonNegativeProducts(numerator, Signed832.ExtendValue(Signed576.ExtendValue(lowerD)), |
| | 1755 | 317 | | a, Signed832.ExtendValue(Signed576.ExtendValue(lowerN))) < 0 |
| | 1755 | 318 | | || WideArithmetic.CompareNonNegativeProducts(numerator, Signed832.ExtendValue(Signed576.ExtendValue(upperD)) |
| | 1755 | 319 | | a, Signed832.ExtendValue(Signed576.ExtendValue(upperN))) > 0) |
| | 1120 | 320 | | return false; |
| | 635 | 321 | | return WideArithmetic.CompareNonNegativeProducts(b, b, a, c) >= 0; |
| | | 322 | | } |
| | | 323 | | |
| | | 324 | | internal static bool IntersectsQuadratic(Signed832 a, Signed832 b, Signed832 c, |
| | | 325 | | Signed832 lowerN, Signed832 lowerD, Signed832 upperN, Signed832 upperD) |
| | | 326 | | { |
| | 141 | 327 | | if (WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(a, b, c, lowerN, lowerD) <= 0 |
| | 141 | 328 | | || WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(a, b, c, upperN, upperD) <= 0) |
| | 39 | 329 | | return true; |
| | 114 | 330 | | if (a.Sign <= 0 || b.Sign >= 0) return false; |
| | 90 | 331 | | Signed832 numerator = WideArithmetic.SubtractSigned832(default, b); |
| | 90 | 332 | | if (WideArithmetic.CompareNonNegativeProducts(numerator, lowerD, a, lowerN) < 0 |
| | 90 | 333 | | || WideArithmetic.CompareNonNegativeProducts(numerator, upperD, a, upperN) > 0) |
| | 65 | 334 | | return false; |
| | 25 | 335 | | return WideArithmetic.CompareNonNegativeProducts(b, b, a, c) >= 0; |
| | | 336 | | } |
| | | 337 | | |
| | | 338 | | /// <summary>Intersects closed shape bounds with an exact source subsegment before quadratic admission.</summary> |
| | | 339 | | internal static bool IntersectsQuadratic(Signed832 a, Signed832 b, Signed832 c, |
| | | 340 | | Signed320 lowerN, Signed320 lowerD, Signed320 upperN, Signed320 upperD, in RationalSegmentInterval clip) |
| | | 341 | | { |
| | 126 | 342 | | if (!clip.IsClipped) return IntersectsQuadratic(a, b, c, lowerN, lowerD, upperN, upperD); |
| | 168 | 343 | | Signed832 ln = Signed832.ExtendValue(lowerN), ld = Signed832.ExtendValue(lowerD), |
| | 168 | 344 | | un = Signed832.ExtendValue(upperN), ud = Signed832.ExtendValue(upperD); |
| | 84 | 345 | | if (RationalSegmentInterval.CompareParameters(ln, ld, clip.WideLowerNumerator, clip.WideLowerDenominator) < 0) |
| | 86 | 346 | | { ln = clip.WideLowerNumerator; ld = clip.WideLowerDenominator; } |
| | 84 | 347 | | if (RationalSegmentInterval.CompareParameters(un, ud, clip.WideUpperNumerator, clip.WideUpperDenominator) > 0) |
| | 130 | 348 | | { un = clip.WideUpperNumerator; ud = clip.WideUpperDenominator; } |
| | 84 | 349 | | return RationalSegmentInterval.CompareParameters(ln, ld, un, ud) <= 0 |
| | 84 | 350 | | && IntersectsQuadratic(a, b, c, ln, ld, un, ud); |
| | | 351 | | } |
| | | 352 | | |
| | | 353 | | /// <summary>Tests a rational borrowed quadratic on a closed clipped segment.</summary> |
| | | 354 | | internal static bool IntersectsQuadratic(scoped ContactQuadratic a, scoped ContactQuadratic b, |
| | | 355 | | scoped ContactQuadratic c, Signed320 lowerN, Signed320 lowerD, Signed320 upperN, Signed320 upperD) |
| | | 356 | | { |
| | 2636 | 357 | | Signed832 ln = Signed832.ExtendValue(Signed576.ExtendValue(lowerN)); |
| | 2636 | 358 | | Signed832 ld = Signed832.ExtendValue(Signed576.ExtendValue(lowerD)); |
| | 2636 | 359 | | Signed832 un = Signed832.ExtendValue(Signed576.ExtendValue(upperN)); |
| | 2636 | 360 | | Signed832 ud = Signed832.ExtendValue(Signed576.ExtendValue(upperD)); |
| | 2636 | 361 | | if (WideFiniteConeIntersection.GetBorrowedPolynomialSignAtRationalParameter(a, b, c, ln, ld) <= 0 |
| | 2636 | 362 | | || WideFiniteConeIntersection.GetBorrowedPolynomialSignAtRationalParameter(a, b, c, un, ud) <= 0) |
| | 13 | 363 | | return true; |
| | 4582 | 364 | | if (a.Signs[0] <= 0 || b.Signs[0] >= 0) return false; |
| | | 365 | | |
| | | 366 | | // Positive endpoints leave only an interior convex minimum. Preserve |
| | | 367 | | // both complete products, including the discriminant's subtraction bit. |
| | 664 | 368 | | int coefficientWords = Math.Max(a.FieldWords, Math.Max(b.FieldWords, c.FieldWords)); |
| | 664 | 369 | | int words = Math.Max(2 * coefficientWords, coefficientWords + 5) + 1; |
| | 664 | 370 | | Span<ulong> storage = stackalloc ulong[4 * words]; |
| | 664 | 371 | | Span<int> signs = stackalloc int[4]; |
| | 664 | 372 | | ContactQuadratic left = ContactQuadratic.At(storage, signs, 0, words); |
| | 664 | 373 | | ContactQuadratic right = ContactQuadratic.At(storage, signs, 1, words); |
| | 1328 | 374 | | ContactQuadratic.Scale(b, ld, left); left.MultiplySign(-1); |
| | 1328 | 375 | | ContactQuadratic.Scale(a, ln, right); left.Add(right, -1); |
| | 665 | 376 | | if (left.Signs[0] < 0) return false; |
| | 1326 | 377 | | ContactQuadratic.Scale(b, ud, left); left.MultiplySign(-1); |
| | 1326 | 378 | | ContactQuadratic.Scale(a, un, right); left.Add(right, -1); |
| | 1223 | 379 | | if (left.Signs[0] > 0) return false; |
| | 103 | 380 | | ContactQuadratic.Multiply(b, b, ReadOnlySpan<ulong>.Empty, left); |
| | 206 | 381 | | ContactQuadratic.Multiply(a, c, ReadOnlySpan<ulong>.Empty, right); left.Add(right, -1); |
| | 103 | 382 | | return left.Signs[0] >= 0; |
| | | 383 | | } |
| | | 384 | | |
| | 27113 | 385 | | private static Signed832 DotWide(WideAxis3 left, WideAxis3 right) => WideArithmetic.AddSigned832( |
| | 27113 | 386 | | WideArithmetic.AddSigned832(Product(left.X, right.X), Product(left.Y, right.Y)), Product(left.Z, right.Z)); |
| | | 387 | | |
| | | 388 | | private static Signed832 Product(Signed320 first, Signed320 second) => |
| | 81519 | 389 | | WideArithmetic.MultiplySigned576ToSigned832(Signed576.ExtendValue(first), second); |
| | | 390 | | |
| | | 391 | | private static int PolynomialSign(Signed576 a, Signed576 b, Signed576 c, Signed320 n, Signed320 d) => |
| | 4154 | 392 | | WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(Signed832.ExtendValue(a), |
| | 4154 | 393 | | Signed832.ExtendValue(b), Signed832.ExtendValue(c), n, d); |
| | | 394 | | |
| | | 395 | | /// <summary>Retains the first point of an already admitted convex quadratic interval.</summary> |
| | | 396 | | internal static void GetFirstQuadraticParameter(Signed576 a, Signed576 b, Signed576 c, |
| | | 397 | | Signed320 lowerN, Signed320 lowerD, ContactQuadratic qa, ContactQuadratic qb, ContactQuadratic qc, |
| | | 398 | | ContactQuadratic numerator, ContactQuadratic denominator, Span<ulong> root) |
| | | 399 | | { |
| | 21 | 400 | | if (PolynomialSign(a, b, c, lowerN, lowerD) <= 0) |
| | | 401 | | { |
| | 18 | 402 | | numerator.Set(Signed576.ExtendValue(lowerN)); denominator.Set(Signed576.ExtendValue(lowerD)); |
| | 9 | 403 | | return; |
| | | 404 | | } |
| | 36 | 405 | | qa.Set(a); qb.Set(b); qc.Set(c); |
| | | 406 | | // The lower root starts the admitted interval. Repeated roots use |
| | | 407 | | // the sole +1 branch and remain closed point intersections. |
| | 12 | 408 | | bool retained = ContactQuadratic.TryGetParameter(qa, qb, qc, -1, root, numerator, denominator); |
| | 14 | 409 | | if (!retained) retained = ContactQuadratic.TryGetParameter(qa, qb, qc, 1, root, numerator, denominator); |
| | | 410 | | System.Diagnostics.Debug.Assert(retained); |
| | 12 | 411 | | } |
| | | 412 | | |
| | | 413 | | private static int CompareProducts(Signed576 left, Signed320 leftScale, Signed576 right, Signed320 rightScale) => |
| | 2849 | 414 | | WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576ToSigned832(left, leftScale), |
| | 2849 | 415 | | WideArithmetic.MultiplySigned576ToSigned832(right, rightScale)).Sign; |
| | | 416 | | |
| | | 417 | | private static Signed576 Dot(WideAxis3 left, WideAxis3 right) => |
| | 5511 | 418 | | WideArithmetic.AddSigned576(WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned320(left.X, right.X), |
| | 5511 | 419 | | WideArithmetic.MultiplySigned320(left.Y, right.Y)), WideArithmetic.MultiplySigned320(left.Z, right.Z)); |
| | | 420 | | |
| | | 421 | | private static WideAxis3 Difference(WideAxis3 end, WideAxis3 start) => |
| | 305 | 422 | | new(WideArithmetic.SubtractSigned320(end.X, start.X), WideArithmetic.SubtractSigned320(end.Y, start.Y), |
| | 305 | 423 | | WideArithmetic.SubtractSigned320(end.Z, start.Z)); |
| | | 424 | | } |