| | | 1 | | //======================================================================= |
| | | 2 | | // NativePrismYawFrame3d.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>Preserves the scalar Sin/Cos prism chart without asserting a rigid inverse.</summary> |
| | | 15 | | internal readonly struct NativePrismYawFrame3d : IForwardHullFrame3d |
| | | 16 | | { |
| | 1 | 17 | | private static readonly Signed192 Unit = Signed192.Raw(Fixed64.One); |
| | 1 | 18 | | private static readonly Signed320 CoordinateDenominator = new(0UL, 0UL, 0UL, 1UL, 0UL); |
| | | 19 | | private readonly RationalPoint3d _origin; |
| | | 20 | | private readonly WideConvex2dRelations.RotationFrame2d _yaw; |
| | | 21 | | |
| | | 22 | | internal NativePrismYawFrame3d(Vector3d origin, Fixed64 yaw) |
| | | 23 | | { |
| | 84 | 24 | | _origin = RationalPoint3d.FromScalarPoint(origin); |
| | 84 | 25 | | _yaw = new(yaw); |
| | 84 | 26 | | } |
| | | 27 | | |
| | 10706 | 28 | | public Signed192 RotationDenominator => Signed192.Signed(1); |
| | 43698 | 29 | | public Signed320 Denominator => CoordinateDenominator; |
| | | 30 | | |
| | | 31 | | public RationalPoint3d GetPoint(Vector3d point) |
| | | 32 | | { |
| | 43643 | 33 | | WideConvex2dRelations.GetRotatedOffset(_yaw, new(point.X, point.Z), out Signed192 x, out Signed192 z); |
| | 43643 | 34 | | RationalPoint3d scalar = RationalPoint3d.FromScalarPoint(point); |
| | | 35 | | // Native products have one Q denominator; lift them into the existing |
| | | 36 | | // Q² point domain before adding the absolute origin (under 129 bits). |
| | 43643 | 37 | | return new( |
| | 43643 | 38 | | WideArithmetic.AddSigned320(_origin.X, WideArithmetic.MultiplySigned192(x, Unit)), |
| | 43643 | 39 | | WideArithmetic.AddSigned320(_origin.Y, scalar.Y), |
| | 43643 | 40 | | WideArithmetic.AddSigned320(_origin.Z, WideArithmetic.MultiplySigned192(z, Unit)), Denominator); |
| | | 41 | | } |
| | | 42 | | |
| | | 43 | | public WideDirection3d GetEdgeDirection(Vector3d first, Vector3d second) |
| | | 44 | | { |
| | 11031 | 45 | | WideConvex2dRelations.GetRotatedOffset(_yaw, new(first.X, first.Z), out Signed192 ax, out Signed192 az); |
| | 11031 | 46 | | WideConvex2dRelations.GetRotatedOffset(_yaw, new(second.X, second.Z), out Signed192 bx, out Signed192 bz); |
| | 11031 | 47 | | Signed192 y = WideArithmetic.SubtractSigned192(Signed192.Raw(second.Y), Signed192.Raw(first.Y)); |
| | 11031 | 48 | | return new(Signed576.ExtendValue(WideArithmetic.SubtractSigned192(bx, ax)), |
| | 11031 | 49 | | WideArithmetic.MultiplySigned320(Signed320.ExtendValue(y), Unit), |
| | 11031 | 50 | | Signed576.ExtendValue(WideArithmetic.SubtractSigned192(bz, az))); |
| | | 51 | | } |
| | | 52 | | |
| | | 53 | | public WideDirection3d GetFaceNormal(in FixedTriangle triangle) |
| | | 54 | | { |
| | 2639 | 55 | | triangle.GetExactNormal(out Signed192 x, out Signed192 y, out Signed192 z, out _); |
| | 2639 | 56 | | Signed320 nx = WideArithmetic.SubtractSigned320(WideArithmetic.MultiplySigned192(x, _yaw.Cosine), |
| | 2639 | 57 | | WideArithmetic.MultiplySigned192(z, _yaw.Sine)); |
| | 2639 | 58 | | Signed320 nz = WideArithmetic.AddSigned320(WideArithmetic.MultiplySigned192(x, _yaw.Sine), |
| | 2639 | 59 | | WideArithmetic.MultiplySigned192(z, _yaw.Cosine)); |
| | 2639 | 60 | | Signed320 radialMetric = WideArithmetic.AddSigned320(WideArithmetic.MultiplySigned192(_yaw.Cosine, _yaw.Cosine), |
| | 2639 | 61 | | WideArithmetic.MultiplySigned192(_yaw.Sine, _yaw.Sine)); |
| | | 62 | | // Cofactors preserve the actual nonunit scalar matrix: c²+s² need |
| | | 63 | | // not equal Q². Raw face normals need <131 bits, so outputs need <197. |
| | 2639 | 64 | | return new(WideArithmetic.MultiplySigned320(nx, Unit), |
| | 2639 | 65 | | WideArithmetic.MultiplySigned320(radialMetric, y), WideArithmetic.MultiplySigned320(nz, Unit)); |
| | | 66 | | } |
| | | 67 | | |
| | | 68 | | public WideDirection3d GetInwardAxis(in FixedTriangle triangle, Vector3d first, Vector3d second) => |
| | 6 | 69 | | WideDirection3d.Cross(GetFaceNormal(triangle), GetEdgeDirection(first, second)); |
| | | 70 | | |
| | | 71 | | public bool TryGetTriangleHalfPlanes(in FixedTriangle triangle, in RationalPoint3d point, Span<Signed832> values) |
| | | 72 | | { |
| | 1799 | 73 | | WideDirection3d normal = GetFaceNormal(triangle); |
| | 1803 | 74 | | if (normal.IsZero) return false; |
| | 1795 | 75 | | Span<Vector3d> vertices = stackalloc Vector3d[3] { triangle.A, triangle.B, triangle.C }; |
| | 14360 | 76 | | for (int edge = 0; edge < 3; edge++) |
| | | 77 | | { |
| | 10770 | 78 | | Vector3d first = vertices[edge], second = vertices[(edge + 1) % 3]; |
| | 5385 | 79 | | WideDirection3d inward = WideDirection3d.Cross(normal, GetEdgeDirection(first, second)); |
| | 5385 | 80 | | WideDirection3d delta = WideTriangleRelations.GetScaledPointDifference(point, GetPoint(first), RotationDenom |
| | | 81 | | // Native inward axes need <295 bits; even affine point differences |
| | | 82 | | // keep the complete dot below the existing 832-bit half-plane bank. |
| | 5385 | 83 | | values[edge] = WideDirection3d.Dot(inward, delta); |
| | | 84 | | } |
| | 1795 | 85 | | return true; |
| | | 86 | | } |
| | | 87 | | } |