| | | 1 | | //======================================================================= |
| | | 2 | | // ConePlaneRayFrame.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>One canonical authored plane and exact cone frame shared by its finite sections.</summary> |
| | | 13 | | internal readonly struct ConePlaneRayFrame |
| | | 14 | | { |
| | | 15 | | internal readonly Vector3d ConeCenter; |
| | | 16 | | internal readonly FixedQuaternion ConeRotation; |
| | | 17 | | internal readonly Fixed64 Height, Radius; |
| | | 18 | | internal readonly ConeFiniteSectionFrame Finite; |
| | | 19 | | internal readonly WideDirection3d Normal; |
| | | 20 | | internal readonly Signed832 PlaneConstant, NormalSquared; |
| | | 21 | | private readonly WideAxis3 authoredNormal; |
| | | 22 | | private readonly bool _wideParameters; |
| | 43 | 23 | | internal ScaledRigidFrame3d SourceFrame => Finite.ShapeFrame.SourceFrame; |
| | 1222365 | 24 | | internal bool UsesWideBanks => Finite.ShapeFrame.HasExactScale || _wideParameters; |
| | 309247 | 25 | | internal int FieldWords => UsesWideBanks ? 192 : ConePlaneRaySelection.FieldWords; |
| | 174878 | 26 | | internal int RootWords => UsesWideBanks ? 120 : ConePlaneRaySelection.RootWords; |
| | 724192 | 27 | | internal int CircleWords => UsesWideBanks ? 60 : 20; |
| | 26917 | 28 | | internal int PointStorageWords => 8 * FieldWords; |
| | 151 | 29 | | internal int SelectionStorageWords => 12 * FieldWords + RootWords; |
| | 11126 | 30 | | internal int AdmissionWords => UsesWideBanks ? 432 : 144; |
| | 2904 | 31 | | internal int VerificationWords => UsesWideBanks ? 528 : 176; |
| | 6 | 32 | | internal ConePlaneRayFrame WithWideParameters() => UsesWideBanks ? this : new ConePlaneRayFrame(this); |
| | | 33 | | |
| | | 34 | | // Numeric native charts are already cone-local. Materialization supplies |
| | | 35 | | // their actual physical pose without retransforming or replaying an event. |
| | 10 | 36 | | internal ConePlaneRayFrame WithConePose(Vector3d center, FixedQuaternion rotation) => new(this, center, rotation); |
| | | 37 | | private ConePlaneRayFrame(in ConePlaneRayFrame source, Vector3d center, FixedQuaternion rotation) |
| | 40 | 38 | | { this = source; ConeCenter = center; ConeRotation = rotation; } |
| | | 39 | | |
| | | 40 | | private ConePlaneRayFrame(in ConePlaneRayFrame source) |
| | | 41 | | { |
| | 5 | 42 | | this = source; |
| | 5 | 43 | | _wideParameters = true; |
| | 5 | 44 | | } |
| | | 45 | | |
| | | 46 | | /// <summary>Shares an exact primitive-local native plane without a rounded source transform.</summary> |
| | | 47 | | internal ConePlaneRayFrame(Fixed64 height, Fixed64 radius, Signed320 coordinateDenominator, |
| | | 48 | | in WideDirection3d centeredNormal, Signed832 doubledCenteredConstant) |
| | | 49 | | { |
| | 12 | 50 | | this = default; |
| | 36 | 51 | | ConeRotation = FixedQuaternion.Identity; Height = height; Radius = radius; |
| | 12 | 52 | | Finite = new ConeFiniteSectionFrame(height, radius, coordinateDenominator); |
| | 12 | 53 | | Normal = new(centeredNormal.X, WideArithmetic.Negate(centeredNormal.Y), centeredNormal.Z); |
| | 12 | 54 | | PlaneConstant = WideArithmetic.SubtractSigned832(doubledCenteredConstant, |
| | 12 | 55 | | WideArithmetic.MultiplySigned576ToSigned832(centeredNormal.Y, Finite.ShapeFrame.Cap)); |
| | 12 | 56 | | NormalSquared = Normal.SquaredLength; |
| | 12 | 57 | | authoredNormal = new(Signed320.NarrowValue(centeredNormal.X), Signed320.NarrowValue(centeredNormal.Y), Signed320 |
| | 12 | 58 | | } |
| | | 59 | | |
| | | 60 | | internal ConePlaneRayFrame(FixedTriangle planeTriangle, Vector3d origin, FixedQuaternion rotation, |
| | | 61 | | Vector3d coneCenter, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius) |
| | | 62 | | { |
| | 723 | 63 | | _wideParameters = false; |
| | 2892 | 64 | | ConeCenter = coneCenter; ConeRotation = coneRotation; Height = height; Radius = radius; |
| | 723 | 65 | | Finite = new ConeFiniteSectionFrame(origin, rotation, coneCenter, coneRotation, height, radius); |
| | 721 | 66 | | planeTriangle.GetExactNormal(out Signed192 nx, out Signed192 ny, out Signed192 nz, out _); |
| | 721 | 67 | | Span<Signed320> primitive = stackalloc Signed320[3] |
| | 721 | 68 | | { |
| | 721 | 69 | | Signed320.ExtendValue(nx), Signed320.ExtendValue(ny), Signed320.ExtendValue(nz) |
| | 721 | 70 | | }; |
| | 721 | 71 | | if (!nx.IsZero || !ny.IsZero || !nz.IsZero) |
| | 714 | 72 | | WideArithmetic.ReduceCommonScale(primitive); |
| | 721 | 73 | | int orientation = primitive[0].Sign != 0 ? primitive[0].Sign |
| | 721 | 74 | | : primitive[1].Sign != 0 ? primitive[1].Sign : primitive[2].Sign; |
| | 721 | 75 | | if (orientation < 0) |
| | 1480 | 76 | | for (int axis = 0; axis < 3; axis++) |
| | 555 | 77 | | primitive[axis] = WideArithmetic.Negate(primitive[axis]); |
| | 721 | 78 | | authoredNormal = new WideAxis3(primitive[0], primitive[1], primitive[2]); |
| | 721 | 79 | | WideAxis3 localNormal = WideRigidProjection.TransformLocalAxis(Finite.ShapeFrame.Basis, |
| | 721 | 80 | | Signed192.NarrowProven(primitive[0]), Signed192.NarrowProven(primitive[1]), Signed192.NarrowProven(primitive |
| | | 81 | | // Reflect the centered +Y coordinates into the apex-to-base +Y |
| | | 82 | | // cone frame. This is an exact coordinate convention, not rounding. |
| | 2163 | 83 | | primitive[0] = localNormal.X; primitive[1] = WideArithmetic.Negate(localNormal.Y); primitive[2] = localNormal.Z; |
| | | 84 | | // A positive common scale carries no plane or ray geometry. Remove |
| | | 85 | | // the quaternion basis denominator here so it does not multiply every |
| | | 86 | | // critical-event coefficient, while preserving the authored orientation. |
| | 1435 | 87 | | if (!localNormal.IsZero) WideArithmetic.ReduceCommonScale(primitive); |
| | 721 | 88 | | Normal = new WideDirection3d(new WideAxis3(primitive[0], primitive[1], primitive[2])); |
| | 721 | 89 | | PlaneConstant = WideDirection3d.Dot(Normal, Transform(planeTriangle.A)); |
| | 721 | 90 | | NormalSquared = Normal.SquaredLength; |
| | 721 | 91 | | } |
| | | 92 | | |
| | | 93 | | internal ConePlaneRayFrame(FixedTriangle planeTriangle, in ScaledRigidFrame3d sourceFrame, |
| | | 94 | | Vector3d coneCenter, FixedQuaternion coneRotation, Fixed64 height, Fixed64 radius) |
| | | 95 | | { |
| | 33 | 96 | | if (sourceFrame.IsRigid) |
| | | 97 | | { |
| | 12 | 98 | | this = new ConePlaneRayFrame(planeTriangle, sourceFrame.RigidOrigin, sourceFrame.Rotation, |
| | 12 | 99 | | coneCenter, coneRotation, height, radius); |
| | 12 | 100 | | return; |
| | | 101 | | } |
| | 84 | 102 | | ConeCenter = coneCenter; ConeRotation = coneRotation; Height = height; Radius = radius; |
| | 21 | 103 | | _wideParameters = false; |
| | 21 | 104 | | Finite = new ConeFiniteSectionFrame(sourceFrame, coneCenter, coneRotation, height, radius); |
| | 21 | 105 | | WideDirection3d edge = WideDirection3d.GetLocalEdge(sourceFrame, planeTriangle.A, planeTriangle.B); |
| | 21 | 106 | | WideDirection3d side = WideDirection3d.GetLocalEdge(sourceFrame, planeTriangle.A, planeTriangle.C); |
| | 21 | 107 | | WideDirection3d cross = WideDirection3d.Cross(edge, side); |
| | 21 | 108 | | Span<Signed320> primitive = stackalloc Signed320[3] |
| | 21 | 109 | | { |
| | 21 | 110 | | Signed320.NarrowValue(cross.X), Signed320.NarrowValue(cross.Y), Signed320.NarrowValue(cross.Z) |
| | 21 | 111 | | }; |
| | 41 | 112 | | if (!cross.IsZero) WideArithmetic.ReduceCommonScale(primitive); |
| | 21 | 113 | | int orientation = primitive[0].Sign != 0 ? primitive[0].Sign |
| | 21 | 114 | | : primitive[1].Sign != 0 ? primitive[1].Sign : primitive[2].Sign; |
| | 21 | 115 | | if (orientation < 0) |
| | 198 | 116 | | for (int axis = 0; axis < 3; axis++) primitive[axis] = WideArithmetic.Negate(primitive[axis]); |
| | 21 | 117 | | authoredNormal = new WideAxis3(primitive[0], primitive[1], primitive[2]); |
| | 21 | 118 | | WideDirection3d local = new WideDirection3d(authoredNormal).Transform(Finite.ShapeFrame.Basis); |
| | 21 | 119 | | Span<Signed576> localComponents = stackalloc Signed576[3] |
| | 21 | 120 | | { local.X, WideArithmetic.SubtractSigned576(default, local.Y), local.Z }; |
| | 41 | 121 | | if (!local.IsZero) WideArithmetic.ReduceCommonScale(localComponents); |
| | 21 | 122 | | Normal = new WideDirection3d(localComponents[0], localComponents[1], localComponents[2]); |
| | | 123 | | // Scaled edges <110 bits give an authored normal <221; a relative |
| | | 124 | | // rigid basis adds <130. Plane and norm² stay below 660/705 bits. |
| | 21 | 125 | | PlaneConstant = WideDirection3d.Dot(Normal, Transform(planeTriangle.A)); |
| | 21 | 126 | | NormalSquared = Normal.SquaredLength; |
| | 21 | 127 | | } |
| | | 128 | | |
| | | 129 | | /// <summary>Gets the exact canonical covector in the original authored frame, before normal rounding.</summary> |
| | 2 | 130 | | internal WideAxis3 AuthoredNormal => authoredNormal; |
| | | 131 | | |
| | | 132 | | /// <summary>Materializes the canonical plane normal in its original authored rigid frame.</summary> |
| | | 133 | | /// <remarks>The rotation is the same validated authored rotation used to construct this frame.</remarks> |
| | | 134 | | internal Vector3d GetWorldNormal(FixedQuaternion authoredRotation) |
| | | 135 | | { |
| | 10 | 136 | | if (Finite.ShapeFrame.HasExactScale) |
| | 5 | 137 | | return new WideDirection3d(authoredNormal).Transform(SourceFrame.GeometryBasis).GetNormal(false); |
| | | 138 | | // Primitive authored normal components are <130 bits and a normalized |
| | | 139 | | // quaternion's exact basis numerators are <65 bits. The three-term |
| | | 140 | | // transform stays <197 bits, within Signed320, without rounding a |
| | | 141 | | // local normal first or losing its canonical leading-component sign. |
| | 5 | 142 | | WideAxis3 normal = WideRigidProjection.TransformLocalAxis(new WideRationalBasis3d(authoredRotation), |
| | 5 | 143 | | Signed192.NarrowProven(authoredNormal.X), Signed192.NarrowProven(authoredNormal.Y), |
| | 5 | 144 | | Signed192.NarrowProven(authoredNormal.Z)); |
| | 5 | 145 | | return WideNormalization.GetNormalized(normal.X, normal.Y, normal.Z); |
| | | 146 | | } |
| | | 147 | | |
| | 49501 | 148 | | internal WideAxis3 Transform(Vector3d point) => Finite.Transform(point); |
| | | 149 | | |
| | | 150 | | internal bool ContainsPoint(Vector3d point) |
| | | 151 | | { |
| | 4 | 152 | | WideAxis3 p = Finite.Transform(point); |
| | 4 | 153 | | return IsCoplanar(p) && Finite.ContainsTransformedPoint(p); |
| | | 154 | | } |
| | | 155 | | |
| | | 156 | | internal bool IntersectsSegment(FixedSegment segment) |
| | | 157 | | { |
| | 248 | 158 | | WideAxis3 start = Finite.Transform(segment.Start), end = Finite.Transform(segment.End); |
| | 124 | 159 | | return IsCoplanar(start) && IsCoplanar(end) && Finite.IntersectsTransformedSegment(start, end); |
| | | 160 | | } |
| | | 161 | | |
| | | 162 | | internal bool IsCoplanar(WideAxis3 point) => |
| | 261 | 163 | | WideArithmetic.SubtractSigned832(WideDirection3d.Dot(Normal, point), PlaneConstant).IsZero; |
| | | 164 | | |
| | | 165 | | internal void GetTriangleEdgeWall(FixedTriangle triangle, int edgeIndex, |
| | | 166 | | out Signed576 x, out Signed576 y, out Signed576 z, out Signed832 offset) |
| | | 167 | | { |
| | 12032 | 168 | | FixedSegment edge = triangle.GetEdge(edgeIndex); |
| | 12032 | 169 | | GetEdgeLine(edge, out x, out y, out z, out offset); |
| | 12032 | 170 | | WideAxis3 opposite = Transform(edgeIndex == 0 ? triangle.C : edgeIndex == 1 ? triangle.A : triangle.B); |
| | 12032 | 171 | | if (WideArithmetic.SubtractSigned832(DotWall(x, y, z, opposite), offset).Sign < 0) |
| | | 172 | | { |
| | 846 | 173 | | x = WideArithmetic.SubtractSigned576(default, x); |
| | 846 | 174 | | y = WideArithmetic.SubtractSigned576(default, y); |
| | 846 | 175 | | z = WideArithmetic.SubtractSigned576(default, z); |
| | 846 | 176 | | offset = WideArithmetic.SubtractSigned832(default, offset); |
| | | 177 | | } |
| | 12032 | 178 | | } |
| | | 179 | | |
| | | 180 | | // The sign of an exact line equation is immaterial for its roots. Segment |
| | | 181 | | // sources therefore need no invented opposite vertex or enclosing triangle. |
| | | 182 | | internal void GetEdgeLine(FixedSegment edge, |
| | | 183 | | out Signed576 x, out Signed576 y, out Signed576 z, out Signed832 offset) |
| | | 184 | | { |
| | 21646 | 185 | | if (Finite.ShapeFrame.HasExactScale) |
| | | 186 | | { |
| | 21 | 187 | | WideDirection3d direction = WideDirection3d.GetLocalEdge(SourceFrame, edge.Start, edge.End); |
| | 21 | 188 | | WideDirection3d scaledWall = WideDirection3d.Cross(new WideDirection3d(authoredNormal), direction) |
| | 21 | 189 | | .Transform(Finite.ShapeFrame.Basis); |
| | 63 | 190 | | x = scaledWall.X; y = WideArithmetic.SubtractSigned576(default, scaledWall.Y); z = scaledWall.Z; |
| | 21 | 191 | | offset = DotWall(x, y, z, Transform(edge.Start)); |
| | 21 | 192 | | return; |
| | | 193 | | } |
| | 21625 | 194 | | WideAxis3 delta = new( |
| | 21625 | 195 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(edge.End.X), Signed192.Raw(edge.Start.X |
| | 21625 | 196 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(edge.End.Y), Signed192.Raw(edge.Start.Y |
| | 21625 | 197 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(edge.End.Z), Signed192.Raw(edge.Start.Z |
| | | 198 | | // Form the wall in the authored frame first: primitive normal <130, |
| | | 199 | | // edge <65, wall <196, exact relative basis <130. A direct cross of |
| | | 200 | | // the transformed normal and transformed edge would need >320 bits. |
| | 21625 | 201 | | WideAxis3 wall = WideAxis3.Cross(authoredNormal, delta); |
| | 21625 | 202 | | WideRigidProjection.TransformLocalAxis(Finite.ShapeFrame.Basis, wall, out x, out y, out z); |
| | 21625 | 203 | | y = WideArithmetic.SubtractSigned576(default, y); |
| | 21625 | 204 | | offset = DotWall(x, y, z, Transform(edge.Start)); |
| | 21625 | 205 | | } |
| | | 206 | | |
| | | 207 | | private static Signed832 DotWall(Signed576 x, Signed576 y, Signed576 z, WideAxis3 point) |
| | | 208 | | { |
| | | 209 | | // Each transformed authored wall component <326 and point <198; |
| | | 210 | | // three signed products need <526 bits, safely inside Signed576. |
| | 33678 | 211 | | return WideArithmetic.AddSigned832( |
| | 33678 | 212 | | WideArithmetic.AddSigned832(WideArithmetic.MultiplySigned576ToSigned832(x, point.X), |
| | 33678 | 213 | | WideArithmetic.MultiplySigned576ToSigned832(y, point.Y)), |
| | 33678 | 214 | | WideArithmetic.MultiplySigned576ToSigned832(z, point.Z)); |
| | | 215 | | } |
| | | 216 | | } |