| | | 1 | | //======================================================================= |
| | | 2 | | // ContinuousCollisionSweepRange.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; |
| | | 10 | | using System.Runtime.CompilerServices; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.CollisionHandling; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Validates the scalar-distance contract required by continuous sweep queries. |
| | | 16 | | /// </summary> |
| | | 17 | | internal static class ContinuousCollisionSweepRange |
| | | 18 | | { |
| | | 19 | | private const string RangeMessage = |
| | | 20 | | "Continuous collision motion must have a component-exact difference and a representable Euclidean length."; |
| | | 21 | | |
| | | 22 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 23 | | public static Vector2d ValidateEndpoint(Vector2d start, Vector2d end, out Fixed64 length) |
| | | 24 | | { |
| | 1432 | 25 | | if (!Vector2d.TrySubtract(end, start, out Vector2d displacement) |
| | 1432 | 26 | | || !Vector2d.TryGetMagnitude(displacement, out length)) |
| | | 27 | | { |
| | 4 | 28 | | throw new ArgumentOutOfRangeException(nameof(end), RangeMessage); |
| | | 29 | | } |
| | | 30 | | |
| | 1428 | 31 | | return displacement; |
| | | 32 | | } |
| | | 33 | | |
| | | 34 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 35 | | public static Vector3d ValidateEndpoint(Vector3d start, Vector3d end, out Fixed64 length) |
| | | 36 | | { |
| | 20066 | 37 | | if (!Vector3d.TrySubtract(end, start, out Vector3d displacement) |
| | 20066 | 38 | | || !Vector3d.TryGetMagnitude(displacement, out length)) |
| | | 39 | | { |
| | 4 | 40 | | throw new ArgumentOutOfRangeException(nameof(end), RangeMessage); |
| | | 41 | | } |
| | | 42 | | |
| | 20062 | 43 | | return displacement; |
| | | 44 | | } |
| | | 45 | | |
| | | 46 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 47 | | public static Vector2d ValidateEndpoint( |
| | | 48 | | Vector2d start, |
| | | 49 | | Vector2d end, |
| | | 50 | | Vector2d requestedDisplacement, |
| | | 51 | | out Fixed64 length) |
| | | 52 | | { |
| | 581 | 53 | | Vector2d displacement = ValidateEndpoint(start, end, out length); |
| | 579 | 54 | | if (displacement != requestedDisplacement) |
| | 1 | 55 | | throw new ArgumentOutOfRangeException(nameof(end), RangeMessage); |
| | | 56 | | |
| | 578 | 57 | | return displacement; |
| | | 58 | | } |
| | | 59 | | |
| | | 60 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 61 | | public static Vector3d ValidateEndpoint( |
| | | 62 | | Vector3d start, |
| | | 63 | | Vector3d end, |
| | | 64 | | Vector3d requestedDisplacement, |
| | | 65 | | out Fixed64 length) |
| | | 66 | | { |
| | 801 | 67 | | Vector3d displacement = ValidateEndpoint(start, end, out length); |
| | 800 | 68 | | if (displacement != requestedDisplacement) |
| | 1 | 69 | | throw new ArgumentOutOfRangeException(nameof(end), RangeMessage); |
| | | 70 | | |
| | 799 | 71 | | return displacement; |
| | | 72 | | } |
| | | 73 | | |
| | | 74 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 75 | | public static Vector2d ValidateRelativeDisplacement( |
| | | 76 | | Vector2d sourceDisplacement, |
| | | 77 | | Vector2d targetDisplacement, |
| | | 78 | | out Fixed64 length) |
| | | 79 | | { |
| | 313 | 80 | | if (!Vector2d.TrySubtract(sourceDisplacement, targetDisplacement, out Vector2d relativeDisplacement) |
| | 313 | 81 | | || !Vector2d.TryGetMagnitude(relativeDisplacement, out length)) |
| | | 82 | | { |
| | 3 | 83 | | throw new ArgumentOutOfRangeException(nameof(sourceDisplacement), RangeMessage); |
| | | 84 | | } |
| | | 85 | | |
| | 310 | 86 | | return relativeDisplacement; |
| | | 87 | | } |
| | | 88 | | |
| | | 89 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 90 | | public static Vector3d ValidateRelativeDisplacement( |
| | | 91 | | Vector3d sourceDisplacement, |
| | | 92 | | Vector3d targetDisplacement, |
| | | 93 | | out Fixed64 length) |
| | | 94 | | { |
| | 328 | 95 | | if (!Vector3d.TrySubtract(sourceDisplacement, targetDisplacement, out Vector3d relativeDisplacement) |
| | 328 | 96 | | || !Vector3d.TryGetMagnitude(relativeDisplacement, out length)) |
| | | 97 | | { |
| | 3 | 98 | | throw new ArgumentOutOfRangeException(nameof(sourceDisplacement), RangeMessage); |
| | | 99 | | } |
| | | 100 | | |
| | 325 | 101 | | return relativeDisplacement; |
| | | 102 | | } |
| | | 103 | | } |