| | | 1 | | //======================================================================= |
| | | 2 | | // ScaledRigidFrame3d.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 scaled source points and a separately rotated placement without |
| | | 14 | | /// rounding either scale products, reference subtraction, or frame origins. |
| | | 15 | | /// </summary> |
| | | 16 | | /// <remarks> |
| | | 17 | | /// The exact point is T + Rp(offset * ownerScale) + |
| | | 18 | | /// Rg((source - reference) * ownerScale * partScale). Scale factors may be |
| | | 19 | | /// signed or zero; rotations must be normalized. All inputs use their complete |
| | | 20 | | /// scalar domains. Only final Q32.32 materialization can fail representability. |
| | | 21 | | /// </remarks> |
| | | 22 | | internal readonly struct ScaledRigidFrame3d : IForwardHullFrame3d |
| | | 23 | | { |
| | 1 | 24 | | private static readonly Signed192 ScaleDenominator = new(0UL, 1UL, 0UL); |
| | | 25 | | private readonly Vector3d _reference; |
| | | 26 | | private readonly Vector3d _translation; |
| | | 27 | | private readonly Signed192 _scaleX, _scaleY, _scaleZ; |
| | | 28 | | private readonly Signed192 _offsetX, _offsetY, _offsetZ; |
| | | 29 | | private readonly Signed320 _translationX, _translationY, _translationZ; |
| | | 30 | | private readonly Signed192 _placementDenominator; |
| | | 31 | | private readonly bool _sameRotation, _identityUnit, _isRigid; |
| | | 32 | | |
| | | 33 | | internal readonly WideRationalBasis3d GeometryBasis; |
| | | 34 | | internal readonly FixedQuaternion GeometryRotation; |
| | | 35 | | internal readonly Signed192 RotationDenominator; |
| | | 36 | | internal readonly Signed320 Denominator; |
| | | 37 | | |
| | 158688 | 38 | | internal bool IsRigid => _isRigid; |
| | | 39 | | |
| | 651 | 40 | | internal Vector3d RigidOrigin => _translation; |
| | 616 | 41 | | internal FixedQuaternion Rotation => GeometryRotation; |
| | 2 | 42 | | internal bool HasLocalPlacement => !(_offsetX.IsZero & _offsetY.IsZero & _offsetZ.IsZero); |
| | | 43 | | |
| | 11127 | 44 | | Signed192 IForwardHullFrame3d.RotationDenominator => RotationDenominator; |
| | 556 | 45 | | Signed320 IForwardHullFrame3d.Denominator => Denominator; |
| | 100940 | 46 | | RationalPoint3d IForwardHullFrame3d.GetPoint(Vector3d point) => GetPoint(point); |
| | | 47 | | WideDirection3d IForwardHullFrame3d.GetFaceNormal(in FixedTriangle triangle) => |
| | 1956 | 48 | | WideTriangleRelations.GetScaledTriangleLocalNormal(triangle, this).Transform(GeometryBasis); |
| | | 49 | | WideDirection3d IForwardHullFrame3d.GetEdgeDirection(Vector3d first, Vector3d second) => |
| | 4301 | 50 | | WideDirection3d.GetLocalEdge(this, first, second).Transform(GeometryBasis); |
| | | 51 | | WideDirection3d IForwardHullFrame3d.GetInwardAxis(in FixedTriangle triangle, Vector3d first, Vector3d second) => |
| | 30 | 52 | | WideDirection3d.Cross(WideTriangleRelations.GetScaledTriangleLocalNormal(triangle, this), |
| | 30 | 53 | | WideDirection3d.GetLocalEdge(this, first, second)).Transform(GeometryBasis); |
| | | 54 | | bool IForwardHullFrame3d.TryGetTriangleHalfPlanes(in FixedTriangle triangle, in RationalPoint3d point, |
| | 2794 | 55 | | Span<Signed832> values) => WideTriangleRelations.GetScaledTriangleHalfPlanes(triangle, this, point, values); |
| | | 56 | | |
| | | 57 | | internal static ScaledRigidFrame3d Rigid(Vector3d origin, FixedQuaternion rotation) => |
| | 177 | 58 | | new(origin, rotation, Vector3d.Zero, Vector3d.One, rotation, Vector3d.Zero, Vector3d.One); |
| | | 59 | | |
| | | 60 | | internal ScaledRigidFrame3d( |
| | | 61 | | Vector3d translation, |
| | | 62 | | FixedQuaternion placementRotation, |
| | | 63 | | Vector3d offset, |
| | | 64 | | Vector3d ownerScale, |
| | | 65 | | FixedQuaternion geometryRotation, |
| | | 66 | | Vector3d reference, |
| | | 67 | | Vector3d partScale) |
| | | 68 | | { |
| | 724 | 69 | | this = default; |
| | 724 | 70 | | if (!placementRotation.IsNormalized()) |
| | 1 | 71 | | throw new ArgumentException("The placement rotation must be normalized.", nameof(placementRotation)); |
| | 723 | 72 | | if (!geometryRotation.IsNormalized()) |
| | 1 | 73 | | throw new ArgumentException("The geometry rotation must be normalized.", nameof(geometryRotation)); |
| | | 74 | | |
| | 722 | 75 | | _reference = reference; |
| | 722 | 76 | | _translation = translation; |
| | 722 | 77 | | _sameRotation = placementRotation == geometryRotation; |
| | 722 | 78 | | _identityUnit = placementRotation == FixedQuaternion.Identity |
| | 722 | 79 | | && geometryRotation == FixedQuaternion.Identity |
| | 722 | 80 | | && ownerScale == Vector3d.One && partScale == Vector3d.One; |
| | 722 | 81 | | GeometryBasis = new WideRationalBasis3d(geometryRotation); |
| | 722 | 82 | | GeometryRotation = geometryRotation; |
| | 722 | 83 | | _scaleX = Fixed64.GetExactRawProduct(ownerScale.X, partScale.X); |
| | 722 | 84 | | _scaleY = Fixed64.GetExactRawProduct(ownerScale.Y, partScale.Y); |
| | 722 | 85 | | _scaleZ = Fixed64.GetExactRawProduct(ownerScale.Z, partScale.Z); |
| | | 86 | | // Placement consumes owner scale only. Its Q denominator is lifted |
| | | 87 | | // to the same Q^2 denominator as the centered three-factor product. |
| | 722 | 88 | | _offsetX = WideArithmetic.ShiftLeftSigned192By32(Fixed64.GetExactRawProduct(offset.X, ownerScale.X)); |
| | 722 | 89 | | _offsetY = WideArithmetic.ShiftLeftSigned192By32(Fixed64.GetExactRawProduct(offset.Y, ownerScale.Y)); |
| | 722 | 90 | | _offsetZ = WideArithmetic.ShiftLeftSigned192By32(Fixed64.GetExactRawProduct(offset.Z, ownerScale.Z)); |
| | | 91 | | |
| | | 92 | | // Frame inputs are immutable. Classify their exact rigid mapping once, |
| | | 93 | | // rather than repeating scale/reference products for every support query. |
| | 722 | 94 | | _isRigid = _sameRotation && _scaleX.Equals(ScaleDenominator) |
| | 722 | 95 | | && _scaleY.Equals(ScaleDenominator) && _scaleZ.Equals(ScaleDenominator) |
| | 722 | 96 | | && _offsetX.Equals(ScaleReference(_reference.X)) |
| | 722 | 97 | | && _offsetY.Equals(ScaleReference(_reference.Y)) |
| | 722 | 98 | | && _offsetZ.Equals(ScaleReference(_reference.Z)); |
| | | 99 | | |
| | 722 | 100 | | WideRationalBasis3d placementBasis = _sameRotation ? GeometryBasis : new(placementRotation); |
| | 722 | 101 | | _placementDenominator = _sameRotation ? Signed192.Signed(1L) : placementBasis.Denominator; |
| | 722 | 102 | | RotationDenominator = Signed192.NarrowProven( |
| | 722 | 103 | | WideArithmetic.MultiplySigned192(GeometryBasis.Denominator, _placementDenominator)); |
| | 722 | 104 | | Denominator = WideArithmetic.MultiplySigned192(RotationDenominator, ScaleDenominator); |
| | 722 | 105 | | _translationX = GetTranslation(translation.X, placementBasis.Xx, placementBasis.Yx, placementBasis.Zx); |
| | 722 | 106 | | _translationY = GetTranslation(translation.Y, placementBasis.Xy, placementBasis.Yy, placementBasis.Zy); |
| | 722 | 107 | | _translationZ = GetTranslation(translation.Z, placementBasis.Xz, placementBasis.Yz, placementBasis.Zz); |
| | 722 | 108 | | } |
| | | 109 | | |
| | | 110 | | /// <summary> |
| | | 111 | | /// Returns centered scaled raw-coordinate numerators over the shared Q^2 |
| | | 112 | | /// denominator. A full-domain difference times two scalar factors fits 192 bits. |
| | | 113 | | /// </summary> |
| | | 114 | | internal void GetScaledLocalPointNumerators(Vector3d source, out Signed192 x, out Signed192 y, out Signed192 z) |
| | | 115 | | { |
| | 287710 | 116 | | x = GetScaledDifference(source.X, _reference.X, _scaleX); |
| | 287710 | 117 | | y = GetScaledDifference(source.Y, _reference.Y, _scaleY); |
| | 287710 | 118 | | z = GetScaledDifference(source.Z, _reference.Z, _scaleZ); |
| | 287710 | 119 | | } |
| | | 120 | | |
| | | 121 | | internal RationalPoint3d GetPoint(Vector3d source) |
| | | 122 | | { |
| | 275502 | 123 | | if (_identityUnit) |
| | | 124 | | { |
| | | 125 | | // Preserve the shared Q^2 denominator with shifts, without rotation products. |
| | 67830 | 126 | | return new RationalPoint3d(GetOrdinaryCoordinate(source.X, _reference.X, _offsetX, _translationX), |
| | 67830 | 127 | | GetOrdinaryCoordinate(source.Y, _reference.Y, _offsetY, _translationY), |
| | 67830 | 128 | | GetOrdinaryCoordinate(source.Z, _reference.Z, _offsetZ, _translationZ), Denominator); |
| | | 129 | | } |
| | | 130 | | |
| | 207672 | 131 | | GetScaledLocalPointNumerators(source, out Signed192 x, out Signed192 y, out Signed192 z); |
| | 207672 | 132 | | if (_sameRotation) |
| | | 133 | | { |
| | 175931 | 134 | | x = WideArithmetic.AddSigned192(x, _offsetX); |
| | 175931 | 135 | | y = WideArithmetic.AddSigned192(y, _offsetY); |
| | 175931 | 136 | | z = WideArithmetic.AddSigned192(z, _offsetZ); |
| | | 137 | | } |
| | 207672 | 138 | | return new RationalPoint3d( |
| | 207672 | 139 | | GetWorldCoordinate(x, y, z, GeometryBasis.Xx, GeometryBasis.Yx, GeometryBasis.Zx, _translationX), |
| | 207672 | 140 | | GetWorldCoordinate(x, y, z, GeometryBasis.Xy, GeometryBasis.Yy, GeometryBasis.Zy, _translationY), |
| | 207672 | 141 | | GetWorldCoordinate(x, y, z, GeometryBasis.Xz, GeometryBasis.Yz, GeometryBasis.Zz, _translationZ), |
| | 207672 | 142 | | Denominator); |
| | | 143 | | } |
| | | 144 | | |
| | 2963 | 145 | | internal bool TryGetPoint(Vector3d source, out Vector3d point) => GetPoint(source).TryGetPoint(out point); |
| | | 146 | | |
| | | 147 | | internal RationalPoint3d GetScaledLocalPoint(Vector3d scaledLocalPoint) |
| | | 148 | | { |
| | 8 | 149 | | Signed192 x = ScaleReference(scaledLocalPoint.X); |
| | 8 | 150 | | Signed192 y = ScaleReference(scaledLocalPoint.Y); |
| | 8 | 151 | | Signed192 z = ScaleReference(scaledLocalPoint.Z); |
| | 8 | 152 | | if (_sameRotation) |
| | | 153 | | { |
| | 2 | 154 | | x = WideArithmetic.AddSigned192(x, _offsetX); |
| | 2 | 155 | | y = WideArithmetic.AddSigned192(y, _offsetY); |
| | 2 | 156 | | z = WideArithmetic.AddSigned192(z, _offsetZ); |
| | | 157 | | } |
| | 8 | 158 | | return new RationalPoint3d( |
| | 8 | 159 | | GetWorldCoordinate(x, y, z, GeometryBasis.Xx, GeometryBasis.Yx, GeometryBasis.Zx, _translationX), |
| | 8 | 160 | | GetWorldCoordinate(x, y, z, GeometryBasis.Xy, GeometryBasis.Yy, GeometryBasis.Zy, _translationY), |
| | 8 | 161 | | GetWorldCoordinate(x, y, z, GeometryBasis.Xz, GeometryBasis.Yz, GeometryBasis.Zz, _translationZ), Denominato |
| | | 162 | | } |
| | | 163 | | |
| | | 164 | | internal bool TryGetScaledLocalPointIn(Vector3d worldPoint, out Vector3d scaledLocalPoint) |
| | | 165 | | { |
| | 14 | 166 | | GetScaledLocalPointNumeratorsIn(worldPoint, out Signed576 x, out Signed576 y, out Signed576 z, |
| | 14 | 167 | | out Signed320 denominator); |
| | 14 | 168 | | Signed576 d = Signed576.ExtendValue(denominator); |
| | 14 | 169 | | bool representable = Fixed64.TryGetSignedRawRatio(x, d, out Fixed64 px) |
| | 14 | 170 | | & Fixed64.TryGetSignedRawRatio(y, d, out Fixed64 py) |
| | 14 | 171 | | & Fixed64.TryGetSignedRawRatio(z, d, out Fixed64 pz); |
| | 14 | 172 | | scaledLocalPoint = representable ? new Vector3d(px, py, pz) : default; |
| | 14 | 173 | | return representable; |
| | | 174 | | } |
| | | 175 | | |
| | | 176 | | internal bool IsScaledLocalPointExact(Vector3d source, Vector3d roundedCentered) |
| | | 177 | | { |
| | 4 | 178 | | GetScaledLocalPointNumerators(source, out Signed192 x, out Signed192 y, out Signed192 z); |
| | 4 | 179 | | return x.Equals(ScaleReference(roundedCentered.X)) |
| | 4 | 180 | | & y.Equals(ScaleReference(roundedCentered.Y)) |
| | 4 | 181 | | & z.Equals(ScaleReference(roundedCentered.Z)); |
| | | 182 | | } |
| | | 183 | | |
| | | 184 | | internal WideAxis3 GetSourceDirectionNumerators(Vector3d worldDirection) |
| | | 185 | | { |
| | 24 | 186 | | Signed192 x = Signed192.Raw(worldDirection.X), y = Signed192.Raw(worldDirection.Y), z = Signed192.Raw(worldDirec |
| | | 187 | | // Rotation coefficients <65 bits, direction components <=64 and scale |
| | | 188 | | // products <=127 keep each complete source covector below 258 bits. |
| | 8 | 189 | | return new WideAxis3( |
| | 8 | 190 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 8 | 191 | | Rotate(x, y, z, GeometryBasis.Xx, GeometryBasis.Xy, GeometryBasis.Xz), _scaleX)), |
| | 8 | 192 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 8 | 193 | | Rotate(x, y, z, GeometryBasis.Yx, GeometryBasis.Yy, GeometryBasis.Yz), _scaleY)), |
| | 8 | 194 | | Signed320.NarrowValue(WideArithmetic.MultiplySigned320( |
| | 8 | 195 | | Rotate(x, y, z, GeometryBasis.Zx, GeometryBasis.Zy, GeometryBasis.Zz), _scaleZ))); |
| | | 196 | | } |
| | | 197 | | |
| | | 198 | | /// <summary>Transforms an authored endpoint difference, whose components have magnitude below 2^64.</summary> |
| | | 199 | | internal WideAxis3 GetWorldDirectionNumerators(in WideAxis3 authoredRay) |
| | | 200 | | { |
| | | 201 | | // A halfspace direction has no metric scale. Rigid endpoint |
| | | 202 | | // differences need only the rotation numerator; retaining unit Q^2 |
| | | 203 | | // scale factors needlessly enlarges every downstream fan product. |
| | 3 | 204 | | if (IsRigid) |
| | 1 | 205 | | return WideRigidProjection.TransformLocalAxis(GeometryBasis, |
| | 1 | 206 | | Signed192.NarrowProven(authoredRay.X), Signed192.NarrowProven(authoredRay.Y), Signed192.NarrowProven(aut |
| | 2 | 207 | | Signed192 x = Signed192.NarrowValue(WideArithmetic.MultiplySigned320(authoredRay.X, _scaleX)); |
| | 2 | 208 | | Signed192 y = Signed192.NarrowValue(WideArithmetic.MultiplySigned320(authoredRay.Y, _scaleY)); |
| | 2 | 209 | | Signed192 z = Signed192.NarrowValue(WideArithmetic.MultiplySigned320(authoredRay.Z, _scaleZ)); |
| | 2 | 210 | | return new WideAxis3(Rotate(x, y, z, GeometryBasis.Xx, GeometryBasis.Yx, GeometryBasis.Zx), |
| | 2 | 211 | | Rotate(x, y, z, GeometryBasis.Xy, GeometryBasis.Yy, GeometryBasis.Zy), |
| | 2 | 212 | | Rotate(x, y, z, GeometryBasis.Xz, GeometryBasis.Yz, GeometryBasis.Zz)); |
| | | 213 | | } |
| | | 214 | | |
| | | 215 | | internal FixedBoundBox GetWorldBounds(Vector3d sourceMin, Vector3d sourceMax) |
| | | 216 | | { |
| | 17 | 217 | | if (IsRigid) |
| | 8 | 218 | | return FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain(RigidOrigin, Rotation, sourceMin, sourceMax, |
| | 8 | 219 | | Vector3d.Zero, FixedQuaternion.Identity); |
| | 9 | 220 | | Vector3d min = new(Fixed64.MaxValue, Fixed64.MaxValue, Fixed64.MaxValue); |
| | 9 | 221 | | Vector3d max = new(Fixed64.MinValue, Fixed64.MinValue, Fixed64.MinValue); |
| | 162 | 222 | | for (int corner = 0; corner < 8; corner++) |
| | | 223 | | { |
| | 72 | 224 | | RationalPoint3d point = GetPoint(GetCorner(sourceMin, sourceMax, corner)); |
| | 72 | 225 | | Signed576 denominator = Signed576.ExtendValue(point.Denominator); |
| | 72 | 226 | | IncludeBound(Signed576.ExtendValue(point.X), denominator, ref min.X, ref max.X); |
| | 72 | 227 | | IncludeBound(Signed576.ExtendValue(point.Y), denominator, ref min.Y, ref max.Y); |
| | 72 | 228 | | IncludeBound(Signed576.ExtendValue(point.Z), denominator, ref min.Z, ref max.Z); |
| | | 229 | | } |
| | 9 | 230 | | return FixedBoundBox.FromMinMax(min, max); |
| | | 231 | | } |
| | | 232 | | |
| | | 233 | | internal bool TryGetSourceBounds(Vector3d worldMin, Vector3d worldMax, out FixedBoundBox bounds) |
| | | 234 | | { |
| | 13 | 235 | | if (_scaleX.IsZero || _scaleY.IsZero || _scaleZ.IsZero) |
| | | 236 | | { |
| | 3 | 237 | | bounds = default; |
| | 3 | 238 | | return false; |
| | | 239 | | } |
| | 10 | 240 | | Vector3d min = new(Fixed64.MaxValue, Fixed64.MaxValue, Fixed64.MaxValue); |
| | 10 | 241 | | Vector3d max = new(Fixed64.MinValue, Fixed64.MinValue, Fixed64.MinValue); |
| | 180 | 242 | | for (int corner = 0; corner < 8; corner++) |
| | | 243 | | { |
| | 80 | 244 | | GetSourcePointNumerators(GetCorner(worldMin, worldMax, corner), |
| | 80 | 245 | | out Signed576 x, out Signed576 y, out Signed576 z, |
| | 80 | 246 | | out Signed576 dx, out Signed576 dy, out Signed576 dz); |
| | 80 | 247 | | IncludeBound(x, dx, ref min.X, ref max.X); |
| | 80 | 248 | | IncludeBound(y, dy, ref min.Y, ref max.Y); |
| | 80 | 249 | | IncludeBound(z, dz, ref min.Z, ref max.Z); |
| | | 250 | | } |
| | 10 | 251 | | bounds = FixedBoundBox.FromMinMax(min, max); |
| | 10 | 252 | | return true; |
| | | 253 | | } |
| | | 254 | | |
| | | 255 | | internal bool TryGetBoundsIn(Vector3d sourceMin, Vector3d sourceMax, |
| | | 256 | | in ScaledRigidFrame3d target, out FixedBoundBox bounds) |
| | | 257 | | { |
| | 6 | 258 | | if (target._scaleX.IsZero || target._scaleY.IsZero || target._scaleZ.IsZero) |
| | | 259 | | { |
| | 4 | 260 | | bounds = default; |
| | 4 | 261 | | return false; |
| | | 262 | | } |
| | 2 | 263 | | Vector3d min = new(Fixed64.MaxValue, Fixed64.MaxValue, Fixed64.MaxValue); |
| | 2 | 264 | | Vector3d max = new(Fixed64.MinValue, Fixed64.MinValue, Fixed64.MinValue); |
| | 36 | 265 | | for (int corner = 0; corner < 8; corner++) |
| | | 266 | | { |
| | 16 | 267 | | RationalPoint3d world = GetPoint(GetCorner(sourceMin, sourceMax, corner)); |
| | 128 | 268 | | for (int axis = 0; axis < 3; axis++) |
| | | 269 | | { |
| | 48 | 270 | | target.GetSourceCoordinateTransform(axis, out Signed576 x, out Signed576 y, out Signed576 z, |
| | 48 | 271 | | out Signed576 offset, out Signed576 divisor); |
| | 48 | 272 | | Signed832 numerator = WideArithmetic.AddSigned832(WideArithmetic.AddSigned832( |
| | 48 | 273 | | WideArithmetic.MultiplySigned576ToSigned832(x, world.X), |
| | 48 | 274 | | WideArithmetic.MultiplySigned576ToSigned832(y, world.Y)), |
| | 48 | 275 | | WideArithmetic.AddSigned832(WideArithmetic.MultiplySigned576ToSigned832(z, world.Z), |
| | 48 | 276 | | WideArithmetic.MultiplySigned576ToSigned832(offset, world.Denominator))); |
| | 48 | 277 | | Signed832 denominator = WideArithmetic.MultiplySigned576ToSigned832(divisor, world.Denominator); |
| | | 278 | | // Both complete affine ratios retain the source's Q^2 factor. |
| | | 279 | | // Cancel it exactly before the bounded 576-bit bound owner. |
| | 96 | 280 | | Signed576 n = RemoveScaleDenominator(numerator), d = RemoveScaleDenominator(denominator); |
| | 48 | 281 | | min[axis] = FixedMath.Min(min[axis], WideGeometry.GetRationalBoundClippedToDomain(n, d, true)); |
| | 48 | 282 | | max[axis] = FixedMath.Max(max[axis], WideGeometry.GetRationalBoundClippedToDomain(n, d, false)); |
| | | 283 | | } |
| | | 284 | | } |
| | 2 | 285 | | bounds = FixedBoundBox.FromMinMax(min, max); |
| | 2 | 286 | | return true; |
| | | 287 | | } |
| | | 288 | | |
| | | 289 | | /// <summary> |
| | | 290 | | /// Inverts this frame without materializing its transformed reference. |
| | | 291 | | /// Each source coordinate has its own scale denominator; zero scale makes |
| | | 292 | | /// that coordinate undefined and therefore makes materialization fail. |
| | | 293 | | /// </summary> |
| | | 294 | | internal void GetSourcePointNumerators(Vector3d worldPoint, |
| | | 295 | | out Signed576 x, out Signed576 y, out Signed576 z, |
| | | 296 | | out Signed576 denominatorX, out Signed576 denominatorY, out Signed576 denominatorZ) |
| | | 297 | | { |
| | 88 | 298 | | GetScaledLocalPointNumeratorsIn(worldPoint, out Signed576 inverseX, out Signed576 inverseY, |
| | 88 | 299 | | out Signed576 inverseZ, out Signed320 rotationDenominator); |
| | 88 | 300 | | GetSourceCoordinate(inverseX, rotationDenominator, |
| | 88 | 301 | | _reference.X, _scaleX, out x, out denominatorX); |
| | 88 | 302 | | GetSourceCoordinate(inverseY, rotationDenominator, |
| | 88 | 303 | | _reference.Y, _scaleY, out y, out denominatorY); |
| | 88 | 304 | | GetSourceCoordinate(inverseZ, rotationDenominator, |
| | 88 | 305 | | _reference.Z, _scaleZ, out z, out denominatorZ); |
| | 88 | 306 | | } |
| | | 307 | | |
| | | 308 | | internal bool TryGetSourceBoundsFromScaledLocal(Vector3d scaledMin, Vector3d scaledMax, out FixedBoundBox bounds) |
| | | 309 | | { |
| | 17 | 310 | | if (_scaleX.IsZero || _scaleY.IsZero || _scaleZ.IsZero) { bounds = default; return false; } |
| | 16 | 311 | | Vector3d min = default, max = default; |
| | 8 | 312 | | Signed320 one = Signed320.ExtendValue(Signed192.Signed(1)); |
| | 64 | 313 | | for (int axis = 0; axis < 3; axis++) |
| | | 314 | | { |
| | 48 | 315 | | min[axis] = Fixed64.MaxValue; max[axis] = Fixed64.MinValue; |
| | 144 | 316 | | for (int end = 0; end < 2; end++) |
| | | 317 | | { |
| | 48 | 318 | | Fixed64 value = end == 0 ? scaledMin[axis] : scaledMax[axis]; |
| | 48 | 319 | | GetSourceCoordinate(Signed576.ExtendValue(Signed320.ExtendValue(Signed192.Raw(value))), one, |
| | 48 | 320 | | _reference[axis], GetScale(axis), out Signed576 numerator, out Signed576 denominator); |
| | 48 | 321 | | min[axis] = FixedMath.Min(min[axis], WideGeometry.GetRationalBoundClippedToDomain(numerator, denominator |
| | 48 | 322 | | max[axis] = FixedMath.Max(max[axis], WideGeometry.GetRationalBoundClippedToDomain(numerator, denominator |
| | | 323 | | } |
| | | 324 | | } |
| | 8 | 325 | | bounds = FixedBoundBox.FromMinMax(min, max); |
| | 8 | 326 | | return true; |
| | | 327 | | } |
| | | 328 | | internal bool HasSameMapping(in ScaledRigidFrame3d other) |
| | | 329 | | { |
| | 110 | 330 | | for (int axis = 0; axis < 3; axis++) |
| | | 331 | | { |
| | 90 | 332 | | WideAxis3 first = GeometryBasis.GetAxis(axis), second = other.GeometryBasis.GetAxis(axis); |
| | 90 | 333 | | Signed192 firstScale = GetScale(axis), secondScale = other.GetScale(axis); |
| | 45 | 334 | | if (!SameLinearCoordinate(first.X, firstScale, other.GeometryBasis.Denominator, |
| | 45 | 335 | | second.X, secondScale, GeometryBasis.Denominator) |
| | 45 | 336 | | || !SameLinearCoordinate(first.Y, firstScale, other.GeometryBasis.Denominator, |
| | 45 | 337 | | second.Y, secondScale, GeometryBasis.Denominator) |
| | 45 | 338 | | || !SameLinearCoordinate(first.Z, firstScale, other.GeometryBasis.Denominator, |
| | 57 | 339 | | second.Z, secondScale, GeometryBasis.Denominator)) return false; |
| | | 340 | | } |
| | 20 | 341 | | RationalPoint3d origin = GetPoint(Vector3d.Zero), otherOrigin = other.GetPoint(Vector3d.Zero); |
| | | 342 | | // Both point denominators contain the same Q^2 factor; cancel it |
| | | 343 | | // before comparing independently retained placement origins. |
| | 10 | 344 | | return SameMappedCoordinate(origin.X, other.RotationDenominator, otherOrigin.X, RotationDenominator) |
| | 10 | 345 | | & SameMappedCoordinate(origin.Y, other.RotationDenominator, otherOrigin.Y, RotationDenominator) |
| | 10 | 346 | | & SameMappedCoordinate(origin.Z, other.RotationDenominator, otherOrigin.Z, RotationDenominator); |
| | | 347 | | } |
| | | 348 | | |
| | | 349 | | /// <summary> |
| | | 350 | | /// Returns one inverse coordinate as (x*worldX + y*worldY + z*worldZ + |
| | | 351 | | /// offset)/divisor, where world components and the result are raw coordinates. |
| | | 352 | | /// The axis is in [0,2]; a zero scale produces a zero divisor. |
| | | 353 | | /// </summary> |
| | | 354 | | internal void GetSourceCoordinateTransform(int axis, out Signed576 x, out Signed576 y, out Signed576 z, |
| | | 355 | | out Signed576 offset, out Signed576 divisor) |
| | | 356 | | { |
| | 128 | 357 | | WideAxis3 column = GeometryBasis.GetAxis(axis); |
| | | 358 | | // Every inverse coefficient has a common Q^2 factor; cancel it here |
| | | 359 | | // so chained geometry does not multiply redundant scale denominators. |
| | 128 | 360 | | x = WideArithmetic.MultiplySigned320(column.X, Denominator); |
| | 128 | 361 | | y = WideArithmetic.MultiplySigned320(column.Y, Denominator); |
| | 128 | 362 | | z = WideArithmetic.MultiplySigned320(column.Z, Denominator); |
| | 128 | 363 | | divisor = WideArithmetic.MultiplySigned576( |
| | 128 | 364 | | Signed576.ExtendValue(WideArithmetic.MultiplySigned192(RotationDenominator, GeometryBasis.Denominator)), Get |
| | 128 | 365 | | RationalPoint3d origin = GetPoint(_reference); |
| | 128 | 366 | | Signed576 projectedOrigin = WideArithmetic.AddSigned576(WideArithmetic.AddSigned576( |
| | 128 | 367 | | WideArithmetic.MultiplySigned320(origin.X, column.X), WideArithmetic.MultiplySigned320(origin.Y, column.Y)), |
| | 128 | 368 | | WideArithmetic.MultiplySigned320(origin.Z, column.Z)); |
| | 128 | 369 | | offset = WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned576(divisor, _reference[axis].m_rawValue) |
| | 128 | 370 | | projectedOrigin); |
| | 128 | 371 | | } |
| | | 372 | | |
| | 532 | 373 | | private Signed192 GetScale(int axis) => axis switch { 0 => _scaleX, 1 => _scaleY, _ => _scaleZ }; |
| | | 374 | | |
| | | 375 | | private static bool SameLinearCoordinate(Signed320 first, Signed192 firstScale, Signed192 otherDenominator, |
| | | 376 | | Signed320 second, Signed192 secondScale, Signed192 denominator) => |
| | 114 | 377 | | WideArithmetic.SubtractSigned576( |
| | 114 | 378 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(first, firstScale), otherDenominator), |
| | 114 | 379 | | WideArithmetic.MultiplySigned576(WideArithmetic.MultiplySigned320(second, secondScale), denominator)).IsZero |
| | | 380 | | |
| | | 381 | | private static bool SameMappedCoordinate(Signed320 first, Signed192 otherDenominator, |
| | | 382 | | Signed320 second, Signed192 denominator) => |
| | 30 | 383 | | WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(first, otherDenominator), |
| | 30 | 384 | | WideArithmetic.MultiplySigned320(second, denominator)).IsZero; |
| | | 385 | | |
| | | 386 | | private static Vector3d GetCorner(Vector3d min, Vector3d max, int corner) => |
| | 168 | 387 | | new((corner & 1) == 0 ? min.X : max.X, (corner & 2) == 0 ? min.Y : max.Y, |
| | 168 | 388 | | (corner & 4) == 0 ? min.Z : max.Z); |
| | | 389 | | |
| | | 390 | | private static Signed576 RemoveScaleDenominator(Signed832 value) => |
| | 96 | 391 | | new(value.Word9, value.Word8, value.Word7, value.Word6, value.Word5, |
| | 96 | 392 | | value.Word4, value.Word3, value.Word2, value.Word1); |
| | | 393 | | |
| | | 394 | | private static void IncludeBound(Signed576 numerator, Signed576 denominator, ref Fixed64 min, ref Fixed64 max) |
| | | 395 | | { |
| | 456 | 396 | | min = FixedMath.Min(min, WideGeometry.GetRationalBoundClippedToDomain(numerator, denominator, minimum: true)); |
| | 456 | 397 | | max = FixedMath.Max(max, WideGeometry.GetRationalBoundClippedToDomain(numerator, denominator, minimum: false)); |
| | 456 | 398 | | } |
| | | 399 | | |
| | | 400 | | private static Signed192 ScaleReference(Fixed64 value) => |
| | 908 | 401 | | WideArithmetic.ShiftLeftSigned192By32(WideArithmetic.ShiftLeftSigned192By32(Signed192.Raw(value))); |
| | | 402 | | |
| | | 403 | | private void GetScaledLocalPointNumeratorsIn(Vector3d worldPoint, |
| | | 404 | | out Signed576 x, out Signed576 y, out Signed576 z, out Signed320 denominator) |
| | | 405 | | { |
| | 102 | 406 | | GetPoint(_reference).GetLocalPointNumerators(worldPoint, GeometryRotation, |
| | 102 | 407 | | out Signed576 inverseX, out Signed576 inverseY, out Signed576 inverseZ, out denominator); |
| | | 408 | | // The rigid projection gives reference-worldPoint; negate to obtain the |
| | | 409 | | // displacement from the exact geometry origin to the requested point. |
| | 102 | 410 | | x = WideArithmetic.SubtractSigned576(default, inverseX); |
| | 102 | 411 | | y = WideArithmetic.SubtractSigned576(default, inverseY); |
| | 102 | 412 | | z = WideArithmetic.SubtractSigned576(default, inverseZ); |
| | 102 | 413 | | } |
| | | 414 | | |
| | | 415 | | private static void GetSourceCoordinate(Signed576 localNumerator, Signed320 localDenominator, |
| | | 416 | | Fixed64 reference, Signed192 scale, out Signed576 numerator, out Signed576 denominator) |
| | | 417 | | { |
| | 312 | 418 | | denominator = WideArithmetic.MultiplySigned320(localDenominator, scale); |
| | | 419 | | // Three source/target rotations, two scalar factors and reference |
| | | 420 | | // multiplication keep these inverse numerators below 452 bits. |
| | 312 | 421 | | numerator = WideArithmetic.AddSigned576( |
| | 312 | 422 | | WideArithmetic.MultiplySigned576(localNumerator, ScaleDenominator), |
| | 312 | 423 | | WideArithmetic.MultiplySigned576(denominator, reference.m_rawValue)); |
| | 312 | 424 | | } |
| | | 425 | | |
| | | 426 | | private Signed320 GetTranslation(Fixed64 translation, Signed192 axisX, Signed192 axisY, Signed192 axisZ) |
| | | 427 | | { |
| | 2166 | 428 | | Signed576 result = WideArithmetic.MultiplySigned320(Denominator, Signed192.Raw(translation)); |
| | 2166 | 429 | | if (!_sameRotation) |
| | | 430 | | { |
| | 294 | 431 | | Signed320 rotated = Rotate(_offsetX, _offsetY, _offsetZ, axisX, axisY, axisZ); |
| | 294 | 432 | | result = WideArithmetic.AddSigned576(result, |
| | 294 | 433 | | WideArithmetic.MultiplySigned320(rotated, GeometryBasis.Denominator)); |
| | | 434 | | } |
| | 2166 | 435 | | return Signed320.NarrowValue(result); |
| | | 436 | | } |
| | | 437 | | |
| | | 438 | | private Signed320 GetWorldCoordinate(Signed192 x, Signed192 y, Signed192 z, |
| | | 439 | | Signed192 axisX, Signed192 axisY, Signed192 axisZ, Signed320 translation) |
| | | 440 | | { |
| | 623040 | 441 | | Signed576 result = WideArithmetic.AddSigned576(Signed576.ExtendValue(translation), |
| | 623040 | 442 | | WideArithmetic.MultiplySigned320(Rotate(x, y, z, axisX, axisY, axisZ), _placementDenominator)); |
| | | 443 | | // Normalized quaternion D < Q^2 + 258Q < (17/16)Q^2. |
| | | 444 | | // The geometry numerator vector has norm < sqrt(3)*2^190. |
| | | 445 | | // Two basis denominators give magnitude < (7/4)*(17/16)^2*2^318, |
| | | 446 | | // leaving more than 2^312 of headroom below the signed 320-bit limit. |
| | | 447 | | // Full-domain placement and translation together are below 2^290. |
| | 623040 | 448 | | return Signed320.NarrowValue(result); |
| | | 449 | | } |
| | | 450 | | |
| | | 451 | | private static Signed192 GetScaledDifference(Fixed64 source, Fixed64 reference, Signed192 scale) => |
| | 863130 | 452 | | Signed192.NarrowProven(WideArithmetic.MultiplySigned192( |
| | 863130 | 453 | | WideArithmetic.Difference(source, reference), scale)); |
| | | 454 | | |
| | | 455 | | private static Signed320 GetOrdinaryCoordinate(Fixed64 source, Fixed64 reference, Signed192 offset, Signed320 transl |
| | 203490 | 456 | | WideArithmetic.AddSigned320(translation, Signed320.ExtendValue( |
| | 203490 | 457 | | WideArithmetic.ShiftLeftSigned192By32(WideArithmetic.ShiftLeftSigned192By32( |
| | 203490 | 458 | | WideArithmetic.AddSigned192(WideArithmetic.Difference(source, reference), |
| | 203490 | 459 | | Signed192.Signed(unchecked((long)offset.Middle))))))); |
| | | 460 | | |
| | | 461 | | private static Signed320 Rotate(Signed192 x, Signed192 y, Signed192 z, |
| | | 462 | | Signed192 axisX, Signed192 axisY, Signed192 axisZ) => |
| | 623364 | 463 | | WideArithmetic.AddSigned320(WideArithmetic.AddSigned320( |
| | 623364 | 464 | | WideArithmetic.MultiplySigned192(x, axisX), WideArithmetic.MultiplySigned192(y, axisY)), |
| | 623364 | 465 | | WideArithmetic.MultiplySigned192(z, axisZ)); |
| | | 466 | | } |