| | | 1 | | //======================================================================= |
| | | 2 | | // AxisTriangleContactFeature.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>Retains exact closest-point feature identity and separation before contact rounding.</summary> |
| | | 13 | | internal readonly struct AxisTriangleContactFeature |
| | | 14 | | { |
| | | 15 | | private readonly Signed832 _separationX, _separationY, _separationZ; |
| | | 16 | | private readonly bool _wide; |
| | | 17 | | |
| | | 18 | | internal AxisTriangleContactFeature(int edgeMask, Signed576 separationX, |
| | | 19 | | Signed576 separationY, Signed576 separationZ, int triangleVertex = -1) |
| | | 20 | | { |
| | 60 | 21 | | _wide = false; |
| | 60 | 22 | | EdgeMask = edgeMask; |
| | 60 | 23 | | TriangleVertex = triangleVertex; |
| | 180 | 24 | | _separationX = Signed832.ExtendValue(separationX); _separationY = Signed832.ExtendValue(separationY); _separatio |
| | 60 | 25 | | } |
| | | 26 | | |
| | | 27 | | internal AxisTriangleContactFeature(int edgeMask, Signed832 x, Signed832 y, Signed832 z, int triangleVertex = -1) |
| | | 28 | | { |
| | 41 | 29 | | _wide = true; |
| | 82 | 30 | | EdgeMask = edgeMask; TriangleVertex = triangleVertex; |
| | 123 | 31 | | _separationX = x; _separationY = y; _separationZ = z; |
| | 41 | 32 | | } |
| | | 33 | | |
| | | 34 | | /// <summary>Maps a retained rigid-local separation into world orientation without scalar rounding.</summary> |
| | | 35 | | internal AxisTriangleContactFeature GetWorldFeatureFromRigidLocal(FixedQuaternion rotation) |
| | | 36 | | { |
| | 3 | 37 | | if (rotation == FixedQuaternion.Identity) return this; |
| | | 38 | | // This ingress is used only for the legacy local closest chart, whose |
| | | 39 | | // complete components fit Signed576. One normalized basis adds <65 |
| | | 40 | | // bits; the resulting world direction fits Signed832. |
| | 1 | 41 | | var basis = new WideRationalBasis3d(rotation); |
| | 3 | 42 | | Signed576 x = Signed576.NarrowValue(_separationX), y = Signed576.NarrowValue(_separationY), z = Signed576.Narrow |
| | | 43 | | Signed832 Component(Signed192 a, Signed192 b, Signed192 c) => |
| | | 44 | | WideArithmetic.AddSigned832(WideArithmetic.AddSigned832( |
| | | 45 | | WideArithmetic.MultiplySigned576ToSigned832(x, Signed320.ExtendValue(a)), |
| | | 46 | | WideArithmetic.MultiplySigned576ToSigned832(y, Signed320.ExtendValue(b))), |
| | | 47 | | WideArithmetic.MultiplySigned576ToSigned832(z, Signed320.ExtendValue(c))); |
| | 1 | 48 | | return new AxisTriangleContactFeature(EdgeMask, Component(basis.Xx, basis.Yx, basis.Zx), |
| | 1 | 49 | | Component(basis.Xy, basis.Yy, basis.Zy), Component(basis.Xz, basis.Yz, basis.Zz), TriangleVertex); |
| | | 50 | | } |
| | | 51 | | /// <summary>Zero edge halfspaces: AB=1, BC=2, CA=4; 7 declines degenerate classification.</summary> |
| | | 52 | | internal int EdgeMask { get; } |
| | | 53 | | |
| | | 54 | | /// <summary>Exact authored vertex ownership; -1 denotes a nonvertex witness.</summary> |
| | | 55 | | internal int TriangleVertex { get; } |
| | | 56 | | |
| | | 57 | | /// <summary>Whether the retained core and triangle witnesses coincide exactly.</summary> |
| | 24 | 58 | | internal bool IsZeroSeparation => _separationX.IsZero && _separationY.IsZero && _separationZ.IsZero; |
| | | 59 | | |
| | | 60 | | /// <summary>Gets the exact separation sign along a wide direction in the selected contact chart.</summary> |
| | | 61 | | internal int CompareSeparationProjection(Signed192 x, Signed192 y, Signed192 z) |
| | | 62 | | { |
| | 26 | 63 | | if (_wide) return CompareSeparationProjection(new WideAxis3(Signed320.ExtendValue(x), Signed320.ExtendValue(y), |
| | 10 | 64 | | return WideArithmetic.AddSigned832(WideArithmetic.AddSigned832( |
| | 10 | 65 | | WideArithmetic.MultiplySigned576ToSigned832(Signed576.NarrowValue(_separationX), Signed320.ExtendValue(x)), |
| | 10 | 66 | | WideArithmetic.MultiplySigned576ToSigned832(Signed576.NarrowValue(_separationY), Signed320.ExtendValue(y))), |
| | 10 | 67 | | WideArithmetic.MultiplySigned576ToSigned832(Signed576.NarrowValue(_separationZ), Signed320.ExtendValue(z))). |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | /// <summary>Gets the same exact sign for a rigidly transformed wide axis.</summary> |
| | | 71 | | internal int CompareSeparationProjection(in WideAxis3 direction) |
| | | 72 | | { |
| | | 73 | | // Full 832-by-320 products plus carries need 1154 signed bits. The |
| | | 74 | | // existing quadratic owner retains this rational-only dot without |
| | | 75 | | // narrowing exceptional world-chart plane normals to Signed192. |
| | 30 | 76 | | int words = _wide ? 19 : 15; |
| | 30 | 77 | | Span<ulong> values = stackalloc ulong[6 * words]; |
| | 30 | 78 | | Span<int> signs = stackalloc int[6]; |
| | 30 | 79 | | ContactQuadratic sum = ContactQuadratic.At(values, signs, 0, words); |
| | 30 | 80 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 1, words); |
| | 30 | 81 | | ContactQuadratic product = ContactQuadratic.At(values, signs, 2, words); |
| | 30 | 82 | | sum.Clear(); |
| | 30 | 83 | | Span<Signed832> components = stackalloc Signed832[3] { _separationX, _separationY, _separationZ }; |
| | 30 | 84 | | Span<Signed320> axes = stackalloc Signed320[3] { direction.X, direction.Y, direction.Z }; |
| | 240 | 85 | | for (int index = 0; index < 3; index++) |
| | | 86 | | { |
| | 180 | 87 | | if (_wide) term.Set(components[index]); else term.Set(Signed576.NarrowValue(components[index])); |
| | 90 | 88 | | ContactQuadratic.Scale(term, Signed576.ExtendValue(axes[index]), product); |
| | 90 | 89 | | sum.Add(product); |
| | | 90 | | } |
| | 30 | 91 | | return sum.Sign(ReadOnlySpan<ulong>.Empty); |
| | | 92 | | } |
| | | 93 | | |
| | | 94 | | /// <summary>Gets the exact sign of separation dotted with a ray in the selected contact chart.</summary> |
| | | 95 | | internal int CompareSeparationProjection(Vector3d first, Vector3d second) |
| | | 96 | | { |
| | 39 | 97 | | Signed320 x = Signed320.ExtendValue(WideArithmetic.Difference(second.X, first.X)); |
| | 39 | 98 | | Signed320 y = Signed320.ExtendValue(WideArithmetic.Difference(second.Y, first.Y)); |
| | 39 | 99 | | Signed320 z = Signed320.ExtendValue(WideArithmetic.Difference(second.Z, first.Z)); |
| | 39 | 100 | | if (!_wide) |
| | 29 | 101 | | return WideArithmetic.AddSigned704(WideArithmetic.AddSigned704( |
| | 29 | 102 | | WideArithmetic.MultiplySigned576ToSigned704(Signed576.NarrowValue(_separationX), x), |
| | 29 | 103 | | WideArithmetic.MultiplySigned576ToSigned704(Signed576.NarrowValue(_separationY), y)), |
| | 29 | 104 | | WideArithmetic.MultiplySigned576ToSigned704(Signed576.NarrowValue(_separationZ), z)).Sign; |
| | 10 | 105 | | return CompareSeparationProjection(new WideAxis3(x, y, z)); |
| | | 106 | | } |
| | | 107 | | } |