| | | 1 | | //======================================================================= |
| | | 2 | | // WideDirection3d.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 | | namespace FixedMathSharp.Geometry; |
| | | 9 | | |
| | | 10 | | /// <summary>An exact scaled-hull SAT direction; its magnitude is deliberately arbitrary.</summary> |
| | | 11 | | internal readonly struct WideDirection3d |
| | | 12 | | { |
| | | 13 | | internal readonly Signed576 X, Y, Z; |
| | 6764136 | 14 | | internal WideDirection3d(Signed576 x, Signed576 y, Signed576 z) { X = x; Y = y; Z = z; } |
| | | 15 | | internal WideDirection3d(Signed320 x, Signed320 y, Signed320 z) |
| | 816464 | 16 | | : this(Signed576.ExtendValue(x), Signed576.ExtendValue(y), Signed576.ExtendValue(z)) { } |
| | | 17 | | internal WideDirection3d(in WideAxis3 value) |
| | 49070 | 18 | | : this(Signed576.ExtendValue(value.X), Signed576.ExtendValue(value.Y), Signed576.ExtendValue(value.Z)) { } |
| | 619843 | 19 | | internal bool IsZero => X.IsZero && Y.IsZero && Z.IsZero; |
| | 57811 | 20 | | internal Signed832 SquaredLength => WideArithmetic.AddSigned832(WideArithmetic.AddSigned832( |
| | 57811 | 21 | | WideArithmetic.MultiplySigned576ToSigned832(X, X), WideArithmetic.MultiplySigned576ToSigned832(Y, Y)), |
| | 57811 | 22 | | WideArithmetic.MultiplySigned576ToSigned832(Z, Z)); |
| | 164 | 23 | | internal Vector3d GetNormal(bool negate) => WideNormalization.GetNormalized( |
| | 164 | 24 | | negate ? WideArithmetic.SubtractSigned576(default, X) : X, |
| | 164 | 25 | | negate ? WideArithmetic.SubtractSigned576(default, Y) : Y, |
| | 164 | 26 | | negate ? WideArithmetic.SubtractSigned576(default, Z) : Z); |
| | | 27 | | |
| | 54036 | 28 | | internal static WideDirection3d Cross(in WideDirection3d a, in WideDirection3d b) => new( |
| | 54036 | 29 | | DifferenceProduct(a.Y, b.Z, a.Z, b.Y), DifferenceProduct(a.Z, b.X, a.X, b.Z), |
| | 54036 | 30 | | DifferenceProduct(a.X, b.Y, a.Y, b.X)); |
| | | 31 | | internal static Signed832 Dot(in WideDirection3d axis, in WideAxis3 point) => |
| | 34520 | 32 | | WideArithmetic.AddSigned832(WideArithmetic.AddSigned832( |
| | 34520 | 33 | | WideArithmetic.MultiplySigned576ToSigned832(axis.X, point.X), |
| | 34520 | 34 | | WideArithmetic.MultiplySigned576ToSigned832(axis.Y, point.Y)), |
| | 34520 | 35 | | WideArithmetic.MultiplySigned576ToSigned832(axis.Z, point.Z)); |
| | | 36 | | internal static Signed832 Dot(in WideDirection3d first, in WideDirection3d second) => |
| | 515131 | 37 | | WideArithmetic.AddSigned832(WideArithmetic.AddSigned832( |
| | 515131 | 38 | | WideArithmetic.MultiplySigned576ToSigned832(first.X, second.X), |
| | 515131 | 39 | | WideArithmetic.MultiplySigned576ToSigned832(first.Y, second.Y)), |
| | 515131 | 40 | | WideArithmetic.MultiplySigned576ToSigned832(first.Z, second.Z)); |
| | | 41 | | internal static bool AreParallel(in WideDirection3d first, in WideDirection3d second) => |
| | 2887 | 42 | | WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576ToSigned832(first.Y, second.Z), |
| | 2887 | 43 | | WideArithmetic.MultiplySigned576ToSigned832(first.Z, second.Y)).IsZero |
| | 2887 | 44 | | && WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576ToSigned832(first.Z, second.X), |
| | 2887 | 45 | | WideArithmetic.MultiplySigned576ToSigned832(first.X, second.Z)).IsZero |
| | 2887 | 46 | | && WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576ToSigned832(first.X, second.Y), |
| | 2887 | 47 | | WideArithmetic.MultiplySigned576ToSigned832(first.Y, second.X)).IsZero; |
| | | 48 | | /// <summary> |
| | | 49 | | /// Returns axis squared times value minus (axis dot value) times axis. |
| | | 50 | | /// The caller proves the squared norm, dot and result fit 576 bits, and |
| | | 51 | | /// every complete product/difference fits 832 bits. |
| | | 52 | | /// </summary> |
| | | 53 | | internal static WideDirection3d ProjectPerpendicular(in WideDirection3d axis, in WideDirection3d value) |
| | | 54 | | { |
| | 8429 | 55 | | Signed576 squaredAxis = Signed576.NarrowValue(axis.SquaredLength); |
| | 8429 | 56 | | Signed576 projection = Signed576.NarrowValue(Dot(axis, value)); |
| | 8429 | 57 | | return DifferenceScaled(value, squaredAxis, axis, projection); |
| | | 58 | | } |
| | | 59 | | |
| | | 60 | | /// <summary> |
| | | 61 | | /// Returns firstScale times first minus secondScale times second. |
| | | 62 | | /// Complete products/differences must fit 832 bits and result components 576. |
| | | 63 | | /// </summary> |
| | | 64 | | internal static WideDirection3d DifferenceScaled(in WideDirection3d first, Signed576 firstScale, |
| | 9869 | 65 | | in WideDirection3d second, Signed576 secondScale) => new( |
| | 9869 | 66 | | DifferenceProduct(first.X, firstScale, second.X, secondScale), |
| | 9869 | 67 | | DifferenceProduct(first.Y, firstScale, second.Y, secondScale), |
| | 9869 | 68 | | DifferenceProduct(first.Z, firstScale, second.Z, secondScale)); |
| | | 69 | | |
| | 78055 | 70 | | public static WideDirection3d operator -(WideDirection3d value) => new( |
| | 78055 | 71 | | WideArithmetic.SubtractSigned576(default, value.X), WideArithmetic.SubtractSigned576(default, value.Y), |
| | 78055 | 72 | | WideArithmetic.SubtractSigned576(default, value.Z)); |
| | | 73 | | private static Signed576 DifferenceProduct(Signed576 a, Signed576 b, Signed576 c, Signed576 d) => |
| | 191715 | 74 | | Signed576.NarrowValue(WideArithmetic.SubtractSigned832(WideArithmetic.MultiplySigned576ToSigned832(a, b), |
| | 191715 | 75 | | WideArithmetic.MultiplySigned576ToSigned832(c, d))); |
| | | 76 | | |
| | | 77 | | internal static WideDirection3d GetLocalEdge(in ScaledRigidFrame3d frame, Vector3d a, Vector3d b) |
| | | 78 | | { |
| | 39989 | 79 | | frame.GetScaledLocalPointNumerators(a, out Signed192 ax, out Signed192 ay, out Signed192 az); |
| | 39989 | 80 | | frame.GetScaledLocalPointNumerators(b, out Signed192 bx, out Signed192 by, out Signed192 bz); |
| | 39989 | 81 | | return new WideDirection3d(Signed576.ExtendValue(Signed320.ExtendValue(WideArithmetic.SubtractSigned192(bx, ax)) |
| | 39989 | 82 | | Signed576.ExtendValue(Signed320.ExtendValue(WideArithmetic.SubtractSigned192(by, ay))), |
| | 39989 | 83 | | Signed576.ExtendValue(Signed320.ExtendValue(WideArithmetic.SubtractSigned192(bz, az)))); |
| | | 84 | | } |
| | 36396 | 85 | | internal WideDirection3d Transform(in WideRationalBasis3d basis) => new( |
| | 36396 | 86 | | TransformComponent(basis.Xx, basis.Yx, basis.Zx), TransformComponent(basis.Xy, basis.Yy, basis.Zy), |
| | 36396 | 87 | | TransformComponent(basis.Xz, basis.Yz, basis.Zz)); |
| | | 88 | | private Signed576 TransformComponent(Signed192 x, Signed192 y, Signed192 z) => |
| | 109188 | 89 | | WideArithmetic.AddSigned576(WideArithmetic.AddSigned576(WideArithmetic.MultiplySigned576(X, x), |
| | 109188 | 90 | | WideArithmetic.MultiplySigned576(Y, y)), WideArithmetic.MultiplySigned576(Z, z)); |
| | | 91 | | } |