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