| | | 1 | | //======================================================================= |
| | | 2 | | // Physics3DHitSorter.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 3D hits by distance and deterministic tie breaks. |
| | | 15 | | /// </summary> |
| | | 16 | | internal static class Physics3DHitSorter |
| | | 17 | | { |
| | | 18 | | internal static void SortByDistance(SwiftList<Physics3DHit> hits) |
| | | 19 | | { |
| | 30421 | 20 | | SortByDistance(hits, 0, hits.Count); |
| | 30421 | 21 | | } |
| | | 22 | | |
| | | 23 | | internal static void SortByDistance(SwiftList<Physics3DHit> hits, int start, int count) |
| | | 24 | | { |
| | 30421 | 25 | | if (count < 2) |
| | 28927 | 26 | | return; |
| | | 27 | | |
| | 5996 | 28 | | for (int root = (count / 2) - 1; root >= 0; root--) |
| | 1504 | 29 | | SiftDown(hits, start, root, count); |
| | | 30 | | |
| | 6486 | 31 | | for (int end = count - 1; end > 0; end--) |
| | | 32 | | { |
| | 1749 | 33 | | Swap(hits, start, start + end); |
| | 1749 | 34 | | SiftDown(hits, start, 0, end); |
| | | 35 | | } |
| | 1494 | 36 | | } |
| | | 37 | | |
| | | 38 | | private static void SiftDown(SwiftList<Physics3DHit> hits, int start, int root, int count) |
| | | 39 | | { |
| | 1186 | 40 | | while (true) |
| | | 41 | | { |
| | 4439 | 42 | | int child = (root * 2) + 1; |
| | 4439 | 43 | | if (child >= count) |
| | 2668 | 44 | | return; |
| | | 45 | | |
| | 1771 | 46 | | int swapIndex = root; |
| | 1771 | 47 | | if (ComesBefore(hits[start + swapIndex], hits[start + child])) |
| | 1185 | 48 | | swapIndex = child; |
| | | 49 | | |
| | 1771 | 50 | | int right = child + 1; |
| | 1771 | 51 | | if (right < count && ComesBefore(hits[start + swapIndex], hits[start + right])) |
| | 241 | 52 | | swapIndex = right; |
| | | 53 | | |
| | 1771 | 54 | | if (swapIndex == root) |
| | 585 | 55 | | return; |
| | | 56 | | |
| | 1186 | 57 | | Swap(hits, start + root, start + swapIndex); |
| | 1186 | 58 | | root = swapIndex; |
| | | 59 | | } |
| | | 60 | | } |
| | | 61 | | |
| | | 62 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 63 | | internal static bool ComesBefore(Physics3DHit left, Physics3DHit right) |
| | | 64 | | { |
| | 2492 | 65 | | int distanceCompare = left.Distance.CompareTo(right.Distance); |
| | 2492 | 66 | | if (distanceCompare != 0) |
| | 2225 | 67 | | return distanceCompare < 0; |
| | | 68 | | |
| | 267 | 69 | | return (left.Collider?.Id ?? -1) < (right.Collider?.Id ?? -1); |
| | | 70 | | } |
| | | 71 | | |
| | | 72 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 73 | | private static void Swap(SwiftList<Physics3DHit> hits, int left, int right) |
| | | 74 | | { |
| | 2935 | 75 | | Physics3DHit temp = hits[left]; |
| | 2935 | 76 | | hits[left] = hits[right]; |
| | 2935 | 77 | | hits[right] = temp; |
| | 2935 | 78 | | } |
| | | 79 | | } |