| | | 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 | | bool captureRotation = true) |
| | | 24 | | { |
| | 113398 | 25 | | ChronicleGuard.ThrowIfNull(transform, nameof(transform)); |
| | 113398 | 26 | | FixedTransform? current = transform; |
| | 226901 | 27 | | while (current != null) |
| | | 28 | | { |
| | 113518 | 29 | | Validate(current.LocalScaleXZ); |
| | 113503 | 30 | | current = current.Parent; |
| | | 31 | | } |
| | | 32 | | |
| | 113383 | 33 | | Vector2d worldScale = default; |
| | 113383 | 34 | | worldRotation = default; |
| | 113383 | 35 | | bool captured = transform.TryGetLocalToWorldMatrix(out worldMatrix) |
| | 113383 | 36 | | && TryExtractPlanarTransform( |
| | 113383 | 37 | | worldMatrix, |
| | 113383 | 38 | | out worldScale, |
| | 113383 | 39 | | out worldRotation, |
| | 113383 | 40 | | captureRotation); |
| | 113383 | 41 | | ChronicleGuard.ThrowIfArgument( |
| | 113383 | 42 | | !captured, |
| | 113383 | 43 | | nameof(transform), |
| | 113383 | 44 | | "2D collider transform hierarchy must compose to a representable, nonsheared X/Z transform."); |
| | 113372 | 45 | | Validate(worldScale); |
| | 113372 | 46 | | return worldScale; |
| | | 47 | | } |
| | | 48 | | |
| | | 49 | | internal static Vector3d Capture( |
| | | 50 | | FixedTransform transform, |
| | | 51 | | out Fixed4x4 worldMatrix, |
| | | 52 | | out FixedQuaternion worldRotation) |
| | | 53 | | { |
| | 140119 | 54 | | ChronicleGuard.ThrowIfNull(transform, nameof(transform)); |
| | 140119 | 55 | | ValidateAncestry(transform); |
| | | 56 | | |
| | 140101 | 57 | | Vector3d worldScale = default; |
| | 140101 | 58 | | worldRotation = default; |
| | 140101 | 59 | | bool captured = transform.TryGetLocalToWorldMatrix(out worldMatrix) |
| | 140101 | 60 | | && Fixed4x4.Decompose( |
| | 140101 | 61 | | worldMatrix, |
| | 140101 | 62 | | out _, |
| | 140101 | 63 | | out worldRotation, |
| | 140101 | 64 | | out worldScale); |
| | 140101 | 65 | | ChronicleGuard.ThrowIfArgument( |
| | 140101 | 66 | | !captured, |
| | 140101 | 67 | | nameof(transform), |
| | 140101 | 68 | | "Collider transform hierarchy must compose to a representable, nonsheared transform."); |
| | 140099 | 69 | | Validate(worldScale); |
| | 140099 | 70 | | return worldScale; |
| | | 71 | | } |
| | | 72 | | |
| | | 73 | | internal static Vector3d CaptureScale(FixedTransform transform) |
| | | 74 | | { |
| | 306893 | 75 | | ValidateAncestry(transform); |
| | 306888 | 76 | | if (transform.Parent == null) |
| | 306870 | 77 | | return transform.LocalScale; |
| | | 78 | | |
| | 18 | 79 | | Vector3d worldScale = default; |
| | 18 | 80 | | bool capturedMatrix = |
| | 18 | 81 | | transform.TryGetLocalToWorldMatrix(out Fixed4x4 worldMatrix); |
| | 18 | 82 | | bool capturedScale = Fixed4x4.Decompose( |
| | 18 | 83 | | worldMatrix, |
| | 18 | 84 | | out _, |
| | 18 | 85 | | out _, |
| | 18 | 86 | | out worldScale); |
| | 18 | 87 | | ChronicleGuard.ThrowIfArgument( |
| | 18 | 88 | | !(capturedMatrix & capturedScale), |
| | 18 | 89 | | nameof(transform), |
| | 18 | 90 | | "Collider transform hierarchy must compose to a representable, nonsheared transform."); |
| | 15 | 91 | | Validate(worldScale); |
| | 15 | 92 | | return worldScale; |
| | | 93 | | } |
| | | 94 | | |
| | | 95 | | private static void ValidateAncestry(FixedTransform transform) |
| | | 96 | | { |
| | 447012 | 97 | | FixedTransform? current = transform; |
| | 894051 | 98 | | while (current != null) |
| | | 99 | | { |
| | 447062 | 100 | | Validate(current.LocalScale); |
| | 447039 | 101 | | current = current.Parent; |
| | | 102 | | } |
| | 446989 | 103 | | } |
| | | 104 | | |
| | | 105 | | private static bool TryExtractPlanarTransform( |
| | | 106 | | Fixed4x4 matrix, |
| | | 107 | | out Vector2d scale, |
| | | 108 | | out Fixed64 rotation, |
| | | 109 | | bool captureRotation) |
| | | 110 | | { |
| | | 111 | | // A planar collider requires a block-diagonal X/Z and Y basis. Checking |
| | | 112 | | // both directions prevents scale extraction from silently discarding |
| | | 113 | | // hierarchy coupling that happens not to move local Y=0 points. |
| | 113381 | 114 | | if ((matrix.M12 != Fixed64.Zero) |
| | 113381 | 115 | | | (matrix.M21 != Fixed64.Zero) |
| | 113381 | 116 | | | (matrix.M23 != Fixed64.Zero) |
| | 113381 | 117 | | | (matrix.M32 != Fixed64.Zero)) |
| | | 118 | | { |
| | 5 | 119 | | scale = Vector2d.Zero; |
| | 5 | 120 | | rotation = Fixed64.Zero; |
| | 5 | 121 | | return false; |
| | | 122 | | } |
| | | 123 | | |
| | | 124 | | // The exact unit basis needs neither decomposition nor yaw materialization. |
| | 113376 | 125 | | if ((matrix.M11 == Fixed64.One) & (matrix.M13 == Fixed64.Zero) |
| | 113376 | 126 | | & (matrix.M31 == Fixed64.Zero) & (matrix.M33 == Fixed64.One)) |
| | | 127 | | { |
| | 108407 | 128 | | scale = Vector2d.One; |
| | 108407 | 129 | | rotation = Fixed64.Zero; |
| | 108407 | 130 | | return true; |
| | | 131 | | } |
| | | 132 | | |
| | 4969 | 133 | | Fixed4x4 planarMatrix = Fixed4x4.Identity; |
| | 4969 | 134 | | planarMatrix.M11 = matrix.M11; |
| | 4969 | 135 | | planarMatrix.M13 = matrix.M13; |
| | 4969 | 136 | | planarMatrix.M31 = matrix.M31; |
| | 4969 | 137 | | planarMatrix.M33 = matrix.M33; |
| | 4969 | 138 | | if (!Fixed4x4.Decompose( |
| | 4969 | 139 | | planarMatrix, |
| | 4969 | 140 | | out _, |
| | 4969 | 141 | | out FixedQuaternion planarRotation, |
| | 4969 | 142 | | out Vector3d planarScale) |
| | 4969 | 143 | | || planarScale.X <= Fixed64.Zero) |
| | | 144 | | { |
| | 4 | 145 | | scale = Vector2d.Zero; |
| | 4 | 146 | | rotation = Fixed64.Zero; |
| | 4 | 147 | | return false; |
| | | 148 | | } |
| | | 149 | | |
| | 4965 | 150 | | scale = planarScale.ToVector2d(); |
| | 4965 | 151 | | rotation = Fixed64.Zero; |
| | | 152 | | // Body-owned poses replace host yaw. Keep the same decomposition and |
| | | 153 | | // admission checks, but omit yaw materialization when it is unused. |
| | 4965 | 154 | | if (captureRotation) |
| | | 155 | | { |
| | 171 | 156 | | Vector3d right = planarRotation.Rotate(Vector3d.Right); |
| | 171 | 157 | | rotation = PlanarRotation.Canonicalize( |
| | 171 | 158 | | FixedMath.Atan2(right.Z, right.X)); |
| | | 159 | | } |
| | 4965 | 160 | | return true; |
| | | 161 | | } |
| | | 162 | | |
| | | 163 | | internal static void Validate(Vector2d scale) |
| | | 164 | | { |
| | 228685 | 165 | | ChronicleGuard.ThrowIfArgument( |
| | 228685 | 166 | | !IsPositive(scale), |
| | 228685 | 167 | | nameof(scale), |
| | 228685 | 168 | | "2D collider scale must be greater than zero on the X and Z axes throughout its transform ancestry."); |
| | 228670 | 169 | | } |
| | | 170 | | |
| | | 171 | | internal static void Validate(Vector3d scale) |
| | | 172 | | { |
| | 649129 | 173 | | ChronicleGuard.ThrowIfArgument( |
| | 649129 | 174 | | !IsPositive(scale), |
| | 649129 | 175 | | nameof(scale), |
| | 649129 | 176 | | "3D collider scale must be greater than zero on every axis throughout its transform ancestry."); |
| | 649098 | 177 | | } |
| | | 178 | | |
| | | 179 | | private static bool IsPositive(Vector2d scale) => |
| | 228685 | 180 | | scale.X > Fixed64.Zero && scale.Y > Fixed64.Zero; |
| | | 181 | | |
| | | 182 | | private static bool IsPositive(Vector3d scale) => |
| | 649129 | 183 | | scale.X > Fixed64.Zero |
| | 649129 | 184 | | && scale.Y > Fixed64.Zero |
| | 649129 | 185 | | && scale.Z > Fixed64.Zero; |
| | | 186 | | |
| | | 187 | | internal static Fixed64 Scale( |
| | | 188 | | Fixed64 value, |
| | | 189 | | Fixed64 ownerScale, |
| | | 190 | | Fixed64 partScale) => |
| | 365350 | 191 | | Scale(value, ownerScale, partScale, Fixed64.One); |
| | | 192 | | |
| | | 193 | | internal static Fixed64 Scale( |
| | | 194 | | Fixed64 value, |
| | | 195 | | Fixed64 ownerScale, |
| | | 196 | | Fixed64 partScale, |
| | | 197 | | Fixed64 divisor) |
| | | 198 | | { |
| | 608111 | 199 | | ChronicleGuard.ThrowIfArgument( |
| | 608111 | 200 | | !Fixed64.TryMultiplyDivide(value, ownerScale, partScale, divisor, out Fixed64 result), |
| | 608111 | 201 | | nameof(value), |
| | 608111 | 202 | | "Scaled collider geometry must be representable."); |
| | 608111 | 203 | | return result; |
| | | 204 | | } |
| | | 205 | | |
| | | 206 | | internal static Vector2d Scale( |
| | | 207 | | Vector2d value, |
| | | 208 | | Vector2d ownerScale, |
| | | 209 | | Vector2d partScale) => |
| | 63761 | 210 | | Scale(value, ownerScale, partScale, Fixed64.One); |
| | | 211 | | |
| | | 212 | | internal static Vector2d Scale( |
| | | 213 | | Vector2d value, |
| | | 214 | | Vector2d ownerScale, |
| | | 215 | | Vector2d partScale, |
| | | 216 | | Fixed64 divisor) => |
| | 76012 | 217 | | new( |
| | 76012 | 218 | | Scale(value.X, ownerScale.X, partScale.X, divisor), |
| | 76012 | 219 | | Scale(value.Y, ownerScale.Y, partScale.Y, divisor)); |
| | | 220 | | |
| | | 221 | | internal static Fixed64 ScalePositive( |
| | | 222 | | Fixed64 dimension, |
| | | 223 | | Fixed64 ownerScale, |
| | | 224 | | Fixed64 partScale) |
| | | 225 | | { |
| | 354182 | 226 | | Fixed64 result = Scale(dimension, ownerScale, partScale); |
| | 354182 | 227 | | ChronicleGuard.ThrowIfArgument( |
| | 354182 | 228 | | result <= Fixed64.Zero, |
| | 354182 | 229 | | nameof(dimension), |
| | 354182 | 230 | | "Scaled collider dimensions must remain greater than zero."); |
| | 354176 | 231 | | return result; |
| | | 232 | | } |
| | | 233 | | |
| | | 234 | | internal static Vector2d ScalePositive( |
| | | 235 | | Vector2d dimensions, |
| | | 236 | | Vector2d ownerScale, |
| | | 237 | | Vector2d partScale, |
| | | 238 | | Fixed64 divisor) => |
| | 4228 | 239 | | new( |
| | 4228 | 240 | | ScalePositive(dimensions.X, ownerScale.X, partScale.X, divisor), |
| | 4228 | 241 | | ScalePositive(dimensions.Y, ownerScale.Y, partScale.Y, divisor)); |
| | | 242 | | |
| | | 243 | | internal static Vector3d ScalePositive( |
| | | 244 | | Vector3d dimensions, |
| | | 245 | | Vector3d ownerScale, |
| | | 246 | | Vector3d partScale, |
| | | 247 | | Fixed64 divisor) => |
| | 27427 | 248 | | new( |
| | 27427 | 249 | | ScalePositive(dimensions.X, ownerScale.X, partScale.X, divisor), |
| | 27427 | 250 | | ScalePositive(dimensions.Y, ownerScale.Y, partScale.Y, divisor), |
| | 27427 | 251 | | ScalePositive(dimensions.Z, ownerScale.Z, partScale.Z, divisor)); |
| | | 252 | | |
| | | 253 | | private static Fixed64 ScalePositive( |
| | | 254 | | Fixed64 dimension, |
| | | 255 | | Fixed64 ownerScale, |
| | | 256 | | Fixed64 partScale, |
| | | 257 | | Fixed64 divisor) |
| | | 258 | | { |
| | 90737 | 259 | | Fixed64 result = Scale(dimension, ownerScale, partScale, divisor); |
| | 90737 | 260 | | ChronicleGuard.ThrowIfArgument( |
| | 90737 | 261 | | result <= Fixed64.Zero, |
| | 90737 | 262 | | nameof(dimension), |
| | 90737 | 263 | | "Scaled collider dimensions must remain greater than zero."); |
| | 90737 | 264 | | return result; |
| | | 265 | | } |
| | | 266 | | |
| | | 267 | | } |