| | | 1 | | //======================================================================= |
| | | 2 | | // WideTriangleRelations.ScaledFrame.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 | | /// <content>Finite triangle predicates and final witnesses over exact authored scaled frames.</content> |
| | | 13 | | internal static partial class WideTriangleRelations |
| | | 14 | | { |
| | | 15 | | internal static bool TryGetClosestNonparallelSegmentParameters(in RationalPoint3d firstStart, |
| | | 16 | | in RationalPoint3d firstEnd, in RationalPoint3d secondStart, in RationalPoint3d secondEnd, |
| | | 17 | | ContactQuadratic firstN, ContactQuadratic firstD, ContactQuadratic secondN, ContactQuadratic secondD) |
| | | 18 | | { |
| | 309 | 19 | | Signed192 firstRotationD = WideArithmetic.TruncateSigned320By64(firstStart.Denominator), |
| | 309 | 20 | | secondRotationD = WideArithmetic.TruncateSigned320By64(secondStart.Denominator); |
| | 618 | 21 | | WideDirection3d a = Common(firstStart, secondRotationD), b = Common(firstEnd, secondRotationD), |
| | 618 | 22 | | c = Common(secondStart, firstRotationD), d = Common(secondEnd, firstRotationD); |
| | 927 | 23 | | WideDirection3d e = Difference(b, a), f = Difference(d, c), r = Difference(a, c); |
| | | 24 | | // All points share Q^2*R1*R2. The line Gram minors need at most |
| | | 25 | | // twice a full 832-bit coefficient; keep them borrowed until selection. |
| | | 26 | | const int words = 27; |
| | 618 | 27 | | Span<ulong> values = stackalloc ulong[14 * words]; Span<int> signs = stackalloc int[14]; |
| | 309 | 28 | | var aa = ContactQuadratic.At(values, signs, 0, words); |
| | 309 | 29 | | var ab = ContactQuadratic.At(values, signs, 1, words); |
| | 309 | 30 | | var bb = ContactQuadratic.At(values, signs, 2, words); |
| | 309 | 31 | | var ar = ContactQuadratic.At(values, signs, 3, words); |
| | 309 | 32 | | var br = ContactQuadratic.At(values, signs, 4, words); |
| | 309 | 33 | | var one = ContactQuadratic.At(values, signs, 5, words); |
| | 309 | 34 | | var two = ContactQuadratic.At(values, signs, 6, words); |
| | 927 | 35 | | aa.Set(WideDirection3d.Dot(e, e)); ab.Set(WideDirection3d.Dot(e, f)); bb.Set(WideDirection3d.Dot(f, f)); |
| | 618 | 36 | | ar.Set(WideDirection3d.Dot(e, r)); br.Set(WideDirection3d.Dot(f, r)); |
| | 309 | 37 | | ContactQuadratic.Multiply(aa, bb, ReadOnlySpan<ulong>.Empty, one); |
| | 618 | 38 | | ContactQuadratic.Multiply(ab, ab, ReadOnlySpan<ulong>.Empty, two); one.Add(two, -1); |
| | 491 | 39 | | if (one.Signs[0] <= 0) return false; |
| | 254 | 40 | | one.CopyTo(firstD); one.CopyTo(secondD); |
| | 127 | 41 | | ContactQuadratic.Multiply(ab, br, ReadOnlySpan<ulong>.Empty, one); |
| | 254 | 42 | | ContactQuadratic.Multiply(bb, ar, ReadOnlySpan<ulong>.Empty, two); one.Add(two, -1); |
| | 158 | 43 | | if (one.Signs[0] < 0) return false; |
| | 288 | 44 | | one.CopyTo(firstN); firstD.CopyTo(two); two.Add(firstN, -1); |
| | 127 | 45 | | if (two.Signs[0] < 0) return false; |
| | 65 | 46 | | ContactQuadratic.Multiply(aa, br, ReadOnlySpan<ulong>.Empty, one); |
| | 130 | 47 | | ContactQuadratic.Multiply(ab, ar, ReadOnlySpan<ulong>.Empty, two); one.Add(two, -1); |
| | 82 | 48 | | if (one.Signs[0] < 0) return false; |
| | 144 | 49 | | one.CopyTo(secondN); secondD.CopyTo(two); two.Add(secondN, -1); |
| | 48 | 50 | | return two.Signs[0] >= 0; |
| | | 51 | | |
| | | 52 | | static WideDirection3d Common(in RationalPoint3d point, Signed192 otherD) => new( |
| | | 53 | | WideArithmetic.MultiplySigned320(point.X, otherD), WideArithmetic.MultiplySigned320(point.Y, otherD), |
| | | 54 | | WideArithmetic.MultiplySigned320(point.Z, otherD)); |
| | | 55 | | static WideDirection3d Difference(in WideDirection3d end, in WideDirection3d start) => new( |
| | | 56 | | WideArithmetic.SubtractSigned576(end.X, start.X), WideArithmetic.SubtractSigned576(end.Y, start.Y), |
| | | 57 | | WideArithmetic.SubtractSigned576(end.Z, start.Z)); |
| | | 58 | | } |
| | | 59 | | |
| | | 60 | | internal static bool TryGetSegmentPointAnchor(in ScaledRigidFrame3d frame, in FixedSegment segment, |
| | | 61 | | scoped ContactQuadratic n, scoped ContactQuadratic d, Vector3d origin, out FixedPointAnchor anchor) => |
| | 29 | 62 | | TryGetSegmentPointAnchor<ScaledRigidFrame3d>(frame, segment, n, d, origin, out anchor); |
| | | 63 | | |
| | | 64 | | internal static bool TryGetSegmentPointAnchor<TFrame>(in TFrame frame, in FixedSegment segment, |
| | | 65 | | scoped ContactQuadratic n, scoped ContactQuadratic d, Vector3d origin, out FixedPointAnchor anchor) |
| | | 66 | | where TFrame : struct, IForwardHullFrame3d => |
| | 34 | 67 | | TryGetSegmentPointAnchor(frame.GetPoint(segment.Start), frame.GetPoint(segment.End), n, d, origin, out anchor); |
| | | 68 | | |
| | | 69 | | private static bool TryGetSegmentPointAnchor(in RationalPoint3d a, in RationalPoint3d b, |
| | | 70 | | scoped ContactQuadratic n, scoped ContactQuadratic d, Vector3d origin, out FixedPointAnchor anchor) |
| | | 71 | | { |
| | 57 | 72 | | anchor = default; |
| | | 73 | | System.Diagnostics.Debug.Assert(a.Denominator.Equals(b.Denominator)); |
| | 57 | 74 | | a.GetLocalPointNumerators(origin, FixedQuaternion.Identity, out Signed576 ax, out Signed576 ay, out Signed576 az |
| | 57 | 75 | | b.GetLocalPointNumerators(origin, FixedQuaternion.Identity, out Signed576 bx, out Signed576 by, out Signed576 bz |
| | 57 | 76 | | int words = Math.Max(ContactQuadratic.ActiveWords(n), ContactQuadratic.ActiveWords(d)) + 7; |
| | 114 | 77 | | Span<ulong> values = stackalloc ulong[8 * words]; Span<int> signs = stackalloc int[8]; |
| | 57 | 78 | | var remaining = ContactQuadratic.At(values, signs, 0, words); |
| | 57 | 79 | | var numerator = ContactQuadratic.At(values, signs, 1, words); |
| | 57 | 80 | | var term = ContactQuadratic.At(values, signs, 2, words); |
| | 57 | 81 | | var denominator = ContactQuadratic.At(values, signs, 3, words); |
| | 114 | 82 | | d.CopyTo(remaining); remaining.Add(n, -1); |
| | 57 | 83 | | ContactQuadratic.Scale(d, Signed576.ExtendValue(a.Denominator), denominator); |
| | 114 | 84 | | Span<Signed576> first = stackalloc Signed576[3] { ax, ay, az }, second = stackalloc Signed576[3] { bx, by, bz }; |
| | 57 | 85 | | Vector3d local = default; |
| | 414 | 86 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 87 | | { |
| | 157 | 88 | | EvaluateScaledEdgePoint(first[coordinate], second[coordinate], remaining, n, numerator, term); |
| | 157 | 89 | | if (!Fixed64.TryGetSignedRawRatio(numerator.Rational, denominator.Rational, |
| | 164 | 90 | | numerator.Signs[0] < 0, out Fixed64 scalar)) return false; |
| | 150 | 91 | | local[coordinate] = scalar; |
| | | 92 | | } |
| | 50 | 93 | | anchor = new FixedPointAnchor(origin, FixedQuaternion.Identity, local); |
| | 50 | 94 | | return true; |
| | | 95 | | } |
| | | 96 | | |
| | | 97 | | internal static int LocateEdgePoint(in FixedSegment source, in ScaledRigidFrame3d sourceFrame, |
| | | 98 | | Signed832 n, Signed832 d, in FixedSegment target, in ScaledRigidFrame3d targetFrame, |
| | | 99 | | in RationalSegmentInterval targetInterval = default) |
| | | 100 | | { |
| | 120 | 101 | | Span<ulong> values = stackalloc ulong[52]; Span<int> signs = stackalloc int[4]; |
| | 60 | 102 | | var numerator = ContactQuadratic.At(values, signs, 0, 13); |
| | 60 | 103 | | var denominator = ContactQuadratic.At(values, signs, 1, 13); |
| | 120 | 104 | | numerator.Set(n); denominator.Set(d); |
| | 60 | 105 | | return LocateEdgePoint(source, sourceFrame, numerator, denominator, target, targetFrame, targetInterval); |
| | | 106 | | } |
| | | 107 | | |
| | | 108 | | /// <summary>Locates a retained rational edge parameter without narrowing its transient numerator or denominator.</s |
| | | 109 | | internal static int LocateEdgePoint(in FixedSegment source, in ScaledRigidFrame3d sourceFrame, |
| | | 110 | | scoped ContactQuadratic n, scoped ContactQuadratic d, in FixedSegment target, in ScaledRigidFrame3d targetFrame, |
| | | 111 | | in RationalSegmentInterval targetInterval = default) => |
| | 60 | 112 | | LocateFrameEdgePoint(source, sourceFrame, n, d, target, targetFrame, targetInterval); |
| | | 113 | | |
| | | 114 | | /// <summary>Locates an exact retained point across forward-only scalar and authored charts.</summary> |
| | | 115 | | internal static int LocateFrameEdgePoint<TSource, TTarget>(in FixedSegment source, in TSource sourceFrame, |
| | | 116 | | scoped ContactQuadratic n, scoped ContactQuadratic d, in FixedSegment target, in TTarget targetFrame, |
| | | 117 | | in RationalSegmentInterval targetInterval = default) |
| | | 118 | | where TSource : struct, IForwardHullFrame3d |
| | | 119 | | where TTarget : struct, IForwardHullFrame3d |
| | | 120 | | { |
| | 470 | 121 | | RationalPoint3d a = sourceFrame.GetPoint(source.Start), b = sourceFrame.GetPoint(source.End); |
| | 235 | 122 | | return LocateFrameEdgePoint(a, b, n, d, target, targetFrame, targetInterval); |
| | | 123 | | } |
| | | 124 | | |
| | | 125 | | /// <summary>Locates an exact anchored point without rounding its retained feature term.</summary> |
| | | 126 | | internal static int LocatePointOnFrameEdge<TFrame>(in RationalPoint3d point, |
| | | 127 | | in FixedSegment target, in TFrame frame, in RationalSegmentInterval interval = default) |
| | | 128 | | where TFrame : struct, IForwardHullFrame3d |
| | | 129 | | { |
| | 32 | 130 | | Span<ulong> values = stackalloc ulong[52]; Span<int> signs = stackalloc int[4]; |
| | 32 | 131 | | var n = ContactQuadratic.At(values, signs, 0, 13); var d = ContactQuadratic.At(values, signs, 1, 13); |
| | 32 | 132 | | n.Clear(); d.Set(Signed832.ExtendValue(Signed192.Signed(1))); |
| | 16 | 133 | | return LocateFrameEdgePoint(point, point, n, d, target, frame, interval); |
| | | 134 | | } |
| | | 135 | | |
| | | 136 | | private static int LocateFrameEdgePoint<TTarget>(in RationalPoint3d a, in RationalPoint3d b, |
| | | 137 | | scoped ContactQuadratic n, scoped ContactQuadratic d, in FixedSegment target, in TTarget targetFrame, |
| | | 138 | | in RationalSegmentInterval targetInterval) where TTarget : struct, IForwardHullFrame3d |
| | | 139 | | { |
| | 502 | 140 | | RationalPoint3d targetA = targetFrame.GetPoint(target.Start), targetB = targetFrame.GetPoint(target.End); |
| | 251 | 141 | | WideDirection3d first = GetScaledPointDifference(a, targetA, targetFrame.RotationDenominator), |
| | 251 | 142 | | second = GetScaledPointDifference(b, targetA, targetFrame.RotationDenominator); |
| | 251 | 143 | | WideDirection3d edge = targetFrame.GetEdgeDirection(target.Start, target.End); |
| | 251 | 144 | | int words = Math.Max(ContactQuadratic.ActiveWords(n), ContactQuadratic.ActiveWords(d)) + 11; |
| | 502 | 145 | | Span<ulong> values = stackalloc ulong[12 * words]; Span<int> signs = stackalloc int[12]; |
| | 251 | 146 | | var remaining = ContactQuadratic.At(values, signs, 0, words); |
| | 251 | 147 | | var value = ContactQuadratic.At(values, signs, 1, words); |
| | 251 | 148 | | var term = ContactQuadratic.At(values, signs, 2, words); |
| | 251 | 149 | | var start = ContactQuadratic.At(values, signs, 3, words); |
| | 251 | 150 | | var end = ContactQuadratic.At(values, signs, 4, words); |
| | 251 | 151 | | var edgeD = ContactQuadratic.At(values, signs, 5, words); |
| | 502 | 152 | | d.CopyTo(remaining); remaining.Add(n, -1); |
| | 502 | 153 | | WideDirection3d firstCross = WideDirection3d.Cross(edge, first), secondCross = WideDirection3d.Cross(edge, secon |
| | 251 | 154 | | Span<Signed576> crossA = stackalloc Signed576[3] { firstCross.X, firstCross.Y, firstCross.Z }; |
| | 251 | 155 | | Span<Signed576> crossB = stackalloc Signed576[3] { secondCross.X, secondCross.Y, secondCross.Z }; |
| | 1984 | 156 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 157 | | { |
| | 749 | 158 | | EvaluateScaledEdgePoint(crossA[coordinate], crossB[coordinate], remaining, n, value, term); |
| | 757 | 159 | | if (value.Signs[0] != 0) return -1; |
| | | 160 | | } |
| | 243 | 161 | | if (edge.IsZero) |
| | | 162 | | { |
| | 12 | 163 | | Span<Signed576> firstCoordinates = stackalloc Signed576[3] { first.X, first.Y, first.Z }; |
| | 12 | 164 | | Span<Signed576> secondCoordinates = stackalloc Signed576[3] { second.X, second.Y, second.Z }; |
| | 72 | 165 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 166 | | { |
| | 30 | 167 | | EvaluateScaledEdgePoint(firstCoordinates[coordinate], secondCoordinates[coordinate], remaining, n, value |
| | 36 | 168 | | if (value.Signs[0] != 0) return -1; |
| | | 169 | | } |
| | 6 | 170 | | return 0; |
| | | 171 | | } |
| | 231 | 172 | | int axis = !edge.X.IsZero ? 0 : !edge.Y.IsZero ? 1 : 2; |
| | 231 | 173 | | Signed576 firstValue = axis == 0 ? first.X : axis == 1 ? first.Y : first.Z; |
| | 231 | 174 | | Signed576 secondValue = axis == 0 ? second.X : axis == 1 ? second.Y : second.Z; |
| | 231 | 175 | | EvaluateScaledEdgePoint(firstValue, secondValue, remaining, n, start, term); |
| | 231 | 176 | | first = GetScaledPointDifference(a, targetB, targetFrame.RotationDenominator); |
| | 231 | 177 | | second = GetScaledPointDifference(b, targetB, targetFrame.RotationDenominator); |
| | 231 | 178 | | firstValue = axis == 0 ? first.X : axis == 1 ? first.Y : first.Z; |
| | 231 | 179 | | secondValue = axis == 0 ? second.X : axis == 1 ? second.Y : second.Z; |
| | 231 | 180 | | EvaluateScaledEdgePoint(firstValue, secondValue, remaining, n, end, term); |
| | 231 | 181 | | int direction = (axis == 0 ? edge.X : axis == 1 ? edge.Y : edge.Z).Sign; |
| | 462 | 182 | | int startSign = start.Signs[0], endSign = end.Signs[0]; |
| | 298 | 183 | | if (startSign != 0 && endSign != 0 && (startSign != direction || endSign != -direction)) return -1; |
| | 164 | 184 | | if (targetInterval.IsClipped) |
| | | 185 | | { |
| | 130 | 186 | | start.CopyTo(edgeD); edgeD.Add(end, -1); |
| | 117 | 187 | | if (direction < 0) { start.MultiplySign(-1); edgeD.MultiplySign(-1); } |
| | 98 | 188 | | if (!targetInterval.Contains(start, edgeD, ReadOnlySpan<ulong>.Empty)) return -1; |
| | | 189 | | } |
| | 131 | 190 | | return startSign == 0 ? 0 : endSign == 0 ? 2 : 1; |
| | | 191 | | } |
| | | 192 | | |
| | | 193 | | private static void EvaluateScaledEdgePoint(Signed576 first, Signed576 second, |
| | | 194 | | ContactQuadratic remaining, ContactQuadratic n, ContactQuadratic value, ContactQuadratic term) |
| | | 195 | | { |
| | 4194 | 196 | | ContactQuadratic.Scale(remaining, first, value); ContactQuadratic.Scale(n, second, term); value.Add(term); |
| | 1398 | 197 | | } |
| | | 198 | | |
| | | 199 | | /// <summary> |
| | | 200 | | /// Clips an authored edge against an independently scaled finite triangle. |
| | | 201 | | /// The side bits and closed unit interval match the rigid coverage contract. |
| | | 202 | | /// </summary> |
| | | 203 | | internal static bool TryGetEdgeCoverage(in FixedTriangle source, in ScaledRigidFrame3d sourceFrame, |
| | | 204 | | in FixedTriangle target, in ScaledRigidFrame3d targetFrame, out Signed832 lowerN, |
| | | 205 | | out Signed832 lowerD, out Signed832 upperN, out Signed832 upperD, out int sides) |
| | | 206 | | { |
| | 39 | 207 | | lowerN = default; lowerD = upperN = upperD = Signed832.ExtendValue(Signed192.One); sides = 0; |
| | 13 | 208 | | if (sourceFrame.HasSameMapping(targetFrame)) |
| | | 209 | | { |
| | | 210 | | // An invertible affine map preserves incidence, filled sides and |
| | | 211 | | // the original edge parameter. Remove it before clipping at all. |
| | 4 | 212 | | if (!TryGetEdgeCoverage(source, Vector3d.Zero, FixedQuaternion.Identity, target, |
| | 6 | 213 | | Vector3d.Zero, FixedQuaternion.Identity, out RationalSegmentInterval interval, out sides)) return fa |
| | 2 | 214 | | if (interval.IsClipped) |
| | | 215 | | { |
| | 1 | 216 | | lowerN = Signed832.ExtendValue(Signed576.ExtendValue(interval.LowerNumerator)); |
| | 1 | 217 | | lowerD = Signed832.ExtendValue(Signed576.ExtendValue(interval.LowerDenominator)); |
| | 1 | 218 | | upperN = Signed832.ExtendValue(Signed576.ExtendValue(interval.UpperNumerator)); |
| | 1 | 219 | | upperD = Signed832.ExtendValue(Signed576.ExtendValue(interval.UpperDenominator)); |
| | | 220 | | } |
| | 2 | 221 | | return true; |
| | | 222 | | } |
| | 9 | 223 | | WideDirection3d sourceLocalNormal = GetScaledTriangleLocalNormal(source, sourceFrame); |
| | 9 | 224 | | WideDirection3d targetLocalNormal = GetScaledTriangleLocalNormal(target, targetFrame); |
| | 12 | 225 | | if (sourceLocalNormal.IsZero || targetLocalNormal.IsZero) return false; |
| | 6 | 226 | | WideDirection3d targetNormal = targetLocalNormal.Transform(targetFrame.GeometryBasis); |
| | 12 | 227 | | RationalPoint3d a = sourceFrame.GetPoint(source.A), b = sourceFrame.GetPoint(source.B), |
| | 12 | 228 | | c = sourceFrame.GetPoint(source.C), targetOrigin = targetFrame.GetPoint(target.A); |
| | 6 | 229 | | if (!DotScaledDirections(targetNormal, GetScaledPointDifference(a, targetOrigin, targetFrame.RotationDenominator |
| | 6 | 230 | | || !DotScaledDirections(targetNormal, GetScaledPointDifference(b, targetOrigin, targetFrame.RotationDenomina |
| | 2 | 231 | | return false; |
| | 4 | 232 | | bool folded = !DotScaledDirections(targetNormal, |
| | 4 | 233 | | GetScaledPointDifference(c, targetOrigin, targetFrame.RotationDenominator)).IsZero; |
| | 4 | 234 | | if (!TryGetProjectedSegmentInterval(target, targetFrame, a, b, out lowerN, out lowerD, out upperN, out upperD)) |
| | 1 | 235 | | return false; |
| | 5 | 236 | | if (folded) { sides = 4; return true; } |
| | 2 | 237 | | WideDirection3d inward = WideDirection3d.Cross(sourceLocalNormal, |
| | 2 | 238 | | WideDirection3d.GetLocalEdge(sourceFrame, source.A, source.B)).Transform(sourceFrame.GeometryBasis); |
| | 2 | 239 | | Span<Vector3d> vertices = stackalloc Vector3d[3] { target.A, target.B, target.C }; |
| | 16 | 240 | | foreach (Vector3d vertex in vertices) |
| | | 241 | | { |
| | 6 | 242 | | int side = DotScaledDirections(inward, GetScaledPointDifference(targetFrame.GetPoint(vertex), a, |
| | 6 | 243 | | sourceFrame.RotationDenominator)).Sign; |
| | 7 | 244 | | if (side > 0) sides |= 1; |
| | 7 | 245 | | if (side < 0) sides |= 2; |
| | | 246 | | } |
| | 2 | 247 | | return true; |
| | | 248 | | } |
| | | 249 | | |
| | | 250 | | /// <summary>Preserves the triangle pair's SAT and successive finite closest-witness contract.</summary> |
| | | 251 | | internal static bool TryGetContact(in FixedTriangle first, in ScaledRigidFrame3d firstFrame, Vector3d firstOrigin, |
| | | 252 | | in FixedTriangle second, in ScaledRigidFrame3d secondFrame, Vector3d secondOrigin, out FixedContactAnchors conta |
| | | 253 | | { |
| | 8 | 254 | | if (firstFrame.IsRigid && secondFrame.IsRigid) |
| | 1 | 255 | | return TryGetContact(first,firstFrame.RigidOrigin,firstFrame.Rotation, |
| | 1 | 256 | | second,secondFrame.RigidOrigin,secondFrame.Rotation,out contact); |
| | 7 | 257 | | contact=default; |
| | 7 | 258 | | if (GetScaledTriangleLocalNormal(first,firstFrame).IsZero |
| | 10 | 259 | | || GetScaledTriangleLocalNormal(second,secondFrame).IsZero) return false; |
| | 4 | 260 | | Span<Vector3d> firstPoints=stackalloc Vector3d[3]{first.A,first.B,first.C}; |
| | 4 | 261 | | Span<Vector3d> secondPoints=stackalloc Vector3d[3]{second.A,second.B,second.C}; |
| | 8 | 262 | | ReadOnlySpan<int> faces=stackalloc int[3]{0,1,2},edges=stackalloc int[6]{0,1,1,2,2,0}; |
| | 4 | 263 | | if(!WideOrientedBox.TryGetConvexHullPenetration(firstFrame,firstPoints,faces,edges, |
| | 4 | 264 | | secondFrame,secondPoints,faces,edges,out WideScaledHullPenetration penetration,planarPair:true) |
| | 4 | 265 | | ||!TryGetClosestPointAnchor(first,firstFrame,secondFrame.GetPoint(second.Centroid),firstOrigin,out FixedPoin |
| | 7 | 266 | | ||!TryGetClosestPointAnchor(second,secondFrame,RationalPoint3d.FromAnchor(a),secondOrigin,out FixedPointAnch |
| | 1 | 267 | | Fixed64 depth=penetration.GetRoundedDepth(out bool clamped); |
| | 2 | 268 | | contact=new FixedContactAnchors(a,b,penetration.GetNormal(),depth,clamped);return true; |
| | | 269 | | } |
| | | 270 | | |
| | | 271 | | /// <summary>Tests the finite orthogonal projection before rounding any vertex or witness.</summary> |
| | | 272 | | internal static bool ContainsProjection(in FixedTriangle triangle, in ScaledRigidFrame3d frame, |
| | | 273 | | in RationalPoint3d point) => |
| | 7 | 274 | | ContainsProjection<ScaledRigidFrame3d>(triangle, frame, point); |
| | | 275 | | |
| | | 276 | | internal static bool ContainsProjection<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 277 | | in RationalPoint3d point) |
| | | 278 | | where TFrame : struct, IForwardHullFrame3d |
| | | 279 | | { |
| | 258 | 280 | | Span<Signed832> values = stackalloc Signed832[3]; |
| | 258 | 281 | | return GetScaledTriangleHalfPlanes(triangle, frame, point, values) |
| | 258 | 282 | | && values[0].Sign >= 0 && values[1].Sign >= 0 && values[2].Sign >= 0; |
| | | 283 | | } |
| | | 284 | | |
| | | 285 | | /// <summary> |
| | | 286 | | /// Projects onto a finite exact face and rounds only the final witness in a |
| | | 287 | | /// caller-selected translated chart. Returns false outside the finite face |
| | | 288 | | /// or when the translated witness cannot fit a scalar coordinate. |
| | | 289 | | /// </summary> |
| | | 290 | | internal static bool TryGetProjectedPointAnchor(in FixedTriangle triangle, in ScaledRigidFrame3d frame, |
| | | 291 | | in RationalPoint3d point, Vector3d materializationOrigin, out FixedPointAnchor anchor) => |
| | 22 | 292 | | TryGetProjectedPointAnchor<ScaledRigidFrame3d>(triangle, frame, point, materializationOrigin, out anchor); |
| | | 293 | | |
| | | 294 | | internal static bool TryGetProjectedPointAnchor<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 295 | | in RationalPoint3d point, Vector3d materializationOrigin, out FixedPointAnchor anchor) |
| | | 296 | | where TFrame : struct, IForwardHullFrame3d => |
| | 42 | 297 | | TryGetProjectedPointAnchor(triangle, frame, point, materializationOrigin, default, default, out anchor); |
| | | 298 | | |
| | | 299 | | internal static bool TryGetProjectedPlanePointAnchor<TFrame>(in FixedTriangle plane, in TFrame frame, |
| | | 300 | | in RationalPoint3d point, Vector3d materializationOrigin, out FixedPointAnchor anchor) |
| | | 301 | | where TFrame : struct, IForwardHullFrame3d => |
| | 37 | 302 | | TryGetProjectedPointAnchor(plane, frame, point, materializationOrigin, default, default, out anchor, finite: fal |
| | | 303 | | |
| | | 304 | | private static bool TryGetProjectedPointAnchor<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 305 | | in RationalPoint3d point, Vector3d materializationOrigin, |
| | | 306 | | scoped ContactQuadratic squaredDistanceNumerator, scoped ContactQuadratic squaredDistanceDenominator, |
| | | 307 | | out FixedPointAnchor anchor, bool finite = true) |
| | | 308 | | where TFrame : struct, IForwardHullFrame3d |
| | | 309 | | { |
| | 101 | 310 | | anchor = default; |
| | 124 | 311 | | if (finite && !ContainsProjection(triangle, frame, point)) return false; |
| | 78 | 312 | | WideDirection3d normal = frame.GetFaceNormal(triangle); |
| | 79 | 313 | | if (normal.IsZero) return false; |
| | 77 | 314 | | WideDirection3d delta = GetScaledPointDifference(point, frame.GetPoint(triangle.A), frame.RotationDenominator); |
| | 154 | 315 | | Signed832 plane = DotScaledDirections(normal, delta), squared = normal.SquaredLength; |
| | 77 | 316 | | Signed576 commonD = WideArithmetic.MultiplySigned320(point.Denominator, frame.RotationDenominator); |
| | | 317 | | // A projected witness needs <962 numerator bits and <897 denominator |
| | | 318 | | // bits. Keep those products transient; no wider persistent integer is needed. |
| | | 319 | | const int words = 16; |
| | 154 | 320 | | Span<ulong> storage = stackalloc ulong[12 * words]; Span<int> signs = stackalloc int[12]; |
| | 77 | 321 | | ContactQuadratic metric = ContactQuadratic.At(storage, signs, 0, words), |
| | 77 | 322 | | divisor = ContactQuadratic.At(storage, signs, 1, words), |
| | 77 | 323 | | denominator = ContactQuadratic.At(storage, signs, 2, words), |
| | 77 | 324 | | numerator = ContactQuadratic.At(storage, signs, 3, words), |
| | 77 | 325 | | planeValue = ContactQuadratic.At(storage, signs, 4, words), |
| | 77 | 326 | | term = ContactQuadratic.At(storage, signs, 5, words); |
| | 231 | 327 | | metric.Set(squared); divisor.Set(commonD); planeValue.Set(plane); |
| | 77 | 328 | | if (!squaredDistanceNumerator.Values.IsEmpty) |
| | | 329 | | { |
| | 3 | 330 | | ContactQuadratic.Multiply(planeValue, planeValue, ReadOnlySpan<ulong>.Empty, squaredDistanceNumerator); |
| | 3 | 331 | | ContactQuadratic.Multiply(divisor, divisor, ReadOnlySpan<ulong>.Empty, term); |
| | 3 | 332 | | ContactQuadratic.Multiply(metric, term, ReadOnlySpan<ulong>.Empty, squaredDistanceDenominator); |
| | | 333 | | } |
| | 77 | 334 | | ContactQuadratic.Multiply(metric, divisor, ReadOnlySpan<ulong>.Empty, denominator); |
| | 77 | 335 | | Vector3d local = default; |
| | 580 | 336 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 337 | | { |
| | 219 | 338 | | Signed320 raw = coordinate == 0 ? point.X : coordinate == 1 ? point.Y : point.Z; |
| | 219 | 339 | | Signed576 relative = WideArithmetic.SubtractSigned576( |
| | 219 | 340 | | WideArithmetic.MultiplySigned320(raw, frame.RotationDenominator), |
| | 219 | 341 | | WideArithmetic.MultiplySigned576(commonD, Signed192.Raw(materializationOrigin[coordinate]))); |
| | 219 | 342 | | Signed576 component = coordinate == 0 ? normal.X : coordinate == 1 ? normal.Y : normal.Z; |
| | 219 | 343 | | ContactQuadratic.Scale(metric, relative, numerator); |
| | 438 | 344 | | ContactQuadratic.Scale(planeValue, component, term); numerator.Add(term, -1); |
| | 219 | 345 | | if (!Fixed64.TryGetSignedRawRatio(numerator.Rational, denominator.Rational, |
| | 225 | 346 | | numerator.Signs[0] < 0, out Fixed64 value)) return false; |
| | 213 | 347 | | local[coordinate] = value; |
| | | 348 | | } |
| | 71 | 349 | | anchor = new FixedPointAnchor(materializationOrigin, FixedQuaternion.Identity, local); |
| | 71 | 350 | | return true; |
| | | 351 | | } |
| | | 352 | | |
| | | 353 | | /// <summary>Clips a same-frame rational segment's projection to the exact finite triangle.</summary> |
| | | 354 | | internal static bool TryGetProjectedSegmentInterval(in FixedTriangle triangle, in ScaledRigidFrame3d frame, |
| | | 355 | | in RationalPoint3d first, in RationalPoint3d second, |
| | | 356 | | out Signed832 lowerN, out Signed832 lowerD, out Signed832 upperN, out Signed832 upperD) => |
| | 87 | 357 | | TryGetProjectedSegmentInterval<ScaledRigidFrame3d>(triangle, frame, first, second, out lowerN, out lowerD, out u |
| | | 358 | | |
| | | 359 | | internal static bool TryGetProjectedSegmentInterval<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 360 | | in RationalPoint3d first, in RationalPoint3d second, |
| | | 361 | | out Signed832 lowerN, out Signed832 lowerD, out Signed832 upperN, out Signed832 upperD) |
| | | 362 | | where TFrame : struct, IForwardHullFrame3d |
| | | 363 | | { |
| | | 364 | | System.Diagnostics.Debug.Assert(first.Denominator.Equals(second.Denominator)); |
| | 1340 | 365 | | Span<Signed832> start = stackalloc Signed832[3], end = stackalloc Signed832[3]; |
| | 1340 | 366 | | lowerN = default; lowerD = upperN = upperD = Signed832.ExtendValue(Signed192.One); |
| | 708 | 367 | | if (!GetScaledTriangleHalfPlanes(triangle, frame, first, start)) return false; |
| | | 368 | | // Both points share the same nondegenerate face; only their values differ. |
| | 632 | 369 | | GetScaledTriangleHalfPlanes(triangle, frame, second, end); |
| | 4130 | 370 | | for (int edge = 0; edge < 3; edge++) |
| | 1931 | 371 | | if (!ClipScaledHalfPlane(start[edge], end[edge], ref lowerN, ref lowerD, ref upperN, ref upperD)) return fal |
| | 383 | 372 | | return true; |
| | | 373 | | } |
| | | 374 | | |
| | | 375 | | /// <summary>Clips a segment's orthogonal projection to A+u(B-A)+v(C-A), with retained u and v in [0,1].</summary> |
| | | 376 | | /// <remarks> |
| | | 377 | | /// Both source points must share a positive denominator. The four disjoint output |
| | | 378 | | /// banks require 32 words each: full 832-bit cuts times exact face half-planes |
| | | 379 | | /// need fewer than 1,667 bits, even when the resulting interval cannot fit |
| | | 380 | | /// Signed832. Fractions remain borrowed; no endpoint is materialized. |
| | | 381 | | /// </remarks> |
| | | 382 | | internal static bool TryGetProjectedRectangleInterval(in FixedTriangle rectangleBasis, in ScaledRigidFrame3d frame, |
| | | 383 | | in RationalPoint3d first, in RationalPoint3d second, in RationalSegmentInterval tangentClip, |
| | | 384 | | scoped ContactQuadratic lowerN, scoped ContactQuadratic lowerD, |
| | | 385 | | scoped ContactQuadratic upperN, scoped ContactQuadratic upperD) => |
| | 57 | 386 | | TryGetProjectedRectangleInterval<ScaledRigidFrame3d>(rectangleBasis, frame, first, second, tangentClip, lowerN, |
| | | 387 | | |
| | | 388 | | internal static bool TryGetProjectedRectangleInterval<TFrame>(in FixedTriangle rectangleBasis, in TFrame frame, |
| | | 389 | | in RationalPoint3d first, in RationalPoint3d second, in RationalSegmentInterval tangentClip, |
| | | 390 | | scoped ContactQuadratic lowerN, scoped ContactQuadratic lowerD, |
| | | 391 | | scoped ContactQuadratic upperN, scoped ContactQuadratic upperD) |
| | | 392 | | where TFrame : struct, IForwardHullFrame3d |
| | | 393 | | { |
| | | 394 | | System.Diagnostics.Debug.Assert(first.Denominator.Equals(second.Denominator)); |
| | 979 | 395 | | Signed832 one = Signed832.ExtendValue(Signed192.One); |
| | 3916 | 396 | | Signed832 cutLowerN = default, cutLowerD = one, cutUpperN = one, cutUpperD = one; |
| | 979 | 397 | | if (tangentClip.IsClipped) |
| | | 398 | | { |
| | 1186 | 399 | | cutLowerN = tangentClip.WideLowerNumerator; cutLowerD = tangentClip.WideLowerDenominator; |
| | 1186 | 400 | | cutUpperN = tangentClip.WideUpperNumerator; cutUpperD = tangentClip.WideUpperDenominator; |
| | | 401 | | } |
| | 3916 | 402 | | lowerN.Clear(); lowerD.Set(one); upperN.Set(one); upperD.Set(one); |
| | 1958 | 403 | | Span<Signed832> start = stackalloc Signed832[3], end = stackalloc Signed832[3]; |
| | 983 | 404 | | if (!GetScaledTriangleHalfPlanes(rectangleBasis, frame, first, start)) return false; |
| | 975 | 405 | | GetScaledTriangleHalfPlanes(rectangleBasis, frame, second, end); |
| | | 406 | | const int words = 32; |
| | 1950 | 407 | | Span<ulong> values = stackalloc ulong[12 * words]; Span<int> signs = stackalloc int[12]; |
| | 975 | 408 | | var firstSum = ContactQuadratic.At(values, signs, 0, words); |
| | 975 | 409 | | var secondSum = ContactQuadratic.At(values, signs, 1, words); |
| | 975 | 410 | | var firstWall = ContactQuadratic.At(values, signs, 2, words); |
| | 975 | 411 | | var secondWall = ContactQuadratic.At(values, signs, 3, words); |
| | 975 | 412 | | var coefficient = ContactQuadratic.At(values, signs, 4, words); |
| | 975 | 413 | | var term = ContactQuadratic.At(values, signs, 5, words); |
| | 1950 | 414 | | firstSum.Set(start[0]); secondSum.Set(end[0]); |
| | 5850 | 415 | | for (int index = 1; index < 3; index++) |
| | | 416 | | { |
| | 3900 | 417 | | coefficient.Set(start[index]); firstSum.Add(coefficient); |
| | 3900 | 418 | | coefficient.Set(end[index]); secondSum.Add(coefficient); |
| | | 419 | | } |
| | | 420 | | // Half-planes opposite B and C are u and v times their common metric; |
| | | 421 | | // the rectangle has no triangular u+v<=1 restriction. |
| | 4910 | 422 | | for (int wall = 0; wall < 4; wall++) |
| | | 423 | | { |
| | 13884 | 424 | | for (int side = 0; side < 2; side++) |
| | | 425 | | { |
| | 4628 | 426 | | var sum = side == 0 ? firstSum : secondSum; |
| | 4628 | 427 | | var output = side == 0 ? firstWall : secondWall; |
| | 4628 | 428 | | ReadOnlySpan<Signed832> point = side == 0 ? start : end; |
| | 4628 | 429 | | if (wall < 2) |
| | | 430 | | { |
| | 3224 | 431 | | Signed832 numerator = wall == 0 ? cutLowerN : cutUpperN; |
| | 3224 | 432 | | Signed832 denominator = wall == 0 ? cutLowerD : cutUpperD; |
| | 6448 | 433 | | coefficient.Set(point[2]); ContactQuadratic.Scale(coefficient, denominator, output); |
| | 6448 | 434 | | ContactQuadratic.Scale(sum, numerator, term); output.Add(term, -1); |
| | 4498 | 435 | | if (wall != 0) output.MultiplySign(-1); |
| | | 436 | | } |
| | 2258 | 437 | | else if (wall == 2) output.Set(point[0]); |
| | 1650 | 438 | | else { sum.CopyTo(output); coefficient.Set(point[0]); output.Add(coefficient, -1); } |
| | | 439 | | } |
| | 3148 | 440 | | if (!ClipProjectedRectangleWall(firstWall, secondWall, lowerN, lowerD, upperN, upperD)) return false; |
| | | 441 | | } |
| | 141 | 442 | | return true; |
| | | 443 | | } |
| | | 444 | | |
| | | 445 | | private static bool ClipProjectedRectangleWall(scoped ContactQuadratic first, scoped ContactQuadratic second, |
| | | 446 | | scoped ContactQuadratic lowerN, scoped ContactQuadratic lowerD, |
| | | 447 | | scoped ContactQuadratic upperN, scoped ContactQuadratic upperD) |
| | | 448 | | { |
| | 4628 | 449 | | int firstSign = first.Signs[0], secondSign = second.Signs[0]; |
| | 3357 | 450 | | if (firstSign >= 0 && secondSign >= 0) return true; |
| | 2101 | 451 | | if (firstSign < 0 && secondSign < 0) return false; |
| | | 452 | | const int words = 32; |
| | 882 | 453 | | Span<ulong> values = stackalloc ulong[4 * words]; Span<int> signs = stackalloc int[4]; |
| | 441 | 454 | | var numerator = ContactQuadratic.At(values, signs, 0, words); |
| | 441 | 455 | | var denominator = ContactQuadratic.At(values, signs, 1, words); |
| | 1323 | 456 | | first.CopyTo(numerator); first.CopyTo(denominator); denominator.Add(second, -1); |
| | 883 | 457 | | if (denominator.Signs[0] < 0) { numerator.MultiplySign(-1); denominator.MultiplySign(-1); } |
| | 441 | 458 | | if (firstSign < 0) |
| | | 459 | | { |
| | 221 | 460 | | if (ContactQuadratic.CompareRatios(numerator, denominator, ReadOnlySpan<ulong>.Empty, |
| | 221 | 461 | | lowerN, lowerD, ReadOnlySpan<ulong>.Empty) > 0) |
| | 430 | 462 | | { numerator.CopyTo(lowerN); denominator.CopyTo(lowerD); } |
| | | 463 | | } |
| | 220 | 464 | | else if (ContactQuadratic.CompareRatios(numerator, denominator, ReadOnlySpan<ulong>.Empty, |
| | 220 | 465 | | upperN, upperD, ReadOnlySpan<ulong>.Empty) < 0) |
| | 436 | 466 | | { numerator.CopyTo(upperN); denominator.CopyTo(upperD); } |
| | 441 | 467 | | return ContactQuadratic.CompareRatios(lowerN, lowerD, ReadOnlySpan<ulong>.Empty, |
| | 441 | 468 | | upperN, upperD, ReadOnlySpan<ulong>.Empty) <= 0; |
| | | 469 | | } |
| | | 470 | | |
| | | 471 | | internal static bool TryGetProjectedSegmentFaceAnchors(in FixedTriangle triangle, in ScaledRigidFrame3d frame, |
| | | 472 | | in RationalPoint3d first, in RationalPoint3d second, Signed832 n, Signed832 d, |
| | | 473 | | Vector3d sourceOrigin, Vector3d targetOrigin, int physicalBoundaryMask, |
| | | 474 | | out FixedPointAnchor source, out FixedPointAnchor target, |
| | | 475 | | bool requirePhysicalTargetBoundary = true, bool materialize = true) => |
| | 8 | 476 | | TryGetProjectedFrameSegmentFaceAnchors<ScaledRigidFrame3d>(triangle, frame, first, second, n, d, sourceOrigin, t |
| | | 477 | | |
| | | 478 | | internal static bool TryGetProjectedFrameSegmentFaceAnchors<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 479 | | in RationalPoint3d first, in RationalPoint3d second, Signed832 n, Signed832 d, |
| | | 480 | | Vector3d sourceOrigin, Vector3d targetOrigin, int physicalBoundaryMask, |
| | | 481 | | out FixedPointAnchor source, out FixedPointAnchor target, |
| | | 482 | | bool requirePhysicalTargetBoundary = true, bool materialize = true, |
| | | 483 | | scoped ContactQuadratic wideN = default, scoped ContactQuadratic wideD = default, bool rectangle = false) |
| | | 484 | | where TFrame : struct, IForwardHullFrame3d |
| | | 485 | | { |
| | 534 | 486 | | source = target = default; |
| | 534 | 487 | | int words = rectangle ? 48 : 32; |
| | 1068 | 488 | | Span<ulong> values = stackalloc ulong[20 * words]; Span<int> signs = stackalloc int[20]; |
| | 534 | 489 | | var sx = ContactQuadratic.At(values, signs, 0, words); |
| | 534 | 490 | | var sy = ContactQuadratic.At(values, signs, 1, words); |
| | 534 | 491 | | var sz = ContactQuadratic.At(values, signs, 2, words); |
| | 534 | 492 | | var sd = ContactQuadratic.At(values, signs, 3, words); |
| | 534 | 493 | | var tx = ContactQuadratic.At(values, signs, 4, words); |
| | 534 | 494 | | var ty = ContactQuadratic.At(values, signs, 5, words); |
| | 534 | 495 | | var tz = ContactQuadratic.At(values, signs, 6, words); |
| | 534 | 496 | | var td = ContactQuadratic.At(values, signs, 7, words); |
| | 534 | 497 | | if (!TryGetProjectedSegmentFaceWitness(triangle, frame, first, second, n, d, |
| | 534 | 498 | | sx, sy, sz, sd, tx, ty, tz, td, out int edgeMask, wideN, wideD, rectangle) |
| | 661 | 499 | | || requirePhysicalTargetBoundary && (edgeMask & physicalBoundaryMask) == 0) return false; |
| | 409 | 500 | | if (!materialize) return true; |
| | 405 | 501 | | var relative = ContactQuadratic.At(values, signs, 8, words); |
| | 405 | 502 | | var term = ContactQuadratic.At(values, signs, 9, words); |
| | 810 | 503 | | Vector3d sourcePoint = default, targetPoint = default; |
| | 3036 | 504 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 505 | | { |
| | 1147 | 506 | | var sourceCoordinate = coordinate == 0 ? sx : coordinate == 1 ? sy : sz; |
| | 1147 | 507 | | var targetCoordinate = coordinate == 0 ? tx : coordinate == 1 ? ty : tz; |
| | 1147 | 508 | | sourceCoordinate.CopyTo(relative); |
| | 2294 | 509 | | ContactQuadratic.Scale(sd, Signed576.ExtendValue(Signed192.Raw(sourceOrigin[coordinate])), term); relative.A |
| | 1147 | 510 | | if (!Fixed64.TryGetSignedRawRatio(relative.Rational, sd.Rational, |
| | 1164 | 511 | | relative.Signs[0] < 0, out Fixed64 scalar)) return false; |
| | 1130 | 512 | | sourcePoint[coordinate] = scalar; |
| | 1130 | 513 | | targetCoordinate.CopyTo(relative); |
| | 2260 | 514 | | ContactQuadratic.Scale(td, Signed576.ExtendValue(Signed192.Raw(targetOrigin[coordinate])), term); relative.A |
| | 1130 | 515 | | if (!Fixed64.TryGetSignedRawRatio(relative.Rational, td.Rational, |
| | 1147 | 516 | | relative.Signs[0] < 0, out scalar)) return false; |
| | 1113 | 517 | | targetPoint[coordinate] = scalar; |
| | | 518 | | } |
| | 371 | 519 | | source = new FixedPointAnchor(sourceOrigin, FixedQuaternion.Identity, sourcePoint); |
| | 371 | 520 | | target = new FixedPointAnchor(targetOrigin, FixedQuaternion.Identity, targetPoint); |
| | 371 | 521 | | return true; |
| | | 522 | | } |
| | | 523 | | |
| | | 524 | | /// <summary>Materializes a rectangle projection from an already clipped borrowed unit parameter.</summary> |
| | | 525 | | internal static bool TryGetProjectedRectangleFaceAnchors<TFrame>(in FixedTriangle rectangleBasis, in TFrame frame, |
| | | 526 | | in RationalPoint3d first, in RationalPoint3d second, scoped ContactQuadratic n, scoped ContactQuadratic d, |
| | | 527 | | Vector3d sourceOrigin, Vector3d targetOrigin, out FixedPointAnchor source, out FixedPointAnchor target) |
| | | 528 | | where TFrame : struct, IForwardHullFrame3d => |
| | 61 | 529 | | TryGetProjectedFrameSegmentFaceAnchors(rectangleBasis, frame, first, second, default, default, |
| | 61 | 530 | | sourceOrigin, targetOrigin, 0, out source, out target, requirePhysicalTargetBoundary: false, |
| | 61 | 531 | | wideN: n, wideD: d, rectangle: true); |
| | | 532 | | |
| | | 533 | | /// <summary> |
| | | 534 | | /// Retains the exact source point and its finite face projection as borrowed |
| | | 535 | | /// world-coordinate ratios. The zero barycentric weights identify opposite |
| | | 536 | | /// triangle edges; no coordinate or separation has been rounded. |
| | | 537 | | /// </summary> |
| | | 538 | | private static bool TryGetProjectedSegmentFaceWitness<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 539 | | in RationalPoint3d first, in RationalPoint3d second, Signed832 n, Signed832 d, |
| | | 540 | | scoped ContactQuadratic sourceX, scoped ContactQuadratic sourceY, scoped ContactQuadratic sourceZ, |
| | | 541 | | scoped ContactQuadratic sourceD, scoped ContactQuadratic targetX, scoped ContactQuadratic targetY, |
| | | 542 | | scoped ContactQuadratic targetZ, scoped ContactQuadratic targetD, out int edgeMask, |
| | | 543 | | scoped ContactQuadratic wideN = default, scoped ContactQuadratic wideD = default, bool rectangle = false) |
| | | 544 | | where TFrame : struct, IForwardHullFrame3d |
| | | 545 | | { |
| | 534 | 546 | | edgeMask = 0; |
| | 1068 | 547 | | Span<Signed832> start = stackalloc Signed832[3], end = stackalloc Signed832[3]; |
| | 535 | 548 | | if (!GetScaledTriangleHalfPlanes(triangle, frame, first, start)) return false; |
| | 533 | 549 | | GetScaledTriangleHalfPlanes(triangle, frame, second, end); |
| | | 550 | | // Compact triangle parameters retain the original 2048-bit banks. |
| | | 551 | | // Full rectangle cuts need <1667 parameter bits; adding an 832-bit |
| | | 552 | | // half-plane and 320-bit coordinate remains below 3072 bits. |
| | 533 | 553 | | int words = rectangle ? 48 : 32; |
| | 1066 | 554 | | Span<ulong> values = stackalloc ulong[20 * words]; Span<int> signs = stackalloc int[20]; |
| | 533 | 555 | | var parameter = ContactQuadratic.At(values, signs, 0, words); |
| | 533 | 556 | | var divisor = ContactQuadratic.At(values, signs, 1, words); |
| | 533 | 557 | | var remaining = ContactQuadratic.At(values, signs, 2, words); |
| | 533 | 558 | | var sum = ContactQuadratic.At(values, signs, 6, words); |
| | 533 | 559 | | var term = ContactQuadratic.At(values, signs, 7, words); |
| | 533 | 560 | | var coefficient = ContactQuadratic.At(values, signs, 8, words); |
| | 655 | 561 | | if (!wideD.Values.IsEmpty) { wideN.CopyTo(parameter); wideD.CopyTo(divisor); } |
| | 944 | 562 | | else { parameter.Set(n); divisor.Set(d); } |
| | 1599 | 563 | | divisor.CopyTo(remaining); remaining.Add(parameter, -1); sum.Clear(); |
| | 4254 | 564 | | for (int corner = 0; corner < 3; corner++) |
| | | 565 | | { |
| | 1597 | 566 | | int edge = (corner + 1) % 3; |
| | 1597 | 567 | | var barycentric = ContactQuadratic.At(values, signs, 3 + corner, words); |
| | 3194 | 568 | | coefficient.Set(start[edge]); ContactQuadratic.Multiply(coefficient, remaining, ReadOnlySpan<ulong>.Empty, b |
| | 3194 | 569 | | coefficient.Set(end[edge]); ContactQuadratic.Multiply(coefficient, parameter, ReadOnlySpan<ulong>.Empty, ter |
| | 1597 | 570 | | barycentric.Add(term); |
| | 1600 | 571 | | if (barycentric.Signs[0] < 0 && (!rectangle || corner != 0)) return false; |
| | 1848 | 572 | | if (barycentric.Signs[0] == 0) edgeMask |= 1 << corner; |
| | 1594 | 573 | | sum.Add(barycentric); |
| | | 574 | | } |
| | | 575 | | // The three barycentric numerators sum to the nonzero face metric |
| | | 576 | | // times the positive input parameter denominator. |
| | | 577 | | System.Diagnostics.Debug.Assert(sum.Signs[0] > 0); |
| | 530 | 578 | | if (rectangle) |
| | 348 | 579 | | for (int corner = 1; corner < 3; corner++) |
| | | 580 | | { |
| | 117 | 581 | | ContactQuadratic.At(values, signs, 3 + corner, words).CopyTo(term); |
| | 117 | 582 | | term.Add(sum, -1); |
| | 119 | 583 | | if (term.Signs[0] > 0) return false; |
| | | 584 | | } |
| | 528 | 585 | | coefficient.Set(Signed576.ExtendValue(first.Denominator)); |
| | 528 | 586 | | ContactQuadratic.Multiply(divisor, coefficient, ReadOnlySpan<ulong>.Empty, sourceD); |
| | 528 | 587 | | coefficient.Set(Signed576.ExtendValue(frame.Denominator)); |
| | 528 | 588 | | ContactQuadratic.Multiply(sum, coefficient, ReadOnlySpan<ulong>.Empty, targetD); |
| | 528 | 589 | | Span<RationalPoint3d> corners = stackalloc RationalPoint3d[3] |
| | 528 | 590 | | { frame.GetPoint(triangle.A), frame.GetPoint(triangle.B), frame.GetPoint(triangle.C) }; |
| | 4224 | 591 | | for (int coordinate = 0; coordinate < 3; coordinate++) |
| | | 592 | | { |
| | 1584 | 593 | | var source = coordinate == 0 ? sourceX : coordinate == 1 ? sourceY : sourceZ; |
| | 1584 | 594 | | var target = coordinate == 0 ? targetX : coordinate == 1 ? targetY : targetZ; |
| | 1584 | 595 | | Signed320 a = coordinate == 0 ? first.X : coordinate == 1 ? first.Y : first.Z; |
| | 1584 | 596 | | Signed320 b = coordinate == 0 ? second.X : coordinate == 1 ? second.Y : second.Z; |
| | 1584 | 597 | | ContactQuadratic.Scale(remaining, Signed576.ExtendValue(a), source); |
| | 3168 | 598 | | ContactQuadratic.Scale(parameter, Signed576.ExtendValue(b), term); source.Add(term); |
| | 1584 | 599 | | target.Clear(); |
| | 12672 | 600 | | for (int corner = 0; corner < 3; corner++) |
| | | 601 | | { |
| | 4752 | 602 | | RationalPoint3d point = corners[corner]; |
| | 4752 | 603 | | Signed320 raw = coordinate == 0 ? point.X : coordinate == 1 ? point.Y : point.Z; |
| | 4752 | 604 | | ContactQuadratic.Scale(ContactQuadratic.At(values, signs, 3 + corner, words), Signed576.ExtendValue(raw) |
| | 4752 | 605 | | target.Add(term); |
| | | 606 | | } |
| | | 607 | | } |
| | 528 | 608 | | return true; |
| | | 609 | | } |
| | | 610 | | |
| | | 611 | | internal static bool ClipScaledHalfPlane(Signed832 first, Signed832 second, |
| | | 612 | | ref Signed832 lowerN, ref Signed832 lowerD, ref Signed832 upperN, ref Signed832 upperD) |
| | | 613 | | { |
| | 62311 | 614 | | if (first.Sign >= 0 && second.Sign >= 0) return true; |
| | 19395 | 615 | | if (first.Sign < 0 && second.Sign < 0) return false; |
| | 1774 | 616 | | Signed832 n = first, d = WideArithmetic.SubtractSigned832(first, second); |
| | 1753 | 617 | | if (d.Sign < 0) { n = WideArithmetic.SubtractSigned832(default, n); d = WideArithmetic.SubtractSigned832(default |
| | 887 | 618 | | if (first.Sign < 0) |
| | | 619 | | { |
| | 1279 | 620 | | if (WideArithmetic.CompareNonNegativeProducts(n, lowerD, lowerN, d) > 0) { lowerN = n; lowerD = d; } |
| | | 621 | | } |
| | 1340 | 622 | | else if (WideArithmetic.CompareNonNegativeProducts(n, upperD, upperN, d) < 0) { upperN = n; upperD = d; } |
| | 887 | 623 | | return WideArithmetic.CompareNonNegativeProducts(lowerN, upperD, upperN, lowerD) <= 0; |
| | | 624 | | } |
| | | 625 | | |
| | | 626 | | internal static bool GetScaledTriangleHalfPlanes<TFrame>(in FixedTriangle triangle, in TFrame frame, |
| | | 627 | | in RationalPoint3d point, Span<Signed832> values) where TFrame : struct, IForwardHullFrame3d => |
| | 4581 | 628 | | frame.TryGetTriangleHalfPlanes(triangle, point, values); |
| | | 629 | | |
| | | 630 | | internal static bool GetScaledTriangleHalfPlanes(in FixedTriangle triangle, in ScaledRigidFrame3d frame, |
| | | 631 | | in RationalPoint3d point, Span<Signed832> values) |
| | | 632 | | { |
| | 2864 | 633 | | WideDirection3d localNormal = GetScaledTriangleLocalNormal(triangle, frame); |
| | 2911 | 634 | | if (localNormal.IsZero) return false; |
| | 2817 | 635 | | Span<Vector3d> vertices = stackalloc Vector3d[3] { triangle.A, triangle.B, triangle.C }; |
| | 22536 | 636 | | for (int edge = 0; edge < 3; edge++) |
| | | 637 | | { |
| | 16902 | 638 | | Vector3d a = vertices[edge], b = vertices[(edge + 1) % 3]; |
| | 8451 | 639 | | WideDirection3d inward = WideDirection3d.Cross(localNormal, WideDirection3d.GetLocalEdge(frame, a, b)) |
| | 8451 | 640 | | .Transform(frame.GeometryBasis); |
| | 8451 | 641 | | WideDirection3d delta = GetScaledPointDifference(point, frame.GetPoint(a), frame.RotationDenominator); |
| | 8451 | 642 | | values[edge] = DotScaledDirections(inward, delta); |
| | | 643 | | } |
| | 2817 | 644 | | return true; |
| | | 645 | | } |
| | | 646 | | |
| | | 647 | | internal static WideDirection3d GetScaledTriangleLocalNormal(in FixedTriangle triangle, in ScaledRigidFrame3d frame) |
| | | 648 | | { |
| | | 649 | | // Rigid geometry already uses physical local vertices. Retain the |
| | | 650 | | // original normal instead of injecting Q^4 through two unit-scale |
| | | 651 | | // edge numerators; that redundant factor also inflates fan minors. |
| | 6479 | 652 | | if (frame.IsRigid) |
| | | 653 | | { |
| | 3532 | 654 | | triangle.GetExactNormal(out Signed192 x, out Signed192 y, out Signed192 z, out _); |
| | 3532 | 655 | | return new WideDirection3d(Signed576.ExtendValue(x), Signed576.ExtendValue(y), Signed576.ExtendValue(z)); |
| | | 656 | | } |
| | 2947 | 657 | | return WideDirection3d.Cross(WideDirection3d.GetLocalEdge(frame, triangle.A, triangle.B), |
| | 2947 | 658 | | WideDirection3d.GetLocalEdge(frame, triangle.A, triangle.C)); |
| | | 659 | | } |
| | | 660 | | |
| | | 661 | | internal static WideDirection3d GetScaledPointDifference(in RationalPoint3d point, |
| | | 662 | | in RationalPoint3d reference, Signed192 referenceRotationDenominator) |
| | | 663 | | { |
| | 16609 | 664 | | Signed192 pointRotationD = WideArithmetic.TruncateSigned320By64(point.Denominator); |
| | 16609 | 665 | | return new WideDirection3d(Difference(point.X, reference.X, referenceRotationDenominator, pointRotationD), |
| | 16609 | 666 | | Difference(point.Y, reference.Y, referenceRotationDenominator, pointRotationD), |
| | 16609 | 667 | | Difference(point.Z, reference.Z, referenceRotationDenominator, pointRotationD)); |
| | | 668 | | static Signed576 Difference(Signed320 point, Signed320 reference, Signed192 referenceD, Signed192 pointD) => |
| | | 669 | | WideArithmetic.SubtractSigned576(WideArithmetic.MultiplySigned320(point, referenceD), |
| | | 670 | | WideArithmetic.MultiplySigned320(reference, pointD)); |
| | | 671 | | } |
| | | 672 | | |
| | | 673 | | internal static Signed832 DotScaledDirections(in WideDirection3d first, in WideDirection3d second) => |
| | 9473 | 674 | | WideArithmetic.AddSigned832(WideArithmetic.AddSigned832(WideArithmetic.MultiplySigned576ToSigned832(first.X, sec |
| | 9473 | 675 | | WideArithmetic.MultiplySigned576ToSigned832(first.Y, second.Y)), WideArithmetic.MultiplySigned576ToSigned832 |
| | | 676 | | } |