| | | 1 | | //======================================================================= |
| | | 2 | | // ConePlaneRayPoint.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>Borrowed homogeneous coordinates in one exact quadratic field.</summary> |
| | | 13 | | internal readonly ref struct ConePlaneRayPoint |
| | | 14 | | { |
| | | 15 | | internal const int FieldWords = ConePlaneRaySelection.FieldWords; |
| | | 16 | | internal const int StorageWords = 8 * FieldWords; |
| | | 17 | | internal const int SignCount = 8; |
| | | 18 | | private readonly Span<ulong> values; |
| | | 19 | | private readonly Span<int> signs; |
| | | 20 | | private readonly int _fieldWords; |
| | 4574 | 21 | | internal int BankWords => _fieldWords; |
| | 143856 | 22 | | internal ContactQuadratic X => ContactQuadratic.At(values, signs, 0, _fieldWords); |
| | 162628 | 23 | | internal ContactQuadratic Y => ContactQuadratic.At(values, signs, 1, _fieldWords); |
| | 140082 | 24 | | internal ContactQuadratic Z => ContactQuadratic.At(values, signs, 2, _fieldWords); |
| | 137560 | 25 | | internal ContactQuadratic Denominator => ContactQuadratic.At(values, signs, 3, _fieldWords); |
| | | 26 | | |
| | | 27 | | internal ConePlaneRayPoint(Span<ulong> values, Span<int> signs, int fieldWords = FieldWords) |
| | | 28 | | { |
| | 51681 | 29 | | if (values.Length < 8 * fieldWords || signs.Length < SignCount) |
| | 2 | 30 | | throw new ArgumentException("The plane-ray point storage is too small."); |
| | 103358 | 31 | | this.values = values[..(8 * fieldWords)]; this.signs = signs[..SignCount]; |
| | 51679 | 32 | | _fieldWords = fieldWords; |
| | 51679 | 33 | | } |
| | | 34 | | |
| | | 35 | | internal void CopyTo(scoped ConePlaneRayPoint target) |
| | | 36 | | { |
| | 67596 | 37 | | X.CopyTo(target.X); Y.CopyTo(target.Y); Z.CopyTo(target.Z); Denominator.CopyTo(target.Denominator); |
| | 16899 | 38 | | } |
| | | 39 | | |
| | | 40 | | /// <summary>Changes only the native scale of coordinates in the same physical cone frame.</summary> |
| | | 41 | | internal void CopyTo(scoped ConePlaneRayPoint target, Signed320 sourceScale, Signed320 targetScale) |
| | | 42 | | { |
| | | 43 | | // Coordinates carry 2D native scale. Cross multiplication adds fewer |
| | | 44 | | // than 132 bits to certified point fields below 3072 bits. |
| | 29 | 45 | | Signed576 scale = Signed576.ExtendValue(targetScale); |
| | 29 | 46 | | ContactQuadratic.Scale(X, scale, target.X); |
| | 29 | 47 | | ContactQuadratic.Scale(Y, scale, target.Y); |
| | 29 | 48 | | ContactQuadratic.Scale(Z, scale, target.Z); |
| | 29 | 49 | | ContactQuadratic.Scale(Denominator, Signed576.ExtendValue(sourceScale), target.Denominator); |
| | 29 | 50 | | } |
| | | 51 | | |
| | | 52 | | internal int CompareTo(scoped ConePlaneRayPoint other, scoped ReadOnlySpan<ulong> root, scoped ReadOnlySpan<ulong> o |
| | | 53 | | { |
| | 13404 | 54 | | for (int axis = 0; axis < 3; axis++) |
| | | 55 | | { |
| | 5498 | 56 | | int order = ContactQuadratic.CompareRatios(axis == 0 ? X : axis == 1 ? Y : Z, Denominator, root, |
| | 5498 | 57 | | axis == 0 ? other.X : axis == 1 ? other.Y : other.Z, other.Denominator, otherRoot); |
| | 7207 | 58 | | if (order != 0) return order; |
| | | 59 | | } |
| | 1204 | 60 | | return 0; |
| | | 61 | | } |
| | | 62 | | |
| | | 63 | | /// <summary>Orders physical cone-frame points across independently scaled authored frames of the same cone.</summar |
| | | 64 | | /// <remarks>Both points have positive homogeneous denominators and the frames describe the same physical cone.</rem |
| | | 65 | | internal int CompareTo(scoped ConePlaneRayPoint other, scoped ReadOnlySpan<ulong> root, |
| | | 66 | | scoped ReadOnlySpan<ulong> otherRoot, in ConePlaneRayFrame frame, in ConePlaneRayFrame otherFrame) |
| | | 67 | | { |
| | 24 | 68 | | Signed320 scale = frame.Finite.ShapeFrame.CoordinateDenominator; |
| | 24 | 69 | | Signed320 otherScale = otherFrame.Finite.ShapeFrame.CoordinateDenominator; |
| | 40 | 70 | | if (scale.Equals(otherScale)) return CompareTo(other, root, otherRoot); |
| | | 71 | | // Each native coordinate is 2D times the physical coordinate. Carry |
| | | 72 | | // D in the homogeneous denominator; at most three extra limbs keep |
| | | 73 | | // the comparison exact even when Q32.32 witnesses coincide. |
| | 8 | 74 | | int words = Math.Max(ContactQuadratic.ActiveWords(Denominator), ContactQuadratic.ActiveWords(other.Denominator)) |
| | 16 | 75 | | Span<ulong> values = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 8 | 76 | | ContactQuadratic denominator = ContactQuadratic.At(values, signs, 0, words); |
| | 8 | 77 | | ContactQuadratic otherDenominator = ContactQuadratic.At(values, signs, 1, words); |
| | 8 | 78 | | ContactQuadratic.Scale(Denominator, Signed576.ExtendValue(scale), denominator); |
| | 8 | 79 | | ContactQuadratic.Scale(other.Denominator, Signed576.ExtendValue(otherScale), otherDenominator); |
| | 40 | 80 | | for (int axis = 0; axis < 3; axis++) |
| | | 81 | | { |
| | 18 | 82 | | int order = ContactQuadratic.CompareRatios(axis == 0 ? X : axis == 1 ? Y : Z, denominator, root, |
| | 18 | 83 | | axis == 0 ? other.X : axis == 1 ? other.Y : other.Z, otherDenominator, otherRoot); |
| | 24 | 84 | | if (order != 0) return order; |
| | | 85 | | } |
| | 2 | 86 | | return 0; |
| | | 87 | | } |
| | | 88 | | |
| | | 89 | | internal void Set(WideAxis3 point) |
| | | 90 | | { |
| | 1080 | 91 | | X.Set(Signed576.ExtendValue(point.X)); Y.Set(Signed576.ExtendValue(point.Y)); |
| | 540 | 92 | | Z.Set(Signed576.ExtendValue(point.Z)); |
| | | 93 | | // Signed*.One is Q32 raw unity; a homogeneous denominator needs |
| | | 94 | | // integer unity instead of the physical fixed-point scale. |
| | 540 | 95 | | Denominator.Set(Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Signed(1)))); |
| | 540 | 96 | | } |
| | | 97 | | |
| | | 98 | | internal void Set(WideDirection3d direction) |
| | | 99 | | { |
| | 20112 | 100 | | X.Set(direction.X); Y.Set(direction.Y); Z.Set(direction.Z); |
| | 6704 | 101 | | Denominator.Set(Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Signed(1)))); |
| | 6704 | 102 | | } |
| | | 103 | | } |