| | | 1 | | //======================================================================= |
| | | 2 | | // RationalPoint3d.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> |
| | | 13 | | /// Retains exact world-space raw-coordinate numerators over one positive |
| | | 14 | | /// denominator; materialization rounds each final coordinate once to nearest even. |
| | | 15 | | /// </summary> |
| | | 16 | | internal readonly struct RationalPoint3d |
| | | 17 | | { |
| | | 18 | | internal readonly Signed320 X, Y, Z, Denominator; |
| | | 19 | | |
| | | 20 | | internal RationalPoint3d(Signed320 x, Signed320 y, Signed320 z, Signed320 denominator) |
| | | 21 | | { |
| | 1453388 | 22 | | X = x; Y = y; Z = z; Denominator = denominator; |
| | 363347 | 23 | | } |
| | | 24 | | |
| | | 25 | | internal static RationalPoint3d FromScalarPoint(Vector3d point) => |
| | 43984 | 26 | | new(ScaleScalar(point.X), ScaleScalar(point.Y), ScaleScalar(point.Z), new Signed320(0UL, 0UL, 0UL, 1UL, 0UL)); |
| | | 27 | | |
| | | 28 | | /// <summary>Retains an existing anchor's exact feature term in the shared Q² point domain.</summary> |
| | | 29 | | internal static RationalPoint3d FromAnchor(in FixedPointAnchor anchor) |
| | | 30 | | { |
| | 75 | 31 | | WideTriangleRelations.GetPointLocalNumerators(anchor, Vector3d.Zero, FixedQuaternion.Identity, |
| | 75 | 32 | | out Signed576 x, out Signed576 y, out Signed576 z, out Signed320 denominator); |
| | | 33 | | // An anchor combines a 65-bit rigid basis with a 34-bit exact local |
| | | 34 | | // term denominator. Its world numerator plus this lift fits 229 bits. |
| | 75 | 35 | | Signed192 qSquared = new(0UL, 1UL, 0UL); |
| | 75 | 36 | | return new RationalPoint3d( |
| | 75 | 37 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned576(x, qSquared)), |
| | 75 | 38 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned576(y, qSquared)), |
| | 75 | 39 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned576(z, qSquared)), |
| | 75 | 40 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320(denominator, qSquared))); |
| | | 41 | | } |
| | | 42 | | |
| | | 43 | | /// <summary>Embeds a complete planar anchor in X/Z with zero Y, preserving sub-lattice features.</summary> |
| | | 44 | | internal static RationalPoint3d FromAnchor(in FixedPointAnchor2d anchor) |
| | | 45 | | { |
| | 31 | 46 | | WidePointAnchor2d.GetExactRelativeOffsetRatio(anchor.Origin, anchor.LocalPoint, anchor.LocalDisplacement, |
| | 31 | 47 | | anchor.ExactLocalTerm, anchor.Rotation, Vector2d.Zero, Vector2d.Zero, Vector2d.Zero, default, |
| | 31 | 48 | | Fixed64.Zero, out Signed320 x, out Signed320 z, out Signed320 rawDenominator); |
| | | 49 | | // Both owners retain raw-coordinate ratios. The anchor's 2 Q² |
| | | 50 | | // denominator already belongs to the shared Q² point domain. |
| | 31 | 51 | | return new(x, default, z, rawDenominator); |
| | | 52 | | } |
| | | 53 | | |
| | | 54 | | /// <summary>Compares exact world projections without materializing either point.</summary> |
| | | 55 | | internal static int CompareProjection(in RationalPoint3d first, in RationalPoint3d second, Vector3d direction) |
| | | 56 | | { |
| | 5 | 57 | | WideDirection3d delta = WideTriangleRelations.GetScaledPointDifference(first, second, |
| | 5 | 58 | | WideArithmetic.TruncateSigned320By64(second.Denominator)); |
| | 5 | 59 | | var axis = new WideAxis3(Signed320.ExtendValue(Signed192.Raw(direction.X)), |
| | 5 | 60 | | Signed320.ExtendValue(Signed192.Raw(direction.Y)), Signed320.ExtendValue(Signed192.Raw(direction.Z))); |
| | 5 | 61 | | return WideDirection3d.Dot(delta, axis).Sign; |
| | | 62 | | } |
| | | 63 | | |
| | | 64 | | private static Signed320 ScaleScalar(Fixed64 coordinate) |
| | | 65 | | { |
| | 131952 | 66 | | ulong extension = coordinate.m_rawValue < 0L ? ulong.MaxValue : 0UL; |
| | 131952 | 67 | | return new Signed320(extension, extension, extension, unchecked((ulong)coordinate.m_rawValue), 0UL); |
| | | 68 | | } |
| | | 69 | | |
| | | 70 | | internal bool TryGetPoint(out Vector3d point) => |
| | 3081 | 71 | | TryMaterialize(Signed576.ExtendValue(X), Signed576.ExtendValue(Y), Signed576.ExtendValue(Z), |
| | 3081 | 72 | | Signed576.ExtendValue(Denominator), out point); |
| | | 73 | | |
| | | 74 | | /// <summary> |
| | | 75 | | /// Transposes a normalized target rotation after exact origin subtraction. |
| | | 76 | | /// Source numerators are below 319 magnitude bits, so the three products |
| | | 77 | | /// and their sum are below 386 bits; 576-bit temporaries retain every term. |
| | | 78 | | /// </summary> |
| | | 79 | | internal void GetLocalPointNumerators(Vector3d targetOrigin, FixedQuaternion targetRotation, |
| | | 80 | | out Signed576 x, out Signed576 y, out Signed576 z, out Signed320 denominator) |
| | | 81 | | { |
| | 149514 | 82 | | if (!targetRotation.IsNormalized()) |
| | 1 | 83 | | throw new ArgumentException("The target rotation must be normalized.", nameof(targetRotation)); |
| | 149513 | 84 | | if (targetRotation == FixedQuaternion.Identity) |
| | | 85 | | { |
| | 44192 | 86 | | x = SubtractOrigin(X, targetOrigin.X); |
| | 44192 | 87 | | y = SubtractOrigin(Y, targetOrigin.Y); |
| | 44192 | 88 | | z = SubtractOrigin(Z, targetOrigin.Z); |
| | 44192 | 89 | | denominator = Denominator; |
| | 44192 | 90 | | return; |
| | | 91 | | } |
| | | 92 | | |
| | 105321 | 93 | | WideRationalBasis3d basis = new(targetRotation); |
| | 105321 | 94 | | Signed576 relativeX = SubtractOrigin(X, targetOrigin.X); |
| | 105321 | 95 | | Signed576 relativeY = SubtractOrigin(Y, targetOrigin.Y); |
| | 105321 | 96 | | Signed576 relativeZ = SubtractOrigin(Z, targetOrigin.Z); |
| | 105321 | 97 | | x = Project(relativeX, relativeY, relativeZ, basis.Xx, basis.Xy, basis.Xz); |
| | 105321 | 98 | | y = Project(relativeX, relativeY, relativeZ, basis.Yx, basis.Yy, basis.Yz); |
| | 105321 | 99 | | z = Project(relativeX, relativeY, relativeZ, basis.Zx, basis.Zy, basis.Zz); |
| | | 100 | | // Q^2 and three normalized quaternion denominators stay below 259 bits. |
| | 105321 | 101 | | denominator = Signed320.NarrowValue(WideArithmetic.MultiplySigned320(Denominator, basis.Denominator)); |
| | 105321 | 102 | | } |
| | | 103 | | |
| | | 104 | | internal bool TryGetLocalPointIn(Vector3d targetOrigin, FixedQuaternion targetRotation, out Vector3d point) |
| | | 105 | | { |
| | 186 | 106 | | GetLocalPointNumerators(targetOrigin, targetRotation, out Signed576 x, out Signed576 y, out Signed576 z, |
| | 186 | 107 | | out Signed320 denominator); |
| | 185 | 108 | | return TryMaterialize(x, y, z, Signed576.ExtendValue(denominator), out point); |
| | | 109 | | } |
| | | 110 | | |
| | | 111 | | /// <summary>Converts only lattice-exact coordinates, without admitting rounded geometry.</summary> |
| | | 112 | | internal bool TryGetExactLocalPointIn(Vector3d targetOrigin, FixedQuaternion targetRotation, out Vector3d point) |
| | | 113 | | { |
| | 12 | 114 | | GetLocalPointNumerators(targetOrigin, targetRotation, out Signed576 x, out Signed576 y, out Signed576 z, |
| | 12 | 115 | | out Signed320 denominator); |
| | 12 | 116 | | return TryMaterialize(x, y, z, Signed576.ExtendValue(denominator), out point) |
| | 12 | 117 | | && x.Equals(WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(point.X))) |
| | 12 | 118 | | && y.Equals(WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(point.Y))) |
| | 12 | 119 | | && z.Equals(WideArithmetic.MultiplySigned320(denominator, Signed192.Raw(point.Z))); |
| | | 120 | | } |
| | | 121 | | private Signed576 SubtractOrigin(Signed320 coordinate, Fixed64 origin) => |
| | 448539 | 122 | | WideArithmetic.SubtractSigned576(Signed576.ExtendValue(coordinate), |
| | 448539 | 123 | | WideArithmetic.MultiplySigned320(Denominator, Signed192.Raw(origin))); |
| | | 124 | | |
| | | 125 | | private static Signed576 Project(Signed576 x, Signed576 y, Signed576 z, |
| | | 126 | | Signed192 axisX, Signed192 axisY, Signed192 axisZ) => |
| | 315963 | 127 | | WideArithmetic.AddSigned576(WideArithmetic.AddSigned576( |
| | 315963 | 128 | | WideArithmetic.MultiplySigned576(x, axisX), WideArithmetic.MultiplySigned576(y, axisY)), |
| | 315963 | 129 | | WideArithmetic.MultiplySigned576(z, axisZ)); |
| | | 130 | | |
| | | 131 | | private static bool TryMaterialize(Signed576 x, Signed576 y, Signed576 z, Signed576 denominator, out Vector3d point) |
| | | 132 | | { |
| | 3278 | 133 | | bool representable = Fixed64.TryGetSignedRawRatio(x, denominator, out Fixed64 px) |
| | 3278 | 134 | | & Fixed64.TryGetSignedRawRatio(y, denominator, out Fixed64 py) |
| | 3278 | 135 | | & Fixed64.TryGetSignedRawRatio(z, denominator, out Fixed64 pz); |
| | 3278 | 136 | | point = representable ? new Vector3d(px, py, pz) : default; |
| | 3278 | 137 | | return representable; |
| | | 138 | | } |
| | | 139 | | } |