| | | 1 | | //======================================================================= |
| | | 2 | | // ColliderScalePolicy.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 | | |
| | | 10 | | namespace Gravitas.Colliders; |
| | | 11 | | |
| | | 12 | | /// <summary> |
| | | 13 | | /// Owns Gravitas's physical-scale admission boundary. FixedTransform permits |
| | | 14 | | /// signed and zero authored scale; collider geometry requires positive authored |
| | | 15 | | /// ancestry and positive resulting world dimensions on every axis it consumes. |
| | | 16 | | /// </summary> |
| | | 17 | | internal static class ColliderScalePolicy |
| | | 18 | | { |
| | | 19 | | internal static Vector2d CapturePlanar( |
| | | 20 | | FixedTransform transform, |
| | | 21 | | out Fixed4x4 worldMatrix, |
| | | 22 | | out Fixed64 worldRotation) |
| | | 23 | | { |
| | 35683 | 24 | | SwiftThrowHelper.ThrowIfNull(transform, nameof(transform)); |
| | 35683 | 25 | | FixedTransform? current = transform; |
| | 71398 | 26 | | while (current != null) |
| | | 27 | | { |
| | 35727 | 28 | | Validate(current.LocalScaleXZ); |
| | 35715 | 29 | | current = current.Parent; |
| | | 30 | | } |
| | | 31 | | |
| | 35671 | 32 | | Vector2d worldScale = default; |
| | 35671 | 33 | | worldRotation = default; |
| | 35671 | 34 | | bool captured = transform.TryGetLocalToWorldMatrix(out worldMatrix) |
| | 35671 | 35 | | && TryExtractPlanarTransform( |
| | 35671 | 36 | | worldMatrix, |
| | 35671 | 37 | | out worldScale, |
| | 35671 | 38 | | out worldRotation); |
| | 35671 | 39 | | SwiftThrowHelper.ThrowIfArgument( |
| | 35671 | 40 | | !captured, |
| | 35671 | 41 | | nameof(transform), |
| | 35671 | 42 | | "2D collider transform hierarchy must compose to a representable, nonsheared X/Z transform."); |
| | 35666 | 43 | | Validate(worldScale); |
| | 35666 | 44 | | return worldScale; |
| | | 45 | | } |
| | | 46 | | |
| | | 47 | | internal static Vector3d Capture( |
| | | 48 | | FixedTransform transform, |
| | | 49 | | out Fixed4x4 worldMatrix, |
| | | 50 | | out FixedQuaternion worldRotation) |
| | | 51 | | { |
| | 1925 | 52 | | SwiftThrowHelper.ThrowIfNull(transform, nameof(transform)); |
| | 1925 | 53 | | ValidateAncestry(transform); |
| | | 54 | | |
| | 1911 | 55 | | Vector3d worldScale = default; |
| | 1911 | 56 | | worldRotation = default; |
| | 1911 | 57 | | bool captured = transform.TryGetLocalToWorldMatrix(out worldMatrix) |
| | 1911 | 58 | | && Fixed4x4.Decompose( |
| | 1911 | 59 | | worldMatrix, |
| | 1911 | 60 | | out _, |
| | 1911 | 61 | | out worldRotation, |
| | 1911 | 62 | | out worldScale); |
| | 1911 | 63 | | SwiftThrowHelper.ThrowIfArgument( |
| | 1911 | 64 | | !captured, |
| | 1911 | 65 | | nameof(transform), |
| | 1911 | 66 | | "Collider transform hierarchy must compose to a representable, nonsheared transform."); |
| | 1909 | 67 | | Validate(worldScale); |
| | 1909 | 68 | | return worldScale; |
| | | 69 | | } |
| | | 70 | | |
| | | 71 | | internal static Vector3d CaptureScale(FixedTransform transform) |
| | | 72 | | { |
| | 72900 | 73 | | ValidateAncestry(transform); |
| | 72895 | 74 | | if (transform.Parent == null) |
| | 72884 | 75 | | return transform.LocalScale; |
| | | 76 | | |
| | 11 | 77 | | Vector3d worldScale = default; |
| | 11 | 78 | | bool capturedMatrix = |
| | 11 | 79 | | transform.TryGetLocalToWorldMatrix(out Fixed4x4 worldMatrix); |
| | 11 | 80 | | bool capturedScale = Fixed4x4.Decompose( |
| | 11 | 81 | | worldMatrix, |
| | 11 | 82 | | out _, |
| | 11 | 83 | | out _, |
| | 11 | 84 | | out worldScale); |
| | 11 | 85 | | SwiftThrowHelper.ThrowIfArgument( |
| | 11 | 86 | | !(capturedMatrix & capturedScale), |
| | 11 | 87 | | nameof(transform), |
| | 11 | 88 | | "Collider transform hierarchy must compose to a representable, nonsheared transform."); |
| | 8 | 89 | | Validate(worldScale); |
| | 8 | 90 | | return worldScale; |
| | | 91 | | } |
| | | 92 | | |
| | | 93 | | private static void ValidateAncestry(FixedTransform transform) |
| | | 94 | | { |
| | 74825 | 95 | | FixedTransform? current = transform; |
| | 149668 | 96 | | while (current != null) |
| | | 97 | | { |
| | 74862 | 98 | | Validate(current.LocalScale); |
| | 74843 | 99 | | current = current.Parent; |
| | | 100 | | } |
| | 74806 | 101 | | } |
| | | 102 | | |
| | | 103 | | private static bool TryExtractPlanarTransform( |
| | | 104 | | Fixed4x4 matrix, |
| | | 105 | | out Vector2d scale, |
| | | 106 | | out Fixed64 rotation) |
| | | 107 | | { |
| | | 108 | | // A planar collider requires a block-diagonal X/Z and Y basis. Checking |
| | | 109 | | // both directions prevents scale extraction from silently discarding |
| | | 110 | | // hierarchy coupling that happens not to move local Y=0 points. |
| | 35670 | 111 | | if ((matrix.M12 != Fixed64.Zero) |
| | 35670 | 112 | | | (matrix.M21 != Fixed64.Zero) |
| | 35670 | 113 | | | (matrix.M23 != Fixed64.Zero) |
| | 35670 | 114 | | | (matrix.M32 != Fixed64.Zero)) |
| | | 115 | | { |
| | 2 | 116 | | scale = Vector2d.Zero; |
| | 2 | 117 | | rotation = Fixed64.Zero; |
| | 2 | 118 | | return false; |
| | | 119 | | } |
| | | 120 | | |
| | 35668 | 121 | | Fixed4x4 planarMatrix = Fixed4x4.Identity; |
| | 35668 | 122 | | planarMatrix.M11 = matrix.M11; |
| | 35668 | 123 | | planarMatrix.M13 = matrix.M13; |
| | 35668 | 124 | | planarMatrix.M31 = matrix.M31; |
| | 35668 | 125 | | planarMatrix.M33 = matrix.M33; |
| | 35668 | 126 | | if (!Fixed4x4.Decompose( |
| | 35668 | 127 | | planarMatrix, |
| | 35668 | 128 | | out _, |
| | 35668 | 129 | | out FixedQuaternion planarRotation, |
| | 35668 | 130 | | out Vector3d planarScale) |
| | 35668 | 131 | | || planarScale.X <= Fixed64.Zero) |
| | | 132 | | { |
| | 2 | 133 | | scale = Vector2d.Zero; |
| | 2 | 134 | | rotation = Fixed64.Zero; |
| | 2 | 135 | | return false; |
| | | 136 | | } |
| | | 137 | | |
| | 35666 | 138 | | scale = planarScale.ToVector2d(); |
| | 35666 | 139 | | Vector3d right = planarRotation.Rotate(Vector3d.Right); |
| | 35666 | 140 | | rotation = PlanarRotation.Canonicalize( |
| | 35666 | 141 | | FixedMath.Atan2(right.Z, right.X)); |
| | 35666 | 142 | | return true; |
| | | 143 | | } |
| | | 144 | | |
| | | 145 | | internal static void Validate(Vector2d scale) |
| | | 146 | | { |
| | 71619 | 147 | | SwiftThrowHelper.ThrowIfArgument( |
| | 71619 | 148 | | !IsPositive(scale), |
| | 71619 | 149 | | nameof(scale), |
| | 71619 | 150 | | "2D collider scale must be greater than zero on the X and Z axes throughout its transform ancestry."); |
| | 71607 | 151 | | } |
| | | 152 | | |
| | | 153 | | internal static void Validate(Vector3d scale) |
| | | 154 | | { |
| | 79209 | 155 | | SwiftThrowHelper.ThrowIfArgument( |
| | 79209 | 156 | | !IsPositive(scale), |
| | 79209 | 157 | | nameof(scale), |
| | 79209 | 158 | | "3D collider scale must be greater than zero on every axis throughout its transform ancestry."); |
| | 79188 | 159 | | } |
| | | 160 | | |
| | | 161 | | private static bool IsPositive(Vector2d scale) => |
| | 71619 | 162 | | scale.X > Fixed64.Zero && scale.Y > Fixed64.Zero; |
| | | 163 | | |
| | | 164 | | private static bool IsPositive(Vector3d scale) => |
| | 79209 | 165 | | scale.X > Fixed64.Zero |
| | 79209 | 166 | | && scale.Y > Fixed64.Zero |
| | 79209 | 167 | | && scale.Z > Fixed64.Zero; |
| | | 168 | | |
| | | 169 | | internal static Fixed64 Scale( |
| | | 170 | | Fixed64 value, |
| | | 171 | | Fixed64 ownerScale, |
| | | 172 | | Fixed64 partScale) => |
| | 109029 | 173 | | Scale(value, ownerScale, partScale, Fixed64.One); |
| | | 174 | | |
| | | 175 | | internal static Fixed64 Scale( |
| | | 176 | | Fixed64 value, |
| | | 177 | | Fixed64 ownerScale, |
| | | 178 | | Fixed64 partScale, |
| | | 179 | | Fixed64 divisor) |
| | | 180 | | { |
| | 207555 | 181 | | SwiftThrowHelper.ThrowIfArgument( |
| | 207555 | 182 | | !Fixed64.TryMultiplyDivide(value, ownerScale, partScale, divisor, out Fixed64 result), |
| | 207555 | 183 | | nameof(value), |
| | 207555 | 184 | | "Scaled collider geometry must be representable."); |
| | 207555 | 185 | | return result; |
| | | 186 | | } |
| | | 187 | | |
| | | 188 | | internal static Vector2d Scale( |
| | | 189 | | Vector2d value, |
| | | 190 | | Vector2d ownerScale, |
| | | 191 | | Vector2d partScale) => |
| | 29172 | 192 | | Scale(value, ownerScale, partScale, Fixed64.One); |
| | | 193 | | |
| | | 194 | | internal static Vector2d Scale( |
| | | 195 | | Vector2d value, |
| | | 196 | | Vector2d ownerScale, |
| | | 197 | | Vector2d partScale, |
| | | 198 | | Fixed64 divisor) => |
| | 36483 | 199 | | new( |
| | 36483 | 200 | | Scale(value.X, ownerScale.X, partScale.X, divisor), |
| | 36483 | 201 | | Scale(value.Y, ownerScale.Y, partScale.Y, divisor)); |
| | | 202 | | |
| | | 203 | | internal static Fixed64 ScalePositive( |
| | | 204 | | Fixed64 dimension, |
| | | 205 | | Fixed64 ownerScale, |
| | | 206 | | Fixed64 partScale) |
| | | 207 | | { |
| | 84215 | 208 | | Fixed64 result = Scale(dimension, ownerScale, partScale); |
| | 84215 | 209 | | SwiftThrowHelper.ThrowIfArgument( |
| | 84215 | 210 | | result <= Fixed64.Zero, |
| | 84215 | 211 | | nameof(dimension), |
| | 84215 | 212 | | "Scaled collider dimensions must remain greater than zero."); |
| | 84210 | 213 | | return result; |
| | | 214 | | } |
| | | 215 | | |
| | | 216 | | internal static Vector2d ScalePositive( |
| | | 217 | | Vector2d dimensions, |
| | | 218 | | Vector2d ownerScale, |
| | | 219 | | Vector2d partScale, |
| | | 220 | | Fixed64 divisor) => |
| | 882 | 221 | | new( |
| | 882 | 222 | | ScalePositive(dimensions.X, ownerScale.X, partScale.X, divisor), |
| | 882 | 223 | | ScalePositive(dimensions.Y, ownerScale.Y, partScale.Y, divisor)); |
| | | 224 | | |
| | | 225 | | internal static Vector3d ScalePositive( |
| | | 226 | | Vector3d dimensions, |
| | | 227 | | Vector3d ownerScale, |
| | | 228 | | Vector3d partScale, |
| | | 229 | | Fixed64 divisor) => |
| | 7932 | 230 | | new( |
| | 7932 | 231 | | ScalePositive(dimensions.X, ownerScale.X, partScale.X, divisor), |
| | 7932 | 232 | | ScalePositive(dimensions.Y, ownerScale.Y, partScale.Y, divisor), |
| | 7932 | 233 | | ScalePositive(dimensions.Z, ownerScale.Z, partScale.Z, divisor)); |
| | | 234 | | |
| | | 235 | | private static Fixed64 ScalePositive( |
| | | 236 | | Fixed64 dimension, |
| | | 237 | | Fixed64 ownerScale, |
| | | 238 | | Fixed64 partScale, |
| | | 239 | | Fixed64 divisor) |
| | | 240 | | { |
| | 25560 | 241 | | Fixed64 result = Scale(dimension, ownerScale, partScale, divisor); |
| | 25560 | 242 | | SwiftThrowHelper.ThrowIfArgument( |
| | 25560 | 243 | | result <= Fixed64.Zero, |
| | 25560 | 244 | | nameof(dimension), |
| | 25560 | 245 | | "Scaled collider dimensions must remain greater than zero."); |
| | 25560 | 246 | | return result; |
| | | 247 | | } |
| | | 248 | | |
| | | 249 | | } |