| | | 1 | | //======================================================================= |
| | | 2 | | // CapsulePlaneRayFrame.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 | | |
| | | 15 | | |
| | | 16 | | |
| | | 17 | | |
| | | 18 | | |
| | | 19 | | |
| | | 20 | | /// <summary>Canonical finite plane and exact capsule-local coordinates for admitted plane rays.</summary> |
| | | 21 | | internal readonly struct CapsulePlaneRayFrame |
| | | 22 | | { |
| | | 23 | | internal readonly CapsuleFiniteSectionFrame Finite; |
| | | 24 | | internal readonly WideDirection3d Normal; |
| | | 25 | | internal readonly WideAxis3 AuthoredNormal; |
| | | 26 | | internal readonly Signed832 PlaneConstant, NormalSquared; |
| | | 27 | | private readonly bool _wideParameters; |
| | 17223 | 28 | | internal bool UsesWideBanks => Finite.ShapeFrame.HasExactScale || _wideParameters; |
| | | 29 | | // Affine event points retain <7680-bit radicands and <12288-bit |
| | | 30 | | // coefficients. Opposite-ray value norms square the coefficients and |
| | | 31 | | // include the point radicand plus <=618 conditioning bits per factor; |
| | | 32 | | // 544 words cover that complete norm. Ordinary banks stay unchanged. |
| | 3450 | 33 | | internal int FieldWords => UsesWideBanks ? 192 : CapsulePlaneRaySelection.FieldWords; |
| | 6534 | 34 | | internal int RootWords => UsesWideBanks ? 120 : CapsulePlaneRaySelection.RootWords; |
| | 5014 | 35 | | internal int ValueWords => UsesWideBanks ? 544 : CapsulePlaneRayValue.PolynomialWords; |
| | 1694 | 36 | | internal int SelectionStorageWords => CapsulePlaneRaySelection.GetStorageWords(FieldWords, RootWords, ValueWords); |
| | 10 | 37 | | internal CapsulePlaneRayFrame WithWideParameters() => UsesWideBanks ? this : new CapsulePlaneRayFrame(this); |
| | 24 | 38 | | private CapsulePlaneRayFrame(in CapsulePlaneRayFrame source) { this = source; _wideParameters = true; } |
| | 508 | 39 | | internal ScaledRigidFrame3d SourceFrame => Finite.ShapeFrame.SourceFrame; |
| | | 40 | | |
| | | 41 | | /// <summary>Prepares a explicit sphere plane ray; its rational base point is supplied separately.</summary> |
| | | 42 | | /// <remarks>Only the rational-point accumulator is valid for this frame, not authored plane/edge reconstruction.</r |
| | | 43 | | internal static CapsulePlaneRayFrame ForSpherePointRay(Fixed64 radius, in WideDirection3d direction, |
| | 15 | 44 | | in Signed832 planeConstant, bool wideParameters = false) => new(radius, direction, planeConstant, wideParameters |
| | | 45 | | |
| | | 46 | | private CapsulePlaneRayFrame(Fixed64 radius, in WideDirection3d direction, in Signed832 planeConstant, bool widePara |
| | | 47 | | { |
| | 15 | 48 | | this = default; |
| | 15 | 49 | | Finite = new CapsuleFiniteSectionFrame(Vector3d.Zero, FixedQuaternion.Identity, |
| | 15 | 50 | | Vector3d.Zero, FixedQuaternion.Identity, Fixed64.Zero, radius); |
| | 30 | 51 | | Normal = direction; PlaneConstant = planeConstant; |
| | 15 | 52 | | AuthoredNormal = new WideAxis3(Signed320.NarrowValue(direction.X), |
| | 15 | 53 | | Signed320.NarrowValue(direction.Y), Signed320.NarrowValue(direction.Z)); |
| | 15 | 54 | | NormalSquared = direction.SquaredLength; |
| | 15 | 55 | | _wideParameters = wideParameters; |
| | 15 | 56 | | } |
| | | 57 | | |
| | | 58 | | /// <summary>Prepares a capsule-local ray chart for exact pretransformed native geometry.</summary> |
| | | 59 | | internal static CapsulePlaneRayFrame ForCapsulePointRay(Fixed64 length, Fixed64 radius, |
| | | 60 | | Signed320 coordinateDenominator, in WideDirection3d direction, in Signed832 planeConstant) => |
| | 45 | 61 | | new(length, radius, coordinateDenominator, direction, planeConstant); |
| | | 62 | | |
| | | 63 | | private CapsulePlaneRayFrame(Fixed64 length, Fixed64 radius, Signed320 coordinateDenominator, |
| | | 64 | | in WideDirection3d direction, in Signed832 planeConstant) |
| | | 65 | | { |
| | 45 | 66 | | this = default; |
| | 45 | 67 | | Finite = new CapsuleFiniteSectionFrame(length, radius, coordinateDenominator); |
| | 135 | 68 | | Normal = direction; PlaneConstant = planeConstant; NormalSquared = direction.SquaredLength; |
| | 45 | 69 | | AuthoredNormal = new WideAxis3(Signed320.NarrowValue(direction.X), |
| | 45 | 70 | | Signed320.NarrowValue(direction.Y), Signed320.NarrowValue(direction.Z)); |
| | 45 | 71 | | } |
| | | 72 | | |
| | | 73 | | internal CapsulePlaneRayFrame(FixedTriangle planeTriangle, in CapsuleFiniteSectionFrame finite) |
| | | 74 | | { |
| | 500 | 75 | | _wideParameters = false; |
| | 500 | 76 | | Finite = finite; |
| | 500 | 77 | | planeTriangle.GetExactNormal(out Signed192 nx, out Signed192 ny, out Signed192 nz, out _); |
| | 500 | 78 | | Span<Signed320> primitive = stackalloc Signed320[3] |
| | 500 | 79 | | { |
| | 500 | 80 | | Signed320.ExtendValue(nx), Signed320.ExtendValue(ny), Signed320.ExtendValue(nz) |
| | 500 | 81 | | }; |
| | 983 | 82 | | if (!nx.IsZero || !ny.IsZero || !nz.IsZero) WideArithmetic.ReduceCommonScale(primitive); |
| | 500 | 83 | | int sign = primitive[0].Sign != 0 ? primitive[0].Sign |
| | 500 | 84 | | : primitive[1].Sign != 0 ? primitive[1].Sign : primitive[2].Sign; |
| | 500 | 85 | | if (sign < 0) |
| | 3443 | 86 | | for (int index = 0; index < 3; index++) primitive[index] = WideArithmetic.Negate(primitive[index]); |
| | 500 | 87 | | AuthoredNormal = new WideAxis3(primitive[0], primitive[1], primitive[2]); |
| | 500 | 88 | | WideAxis3 transformed = WideRigidProjection.TransformLocalAxis(finite.ShapeFrame.Basis, |
| | 500 | 89 | | Signed192.NarrowProven(primitive[0]), Signed192.NarrowProven(primitive[1]), Signed192.NarrowProven(primitive |
| | 1500 | 90 | | primitive[0] = transformed.X; primitive[1] = transformed.Y; primitive[2] = transformed.Z; |
| | 983 | 91 | | if (!transformed.IsZero) WideArithmetic.ReduceCommonScale(primitive); |
| | 500 | 92 | | var compactNormal = new WideAxis3(primitive[0], primitive[1], primitive[2]); |
| | 500 | 93 | | Normal = new WideDirection3d(compactNormal); |
| | 500 | 94 | | PlaneConstant = Signed832.ExtendValue(WideAxis3.Dot(compactNormal, finite.Transform(planeTriangle.A))); |
| | 500 | 95 | | NormalSquared = Signed832.ExtendValue(WideAxis3.Dot(compactNormal, compactNormal)); |
| | 500 | 96 | | } |
| | | 97 | | |
| | | 98 | | internal CapsulePlaneRayFrame(FixedTriangle planeTriangle, in ScaledRigidFrame3d sourceFrame, |
| | | 99 | | in CapsuleFiniteSectionFrame finite) |
| | | 100 | | { |
| | 1068 | 101 | | if (sourceFrame.IsRigid) |
| | | 102 | | { |
| | 4 | 103 | | this = new CapsulePlaneRayFrame(planeTriangle, finite); |
| | 4 | 104 | | return; |
| | | 105 | | } |
| | 1064 | 106 | | _wideParameters = false; |
| | 1064 | 107 | | Finite = finite; |
| | 1064 | 108 | | WideDirection3d first = WideDirection3d.GetLocalEdge(sourceFrame, planeTriangle.A, planeTriangle.B); |
| | 1064 | 109 | | WideDirection3d second = WideDirection3d.GetLocalEdge(sourceFrame, planeTriangle.A, planeTriangle.C); |
| | 1064 | 110 | | WideDirection3d cross = WideDirection3d.Cross(first, second); |
| | 1064 | 111 | | Span<Signed320> primitive = stackalloc Signed320[3] |
| | 1064 | 112 | | { |
| | 1064 | 113 | | Signed320.NarrowValue(cross.X), Signed320.NarrowValue(cross.Y), Signed320.NarrowValue(cross.Z) |
| | 1064 | 114 | | }; |
| | 2126 | 115 | | if (!cross.IsZero) WideArithmetic.ReduceCommonScale(primitive); |
| | 1064 | 116 | | int sign = primitive[0].Sign != 0 ? primitive[0].Sign |
| | 1064 | 117 | | : primitive[1].Sign != 0 ? primitive[1].Sign : primitive[2].Sign; |
| | 1064 | 118 | | if (sign < 0) |
| | 6424 | 119 | | for (int index = 0; index < 3; index++) primitive[index] = WideArithmetic.Negate(primitive[index]); |
| | 1064 | 120 | | AuthoredNormal = new WideAxis3(primitive[0], primitive[1], primitive[2]); |
| | 1064 | 121 | | WideDirection3d transformed = new WideDirection3d(AuthoredNormal).Transform(finite.ShapeFrame.Basis); |
| | 1064 | 122 | | Span<Signed576> reduced = stackalloc Signed576[3] { transformed.X, transformed.Y, transformed.Z }; |
| | 2126 | 123 | | if (!transformed.IsZero) WideArithmetic.ReduceCommonScale(reduced); |
| | 1064 | 124 | | Normal = new WideDirection3d(reduced[0], reduced[1], reduced[2]); |
| | 1064 | 125 | | PlaneConstant = WideDirection3d.Dot(Normal, finite.Transform(planeTriangle.A)); |
| | 1064 | 126 | | NormalSquared = Normal.SquaredLength; |
| | 1064 | 127 | | } |
| | | 128 | | |
| | | 129 | | internal Vector3d GetWorldNormal(FixedQuaternion authoredRotation) |
| | | 130 | | { |
| | 191 | 131 | | if (Finite.ShapeFrame.HasExactScale) |
| | 4 | 132 | | return new WideDirection3d(AuthoredNormal).Transform(SourceFrame.GeometryBasis).GetNormal(false); |
| | 187 | 133 | | WideAxis3 normal = WideRigidProjection.TransformLocalAxis(new WideRationalBasis3d(authoredRotation), |
| | 187 | 134 | | Signed192.NarrowProven(AuthoredNormal.X), Signed192.NarrowProven(AuthoredNormal.Y), |
| | 187 | 135 | | Signed192.NarrowProven(AuthoredNormal.Z)); |
| | 187 | 136 | | return WideNormalization.GetNormalized(normal.X, normal.Y, normal.Z); |
| | | 137 | | } |
| | | 138 | | |
| | | 139 | | /// <summary>Compares exact points in the authored frame; the common translation cancels.</summary> |
| | | 140 | | internal int ComparePoints(scoped ContactQuadratic x, scoped ContactQuadratic y, scoped ContactQuadratic z, |
| | | 141 | | scoped ContactQuadratic d, scoped ReadOnlySpan<ulong> root, scoped ContactQuadratic otherX, |
| | | 142 | | scoped ContactQuadratic otherY, scoped ContactQuadratic otherZ, scoped ContactQuadratic otherD, |
| | | 143 | | scoped ReadOnlySpan<ulong> otherRoot) |
| | | 144 | | { |
| | | 145 | | // Basis covectors add at most 130 bits and two carries; retained bank |
| | | 146 | | // capacity does not determine the width of the actual point products. |
| | 611 | 147 | | int words = Math.Max(9, Math.Max(Math.Max(ContactQuadratic.ActiveWords(x), |
| | 611 | 148 | | Math.Max(ContactQuadratic.ActiveWords(y), ContactQuadratic.ActiveWords(z))), |
| | 611 | 149 | | Math.Max(ContactQuadratic.ActiveWords(otherX), Math.Max(ContactQuadratic.ActiveWords(otherY), |
| | 611 | 150 | | ContactQuadratic.ActiveWords(otherZ)))) + 3); |
| | 1222 | 151 | | Span<ulong> values = stackalloc ulong[6 * words]; Span<int> signs = stackalloc int[6]; |
| | 611 | 152 | | ContactQuadratic first = ContactQuadratic.At(values, signs, 0, words); |
| | 611 | 153 | | ContactQuadratic second = ContactQuadratic.At(values, signs, 1, words); |
| | 611 | 154 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 2, words); |
| | 611 | 155 | | WideRationalBasis3d basis = Finite.ShapeFrame.Basis; |
| | 4582 | 156 | | for (int axis = 0; axis < 3; axis++) |
| | | 157 | | { |
| | 1748 | 158 | | Signed192 bx = axis == 0 ? basis.Xx : axis == 1 ? basis.Yx : basis.Zx; |
| | 1748 | 159 | | Signed192 by = axis == 0 ? basis.Xy : axis == 1 ? basis.Yy : basis.Zy; |
| | 1748 | 160 | | Signed192 bz = axis == 0 ? basis.Xz : axis == 1 ? basis.Yz : basis.Zz; |
| | 1748 | 161 | | DotPoint(Signed576.ExtendValue(Signed320.ExtendValue(bx)), |
| | 1748 | 162 | | Signed576.ExtendValue(Signed320.ExtendValue(by)), Signed576.ExtendValue(Signed320.ExtendValue(bz)), |
| | 1748 | 163 | | x, y, z, first, term); |
| | 1748 | 164 | | DotPoint(Signed576.ExtendValue(Signed320.ExtendValue(bx)), |
| | 1748 | 165 | | Signed576.ExtendValue(Signed320.ExtendValue(by)), Signed576.ExtendValue(Signed320.ExtendValue(bz)), |
| | 1748 | 166 | | otherX, otherY, otherZ, second, term); |
| | 1748 | 167 | | int comparison = ContactQuadratic.CompareRatios(first, d, root, second, otherD, otherRoot); |
| | 1816 | 168 | | if (comparison != 0) return comparison; |
| | | 169 | | } |
| | 543 | 170 | | return 0; |
| | | 171 | | } |
| | | 172 | | |
| | | 173 | | /// <summary>Tests an exact homogeneous capsule-local point against the closed finite triangle.</summary> |
| | | 174 | | internal bool ContainsTrianglePoint(FixedTriangle triangle, ContactQuadratic x, ContactQuadratic y, |
| | | 175 | | ContactQuadratic z, ContactQuadratic denominator, ReadOnlySpan<ulong> root) |
| | | 176 | | { |
| | 695 | 177 | | if (NormalSquared.IsZero || denominator.Sign(root) <= 0) return false; |
| | | 178 | | // Plane/wall products add at most 526 bits to a point coefficient. |
| | | 179 | | // Input banks may be larger than this owner's rational witness bank. |
| | 689 | 180 | | int words = Math.Max(Math.Max(ContactQuadratic.ActiveWords(x), ContactQuadratic.ActiveWords(y)), |
| | 689 | 181 | | Math.Max(ContactQuadratic.ActiveWords(z), ContactQuadratic.ActiveWords(denominator))) |
| | 689 | 182 | | + (Finite.ShapeFrame.HasExactScale ? 13 : 9); |
| | 689 | 183 | | Span<ulong> values = stackalloc ulong[4 * words]; |
| | 689 | 184 | | Span<int> signs = stackalloc int[4]; |
| | 689 | 185 | | ContactQuadratic value = ContactQuadratic.At(values, signs, 0, words); |
| | 689 | 186 | | ContactQuadratic term = ContactQuadratic.At(values, signs, 1, words); |
| | 689 | 187 | | DotPoint(Normal.X, Normal.Y, Normal.Z, |
| | 689 | 188 | | x, y, z, value, term); |
| | 1378 | 189 | | ContactQuadratic.Scale(denominator, PlaneConstant, term); value.Add(term, -1); |
| | 695 | 190 | | if (value.Sign(root) != 0) return false; |
| | 4290 | 191 | | for (int edge = 0; edge < 3; edge++) |
| | | 192 | | { |
| | 1753 | 193 | | GetTriangleEdgeWall(triangle, edge, out Signed832 wx, out Signed832 wy, out Signed832 wz, out Signed832 offs |
| | 1753 | 194 | | DotPoint(wx, wy, wz, x, y, z, value, term); |
| | 3506 | 195 | | ContactQuadratic.Scale(denominator, offset, term); value.Add(term, -1); |
| | 2044 | 196 | | if (value.Sign(root) < 0) return false; |
| | | 197 | | } |
| | 392 | 198 | | return true; |
| | | 199 | | } |
| | | 200 | | |
| | | 201 | | internal void GetTriangleEdgeWall(FixedTriangle triangle, int index, |
| | | 202 | | out Signed832 x, out Signed832 y, out Signed832 z, out Signed832 offset) |
| | | 203 | | { |
| | 1822 | 204 | | FixedSegment edge = triangle.GetEdge(index); |
| | 1822 | 205 | | if (Finite.ShapeFrame.HasExactScale) |
| | | 206 | | { |
| | 504 | 207 | | WideDirection3d deltaExact = WideDirection3d.GetLocalEdge(SourceFrame, edge.Start, edge.End); |
| | 504 | 208 | | WideDirection3d wallExact = WideDirection3d.Cross(new WideDirection3d(AuthoredNormal), deltaExact) |
| | 504 | 209 | | .Transform(Finite.ShapeFrame.Basis); |
| | 1008 | 210 | | x = Signed832.ExtendValue(wallExact.X); y = Signed832.ExtendValue(wallExact.Y); |
| | 504 | 211 | | z = Signed832.ExtendValue(wallExact.Z); |
| | 504 | 212 | | offset = WideDirection3d.Dot(wallExact, Finite.Transform(edge.Start)); |
| | 504 | 213 | | WideAxis3 oppositeExact = Finite.Transform(index == 0 ? triangle.C : index == 1 ? triangle.A : triangle.B); |
| | 504 | 214 | | if (WideArithmetic.SubtractSigned832(WideDirection3d.Dot(wallExact, oppositeExact), offset).Sign < 0) |
| | | 215 | | { |
| | 422 | 216 | | x = WideArithmetic.SubtractSigned832(default, x); y = WideArithmetic.SubtractSigned832(default, y); |
| | 422 | 217 | | z = WideArithmetic.SubtractSigned832(default, z); offset = WideArithmetic.SubtractSigned832(default, off |
| | | 218 | | } |
| | 504 | 219 | | return; |
| | | 220 | | } |
| | 1318 | 221 | | WideAxis3 delta = new( |
| | 1318 | 222 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(edge.End.X), Signed192.Raw(edge.Start.X |
| | 1318 | 223 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(edge.End.Y), Signed192.Raw(edge.Start.Y |
| | 1318 | 224 | | Signed320.ExtendValue(WideArithmetic.SubtractSigned192(Signed192.Raw(edge.End.Z), Signed192.Raw(edge.Start.Z |
| | | 225 | | // Form the authored wall before transforming it: 130-bit normal, |
| | | 226 | | // 65-bit edge, 130-bit relative basis keep the wall below 327 bits. |
| | 1318 | 227 | | WideAxis3 wall = WideAxis3.Cross(AuthoredNormal, delta); |
| | 1318 | 228 | | WideRigidProjection.TransformLocalAxis(Finite.ShapeFrame.Basis, wall, out Signed576 wx, out Signed576 wy, out Si |
| | 3954 | 229 | | x = Signed832.ExtendValue(wx); y = Signed832.ExtendValue(wy); z = Signed832.ExtendValue(wz); |
| | 1318 | 230 | | offset = Signed832.ExtendValue(DotWall(wx, wy, wz, Finite.Transform(edge.Start))); |
| | 1318 | 231 | | WideAxis3 opposite = Finite.Transform(index == 0 ? triangle.C : index == 1 ? triangle.A : triangle.B); |
| | 1318 | 232 | | if (WideArithmetic.SubtractSigned832(Signed832.ExtendValue(DotWall(wx, wy, wz, opposite)), offset).Sign < 0) |
| | | 233 | | { |
| | 1618 | 234 | | x = WideArithmetic.SubtractSigned832(default, x); y = WideArithmetic.SubtractSigned832(default, y); |
| | 1618 | 235 | | z = WideArithmetic.SubtractSigned832(default, z); offset = WideArithmetic.SubtractSigned832(default, offset) |
| | | 236 | | } |
| | 1318 | 237 | | } |
| | | 238 | | |
| | | 239 | | private static Signed576 DotWall(Signed576 x, Signed576 y, Signed576 z, WideAxis3 point) => |
| | 2636 | 240 | | Signed576.NarrowValue(WideArithmetic.AddSigned704( |
| | 2636 | 241 | | WideArithmetic.AddSigned704(WideArithmetic.MultiplySigned576ToSigned704(x, point.X), |
| | 2636 | 242 | | WideArithmetic.MultiplySigned576ToSigned704(y, point.Y)), |
| | 2636 | 243 | | WideArithmetic.MultiplySigned576ToSigned704(z, point.Z))); |
| | | 244 | | |
| | | 245 | | private static void DotPoint(Signed576 wx, Signed576 wy, Signed576 wz, ContactQuadratic x, |
| | | 246 | | ContactQuadratic y, ContactQuadratic z, ContactQuadratic result, ContactQuadratic term) |
| | | 247 | | { |
| | 4185 | 248 | | ContactQuadratic.Scale(x, wx, result); |
| | 8370 | 249 | | ContactQuadratic.Scale(y, wy, term); result.Add(term); |
| | 8370 | 250 | | ContactQuadratic.Scale(z, wz, term); result.Add(term); |
| | 4185 | 251 | | } |
| | | 252 | | |
| | | 253 | | private static void DotPoint(Signed832 wx, Signed832 wy, Signed832 wz, ContactQuadratic x, |
| | | 254 | | ContactQuadratic y, ContactQuadratic z, ContactQuadratic result, ContactQuadratic term) |
| | | 255 | | { |
| | 1753 | 256 | | ContactQuadratic.Scale(x, wx, result); |
| | 3506 | 257 | | ContactQuadratic.Scale(y, wy, term); result.Add(term); |
| | 3506 | 258 | | ContactQuadratic.Scale(z, wz, term); result.Add(term); |
| | 1753 | 259 | | } |
| | | 260 | | } |