| | | 1 | | //======================================================================= |
| | | 2 | | // MeshPlaneSampleReduction.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2026-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Collections.Generic; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using FixedMathSharp.Geometry; |
| | | 12 | | |
| | | 13 | | namespace Gravitas.CollisionHandling; |
| | | 14 | | |
| | | 15 | | |
| | | 16 | | |
| | | 17 | | /// <summary>Ranks finite plane samples after their exact maximum has seeded the pool.</summary> |
| | | 18 | | internal static class MeshPlaneSampleReduction |
| | | 19 | | { |
| | | 20 | | internal static int ChooseNext<T>(ReadOnlySpan<T> pool, ReadOnlySpan<int> chosen, |
| | | 21 | | in WideDirection3d normal, IComparer<T> pointComparer) where T : struct, IPlaneContactSample |
| | | 22 | | { |
| | | 23 | | const int words = 32; |
| | 10356 | 24 | | Span<ulong> values = stackalloc ulong[8 * words]; Span<int> signs = stackalloc int[8]; |
| | 5178 | 25 | | var bestN = ContactQuadratic.At(values, signs, 0, words); |
| | 5178 | 26 | | var bestD = ContactQuadratic.At(values, signs, 1, words); |
| | 5178 | 27 | | var n = ContactQuadratic.At(values, signs, 2, words); |
| | 5178 | 28 | | var d = ContactQuadratic.At(values, signs, 3, words); |
| | 5178 | 29 | | int best = -1; |
| | 41096 | 30 | | for (int candidate = 1; candidate < pool.Length; candidate++) |
| | | 31 | | { |
| | 15370 | 32 | | if (chosen.IndexOf(candidate) >= 0) continue; |
| | 11315 | 33 | | Vector3d p = pool[candidate].Point; |
| | 11315 | 34 | | d.Set(Signed576.ExtendValue(Signed320.ExtendValue(Signed192.One))); |
| | 17187 | 35 | | if (chosen.Length == 1) WidePlaneMetrics.GetSpanSquared(normal, pool[chosen[0]].Point, p, n); |
| | 9105 | 36 | | else if (chosen.Length == 2) WidePlaneMetrics.GetAreaSquared(normal, pool[chosen[0]].Point, pool[chosen[1]]. |
| | 1781 | 37 | | else WidePlaneMetrics.GetTriangleDistanceSquared(normal, p, pool[chosen[0]].Point, |
| | 1781 | 38 | | pool[chosen[1]].Point, pool[chosen[2]].Point, n, d); |
| | 11315 | 39 | | int comparison = best < 0 ? 1 : ContactQuadratic.CompareRatios(n, d, ReadOnlySpan<ulong>.Empty, |
| | 11315 | 40 | | bestN, bestD, ReadOnlySpan<ulong>.Empty); |
| | 11315 | 41 | | if (comparison > 0 || comparison == 0 && pointComparer.Compare(pool[candidate], pool[best]) < 0) |
| | 19011 | 42 | | { best = candidate; n.CopyTo(bestN); d.CopyTo(bestD); } |
| | | 43 | | } |
| | 5178 | 44 | | return best; |
| | | 45 | | } |
| | | 46 | | |
| | | 47 | | internal static int ChooseNext<T>(ReadOnlySpan<T> pool, ReadOnlySpan<int> chosen, |
| | | 48 | | WidePlaneMetrics metrics, IComparer<T> pointComparer) where T : struct, IPlaneContactSample |
| | | 49 | | { |
| | 58144 | 50 | | int best = -1; |
| | 116288 | 51 | | Signed832 bestN = default, bestD = default; |
| | 541126 | 52 | | for (int candidate = 1; candidate < pool.Length; candidate++) |
| | | 53 | | { |
| | 212419 | 54 | | if (chosen.IndexOf(candidate) >= 0) continue; |
| | 160627 | 55 | | Vector3d p = pool[candidate].Point; |
| | 160627 | 56 | | Signed832 n, d = Signed832.ExtendValue(Signed192.Signed(1)); |
| | 235668 | 57 | | if (chosen.Length == 1) n = metrics.GetSpanSquared(pool[chosen[0]].Point, p); |
| | 137275 | 58 | | else if (chosen.Length == 2) n = metrics.GetAreaSquared(pool[chosen[0]].Point, pool[chosen[1]].Point, p); |
| | 33897 | 59 | | else metrics.GetTriangleDistanceSquared(p, pool[chosen[0]].Point, |
| | 33897 | 60 | | pool[chosen[1]].Point, pool[chosen[2]].Point, out n, out d); |
| | 160627 | 61 | | int comparison = best < 0 ? 1 : WidePlaneMetrics.CompareRatios(n, d, bestN, bestD); |
| | | 62 | | // Rounded metrics rank coverage only. Equal metrics retain the |
| | | 63 | | // owner's exact point/provenance order before any solver reduction. |
| | 160627 | 64 | | if (comparison > 0 || comparison == 0 && pointComparer.Compare(pool[candidate], pool[best]) < 0) |
| | 251760 | 65 | | { best = candidate; bestN = n; bestD = d; } |
| | | 66 | | } |
| | 58144 | 67 | | return best; |
| | | 68 | | } |
| | | 69 | | } |