| | | 1 | | //======================================================================= |
| | | 2 | | // FiniteSlabRayFrame.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>One canonical source-plane metric shared by all admitted finite domains.</summary> |
| | | 13 | | /// <content>Prepares exact slab charts and dispatches the shared finite-domain active sets.</content> |
| | | 14 | | internal readonly partial struct FiniteSlabRayFrame |
| | | 15 | | { |
| | | 16 | | internal readonly FiniteSlabSectionFrame Section; |
| | | 17 | | internal readonly CapsulePlaneRayFrame Plane; |
| | | 18 | | internal readonly Vector3d Origin, Center; |
| | | 19 | | internal readonly FixedQuaternion Rotation, SlabRotation; |
| | | 20 | | internal readonly Signed320 Radius, CoreCap; |
| | | 21 | | internal readonly bool HasCore; |
| | | 22 | | internal readonly FixedTriangle SourcePlane; |
| | | 23 | | internal readonly WideDirection3d RayNormal; |
| | | 24 | | internal readonly Signed832 RayNormalSquared; |
| | | 25 | | private readonly bool _wideParameters; |
| | 730 | 26 | | internal bool UsesWideBanks => Section.RadialFrame.ShapeFrame.HasExactScale || _wideParameters; |
| | 724 | 27 | | internal int FieldWords => UsesWideBanks ? FiniteSlabPolynomial.AffineWords : FiniteSlabPolynomial.Words; |
| | 2 | 28 | | internal int SelectionStorageWords => FiniteSlabRaySelection.GetStorageWords(FieldWords); |
| | 4 | 29 | | internal FiniteSlabRayFrame WithWideParameters() => UsesWideBanks ? this : new FiniteSlabRayFrame(this); |
| | 3 | 30 | | private FiniteSlabRayFrame(in FiniteSlabRayFrame source) { this = source; _wideParameters = true; } |
| | | 31 | | // HalfThickness retains the supplied extent; FullHeight is zero for authored half-thickness slabs. |
| | | 32 | | internal readonly Fixed64 RadiusValue, HalfThickness, FullHeight; |
| | | 33 | | |
| | | 34 | | internal FiniteSlabRayFrame(FixedTriangle sourcePlane, Vector3d origin, FixedQuaternion rotation, |
| | | 35 | | Vector3d center, FixedQuaternion slabRotation, Fixed64 length, Fixed64 radius, Fixed64 halfThickness, bool fullH |
| | 234 | 36 | | : this(sourcePlane, origin, rotation, center, slabRotation, length, radius, halfThickness, fullHeight, default, |
| | 234 | 37 | | { } |
| | | 38 | | |
| | | 39 | | internal FiniteSlabRayFrame(FixedTriangle sourcePlane, in ScaledRigidFrame3d sourceFrame, |
| | | 40 | | Vector3d center, FixedQuaternion slabRotation, Fixed64 length, Fixed64 radius, Fixed64 halfThickness, bool fullH |
| | 74 | 41 | | : this(sourcePlane, sourceFrame.RigidOrigin, sourceFrame.Rotation, center, slabRotation, length, radius, |
| | 74 | 42 | | halfThickness, fullHeight, sourceFrame, !sourceFrame.IsRigid) |
| | 74 | 43 | | { } |
| | | 44 | | |
| | | 45 | | private FiniteSlabRayFrame(FixedTriangle sourcePlane, Vector3d origin, FixedQuaternion rotation, |
| | | 46 | | Vector3d center, FixedQuaternion slabRotation, Fixed64 length, Fixed64 radius, Fixed64 halfThickness, |
| | | 47 | | bool fullHeight, in ScaledRigidFrame3d sourceFrame, bool exact) |
| | | 48 | | { |
| | 308 | 49 | | _wideParameters = false; |
| | 308 | 50 | | Section = exact ? new(sourceFrame, center, slabRotation, length, radius, halfThickness, fullHeight) |
| | 308 | 51 | | : new(origin, rotation, center, slabRotation, length, radius, halfThickness, fullHeight); |
| | 308 | 52 | | Plane = exact ? new(sourcePlane, sourceFrame, Section.RadialFrame) : new(sourcePlane, Section.RadialFrame); |
| | 1232 | 53 | | Origin = origin; Rotation = rotation; Center = center; SlabRotation = slabRotation; |
| | 308 | 54 | | Radius = Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 308 | 55 | | Section.RadialFrame.ShapeFrame.CoordinateDenominator, Section.RadialFrame.ShapeFrame.Radius)); |
| | 308 | 56 | | CoreCap = Section.RadialFrame.ShapeFrame.Cap; |
| | 308 | 57 | | HasCore = length != Fixed64.Zero; |
| | 308 | 58 | | SourcePlane = sourcePlane; |
| | 924 | 59 | | RadiusValue = radius; HalfThickness = halfThickness; FullHeight = fullHeight ? halfThickness : Fixed64.Zero; |
| | | 60 | | // Match the established plane-ray variable conditioning: numerator |
| | | 61 | | // coordinates include the rigid frame denominator, so an unscaled |
| | | 62 | | // primitive normal can make ordinary t enormous and u nearly one. |
| | 308 | 63 | | Signed320 bound = WideArithmetic.AddSigned320(WideArithmetic.AddSigned320(Radius, CoreCap), Section.Cap); |
| | 308 | 64 | | Span<ulong> magnitude = stackalloc ulong[13]; |
| | 308 | 65 | | WideArithmetic.GetMagnitude(Signed576.ExtendValue(bound), magnitude); |
| | 308 | 66 | | int boundBits = WideArithmetic.GetMagnitudeBitLength(magnitude); |
| | 308 | 67 | | WideArithmetic.GetMagnitude(Plane.NormalSquared, magnitude); |
| | 308 | 68 | | int normalPower = (WideArithmetic.GetMagnitudeBitLength(magnitude) - 1) / 2; |
| | 308 | 69 | | int shift = Math.Max(0, boundBits + 2 - normalPower); |
| | | 70 | | // The resulting components need at most max(original bits,bound+3), |
| | | 71 | | // below 268 rigid or 352 affine bits; conditioning never adds a |
| | | 72 | | // fixed shift to an already maximal normal. |
| | 308 | 73 | | RayNormal = exact ? new(ScalePowerOfTwo(Plane.Normal.X, shift), ScalePowerOfTwo(Plane.Normal.Y, shift), |
| | 308 | 74 | | ScalePowerOfTwo(Plane.Normal.Z, shift)) |
| | 308 | 75 | | : new WideDirection3d(new WideAxis3(ScalePowerOfTwo(Signed320.NarrowValue(Plane.Normal.X), shift), |
| | 308 | 76 | | ScalePowerOfTwo(Signed320.NarrowValue(Plane.Normal.Y), shift), |
| | 308 | 77 | | ScalePowerOfTwo(Signed320.NarrowValue(Plane.Normal.Z), shift))); |
| | 308 | 78 | | RayNormalSquared = RayNormal.SquaredLength; |
| | 308 | 79 | | } |
| | | 80 | | |
| | | 81 | | internal Vector3d GetNormal(int orientation) => |
| | 1 | 82 | | Plane.GetWorldNormal(Rotation) * (Fixed64)(-orientation); |
| | | 83 | | |
| | | 84 | | /// <summary>Compares the positive and negative whole-plane exit depths before materialization.</summary> |
| | | 85 | | /// <remarks>The caller must establish plane/slab intersection before selecting a nonnegative exit.</remarks> |
| | | 86 | | internal int ComparePlaneExitDepths(ReadOnlySpan<Vector2d> offsets) |
| | | 87 | | { |
| | 144 | 88 | | Signed832 sum = default; |
| | 144 | 89 | | if (!offsets.IsEmpty) |
| | | 90 | | { |
| | 48 | 91 | | Signed320 scale = WideArithmetic.AddSigned320(Section.RadialFrame.ShapeFrame.CoordinateDenominator, |
| | 48 | 92 | | Section.RadialFrame.ShapeFrame.CoordinateDenominator); |
| | 96 | 93 | | Signed832 low = default, high = default; |
| | 384 | 94 | | for (int index = 0; index < offsets.Length; index++) |
| | | 95 | | { |
| | 144 | 96 | | WideAxis3 point = new(Signed320.NarrowValue(WideArithmetic.MultiplySigned320(scale, Signed192.Raw(offset |
| | 144 | 97 | | default, Signed320.NarrowValue(WideArithmetic.MultiplySigned320(scale, Signed192.Raw(offsets[index]. |
| | 144 | 98 | | Signed832 value = WideDirection3d.Dot(Plane.Normal, point); |
| | 216 | 99 | | if (index == 0 || WideArithmetic.SubtractSigned832(value, low).Sign < 0) low = value; |
| | 216 | 100 | | if (index == 0 || WideArithmetic.SubtractSigned832(value, high).Sign > 0) high = value; |
| | | 101 | | } |
| | 48 | 102 | | sum = WideArithmetic.AddSigned832(low, high); |
| | | 103 | | } |
| | | 104 | | // Height caps and rounded footprints are symmetric. Affine coordinates |
| | | 105 | | // and normal components are <352 bits; these dot sums fit within 832 bits. |
| | 144 | 106 | | return WideArithmetic.SubtractSigned832(sum, |
| | 144 | 107 | | WideArithmetic.AddSigned832(Plane.PlaneConstant, Plane.PlaneConstant)).Sign; |
| | | 108 | | } |
| | | 109 | | |
| | | 110 | | /// <summary>Affine dimension of the shape face maximizing the oriented plane functional.</summary> |
| | | 111 | | internal int GetSupportDimension(ReadOnlySpan<Vector2d> prismOffsets, int orientation) |
| | | 112 | | { |
| | 117 | 113 | | if (orientation != -1 && orientation != 1) throw new ArgumentOutOfRangeException(nameof(orientation)); |
| | | 114 | | // A zero normal coefficient leaves that factor of the extrusion free. |
| | | 115 | | // The stadium disk and core share the X/Z plane, whose dimension is at most two. |
| | 115 | 116 | | int height = Plane.Normal.Y.IsZero && Section.Cap.Sign > 0 ? 1 : 0; |
| | 115 | 117 | | bool axial = Plane.Normal.X.IsZero && Plane.Normal.Z.IsZero; |
| | 115 | 118 | | if (prismOffsets.IsEmpty) |
| | | 119 | | { |
| | 71 | 120 | | int footprint = axial && RadiusValue > Fixed64.Zero ? 2 |
| | 71 | 121 | | : HasCore && Plane.Normal.Z.IsZero ? 1 : 0; |
| | 71 | 122 | | return height + footprint; |
| | | 123 | | } |
| | 44 | 124 | | _ = FiniteSlabSectionFrame.GetPrismWinding(prismOffsets); |
| | 46 | 125 | | if (axial) return height + 2; |
| | 126 | 126 | | Signed576 maximum = default; Vector2d vertex = default; bool tied = false; |
| | 404 | 127 | | for (int index = 0; index < prismOffsets.Length; index++) |
| | | 128 | | { |
| | 160 | 129 | | Vector2d offset = prismOffsets[index]; |
| | 160 | 130 | | Signed576 projection = WideArithmetic.AddSigned576( |
| | 160 | 131 | | WideArithmetic.MultiplySigned576(Plane.Normal.X, offset.X.m_rawValue), |
| | 160 | 132 | | WideArithmetic.MultiplySigned576(Plane.Normal.Z, offset.Y.m_rawValue)); |
| | 240 | 133 | | if (orientation < 0) projection = WideArithmetic.SubtractSigned576(default, projection); |
| | 160 | 134 | | int order = WideArithmetic.SubtractSigned576(projection, maximum).Sign; |
| | 379 | 135 | | if (index == 0 || order > 0) { maximum = projection; vertex = offset; tied = false; } |
| | 107 | 136 | | else if (order == 0 && offset != vertex) tied = true; |
| | | 137 | | } |
| | 42 | 138 | | return height + (tied ? 1 : 0); |
| | | 139 | | } |
| | | 140 | | |
| | | 141 | | private static Signed320 ScalePowerOfTwo(Signed320 value, int shift) |
| | | 142 | | { |
| | 1830 | 143 | | while (shift > 0) |
| | | 144 | | { |
| | 963 | 145 | | int step = Math.Min(63, shift); |
| | 963 | 146 | | WideArithmetic.ShiftLeft(value.Word4, value.Word3, value.Word2, value.Word1, value.Word0, step, |
| | 963 | 147 | | out ulong w4, out ulong w3, out ulong w2, out ulong w1, out ulong w0); |
| | 1926 | 148 | | value = new(w4, w3, w2, w1, w0); shift -= step; |
| | | 149 | | } |
| | 867 | 150 | | return value; |
| | | 151 | | } |
| | | 152 | | |
| | | 153 | | private static Signed576 ScalePowerOfTwo(Signed576 value, int shift) |
| | | 154 | | { |
| | 333 | 155 | | while (shift > 0) |
| | | 156 | | { |
| | 222 | 157 | | int step = Math.Min(62, shift); |
| | 222 | 158 | | value = WideArithmetic.MultiplySigned576(value, 1L << step); |
| | 222 | 159 | | shift -= step; |
| | | 160 | | } |
| | 111 | 161 | | return value; |
| | | 162 | | } |
| | | 163 | | } |