| | | 1 | | //======================================================================= |
| | | 2 | | // ConeFiniteSectionFrame.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-cone coordinates, closed admission, and rational segment clipping.</summary> |
| | | 13 | | internal readonly struct ConeFiniteSectionFrame |
| | | 14 | | { |
| | | 15 | | internal readonly CylinderPolytopeFrame ShapeFrame; |
| | | 16 | | internal readonly Fixed64 Height, Radius; |
| | | 17 | | internal readonly Signed320 FullHeight; |
| | | 18 | | |
| | | 19 | | /// <summary>Exact cone-local identity chart supplied by a retained rational native source.</summary> |
| | | 20 | | internal ConeFiniteSectionFrame(Fixed64 height, Fixed64 radius, Signed320 coordinateDenominator) |
| | | 21 | | { |
| | 66 | 22 | | Height = height; Radius = radius; |
| | 33 | 23 | | ShapeFrame = new CylinderPolytopeFrame(height, radius, coordinateDenominator); |
| | 33 | 24 | | FullHeight = WideArithmetic.AddSigned320(ShapeFrame.Cap, ShapeFrame.Cap); |
| | 33 | 25 | | } |
| | | 26 | | |
| | | 27 | | internal ConeFiniteSectionFrame(Vector3d origin, FixedQuaternion rotation, |
| | | 28 | | Vector3d coneCenter, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius) |
| | | 29 | | { |
| | 2820 | 30 | | if (height <= Fixed64.Zero) |
| | 2 | 31 | | throw new ArgumentOutOfRangeException(nameof(height)); |
| | 2818 | 32 | | if (radius < Fixed64.Zero) |
| | 1 | 33 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 2817 | 34 | | if (!rotation.IsNormalized()) |
| | 1 | 35 | | throw new ArgumentException("The authored rotation must be normalized.", nameof(rotation)); |
| | 2816 | 36 | | if (!coneRotation.IsNormalized()) |
| | 1 | 37 | | throw new ArgumentException("The cone rotation must be normalized.", nameof(coneRotation)); |
| | 5630 | 38 | | Height = height; Radius = radius; |
| | 2815 | 39 | | ShapeFrame = new CylinderPolytopeFrame(coneCenter, coneRotation, height, radius, origin, rotation); |
| | 2815 | 40 | | FullHeight = WideArithmetic.AddSigned320(ShapeFrame.Cap, ShapeFrame.Cap); |
| | 2815 | 41 | | } |
| | | 42 | | |
| | | 43 | | internal ConeFiniteSectionFrame(in ScaledRigidFrame3d sourceFrame, |
| | | 44 | | Vector3d coneCenter, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius) |
| | | 45 | | { |
| | 56 | 46 | | if (sourceFrame.IsRigid) |
| | | 47 | | { |
| | 4 | 48 | | this = new ConeFiniteSectionFrame(sourceFrame.RigidOrigin, sourceFrame.Rotation, |
| | 4 | 49 | | coneCenter, coneRotation, height, radius); |
| | 4 | 50 | | return; |
| | | 51 | | } |
| | 52 | 52 | | if (height <= Fixed64.Zero || height > (Fixed64)8192) |
| | 2 | 53 | | throw new ArgumentOutOfRangeException(nameof(height)); |
| | 50 | 54 | | if (radius < Fixed64.Zero || radius > (Fixed64)4096) |
| | 2 | 55 | | throw new ArgumentOutOfRangeException(nameof(radius)); |
| | 48 | 56 | | if (!coneRotation.IsNormalized()) |
| | 1 | 57 | | throw new ArgumentException("The cone rotation must be normalized.", nameof(coneRotation)); |
| | 94 | 58 | | Height = height; Radius = radius; |
| | 47 | 59 | | ShapeFrame = new CylinderPolytopeFrame(sourceFrame, coneCenter, coneRotation, |
| | 47 | 60 | | Signed192.Raw(height), radius); |
| | 47 | 61 | | FullHeight = WideArithmetic.AddSigned320(ShapeFrame.Cap, ShapeFrame.Cap); |
| | 47 | 62 | | } |
| | | 63 | | |
| | | 64 | | internal WideAxis3 Transform(Vector3d point) |
| | | 65 | | { |
| | 59685 | 66 | | WideAxis3 centered = ShapeFrame.Transform(point); |
| | 59685 | 67 | | return new WideAxis3(centered.X, WideArithmetic.SubtractSigned320(ShapeFrame.Cap, centered.Y), centered.Z); |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | internal bool IntersectsTransformedSegment(WideAxis3 start, WideAxis3 end) |
| | | 71 | | { |
| | 4429 | 72 | | WideAxis3 delta = new(WideArithmetic.SubtractSigned320(end.X, start.X), |
| | 4429 | 73 | | WideArithmetic.SubtractSigned320(end.Y, start.Y), WideArithmetic.SubtractSigned320(end.Z, start.Z)); |
| | 4429 | 74 | | if (!TryGetAxialInterval(start.Y, delta.Y, out Signed320 lowerN, out Signed320 lowerD, |
| | 4429 | 75 | | out Signed320 upperN, out Signed320 upperD)) |
| | 23 | 76 | | return false; |
| | 4406 | 77 | | if (ShapeFrame.HasExactScale) |
| | 26 | 78 | | return IntersectsWideQuadratic(start, delta, lowerN, lowerD, upperN, upperD); |
| | 13140 | 79 | | Signed576 a = QuadraticProduct(delta, delta), b = QuadraticProduct(start, delta), c = QuadraticProduct(start, st |
| | 4380 | 80 | | if (WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(Signed832.ExtendValue(a), Signed832.ExtendVa |
| | 4380 | 81 | | || WideFiniteConeIntersection.GetPolynomialSignAtRationalParameter(Signed832.ExtendValue(a), Signed832.Exten |
| | 4090 | 82 | | return true; |
| | | 83 | | // A concave/linear quadratic attains its minimum at an endpoint. |
| | | 84 | | // The remaining convex quadratic has one stationary parameter -b/a. |
| | 290 | 85 | | if (a.Sign <= 0 || b.Sign >= 0) |
| | 20 | 86 | | return false; |
| | 270 | 87 | | Signed576 numerator = WideArithmetic.SubtractSigned576(default, b); |
| | 270 | 88 | | if (CompareProducts(numerator, lowerD, a, lowerN) < 0 |
| | 270 | 89 | | || CompareProducts(numerator, upperD, a, upperN) > 0) |
| | 25 | 90 | | return false; |
| | | 91 | | // F decreases from t=0 to -b/a. The clipped lower endpoint lies |
| | | 92 | | // in that interval and has F>0, so F(0)=c must also be positive. |
| | | 93 | | System.Diagnostics.Debug.Assert(c.Sign > 0); |
| | 245 | 94 | | return WideArithmetic.CompareNonNegativeProducts(Signed832.ExtendValue(b), Signed832.ExtendValue(b), |
| | 245 | 95 | | Signed832.ExtendValue(a), Signed832.ExtendValue(c)) >= 0; |
| | | 96 | | } |
| | | 97 | | |
| | | 98 | | /// <summary>Tests a pretransformed source subsegment, retaining its exact original parameter.</summary> |
| | | 99 | | internal bool IntersectsTransformedSegment(WideAxis3 start, WideAxis3 end, in RationalSegmentInterval clip) |
| | | 100 | | { |
| | 36 | 101 | | if (!clip.IsClipped) return IntersectsTransformedSegment(start, end); |
| | 10 | 102 | | WideAxis3 delta = new(WideArithmetic.SubtractSigned320(end.X, start.X), |
| | 10 | 103 | | WideArithmetic.SubtractSigned320(end.Y, start.Y), WideArithmetic.SubtractSigned320(end.Z, start.Z)); |
| | 10 | 104 | | return TryGetAxialInterval(start.Y, delta.Y, out var ln, out var ld, out var un, out var ud) |
| | 10 | 105 | | && CapsuleFiniteSectionFrame.IntersectsQuadratic(QuadraticProductWide(delta, delta), |
| | 10 | 106 | | QuadraticProductWide(start, delta), QuadraticProductWide(start, start), ln, ld, un, ud, clip); |
| | | 107 | | } |
| | | 108 | | |
| | 1347 | 109 | | internal bool ContainsTransformedPoint(WideAxis3 point) => point.Y.Sign >= 0 |
| | 1347 | 110 | | && WideArithmetic.SubtractSigned320(point.Y, FullHeight).Sign <= 0 |
| | 1347 | 111 | | && (ShapeFrame.HasExactScale ? QuadraticProductWide(point, point).Sign : QuadraticProduct(point, point).Sign) <= |
| | | 112 | | |
| | | 113 | | internal Signed832 QuadraticProductWide(WideAxis3 left, WideAxis3 right) |
| | | 114 | | { |
| | 12473 | 115 | | if (!ShapeFrame.HasExactScale) |
| | 12342 | 116 | | return Signed832.ExtendValue(QuadraticProduct(left, right)); |
| | | 117 | | // Admitted doubled coordinates are <306 bits and H/R raw <46. |
| | | 118 | | // Three squared terms and two extent factors stay below 707 bits. |
| | 131 | 119 | | Signed832 radial = WideArithmetic.AddSigned832(Product(left.X, right.X), Product(left.Z, right.Z)); |
| | 131 | 120 | | Signed832 axial = Product(left.Y, right.Y); |
| | 262 | 121 | | Signed192 height = Signed192.Raw(Height), radius = Signed192.Raw(Radius); |
| | 131 | 122 | | return WideArithmetic.SubtractSigned832( |
| | 131 | 123 | | WideArithmetic.MultiplySigned832(WideArithmetic.MultiplySigned832(radial, height), height), |
| | 131 | 124 | | WideArithmetic.MultiplySigned832(WideArithmetic.MultiplySigned832(axial, radius), radius)); |
| | | 125 | | } |
| | | 126 | | |
| | | 127 | | private bool IntersectsWideQuadratic(WideAxis3 start, WideAxis3 delta, |
| | | 128 | | Signed320 lowerN, Signed320 lowerD, Signed320 upperN, Signed320 upperD) |
| | | 129 | | { |
| | 52 | 130 | | Signed832 a = QuadraticProductWide(delta, delta), b = QuadraticProductWide(start, delta), |
| | 26 | 131 | | c = QuadraticProductWide(start, start); |
| | 26 | 132 | | return CapsuleFiniteSectionFrame.IntersectsQuadratic(a, b, c, lowerN, lowerD, upperN, upperD); |
| | | 133 | | } |
| | | 134 | | |
| | | 135 | | private static Signed832 Product(Signed320 first, Signed320 second) => |
| | 393 | 136 | | WideArithmetic.MultiplySigned576ToSigned832(Signed576.ExtendValue(first), second); |
| | | 137 | | |
| | | 138 | | internal Signed576 QuadraticProduct(WideAxis3 left, WideAxis3 right) |
| | | 139 | | { |
| | | 140 | | // Unreduced doubled rigid coordinates and axial differences <198 |
| | | 141 | | // bits; raw H/R <63. Both products and their signed sum <528 bits. |
| | | 142 | | // Axial clipping excludes the opposite nappe before F<=0 admission. |
| | 26758 | 143 | | Signed576 radial = WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned320(left.X, right.X), |
| | 26758 | 144 | | WideArithmetic.MultiplySigned320(left.Z, right.Z)); |
| | 26758 | 145 | | Signed576 axial = WideArithmetic.MultiplySigned320(left.Y, right.Y); |
| | 26758 | 146 | | return WideArithmetic.SubtractSigned576( |
| | 26758 | 147 | | WideArithmetic.MultiplySigned576(radial, Signed192.Raw(Height), Signed192.Raw(Height)), |
| | 26758 | 148 | | WideArithmetic.MultiplySigned576(axial, Signed192.Raw(Radius), Signed192.Raw(Radius))); |
| | | 149 | | } |
| | | 150 | | |
| | | 151 | | internal bool TryGetAxialInterval(Signed320 start, Signed320 velocity, |
| | | 152 | | out Signed320 lowerN, out Signed320 lowerD, out Signed320 upperN, out Signed320 upperD) |
| | | 153 | | { |
| | 15566 | 154 | | lowerN = default; |
| | 15566 | 155 | | lowerD = upperN = upperD = Signed320.ExtendValue(Signed192.Signed(1)); |
| | 15566 | 156 | | if (velocity.IsZero) |
| | 2812 | 157 | | return start.Sign >= 0 && WideArithmetic.SubtractSigned320(start, FullHeight).Sign <= 0; |
| | 12754 | 158 | | Signed320 first = WideArithmetic.Negate(start); |
| | 12754 | 159 | | Signed320 second = WideArithmetic.SubtractSigned320(FullHeight, start); |
| | 12754 | 160 | | Signed320 divisor = velocity; |
| | 12754 | 161 | | if (velocity.Sign < 0) |
| | | 162 | | { |
| | 5816 | 163 | | (first, second) = (WideArithmetic.Negate(second), WideArithmetic.Negate(first)); |
| | 5816 | 164 | | divisor = WideArithmetic.Negate(divisor); |
| | | 165 | | } |
| | 12754 | 166 | | if (second.Sign < 0 || WideArithmetic.SubtractSigned320(first, divisor).Sign > 0) |
| | 10 | 167 | | return false; |
| | 30382 | 168 | | if (first.Sign > 0) { lowerN = first; lowerD = divisor; } |
| | 30526 | 169 | | if (WideArithmetic.SubtractSigned320(second, divisor).Sign < 0) { upperN = second; upperD = divisor; } |
| | 12744 | 170 | | return true; |
| | | 171 | | } |
| | | 172 | | |
| | | 173 | | private static int CompareProducts(Signed576 left, Signed320 leftScale, Signed576 right, Signed320 rightScale) |
| | | 174 | | { |
| | | 175 | | // <528+198 bits fits Signed832; endpoint polynomial evaluation uses |
| | | 176 | | // the existing wide owner's transient products rather than narrowing. |
| | 529 | 177 | | return WideArithmetic.SubtractSigned832( |
| | 529 | 178 | | WideArithmetic.MultiplySigned576ToSigned832(left, leftScale), |
| | 529 | 179 | | WideArithmetic.MultiplySigned576ToSigned832(right, rightScale)).Sign; |
| | | 180 | | } |
| | | 181 | | } |