| | | 1 | | //======================================================================= |
| | | 2 | | // ColliderGeometryPolicy.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; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | |
| | | 12 | | namespace Gravitas.Colliders; |
| | | 13 | | |
| | | 14 | | /// <summary>Defines the supported effective geometry envelope, independently of world position.</summary> |
| | | 15 | | internal static class ColliderGeometryPolicy |
| | | 16 | | { |
| | 2 | 17 | | internal static readonly Fixed64 MaximumGeometryReach = (Fixed64)4096; |
| | | 18 | | |
| | | 19 | | internal static void ValidateReach(Fixed64 reach, string parameterName) => |
| | 229867 | 20 | | ChronicleGuard.ThrowIfArgument(reach < Fixed64.Zero || reach > MaximumGeometryReach, |
| | 229867 | 21 | | parameterName, "Effective geometry reach must not exceed 4,096 world units."); |
| | | 22 | | |
| | | 23 | | internal static void ValidateReach(Vector2d envelope, string parameterName) => |
| | 3212 | 24 | | ChronicleGuard.ThrowIfArgument(Vector2d.CompareMagnitudeSquared( |
| | 3212 | 25 | | envelope, new Vector2d(MaximumGeometryReach, Fixed64.Zero)) > 0, |
| | 3212 | 26 | | parameterName, "Effective geometry reach must not exceed 4,096 world units."); |
| | | 27 | | |
| | | 28 | | internal static void ValidateFiniteAxisReach(Fixed64 radius, Fixed64 height, string parameterName) => |
| | 550 | 29 | | ChronicleGuard.ThrowIfArgument(Fixed64.CompareMagnitudeSquared( |
| | 550 | 30 | | radius, HalfCeiling(height), radius, Fixed64.Zero, |
| | 550 | 31 | | MaximumGeometryReach, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero) > 0, |
| | 550 | 32 | | parameterName, "Effective finite-axis geometry reach must not exceed 4,096 world units."); |
| | | 33 | | |
| | | 34 | | internal static void ValidateWorldPolygonReach(ReadOnlySpan<Vector2d> vertices) |
| | | 35 | | { |
| | 68 | 36 | | Signed192 sumX = default, sumY = default; |
| | 302 | 37 | | for (int i = 0; i < vertices.Length; i++) |
| | | 38 | | { |
| | 117 | 39 | | sumX = WideArithmetic.AddSigned192(sumX, Signed192.Raw(vertices[i].X)); |
| | 117 | 40 | | sumY = WideArithmetic.AddSigned192(sumY, Signed192.Raw(vertices[i].Y)); |
| | | 41 | | } |
| | | 42 | | // Retain n*v - sum(v): a rounded mean can hide an excess at the cutoff. |
| | | 43 | | // Span counts keep these numerators below 95 bits and their squared sum below 191. |
| | 34 | 44 | | Fixed64 countRaw = Fixed64.FromRaw(vertices.Length); |
| | 34 | 45 | | Signed192 limit = Fixed64.GetExactRawProduct(MaximumGeometryReach, countRaw); |
| | 34 | 46 | | Signed320 limitSquared = WideArithmetic.MultiplySigned192(limit, limit); |
| | 242 | 47 | | for (int i = 0; i < vertices.Length; i++) |
| | | 48 | | { |
| | 102 | 49 | | Signed192 x = WideArithmetic.SubtractSigned192( |
| | 102 | 50 | | Fixed64.GetExactRawProduct(vertices[i].X, countRaw), sumX); |
| | 102 | 51 | | Signed192 y = WideArithmetic.SubtractSigned192( |
| | 102 | 52 | | Fixed64.GetExactRawProduct(vertices[i].Y, countRaw), sumY); |
| | 102 | 53 | | ChronicleGuard.ThrowIfArgument(WideArithmetic.CompareMagnitude( |
| | 102 | 54 | | WideArithmetic.AddProducts(x, x, y, y), limitSquared) > 0, |
| | 102 | 55 | | nameof(vertices), "Effective polygon geometry reach must not exceed 4,096 world units."); |
| | | 56 | | } |
| | 19 | 57 | | } |
| | | 58 | | |
| | | 59 | | internal static Fixed64 Add(Fixed64 attachment, Fixed64 intrinsic) => |
| | 648420 | 60 | | Fixed64.TryAdd(attachment, intrinsic, out Fixed64 reach) ? reach : Fixed64.MaxValue; |
| | | 61 | | |
| | | 62 | | internal static Fixed64 Magnitude(Vector3d vector) => |
| | 25139 | 63 | | vector.TryGetMagnitudeCeiling(out Fixed64 reach) ? reach : Fixed64.MaxValue; |
| | | 64 | | |
| | | 65 | | internal static Fixed64 Magnitude(Vector2d vector) => |
| | 90678 | 66 | | vector.TryGetMagnitudeCeiling(out Fixed64 reach) ? reach : Fixed64.MaxValue; |
| | | 67 | | |
| | | 68 | | internal static Fixed64 Attachment(Vector3d offset, Vector3d scale) => |
| | 221817 | 69 | | offset == Vector3d.Zero ? Fixed64.Zero |
| | 221817 | 70 | | : WideGeometry.TryGetScaledMagnitudeCeiling(offset, scale, out Fixed64 reach) |
| | 221817 | 71 | | ? reach : Fixed64.MaxValue; |
| | | 72 | | |
| | | 73 | | internal static Fixed64 Attachment(Vector2d offset, Vector2d scale) => |
| | 41049 | 74 | | Attachment(new Vector3d(offset.X, offset.Y, Fixed64.Zero), |
| | 41049 | 75 | | new Vector3d(scale.X, scale.Y, Fixed64.One)); |
| | | 76 | | |
| | | 77 | | internal static Fixed64 RotatedReach(Fixed64 reach, bool planar) |
| | | 78 | | { |
| | 339906 | 79 | | if (reach == Fixed64.Zero) |
| | 213308 | 80 | | return Fixed64.Zero; |
| | | 81 | | |
| | | 82 | | // The rational 3D basis preserves norm; final component rounding adds |
| | | 83 | | // less than one raw unit to length. Planar sin/cos has operator norm |
| | | 84 | | // at most 1 + 2e; its scalar product rounding needs another half raw. |
| | 126598 | 85 | | if (planar) |
| | 23276 | 86 | | reach = Fixed64.MultiplyDivide(reach, |
| | 23276 | 87 | | Fixed64.One + Fixed64.Two * FixedMath.CanonicalSinCosErrorBound, |
| | 23276 | 88 | | Fixed64.One, out _); |
| | 126598 | 89 | | return Add(reach, Fixed64.FromRaw(planar ? 2 : 1)); |
| | | 90 | | } |
| | | 91 | | |
| | | 92 | | // Dividing raw lengths outward preserves the half-raw endpoint of an odd core/height. |
| | | 93 | | internal static Fixed64 Capsule(Fixed64 radius, Fixed64 coreLength) => |
| | 31709 | 94 | | Add(radius, HalfCeiling(coreLength)); |
| | | 95 | | |
| | | 96 | | internal static Fixed64 FiniteAxis(Fixed64 radius, Fixed64 height) => |
| | 2082 | 97 | | Magnitude(new Vector3d(radius, HalfCeiling(height), radius)); |
| | | 98 | | |
| | | 99 | | internal static Fixed64 Mixed(Fixed64 planarReach, Fixed64 halfThickness) => |
| | 22852 | 100 | | Magnitude(new Vector2d(planarReach, halfThickness)); |
| | | 101 | | |
| | | 102 | | private static Fixed64 HalfCeiling(Fixed64 length) => |
| | 34341 | 103 | | Fixed64.FromRaw((length.m_rawValue >> 1) + (length.m_rawValue & 1)); |
| | | 104 | | } |