| | | 1 | | //======================================================================= |
| | | 2 | | // PhysicsMixedHitSorter.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.Runtime.CompilerServices; |
| | | 9 | | using SwiftCollections; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Queries; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Orders cross-dimensional hits by distance and deterministic tie breaks. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class PhysicsMixedHitSorter |
| | | 17 | | { |
| | | 18 | | internal static void SortByDistance(SwiftList<PhysicsMixedHit> hits) |
| | | 19 | | { |
| | 8270 | 20 | | SortByDistance(hits, 0, hits.Count); |
| | 8270 | 21 | | } |
| | | 22 | | |
| | | 23 | | internal static void SortByDistance(SwiftList<PhysicsMixedHit> hits, int start, int count) |
| | | 24 | | { |
| | 8270 | 25 | | if (count < 2) |
| | 8152 | 26 | | return; |
| | | 27 | | |
| | 688 | 28 | | for (int root = (count / 2) - 1; root >= 0; root--) |
| | 226 | 29 | | SiftDown(hits, start, root, count); |
| | | 30 | | |
| | 1002 | 31 | | for (int end = count - 1; end > 0; end--) |
| | | 32 | | { |
| | 383 | 33 | | Swap(hits, start, start + end); |
| | 383 | 34 | | SiftDown(hits, start, 0, end); |
| | | 35 | | } |
| | 118 | 36 | | } |
| | | 37 | | |
| | 1074 | 38 | | internal static bool ComesBefore(PhysicsMixedHit left, PhysicsMixedHit right) => Compare(left, right) < 0; |
| | | 39 | | |
| | | 40 | | private static void SiftDown(SwiftList<PhysicsMixedHit> hits, int start, int root, int count) |
| | | 41 | | { |
| | 597 | 42 | | while (true) |
| | | 43 | | { |
| | 1206 | 44 | | int child = (root * 2) + 1; |
| | 1206 | 45 | | if (child >= count) |
| | 571 | 46 | | return; |
| | | 47 | | |
| | 635 | 48 | | int swapIndex = root; |
| | 635 | 49 | | if (ComesBefore(hits[start + swapIndex], hits[start + child])) |
| | 597 | 50 | | swapIndex = child; |
| | | 51 | | |
| | 635 | 52 | | int right = child + 1; |
| | 635 | 53 | | if (right < count && ComesBefore(hits[start + swapIndex], hits[start + right])) |
| | 193 | 54 | | swapIndex = right; |
| | | 55 | | |
| | 635 | 56 | | if (swapIndex == root) |
| | 38 | 57 | | return; |
| | | 58 | | |
| | 597 | 59 | | Swap(hits, start + root, start + swapIndex); |
| | 597 | 60 | | root = swapIndex; |
| | | 61 | | } |
| | | 62 | | } |
| | | 63 | | |
| | | 64 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 65 | | private static int Compare(PhysicsMixedHit left, PhysicsMixedHit right) |
| | | 66 | | { |
| | 1074 | 67 | | int distance = left.Distance.CompareTo(right.Distance); |
| | 1074 | 68 | | if (distance != 0) |
| | 1048 | 69 | | return distance; |
| | | 70 | | |
| | 26 | 71 | | int left3D = left.Collider3D?.Id ?? -1; |
| | 26 | 72 | | int right3D = right.Collider3D?.Id ?? -1; |
| | 26 | 73 | | int collider3D = left3D.CompareTo(right3D); |
| | 26 | 74 | | if (collider3D != 0) |
| | 12 | 75 | | return collider3D; |
| | | 76 | | |
| | 14 | 77 | | int left2D = left.Collider2D?.Id ?? -1; |
| | 14 | 78 | | int right2D = right.Collider2D?.Id ?? -1; |
| | 14 | 79 | | return left2D.CompareTo(right2D); |
| | | 80 | | } |
| | | 81 | | |
| | | 82 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 83 | | private static void Swap(SwiftList<PhysicsMixedHit> hits, int left, int right) |
| | | 84 | | { |
| | 980 | 85 | | PhysicsMixedHit temp = hits[left]; |
| | 980 | 86 | | hits[left] = hits[right]; |
| | 980 | 87 | | hits[right] = temp; |
| | 980 | 88 | | } |
| | | 89 | | } |