| | | 1 | | //======================================================================= |
| | | 2 | | // CapsulePlaneRaySelection.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 | | |
| | | 13 | | |
| | | 14 | | /// <summary>Borrowed exact point and depth storage for an admitted capsule plane ray.</summary> |
| | | 15 | | /// <content>Native event selection, retained coefficient banks and witness reconstruction.</content> |
| | | 16 | | internal ref partial struct CapsulePlaneRaySelection |
| | | 17 | | { |
| | | 18 | | internal const int FieldWords = 64; |
| | | 19 | | internal const int RootWords = 48; |
| | | 20 | | private const int QuadraticStorageWords = 12 * FieldWords + RootWords; |
| | | 21 | | internal const int StorageWords = QuadraticStorageWords + CapsulePlaneRayValue.StorageWords; |
| | | 22 | | internal const int SignCount = 12; |
| | | 23 | | internal const int ValueSignCount = CapsulePlaneRayValue.SignCount; |
| | | 24 | | internal readonly int CoefficientWords, RadicalWords, PolynomialWords; |
| | 960 | 25 | | private readonly int QuadraticWords => 12 * CoefficientWords + RadicalWords; |
| | | 26 | | internal static int GetStorageWords(int fieldWords, int rootWords, int valueWords) => |
| | 8274 | 27 | | 12 * fieldWords + rootWords + CapsulePlaneRayValue.GetStorageWords(valueWords); |
| | | 28 | | internal readonly Span<ulong> Values; |
| | | 29 | | internal readonly Span<int> Signs; |
| | | 30 | | internal readonly Span<sbyte> ValueSigns; |
| | | 31 | | internal bool HasValue; |
| | | 32 | | private bool usesValueRoot; |
| | | 33 | | private CapsulePlaneRayValueMetadata valueMetadata; |
| | | 34 | | internal int Primitive; |
| | | 35 | | internal CapsulePlaneRayEventSource CurrentSource, MaximumSource; |
| | | 36 | | internal CapsulePlaneRayEvent CurrentEvent, MaximumEvent; |
| | 6187 | 37 | | internal readonly ContactQuadratic X => ContactQuadratic.At(Values, Signs, 0, CoefficientWords); |
| | 5790 | 38 | | internal readonly ContactQuadratic Y => ContactQuadratic.At(Values, Signs, 1, CoefficientWords); |
| | 5755 | 39 | | internal readonly ContactQuadratic Z => ContactQuadratic.At(Values, Signs, 2, CoefficientWords); |
| | 6664 | 40 | | internal readonly ContactQuadratic Denominator => ContactQuadratic.At(Values, Signs, 3, CoefficientWords); |
| | 6952 | 41 | | internal readonly ContactQuadratic DepthNumerator => ContactQuadratic.At(Values, Signs, 4, CoefficientWords); |
| | 7972 | 42 | | internal readonly ContactQuadratic DepthDenominator => ContactQuadratic.At(Values, Signs, 5, CoefficientWords); |
| | 9923 | 43 | | internal readonly ReadOnlySpan<ulong> Root => Values.Slice(12 * CoefficientWords, RadicalWords); |
| | 289 | 44 | | internal readonly CapsulePlaneRaySelectionMetadata Metadata => new(HasValue, usesValueRoot, Primitive, |
| | 289 | 45 | | MaximumSource, MaximumEvent, valueMetadata); |
| | | 46 | | |
| | | 47 | | /// <summary>Compares exact base anchors without materializing or evaluating their exits.</summary> |
| | | 48 | | internal readonly int ComparePoint(scoped in CapsulePlaneRaySelection other, scoped in CapsulePlaneRayFrame frame) = |
| | 528 | 49 | | frame.ComparePoints(X, Y, Z, Denominator, Root, other.X, other.Y, other.Z, other.Denominator, other.Root); |
| | | 50 | | |
| | | 51 | | internal CapsulePlaneRaySelection(Span<ulong> values, Span<int> signs, Span<sbyte> valueSigns, |
| | | 52 | | int fieldWords = FieldWords, int rootWords = RootWords, int valueWords = CapsulePlaneRayValue.PolynomialWords) |
| | | 53 | | { |
| | 3291 | 54 | | if (values.Length < GetStorageWords(fieldWords, rootWords, valueWords) || signs.Length < SignCount || valueSigns |
| | 3 | 55 | | throw new ArgumentException("The capsule ray selection storage is too small."); |
| | 9864 | 56 | | CoefficientWords = fieldWords; RadicalWords = rootWords; PolynomialWords = valueWords; |
| | 13152 | 57 | | Values = values[..GetStorageWords(fieldWords, rootWords, valueWords)]; Signs = signs[..SignCount]; HasValue = fa |
| | 9864 | 58 | | ValueSigns = valueSigns[..ValueSignCount]; usesValueRoot = false; valueMetadata = default; |
| | 6576 | 59 | | CurrentSource = MaximumSource = default; CurrentEvent = MaximumEvent = default; |
| | 3288 | 60 | | } |
| | | 61 | | |
| | | 62 | | private readonly CapsulePlaneRayValue Value |
| | | 63 | | { |
| | | 64 | | get |
| | | 65 | | { |
| | 679 | 66 | | var value = new CapsulePlaneRayValue(Values[QuadraticWords..], ValueSigns, PolynomialWords); |
| | 1358 | 67 | | value.Restore(valueMetadata); return value; |
| | | 68 | | } |
| | | 69 | | } |
| | | 70 | | |
| | | 71 | | internal void Keep(scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQu |
| | | 72 | | scoped ContactQuadratic numerator, scoped ContactQuadratic divisor, scoped ReadOnlySpan<ulong> root, int primiti |
| | | 73 | | in CapsulePlaneRayFrame frame) |
| | | 74 | | { |
| | 2055 | 75 | | if (HasValue) |
| | | 76 | | { |
| | 289 | 77 | | int comparison = usesValueRoot ? -Value.CompareQuadratic(numerator, divisor, root) |
| | 289 | 78 | | : ContactQuadratic.CompareRatios(numerator, divisor, root, DepthNumerator, DepthDenominator, Root); |
| | 473 | 79 | | if (comparison < 0) return; |
| | 105 | 80 | | if (comparison == 0) |
| | | 81 | | { |
| | 58 | 82 | | int order = frame.ComparePoints(x, y, z, denominator, root, X, Y, Z, Denominator, Root); |
| | 112 | 83 | | if (order > 0 || order == 0 && primitive >= Primitive) return; |
| | | 84 | | } |
| | | 85 | | } |
| | 7268 | 86 | | x.CopyTo(X); y.CopyTo(Y); z.CopyTo(Z); denominator.CopyTo(Denominator); |
| | 3634 | 87 | | numerator.CopyTo(DepthNumerator); divisor.CopyTo(DepthDenominator); |
| | 1817 | 88 | | Span<ulong> destination = Values.Slice(12 * CoefficientWords, RadicalWords); |
| | 3634 | 89 | | root.CopyTo(destination); destination[root.Length..].Clear(); |
| | 3634 | 90 | | HasValue = true; Primitive = primitive; |
| | 3634 | 91 | | usesValueRoot = false; valueMetadata = default; |
| | 3634 | 92 | | MaximumSource = CurrentSource; MaximumEvent = CurrentEvent; |
| | 1817 | 93 | | } |
| | | 94 | | |
| | | 95 | | internal readonly int CompareDepth(scoped in CapsulePlaneRaySelection other) |
| | | 96 | | { |
| | 874 | 97 | | if (usesValueRoot) |
| | 116 | 98 | | return other.usesValueRoot ? Value.Compare(other.Value) |
| | 116 | 99 | | : Value.CompareQuadratic(other.DepthNumerator, other.DepthDenominator, other.Root); |
| | 758 | 100 | | return other.usesValueRoot ? -other.Value.CompareQuadratic(DepthNumerator, DepthDenominator, Root) |
| | 758 | 101 | | : ContactQuadratic.CompareRatios(DepthNumerator, DepthDenominator, Root, |
| | 758 | 102 | | other.DepthNumerator, other.DepthDenominator, other.Root); |
| | | 103 | | } |
| | | 104 | | |
| | | 105 | | /// <summary>Compares this physical raw depth with a nonnegative exact squared raw-depth ratio.</summary> |
| | | 106 | | internal readonly int CompareSquaredDepth(in CapsulePlaneRayFrame frame, scoped ContactQuadratic numerator, |
| | | 107 | | scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> radical) |
| | | 108 | | { |
| | 18 | 109 | | int words = Math.Max(ContactQuadratic.ActiveWords(numerator), ContactQuadratic.ActiveWords(denominator)) + 14; |
| | 28 | 110 | | if (!usesValueRoot) words = Math.Max(words, |
| | 10 | 111 | | 2 * Math.Max(ContactQuadratic.ActiveWords(DepthNumerator), ContactQuadratic.ActiveWords(DepthDenominator)) |
| | 10 | 112 | | + WideArithmetic.GetActiveMagnitudeLength(Root) + 1); |
| | 72 | 113 | | Span<ulong> values = stackalloc ulong[8 * words], metric = stackalloc ulong[13], scale = stackalloc ulong[5], sq |
| | 18 | 114 | | Span<int> signs = stackalloc int[8]; |
| | 36 | 115 | | var n = ContactQuadratic.At(values, signs, 0, words); var d = ContactQuadratic.At(values, signs, 1, words); |
| | 36 | 116 | | var rn = ContactQuadratic.At(values, signs, 2, words); var rd = ContactQuadratic.At(values, signs, 3, words); |
| | 18 | 117 | | Signed320 doubleD = WideArithmetic.AddSigned320(frame.Finite.ShapeFrame.CoordinateDenominator, frame.Finite.Shap |
| | 18 | 118 | | WideArithmetic.GetMagnitude(doubleD, out scale[4], out scale[3], out scale[2], out scale[1], out scale[0]); |
| | 36 | 119 | | WideArithmetic.MultiplyMagnitudes(scale, scale, square); WideArithmetic.GetMagnitude(frame.NormalSquared, metric |
| | | 120 | | // t² competes with nativeDepth²*(2D)²/|N|² in this ray chart. |
| | 36 | 121 | | ContactQuadratic.Scale(numerator, square, 1, n); ContactQuadratic.Scale(denominator, metric, 1, d); |
| | 26 | 122 | | if (usesValueRoot) return Value.CompareSquaredQuadratic(n, d, radical); |
| | 10 | 123 | | ContactQuadratic.Multiply(DepthNumerator, DepthNumerator, Root, rn); |
| | 10 | 124 | | ContactQuadratic.Multiply(DepthDenominator, DepthDenominator, Root, rd); |
| | 10 | 125 | | return ContactQuadratic.CompareRatios(rn, rd, Root, n, d, radical); |
| | | 126 | | } |
| | | 127 | | |
| | | 128 | | /// <summary> |
| | | 129 | | /// Compares physical depths from certified coplanar member frames in the |
| | | 130 | | /// same capsule chart. Their reduced normal metrics must be identical. |
| | | 131 | | /// </summary> |
| | | 132 | | internal readonly int CompareCoplanarDepth(scoped in CapsulePlaneRaySelection other, |
| | | 133 | | in CapsulePlaneRayFrame frame, in CapsulePlaneRayFrame otherFrame) |
| | | 134 | | { |
| | | 135 | | System.Diagnostics.Debug.Assert(frame.NormalSquared.Equals(otherFrame.NormalSquared)); |
| | 592 | 136 | | Signed320 divisor = frame.Finite.ShapeFrame.CoordinateDenominator; |
| | 592 | 137 | | Signed320 otherDivisor = otherFrame.Finite.ShapeFrame.CoordinateDenominator; |
| | 740 | 138 | | if (divisor.Equals(otherDivisor)) return CompareDepth(other); |
| | 450 | 139 | | if (usesValueRoot && other.usesValueRoot) return Value.Compare(other.Value, divisor, otherDivisor); |
| | | 140 | | |
| | 438 | 141 | | int words = Math.Max(CoefficientWords, other.CoefficientWords) + 5; |
| | 876 | 142 | | Span<ulong> values = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 438 | 143 | | ContactQuadratic first = ContactQuadratic.At(values, signs, 0, words); |
| | 438 | 144 | | ContactQuadratic second = ContactQuadratic.At(values, signs, 1, words); |
| | 438 | 145 | | Signed576 scale = Signed576.ExtendValue(divisor); |
| | 438 | 146 | | Signed576 otherScale = Signed576.ExtendValue(otherDivisor); |
| | 438 | 147 | | if (usesValueRoot) |
| | | 148 | | { |
| | 36 | 149 | | ContactQuadratic.Scale(other.DepthNumerator, scale, first); |
| | 36 | 150 | | ContactQuadratic.Scale(other.DepthDenominator, otherScale, second); |
| | 36 | 151 | | return Value.CompareQuadratic(first, second, other.Root); |
| | | 152 | | } |
| | 402 | 153 | | if (other.usesValueRoot) |
| | | 154 | | { |
| | 36 | 155 | | ContactQuadratic.Scale(DepthNumerator, otherScale, first); |
| | 36 | 156 | | ContactQuadratic.Scale(DepthDenominator, scale, second); |
| | 36 | 157 | | return -other.Value.CompareQuadratic(first, second, Root); |
| | | 158 | | } |
| | 366 | 159 | | ContactQuadratic.Scale(DepthDenominator, scale, first); |
| | 366 | 160 | | ContactQuadratic.Scale(other.DepthDenominator, otherScale, second); |
| | 366 | 161 | | return ContactQuadratic.CompareRatios(DepthNumerator, first, Root, other.DepthNumerator, second, other.Root); |
| | | 162 | | } |
| | | 163 | | |
| | | 164 | | internal void KeepEvaluated(scoped CapsulePlaneRaySelection other, in CapsulePlaneRayFrame frame) |
| | | 165 | | { |
| | 960 | 166 | | if (!other.HasValue) return; |
| | 1916 | 167 | | CurrentSource = other.MaximumSource; CurrentEvent = other.MaximumEvent; |
| | 958 | 168 | | if (other.usesValueRoot) |
| | | 169 | | { |
| | 125 | 170 | | CapsulePlaneRayValue value = other.Value; |
| | 125 | 171 | | KeepValue(other.X, other.Y, other.Z, other.Denominator, other.Root, value, other.Primitive, frame); |
| | 125 | 172 | | return; |
| | | 173 | | } |
| | 833 | 174 | | Keep(other.X, other.Y, other.Z, other.Denominator, other.DepthNumerator, |
| | 833 | 175 | | other.DepthDenominator, other.Root, other.Primitive, frame); |
| | 833 | 176 | | } |
| | | 177 | | |
| | | 178 | | internal void Restore(CapsulePlaneRaySelectionMetadata metadata) |
| | | 179 | | { |
| | 2718 | 180 | | HasValue = metadata.HasValue; usesValueRoot = metadata.UsesValueRoot; Primitive = metadata.Primitive; |
| | 1812 | 181 | | CurrentSource = MaximumSource = metadata.Source; CurrentEvent = MaximumEvent = metadata.Event; |
| | 906 | 182 | | valueMetadata = metadata.Value; |
| | 906 | 183 | | } |
| | | 184 | | |
| | | 185 | | internal void KeepValue(scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, |
| | | 186 | | scoped ContactQuadratic denominator, scoped ReadOnlySpan<ulong> root, |
| | | 187 | | scoped CapsulePlaneRayValue value, int primitive, in CapsulePlaneRayFrame frame) |
| | | 188 | | { |
| | 307 | 189 | | if (HasValue) |
| | | 190 | | { |
| | 40 | 191 | | int comparison = usesValueRoot ? value.Compare(Value) |
| | 40 | 192 | | : value.CompareQuadratic(DepthNumerator, DepthDenominator, Root); |
| | 62 | 193 | | if (comparison < 0) return; |
| | 18 | 194 | | if (comparison == 0) |
| | | 195 | | { |
| | 7 | 196 | | int order = frame.ComparePoints(x, y, z, denominator, root, X, Y, Z, Denominator, Root); |
| | 11 | 197 | | if (order > 0 || order == 0 && primitive >= Primitive) return; |
| | | 198 | | } |
| | | 199 | | } |
| | 1124 | 200 | | x.CopyTo(X); y.CopyTo(Y); z.CopyTo(Z); denominator.CopyTo(Denominator); |
| | 281 | 201 | | Span<ulong> destination = Values.Slice(12 * CoefficientWords, RadicalWords); |
| | 562 | 202 | | root.CopyTo(destination); destination[root.Length..].Clear(); |
| | 281 | 203 | | var retained = new CapsulePlaneRayValue(Values[QuadraticWords..], ValueSigns, PolynomialWords); |
| | 562 | 204 | | retained.CopyFrom(value); valueMetadata = retained.Metadata; |
| | 562 | 205 | | HasValue = usesValueRoot = true; Primitive = primitive; |
| | 562 | 206 | | MaximumSource = CurrentSource; MaximumEvent = CurrentEvent; |
| | 281 | 207 | | } |
| | | 208 | | |
| | | 209 | | internal bool TryGetDepth(in CapsulePlaneRayFrame frame, out Fixed64 depth) |
| | 540 | 210 | | => TryGetDepth(frame, out depth, out _); |
| | | 211 | | |
| | | 212 | | internal bool TryGetDepth(in CapsulePlaneRayFrame frame, out Fixed64 depth, out bool clamped) |
| | | 213 | | { |
| | 1896 | 214 | | depth = default; clamped = false; |
| | 949 | 215 | | if (!HasValue) return false; |
| | 947 | 216 | | Signed320 doubleScale = WideArithmetic.AddSigned320(frame.Finite.ShapeFrame.CoordinateDenominator, frame.Finite. |
| | 1073 | 217 | | if (usesValueRoot) return Value.TryGetDepth(frame.NormalSquared, doubleScale, out depth, out clamped); |
| | 1642 | 218 | | Span<ulong> metric = stackalloc ulong[13]; WideArithmetic.GetMagnitude(frame.NormalSquared, metric); |
| | 1642 | 219 | | Span<ulong> scale = stackalloc ulong[5]; WideArithmetic.GetMagnitude(doubleScale, out scale[4], out scale[3], ou |
| | 821 | 220 | | clamped = !ContactQuadratic.TryRoundRootRatio(DepthNumerator, DepthDenominator, Root, metric, scale, out depth); |
| | 825 | 221 | | if (clamped) depth = Fixed64.MaxValue; |
| | 821 | 222 | | return true; |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | internal bool ContainsTrianglePoint(in CapsulePlaneRayFrame frame, FixedTriangle triangle) => |
| | 349 | 226 | | HasValue && frame.ContainsTrianglePoint(triangle, X, Y, Z, Denominator, Root); |
| | | 227 | | |
| | | 228 | | /// <summary>Materializes the physical source point directly into a requested rigid sampling frame.</summary> |
| | | 229 | | internal bool TryGetPointInRigidFrame(in CapsulePlaneRayFrame frame, Vector3d samplingOrigin, |
| | | 230 | | FixedQuaternion samplingRotation, Vector3d center, FixedQuaternion rotation, out Vector3d point) |
| | | 231 | | { |
| | 38 | 232 | | point = default; |
| | 39 | 233 | | if (!HasValue) return false; |
| | 37 | 234 | | int words = Math.Max(CoefficientWords, ContactQuadratic.ActiveWords(Denominator) + 5); |
| | 74 | 235 | | Span<ulong> values = stackalloc ulong[2 * words]; Span<int> signs = stackalloc int[2]; |
| | 37 | 236 | | ContactQuadratic d = ContactQuadratic.At(values, signs, 0, words); |
| | 37 | 237 | | ContactQuadratic.Scale(Denominator, GetCoordinateScale(frame), d); |
| | 37 | 238 | | return QuadraticPointMaterialization.TryGetPointInRigidFrame(center, rotation, samplingOrigin, samplingRotation, |
| | 37 | 239 | | X, Y, Z, d, Root, out point); |
| | | 240 | | } |
| | | 241 | | internal bool TryGetAuthoredPoint(in CapsulePlaneRayFrame frame, Vector3d meshOrigin, FixedQuaternion meshRotation, |
| | | 242 | | Vector3d center, FixedQuaternion rotation, out Vector3d point) |
| | | 243 | | { |
| | 600 | 244 | | point = default; |
| | 601 | 245 | | if (!HasValue) return false; |
| | 1198 | 246 | | Span<ulong> values = stackalloc ulong[2 * CoefficientWords]; Span<int> signs = stackalloc int[2]; |
| | 599 | 247 | | ContactQuadratic d = ContactQuadratic.At(values, signs, 0, CoefficientWords); |
| | 599 | 248 | | ContactQuadratic.Scale(Denominator, GetCoordinateScale(frame), d); |
| | 599 | 249 | | return QuadraticPointMaterialization.TryGetPointInRigidFrame(center, rotation, meshOrigin, meshRotation, |
| | 599 | 250 | | X, Y, Z, d, Root, out point); |
| | | 251 | | } |
| | | 252 | | |
| | | 253 | | internal bool TryGetExitAnchor(in CapsulePlaneRayFrame frame, int orientation, |
| | | 254 | | Vector3d center, FixedQuaternion rotation, out FixedPointAnchor anchor, |
| | | 255 | | out Fixed64 depth, out bool clamped) |
| | | 256 | | { |
| | 1224 | 257 | | anchor = default; depth = default; clamped = false; |
| | 411 | 258 | | if (orientation is not (1 or -1)) throw new ArgumentOutOfRangeException(nameof(orientation)); |
| | 406 | 259 | | if (!HasValue) return false; |
| | 404 | 260 | | bool depthRepresented = TryGetDepth(frame, out depth, out clamped); |
| | | 261 | | System.Diagnostics.Debug.Assert(depthRepresented); |
| | 808 | 262 | | Span<ulong> values = stackalloc ulong[16 * CoefficientWords]; Span<int> signs = stackalloc int[16]; |
| | 808 | 263 | | ContactQuadratic x = ContactQuadratic.At(values, signs, 0, CoefficientWords), y = ContactQuadratic.At(values, si |
| | 808 | 264 | | ContactQuadratic z = ContactQuadratic.At(values, signs, 2, CoefficientWords), d = ContactQuadratic.At(values, si |
| | 808 | 265 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 4, CoefficientWords), work = ContactQuadratic.At(valu |
| | 808 | 266 | | ContactQuadratic zero = ContactQuadratic.At(values, signs, 6, CoefficientWords), scaled = ContactQuadratic.At(va |
| | 404 | 267 | | zero.Clear(); |
| | 404 | 268 | | Signed320 coordinateScale = WideArithmetic.AddSigned320(frame.Finite.ShapeFrame.CoordinateDenominator, frame.Fin |
| | 404 | 269 | | WideDirection3d direction = orientation > 0 ? frame.Normal : -frame.Normal; |
| | 404 | 270 | | int endpoint = frame.Finite.Length == Fixed64.Zero ? 0 : Primitive == 1 ? -1 : Primitive == 2 ? 1 : 0; |
| | 404 | 271 | | Vector3d axial = default, radial; |
| | 404 | 272 | | if (usesValueRoot) |
| | | 273 | | { |
| | 64 | 274 | | CapsulePlaneRayValue value = Value; |
| | | 275 | | // A body primitive exists only for a positive-length core; |
| | | 276 | | // zero-core producers admit the sphere as the upper cap. |
| | 64 | 277 | | if (Primitive == 0) |
| | | 278 | | { |
| | 18 | 279 | | ContactQuadratic.Scale(Denominator, Signed576.ExtendValue(frame.Finite.ShapeFrame.Cap), work); |
| | 36 | 280 | | Y.CopyTo(term); term.Add(work, -1); |
| | 18 | 281 | | ContactQuadratic.Scale(Denominator, direction.Y, scaled); |
| | 19 | 282 | | if (value.SignLinear(term, scaled, Root) == 0) endpoint = 1; |
| | 52 | 283 | | else { Y.CopyTo(term); term.Add(work); if (value.SignLinear(term, scaled, Root) == 0) endpoint = -1; } |
| | | 284 | | } |
| | 64 | 285 | | if (endpoint == 0 && frame.Finite.Length != Fixed64.Zero) |
| | | 286 | | { |
| | 16 | 287 | | bool axialRepresented = value.TryGetPoint(zero, Y, zero, Denominator, Root, |
| | 16 | 288 | | new WideDirection3d(default, direction.Y, default), coordinateScale, out axial); |
| | | 289 | | System.Diagnostics.Debug.Assert(axialRepresented); |
| | 16 | 290 | | y.Clear(); |
| | | 291 | | } |
| | | 292 | | else |
| | | 293 | | { |
| | 48 | 294 | | Y.CopyTo(y); |
| | 48 | 295 | | Signed320 cap = endpoint < 0 ? WideArithmetic.Negate(frame.Finite.ShapeFrame.Cap) |
| | 48 | 296 | | : endpoint > 0 ? frame.Finite.ShapeFrame.Cap : default; |
| | 96 | 297 | | ContactQuadratic.Scale(Denominator, Signed576.ExtendValue(cap), term); y.Add(term, -1); |
| | | 298 | | } |
| | 64 | 299 | | WideDirection3d radialDirection = endpoint == 0 && frame.Finite.Length != Fixed64.Zero |
| | 64 | 300 | | ? new(direction.X, default, direction.Z) : direction; |
| | 64 | 301 | | bool radialRepresented = value.TryGetPoint(X, y, Z, Denominator, Root, radialDirection, coordinateScale, out |
| | | 302 | | System.Diagnostics.Debug.Assert(radialRepresented); |
| | | 303 | | } |
| | | 304 | | else |
| | | 305 | | { |
| | 340 | 306 | | SetExitCoordinates(frame, orientation, x, y, z, d, term, work); |
| | 340 | 307 | | ContactQuadratic.Scale(d, Signed576.ExtendValue(frame.Finite.ShapeFrame.Cap), work); |
| | 340 | 308 | | if (Primitive == 0) |
| | | 309 | | { |
| | 338 | 310 | | y.CopyTo(term); term.Add(work, -1); |
| | 237 | 311 | | if (term.Sign(Root) == 0) endpoint = 1; |
| | 371 | 312 | | else { y.CopyTo(term); term.Add(work); if (term.Sign(Root) == 0) endpoint = -1; } |
| | | 313 | | } |
| | 340 | 314 | | ContactQuadratic.Scale(d, GetCoordinateScale(frame), scaled); |
| | 340 | 315 | | if (endpoint == 0 && frame.Finite.Length != Fixed64.Zero) |
| | | 316 | | { |
| | 33 | 317 | | bool axialRepresented = QuadraticPointMaterialization.TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Id |
| | 33 | 318 | | zero, y, zero, scaled, Root, out axial); |
| | | 319 | | System.Diagnostics.Debug.Assert(axialRepresented); |
| | 33 | 320 | | y.Clear(); |
| | | 321 | | } |
| | 307 | 322 | | else y.Add(work, -endpoint); |
| | 340 | 323 | | bool radialRepresented = QuadraticPointMaterialization.TryGetWorldPoint(Vector3d.Zero, FixedQuaternion.Ident |
| | 340 | 324 | | x, y, z, scaled, Root, out radial); |
| | | 325 | | System.Diagnostics.Debug.Assert(radialRepresented); |
| | | 326 | | } |
| | 404 | 327 | | FixedPointAnchorTerm3d exactTerm = default; |
| | 404 | 328 | | if (endpoint != 0) |
| | | 329 | | { |
| | 283 | 330 | | Fixed64 signedLength = endpoint > 0 ? frame.Finite.Length : -frame.Finite.Length; |
| | 283 | 331 | | axial = new Vector3d(Fixed64.Zero, signedLength / Fixed64.Two, Fixed64.Zero); |
| | | 332 | | // The endpoint's half-raw remainder stays separate from the |
| | | 333 | | // independently rounded, bounded radial exit. |
| | 283 | 334 | | exactTerm = FixedPointAnchorTerm3d.CreateCenteredAxisSupport(Vector3d.Up, signedLength, |
| | 283 | 335 | | Vector3d.Zero, Fixed64.Zero, axial, Vector3d.Zero); |
| | | 336 | | } |
| | 404 | 337 | | anchor = new FixedPointAnchor(center, rotation, axial, radial, exactTerm); |
| | 404 | 338 | | return true; |
| | | 339 | | } |
| | | 340 | | |
| | | 341 | | private void SetExitCoordinates(in CapsulePlaneRayFrame frame, int orientation, scoped ContactQuadratic x, |
| | | 342 | | scoped ContactQuadratic y, scoped ContactQuadratic z, scoped ContactQuadratic d, |
| | | 343 | | scoped ContactQuadratic term, scoped ContactQuadratic work) |
| | | 344 | | { |
| | 340 | 345 | | ContactQuadratic.Multiply(Denominator, DepthDenominator, Root, d); |
| | 340 | 346 | | ContactQuadratic.Multiply(Denominator, DepthNumerator, Root, term); |
| | 2720 | 347 | | for (int axis = 0; axis < 3; axis++) |
| | | 348 | | { |
| | 1020 | 349 | | ContactQuadratic source = axis == 0 ? X : axis == 1 ? Y : Z; |
| | 1020 | 350 | | ContactQuadratic target = axis == 0 ? x : axis == 1 ? y : z; |
| | 1020 | 351 | | ContactQuadratic.Multiply(source, DepthDenominator, Root, target); |
| | 1020 | 352 | | Signed576 direction = axis == 0 ? frame.Normal.X : axis == 1 ? frame.Normal.Y : frame.Normal.Z; |
| | 2040 | 353 | | ContactQuadratic.Scale(term, direction, work); target.Add(work, orientation); |
| | | 354 | | } |
| | 340 | 355 | | } |
| | | 356 | | |
| | | 357 | | private static Signed576 GetCoordinateScale(in CapsulePlaneRayFrame frame) => |
| | 976 | 358 | | Signed576.ExtendValue(WideArithmetic.AddSigned320(frame.Finite.ShapeFrame.CoordinateDenominator, frame.Finite.Sh |
| | | 359 | | } |