| | | 1 | | //======================================================================= |
| | | 2 | | // CapsulePrismBoundaryContacts.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>Reuses finite capsule events over one already admitted native boundary stratum.</summary> |
| | | 13 | | internal sealed class CapsulePrismBoundaryContacts |
| | | 14 | | { |
| | | 15 | | private readonly CapsulePlaneRayEventVisitor _visitor; |
| | 22 | 16 | | private ulong[] _values = Array.Empty<ulong>(); |
| | 22 | 17 | | private readonly int[] _signs = new int[CapsulePlaneRaySelection.SignCount]; |
| | 22 | 18 | | private readonly sbyte[] _valueSigns = new sbyte[CapsulePlaneRaySelection.ValueSignCount]; |
| | | 19 | | private CapsulePlaneRaySelectionMetadata _winner; |
| | | 20 | | private CapsulePlaneRayFrame _frame; |
| | | 21 | | private Vector3d _center, _origin; |
| | | 22 | | private FixedQuaternion _rotation; |
| | | 23 | | private Fixed64 _length, _radius, _lowerY, _upperY, _yawAngle; |
| | | 24 | | private Vector2d _start, _end; |
| | | 25 | | private WideRationalBasis3d _basis; |
| | | 26 | | private WideConvex2dRelations.RotationFrame2d _yaw; |
| | | 27 | | private Signed320 _baseD, _lowerN, _lowerD, _upperN, _upperD; |
| | | 28 | | private WideDirection3d _worldNormal; |
| | | 29 | | private RationalSegmentInterval _clip, _support; |
| | | 30 | | private bool _supportPrepared, _hasSupport; |
| | | 31 | | |
| | 44 | 32 | | internal CapsulePrismBoundaryContacts() => _visitor = Keep; |
| | | 33 | | |
| | | 34 | | /// <summary>Restores one admitted source chart for connectivity without repeating finite event selection.</summary> |
| | | 35 | | internal bool PrepareSource(Vector3d center, FixedQuaternion rotation, Fixed64 length, Fixed64 radius, |
| | | 36 | | Vector3d origin, Fixed64 yaw, Vector2d start, Vector2d end, Vector2d inside, |
| | | 37 | | in RationalSegmentInterval clip, Fixed64 lowerY, Fixed64 upperY, int verticalNormal, |
| | | 38 | | out WideDirection3d localNormal) |
| | | 39 | | { |
| | 48 | 40 | | localNormal = default; _supportPrepared = _hasSupport = false; |
| | 26 | 41 | | if (lowerY > upperY || start == end) return false; |
| | 110 | 42 | | _center = center; _origin = origin; _rotation = rotation; _length = length; _radius = radius; |
| | 132 | 43 | | _start = start; _end = end; _lowerY = lowerY; _upperY = upperY; _clip = clip; _yawAngle = yaw; |
| | 44 | 44 | | _basis = new WideRationalBasis3d(rotation); _yaw = new WideConvex2dRelations.RotationFrame2d(yaw); |
| | 22 | 45 | | _baseD = WideArithmetic.MultiplySigned192(Signed192.One, _basis.Denominator); |
| | 44 | 46 | | _lowerN = default; _lowerD = _upperN = _upperD = Signed320.ExtendValue(Signed192.Signed(1)); |
| | 22 | 47 | | if (clip.IsClipped) |
| | | 48 | | { |
| | 9 | 49 | | ReduceParameter(clip.WideLowerNumerator, clip.WideLowerDenominator, out _lowerN, out _lowerD); |
| | 9 | 50 | | ReduceParameter(clip.WideUpperNumerator, clip.WideUpperDenominator, out _upperN, out _upperD); |
| | | 51 | | } |
| | 23 | 52 | | if (!GetNormal(inside, verticalNormal, out localNormal)) return false; |
| | 21 | 53 | | if (lowerY != upperY && (!clip.IsClipped || RationalSegmentInterval.CompareParameters(_lowerN, _lowerD, _upperN, |
| | 2 | 54 | | throw new ArgumentException("A boundary stratum must be a line or point.", nameof(clip)); |
| | 19 | 55 | | return true; |
| | | 56 | | } |
| | | 57 | | |
| | | 58 | | /// <summary>Finds the deepest physical ray on one fully admitted closed line or point.</summary> |
| | | 59 | | /// <remarks> |
| | | 60 | | /// At least one of the height range and tangent interval must be degenerate. The caller proves |
| | | 61 | | /// continuity/incident admission over the complete stratum before numerical event selection. |
| | | 62 | | /// A collapsed height and tangent interval represent an isolated boundary point. |
| | | 63 | | /// </remarks> |
| | | 64 | | internal bool TryGetContact(Vector3d center, FixedQuaternion rotation, Fixed64 length, Fixed64 radius, |
| | | 65 | | Vector3d origin, Fixed64 yaw, Vector2d start, Vector2d end, Vector2d inside, |
| | | 66 | | in RationalSegmentInterval clip, Fixed64 lowerY, Fixed64 upperY, int verticalNormal, |
| | | 67 | | out FixedContactAnchors contact) |
| | | 68 | | { |
| | 48 | 69 | | contact = default; _winner = default; |
| | 24 | 70 | | if (!PrepareSource(center, rotation, length, radius, origin, yaw, start, end, inside, clip, |
| | 27 | 71 | | lowerY, upperY, verticalNormal, out WideDirection3d localNormal)) return false; |
| | 19 | 72 | | bool horizontal = lowerY == upperY; |
| | 38 | 73 | | Signed320 zero = default, one = Signed320.ExtendValue(Signed192.Signed(1)); |
| | 19 | 74 | | WideAxis3 first = GetPoint(horizontal ? zero : _lowerN, horizontal ? one : _lowerD, lowerY, out Signed320 denomi |
| | 19 | 75 | | WideAxis3 last = GetPoint(horizontal ? one : _lowerN, horizontal ? one : _lowerD, upperY, out Signed320 lastDeno |
| | | 76 | | System.Diagnostics.Debug.Assert(lastDenominator.Equals(denominator)); |
| | | 77 | | // Native/quaternion products retain removable common scale. Reduce the |
| | | 78 | | // complete line chart together, preserving both points over one exact D. |
| | 19 | 79 | | Span<Signed320> coordinates = stackalloc Signed320[7] |
| | 19 | 80 | | { first.X, first.Y, first.Z, last.X, last.Y, last.Z, denominator }; |
| | 19 | 81 | | WideArithmetic.ReduceCommonScale(coordinates); |
| | 19 | 82 | | first = new(coordinates[0], coordinates[1], coordinates[2]); |
| | 38 | 83 | | last = new(coordinates[3], coordinates[4], coordinates[5]); denominator = coordinates[6]; |
| | 19 | 84 | | _frame = CapsulePlaneRayFrame.ForCapsulePointRay(length, radius, denominator, localNormal, WideDirection3d.Dot(l |
| | 19 | 85 | | Span<ulong> magnitude = stackalloc ulong[13]; |
| | 19 | 86 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(denominator), magnitude); |
| | 38 | 87 | | int pointBits = WideArithmetic.GetMagnitudeBitLength(magnitude) + 48, normalBits = 0; |
| | 152 | 88 | | for (int axis = 0; axis < 3; axis++) |
| | | 89 | | { |
| | 57 | 90 | | WideArithmetic.GetMagnitude(Signed832.ExtendValue(axis == 0 ? localNormal.X : axis == 1 ? localNormal.Y : lo |
| | 57 | 91 | | normalBits = Math.Max(normalBits, WideArithmetic.GetMagnitudeBitLength(magnitude)); |
| | | 92 | | } |
| | | 93 | | // Stationary roots need <6P+2N bits and recovered point coefficients |
| | | 94 | | // <5P+2N. Their complete quadratic-field norm needs <16P+6N. |
| | 20 | 95 | | if (16 * pointBits + 6 * normalBits + 32 > 64 * CapsulePlaneRaySelection.FieldWords) _frame = _frame.WithWidePar |
| | 37 | 96 | | if (_values.Length < _frame.SelectionStorageWords) Array.Resize(ref _values, _frame.SelectionStorageWords); |
| | 19 | 97 | | var source = new CapsulePlaneRayEventSource(new FixedSegment(new(start.X, lowerY, start.Y), new(end.X, upperY, e |
| | 19 | 98 | | CapsulePlaneRayEvents.VisitLocalSegmentEvents(source, _frame, first, last, 1, horizontal ? clip : default, _visi |
| | 20 | 99 | | if (!_winner.HasValue) return false; |
| | 18 | 100 | | var selection = BorrowWinner(); |
| | 18 | 101 | | bool movingRepresented = selection.TryGetExitAnchor(_frame, 1, center, rotation, |
| | 18 | 102 | | out FixedPointAnchor moving, out Fixed64 depth, out bool clamped); |
| | 18 | 103 | | bool targetRepresented = selection.TryGetPointInRigidFrame(_frame, origin, FixedQuaternion.Identity, |
| | 18 | 104 | | center, rotation, out Vector3d target); |
| | | 105 | | System.Diagnostics.Debug.Assert(movingRepresented && targetRepresented); |
| | 18 | 106 | | contact = new FixedContactAnchors(moving, new FixedPointAnchor(origin, FixedQuaternion.Identity, target), |
| | 18 | 107 | | _worldNormal.GetNormal(false), depth, clamped); |
| | 18 | 108 | | return true; |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | /// <summary>Tests whether this finite stratum meets an original support-core interval on its plane.</summary> |
| | | 112 | | internal bool ConnectsSupport(in RationalSegmentInterval coreInterval) |
| | | 113 | | { |
| | 7 | 114 | | if (!_supportPrepared) |
| | | 115 | | { |
| | 2 | 116 | | _supportPrepared = true; |
| | 2 | 117 | | Span<Signed832> points = stackalloc Signed832[6]; |
| | 2 | 118 | | WideConvexPrismRelations.GetProjectedCapsuleSupport(_center, _rotation, _length, _origin, |
| | 2 | 119 | | _yawAngle, new Vector3d(_start.X, _lowerY, _start.Y), _worldNormal, points, out Signed832 denominator); |
| | 2 | 120 | | _hasSupport = WideConvexPrismRelations.TryClipProjectedPrismFace(points, denominator, _yawAngle, |
| | 2 | 121 | | _start, _end, _clip, _lowerY, _upperY, default, out _support); |
| | | 122 | | } |
| | 7 | 123 | | return _hasSupport && (!coreInterval.IsClipped |
| | 7 | 124 | | || RationalSegmentInterval.CompareParameters(_support.WideLowerNumerator, _support.WideLowerDenominator, |
| | 7 | 125 | | coreInterval.WideUpperNumerator, coreInterval.WideUpperDenominator) <= 0 |
| | 7 | 126 | | && RationalSegmentInterval.CompareParameters(coreInterval.WideLowerNumerator, coreInterval.WideLowerDenomina |
| | 7 | 127 | | _support.WideUpperNumerator, _support.WideUpperDenominator) <= 0); |
| | | 128 | | } |
| | | 129 | | |
| | | 130 | | /// <summary>Tests one exact native tangent/height intersection against the complete capsule.</summary> |
| | | 131 | | internal bool ContainsPoint(Signed320 tangentN, Signed320 tangentD, Fixed64 height) |
| | | 132 | | { |
| | 3 | 133 | | WideAxis3 point = GetPoint(tangentN, tangentD, height, out Signed320 denominator); |
| | 3 | 134 | | var finite = new CapsuleFiniteSectionFrame(_length, _radius, denominator); |
| | 3 | 135 | | return finite.ContainsTransformedPoint(point); |
| | | 136 | | } |
| | | 137 | | |
| | | 138 | | private CapsulePlaneRaySelection BorrowWinner() |
| | | 139 | | { |
| | 78 | 140 | | var selection = new CapsulePlaneRaySelection(_values, _signs, _valueSigns, _frame.FieldWords, _frame.RootWords, |
| | 156 | 141 | | selection.Restore(_winner); return selection; |
| | | 142 | | } |
| | | 143 | | |
| | | 144 | | /// <summary>Compares this admitted line maximum with the borrowed native source depth before rounding.</summary> |
| | | 145 | | internal int CompareDepth(scoped in CapsulePrismContactDepth native) |
| | | 146 | | { |
| | | 147 | | System.Diagnostics.Debug.Assert(_winner.HasValue); |
| | 10 | 148 | | return native.Compare(BorrowWinner(), _frame); |
| | | 149 | | } |
| | | 150 | | |
| | | 151 | | private void Keep(in CapsulePlaneRayEventSource source, in CapsulePlaneRayFrame frame, |
| | | 152 | | CapsulePlaneRayEvent descriptor, int orientation, scoped in CapsulePlaneRaySelection candidate) |
| | | 153 | | { |
| | 150 | 154 | | var winner = BorrowWinner(); winner.KeepEvaluated(candidate, frame); _winner = winner.Metadata; |
| | 50 | 155 | | } |
| | | 156 | | |
| | | 157 | | private static void ReduceParameter(Signed832 n, Signed832 d, out Signed320 reducedN, out Signed320 reducedD) |
| | | 158 | | { |
| | 36 | 159 | | Span<ulong> numerator = stackalloc ulong[13], denominator = stackalloc ulong[13]; |
| | 36 | 160 | | WideArithmetic.GetMagnitude(n, numerator); WideArithmetic.GetMagnitude(d, denominator); |
| | 18 | 161 | | RationalSegmentInterval.PackReducedParameter(numerator, denominator, out n, out d); |
| | | 162 | | // Native side joins retain <162-bit coverage ratios before quaternion transformation. |
| | 18 | 163 | | reducedN = new Signed320(n.Word4, n.Word3, n.Word2, n.Word1, n.Word0); |
| | 18 | 164 | | reducedD = new Signed320(d.Word4, d.Word3, d.Word2, d.Word1, d.Word0); |
| | 18 | 165 | | } |
| | | 166 | | |
| | | 167 | | private bool GetNormal(Vector2d inside, int vertical, out WideDirection3d local) |
| | | 168 | | { |
| | 22 | 169 | | local = default; |
| | 22 | 170 | | WideConvex2dRelations.GetRotatedOffset(_yaw, _start, out Signed192 ax, out Signed192 az); |
| | 22 | 171 | | WideConvex2dRelations.GetRotatedOffset(_yaw, _end, out Signed192 bx, out Signed192 bz); |
| | 22 | 172 | | Signed320 side = WideArithmetic.MultiplySubtract(WideArithmetic.Difference(_end.X, _start.X), |
| | 22 | 173 | | WideArithmetic.Difference(inside.Y, _start.Y), WideArithmetic.Difference(_end.Y, _start.Y), |
| | 22 | 174 | | WideArithmetic.Difference(inside.X, _start.X)); |
| | 23 | 175 | | if (side.IsZero) return false; |
| | 21 | 176 | | Span<Signed576> normal = stackalloc Signed576[3] |
| | 21 | 177 | | { |
| | 21 | 178 | | Signed576.ExtendValue(WideArithmetic.SubtractSigned192(az, bz)), default, |
| | 21 | 179 | | Signed576.ExtendValue(WideArithmetic.SubtractSigned192(bx, ax)) |
| | 21 | 180 | | }; |
| | 21 | 181 | | if (side.Sign < 0) |
| | 121 | 182 | | for (int axis = 0; axis < 3; axis++) normal[axis] = WideArithmetic.SubtractSigned576(default, normal[axis]); |
| | 25 | 183 | | if (vertical != 0) { normal[0] = normal[2] = default; normal[1] = Signed576.ExtendValue(Signed192.Signed(vertica |
| | 42 | 184 | | WideArithmetic.ReduceCommonScale(normal); _worldNormal = new WideDirection3d(normal[0], normal[1], normal[2]); |
| | 21 | 185 | | WideRigidProjection.GetLocalAxisProjections(new WideAxis3(Signed320.NarrowValue(normal[0]), |
| | 21 | 186 | | Signed320.NarrowValue(normal[1]), Signed320.NarrowValue(normal[2])), _basis, out normal[0], out normal[1], o |
| | 42 | 187 | | WideArithmetic.ReduceCommonScale(normal); local = new WideDirection3d(normal[0], normal[1], normal[2]); |
| | 21 | 188 | | return true; |
| | | 189 | | } |
| | | 190 | | |
| | | 191 | | private WideAxis3 GetPoint(Signed320 n, Signed320 d, Fixed64 height, out Signed320 denominator) |
| | | 192 | | { |
| | 41 | 193 | | WideConvex2dRelations.GetRotatedOffset(_yaw, _start, out Signed192 ax, out Signed192 az); |
| | 41 | 194 | | WideConvex2dRelations.GetRotatedOffset(_yaw, _end, out Signed192 bx, out Signed192 bz); |
| | 82 | 195 | | Signed192 ex = WideArithmetic.SubtractSigned192(bx, ax), ez = WideArithmetic.SubtractSigned192(bz, az); |
| | 41 | 196 | | ax = WideArithmetic.AddSigned192(ax, Signed192.NarrowProven(WideArithmetic.MultiplySigned192( |
| | 41 | 197 | | WideArithmetic.Difference(_origin.X, _center.X), Signed192.One))); |
| | 41 | 198 | | az = WideArithmetic.AddSigned192(az, Signed192.NarrowProven(WideArithmetic.MultiplySigned192( |
| | 41 | 199 | | WideArithmetic.Difference(_origin.Z, _center.Z), Signed192.One))); |
| | 41 | 200 | | Signed192 ay = Signed192.NarrowProven(WideArithmetic.MultiplySigned192(WideArithmetic.AddSigned192( |
| | 41 | 201 | | WideArithmetic.Difference(_origin.Y, _center.Y), Signed192.Raw(height)), Signed192.One)); |
| | 41 | 202 | | var point = new WideAxis3(Signed320.NarrowValue(WideArithmetic.AddSigned576( |
| | 41 | 203 | | WideArithmetic.MultiplySigned320(d, Signed320.ExtendValue(ax)), WideArithmetic.MultiplySigned320(n, Signed32 |
| | 41 | 204 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(d, Signed320.ExtendValue(ay))), |
| | 41 | 205 | | Signed320.NarrowValue(WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned320(d, Signed320.ExtendValue( |
| | 41 | 206 | | WideArithmetic.MultiplySigned320(n, Signed320.ExtendValue(ez))))); |
| | 41 | 207 | | WideRigidProjection.GetLocalAxisProjections(point, _basis, out Signed576 x, out Signed576 y, out Signed576 z); |
| | 41 | 208 | | denominator = Signed320.NarrowValue(WideArithmetic.MultiplySigned320(_baseD, d)); |
| | | 209 | | // Native points <80 bits, compact cuts <162 and the inverse quaternion |
| | | 210 | | // <65 keep doubled coordinates <309 and their denominator <259 bits. |
| | 41 | 211 | | return new WideAxis3(Signed320.NarrowValue(WideArithmetic.AddSigned576(x, x)), |
| | 41 | 212 | | Signed320.NarrowValue(WideArithmetic.AddSigned576(y, y)), Signed320.NarrowValue(WideArithmetic.AddSigned576( |
| | | 213 | | } |
| | | 214 | | } |