| | | 1 | | //======================================================================= |
| | | 2 | | // SweepBoundsUtility.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 FixedMathSharp; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Queries; |
| | | 12 | | |
| | | 13 | | internal static class SweepBoundsUtility |
| | | 14 | | { |
| | | 15 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 16 | | internal static bool TryClipSegment( |
| | | 17 | | Vector3d start, |
| | | 18 | | Vector3d direction, |
| | | 19 | | Fixed64 length, |
| | | 20 | | Vector3d min, |
| | | 21 | | Vector3d max, |
| | | 22 | | out Fixed64 entry, |
| | | 23 | | out Fixed64 exit) |
| | | 24 | | { |
| | 124 | 25 | | entry = Fixed64.Zero; |
| | 124 | 26 | | exit = length; |
| | 124 | 27 | | bool overlaps = ClipSegmentAxis(start.X, direction.X, min.X, max.X, ref entry, ref exit) |
| | 124 | 28 | | && ClipSegmentAxis(start.Y, direction.Y, min.Y, max.Y, ref entry, ref exit) |
| | 124 | 29 | | && ClipSegmentAxis(start.Z, direction.Z, min.Z, max.Z, ref entry, ref exit); |
| | 124 | 30 | | if (!overlaps) |
| | 16 | 31 | | return false; |
| | | 32 | | |
| | 108 | 33 | | if (entry > exit) |
| | 1 | 34 | | entry = exit = FixedMath.Midpoint(entry, exit); |
| | | 35 | | |
| | 108 | 36 | | return true; |
| | | 37 | | } |
| | | 38 | | |
| | | 39 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 40 | | internal static void CreateSweptBounds( |
| | | 41 | | Vector3d min, |
| | | 42 | | Vector3d max, |
| | | 43 | | Vector3d displacement, |
| | | 44 | | Fixed64 padding, |
| | | 45 | | out Vector3d sweptMin, |
| | | 46 | | out Vector3d sweptMax) |
| | | 47 | | { |
| | 14849 | 48 | | Vector3d endMin = min + displacement; |
| | 14849 | 49 | | Vector3d endMax = max + displacement; |
| | 14849 | 50 | | Vector3d extents = Vector3d.One * padding; |
| | 14849 | 51 | | sweptMin = Vector3d.Min(min, endMin) - extents; |
| | 14849 | 52 | | sweptMax = Vector3d.Max(max, endMax) + extents; |
| | 14849 | 53 | | } |
| | | 54 | | |
| | | 55 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 56 | | internal static void CreateSweptSphereBounds( |
| | | 57 | | Vector3d start, |
| | | 58 | | Vector3d end, |
| | | 59 | | Fixed64 radius, |
| | | 60 | | Fixed64 padding, |
| | | 61 | | out Vector3d sweptMin, |
| | | 62 | | out Vector3d sweptMax) |
| | | 63 | | { |
| | 14479 | 64 | | Vector3d extents = Vector3d.One * (radius + padding); |
| | 14479 | 65 | | sweptMin = Vector3d.Min(start, end) - extents; |
| | 14479 | 66 | | sweptMax = Vector3d.Max(start, end) + extents; |
| | 14479 | 67 | | } |
| | | 68 | | |
| | | 69 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 70 | | internal static bool OverlapsInclusive( |
| | | 71 | | Vector3d firstMin, |
| | | 72 | | Vector3d firstMax, |
| | | 73 | | Vector3d secondMin, |
| | | 74 | | Vector3d secondMax) |
| | | 75 | | { |
| | 10027 | 76 | | return firstMax.X >= secondMin.X |
| | 10027 | 77 | | && firstMin.X <= secondMax.X |
| | 10027 | 78 | | && firstMax.Y >= secondMin.Y |
| | 10027 | 79 | | && firstMin.Y <= secondMax.Y |
| | 10027 | 80 | | && firstMax.Z >= secondMin.Z |
| | 10027 | 81 | | && firstMin.Z <= secondMax.Z; |
| | | 82 | | } |
| | | 83 | | |
| | | 84 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 85 | | private static bool ClipSegmentAxis( |
| | | 86 | | Fixed64 position, |
| | | 87 | | Fixed64 direction, |
| | | 88 | | Fixed64 min, |
| | | 89 | | Fixed64 max, |
| | | 90 | | ref Fixed64 entry, |
| | | 91 | | ref Fixed64 exit) |
| | | 92 | | { |
| | 345 | 93 | | if (direction == Fixed64.Zero) |
| | 224 | 94 | | return position >= min && position <= max; |
| | | 95 | | |
| | 121 | 96 | | Fixed64 first = (min - position) / direction; |
| | 121 | 97 | | Fixed64 second = (max - position) / direction; |
| | 121 | 98 | | if (first > second) |
| | 101 | 99 | | (first, second) = (second, first); |
| | | 100 | | |
| | 121 | 101 | | if (first > entry) |
| | 32 | 102 | | entry = first; |
| | 121 | 103 | | if (second < exit) |
| | 16 | 104 | | exit = second; |
| | 121 | 105 | | return entry <= exit || entry - exit <= Fixed64.Epsilon; |
| | | 106 | | } |
| | | 107 | | } |