| | | 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 SwiftCollections; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Queries; |
| | | 12 | | |
| | | 13 | | internal static class PhysicsMixedHitSorter |
| | | 14 | | { |
| | | 15 | | internal static void SortByDistance(SwiftList<PhysicsMixedHit> hits) |
| | | 16 | | { |
| | 7075 | 17 | | SortByDistance(hits, 0, hits.Count); |
| | 7075 | 18 | | } |
| | | 19 | | |
| | | 20 | | internal static void SortByDistance(SwiftList<PhysicsMixedHit> hits, int start, int count) |
| | | 21 | | { |
| | 7075 | 22 | | if (count < 2) |
| | 6998 | 23 | | return; |
| | | 24 | | |
| | 524 | 25 | | for (int root = (count / 2) - 1; root >= 0; root--) |
| | 185 | 26 | | SiftDown(hits, start, root, count); |
| | | 27 | | |
| | 818 | 28 | | for (int end = count - 1; end > 0; end--) |
| | | 29 | | { |
| | 332 | 30 | | Swap(hits, start, start + end); |
| | 332 | 31 | | SiftDown(hits, start, 0, end); |
| | | 32 | | } |
| | 77 | 33 | | } |
| | | 34 | | |
| | 991 | 35 | | internal static bool ComesBefore(PhysicsMixedHit left, PhysicsMixedHit right) => Compare(left, right) < 0; |
| | | 36 | | |
| | | 37 | | private static void SiftDown(SwiftList<PhysicsMixedHit> hits, int start, int root, int count) |
| | | 38 | | { |
| | 548 | 39 | | while (true) |
| | | 40 | | { |
| | 1065 | 41 | | int child = (root * 2) + 1; |
| | 1065 | 42 | | if (child >= count) |
| | 481 | 43 | | return; |
| | | 44 | | |
| | 584 | 45 | | int swapIndex = root; |
| | 584 | 46 | | if (ComesBefore(hits[start + swapIndex], hits[start + child])) |
| | 548 | 47 | | swapIndex = child; |
| | | 48 | | |
| | 584 | 49 | | int right = child + 1; |
| | 584 | 50 | | if (right < count && ComesBefore(hits[start + swapIndex], hits[start + right])) |
| | 183 | 51 | | swapIndex = right; |
| | | 52 | | |
| | 584 | 53 | | if (swapIndex == root) |
| | 36 | 54 | | return; |
| | | 55 | | |
| | 548 | 56 | | Swap(hits, start + root, start + swapIndex); |
| | 548 | 57 | | root = swapIndex; |
| | | 58 | | } |
| | | 59 | | } |
| | | 60 | | |
| | | 61 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 62 | | private static int Compare(PhysicsMixedHit left, PhysicsMixedHit right) |
| | | 63 | | { |
| | 991 | 64 | | int distance = left.Distance.CompareTo(right.Distance); |
| | 991 | 65 | | if (distance != 0) |
| | 984 | 66 | | return distance; |
| | | 67 | | |
| | 7 | 68 | | int left3D = left.Collider3D?.Id ?? -1; |
| | 7 | 69 | | int right3D = right.Collider3D?.Id ?? -1; |
| | 7 | 70 | | int collider3D = left3D.CompareTo(right3D); |
| | 7 | 71 | | if (collider3D != 0) |
| | 3 | 72 | | return collider3D; |
| | | 73 | | |
| | 4 | 74 | | int left2D = left.Collider2D?.Id ?? -1; |
| | 4 | 75 | | int right2D = right.Collider2D?.Id ?? -1; |
| | 4 | 76 | | return left2D.CompareTo(right2D); |
| | | 77 | | } |
| | | 78 | | |
| | | 79 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 80 | | private static void Swap(SwiftList<PhysicsMixedHit> hits, int left, int right) |
| | | 81 | | { |
| | 880 | 82 | | PhysicsMixedHit temp = hits[left]; |
| | 880 | 83 | | hits[left] = hits[right]; |
| | 880 | 84 | | hits[right] = temp; |
| | 880 | 85 | | } |
| | | 86 | | } |