| | | 1 | | //======================================================================= |
| | | 2 | | // ColliderCanonicalBounds.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 FixedMathSharp.Geometry; |
| | | 10 | | |
| | | 11 | | namespace Gravitas.Colliders; |
| | | 12 | | |
| | | 13 | | /// <summary> |
| | | 14 | | /// Contains methods for computing canonical bounds of colliders, |
| | | 15 | | /// including proxy radii and relative bounds, based on collider type and properties. |
| | | 16 | | /// </summary> |
| | | 17 | | internal static class ColliderCanonicalBounds |
| | | 18 | | { |
| | | 19 | | internal static Fixed64 GetCurrentCenteredProxyRadius( |
| | | 20 | | LSCollider collider) |
| | | 21 | | { |
| | | 22 | | switch (collider) |
| | | 23 | | { |
| | | 24 | | case LSSphereCollider: |
| | 2 | 25 | | return collider.GetCurrentScaledRadius(); |
| | | 26 | | case LSCapsuleCollider capsule: |
| | 2 | 27 | | return GetCurrentCapsuleCenteredProxyRadius(capsule); |
| | | 28 | | case LSCuboidCollider cuboid: |
| | 3 | 29 | | return GetCurrentCuboidCenteredProxyRadius(cuboid); |
| | | 30 | | case LSCylinderCollider cylinder: |
| | 2 | 31 | | return GetCurrentFiniteAxisCenteredProxyRadius( |
| | 2 | 32 | | cylinder, |
| | 2 | 33 | | cylinder.Size.Y); |
| | | 34 | | case LSConeCollider cone: |
| | 1 | 35 | | return GetCurrentFiniteAxisCenteredProxyRadius( |
| | 1 | 36 | | cone, |
| | 1 | 37 | | cone.Size.Y); |
| | | 38 | | case LSMeshCollider mesh: |
| | | 39 | | { |
| | 2 | 40 | | mesh.GetCurrentShapeScales( |
| | 2 | 41 | | out Vector3d ownerScale, |
| | 2 | 42 | | out Vector3d partScale); |
| | 2 | 43 | | return mesh.Mesh.GetScaledLocalRadius( |
| | 2 | 44 | | ownerScale, |
| | 2 | 45 | | partScale); |
| | | 46 | | } |
| | | 47 | | case LSCompoundCollider compound: |
| | 6 | 48 | | return GetCurrentCompoundCenteredProxyRadius( |
| | 6 | 49 | | compound); |
| | | 50 | | default: |
| | 1 | 51 | | return Fixed64.MaxValue; |
| | | 52 | | } |
| | | 53 | | } |
| | | 54 | | |
| | | 55 | | internal static FixedBoundBox GetRelativeBounds( |
| | | 56 | | LSCollider collider, |
| | | 57 | | Vector3d referenceOrigin, |
| | | 58 | | FixedQuaternion referenceRotation = default) |
| | | 59 | | { |
| | 8405 | 60 | | if (referenceRotation == default) |
| | 8086 | 61 | | referenceRotation = FixedQuaternion.Identity; |
| | 8405 | 62 | | if (collider is LSCompoundCollider compound) |
| | | 63 | | { |
| | 165 | 64 | | return GetCompoundRelativeBounds( |
| | 165 | 65 | | compound, |
| | 165 | 66 | | referenceOrigin, |
| | 165 | 67 | | referenceRotation); |
| | | 68 | | } |
| | 8240 | 69 | | if (!TryGetLocalBounds( |
| | 8240 | 70 | | collider, |
| | 8240 | 71 | | out Vector3d localMin, |
| | 8240 | 72 | | out Vector3d localMax)) |
| | | 73 | | { |
| | 1 | 74 | | return FixedBoundBox.FromMinMax( |
| | 1 | 75 | | new Vector3d( |
| | 1 | 76 | | Fixed64.MinValue, |
| | 1 | 77 | | Fixed64.MinValue, |
| | 1 | 78 | | Fixed64.MinValue), |
| | 1 | 79 | | new Vector3d( |
| | 1 | 80 | | Fixed64.MaxValue, |
| | 1 | 81 | | Fixed64.MaxValue, |
| | 1 | 82 | | Fixed64.MaxValue)); |
| | | 83 | | } |
| | 8239 | 84 | | Vector3d sourceOrigin = collider is LSMeshCollider mesh |
| | 8239 | 85 | | ? mesh.Mesh.Origin |
| | 8239 | 86 | | : collider.CanonicalCenter; |
| | 8239 | 87 | | FixedQuaternion sourceRotation = collider is LSMeshCollider meshCollider |
| | 8239 | 88 | | ? meshCollider.Mesh.Rotation |
| | 8239 | 89 | | : collider.CanonicalRotation; |
| | 8239 | 90 | | return FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain( |
| | 8239 | 91 | | sourceOrigin, |
| | 8239 | 92 | | sourceRotation, |
| | 8239 | 93 | | localMin, |
| | 8239 | 94 | | localMax, |
| | 8239 | 95 | | referenceOrigin, |
| | 8239 | 96 | | referenceRotation); |
| | | 97 | | } |
| | | 98 | | |
| | | 99 | | internal static Fixed64 GetCenteredProxyRadius( |
| | | 100 | | LSCollider collider) |
| | | 101 | | { |
| | | 102 | | switch (collider) |
| | | 103 | | { |
| | | 104 | | case LSSphereCollider sphere: |
| | 21759 | 105 | | return sphere.ScaledRadius; |
| | | 106 | | case LSCapsuleCollider capsule: |
| | | 107 | | { |
| | 575 | 108 | | FixedBoundBox localBounds = |
| | 575 | 109 | | FixedBoundBox.FromCenteredCapsuleClippedToDomain( |
| | 575 | 110 | | Vector3d.Zero, |
| | 575 | 111 | | Vector3d.Up, |
| | 575 | 112 | | capsule.AxisLength, |
| | 575 | 113 | | capsule.ScaledRadius); |
| | 575 | 114 | | return localBounds.Max.Y; |
| | | 115 | | } |
| | | 116 | | case LSCuboidCollider cuboid: |
| | 7920 | 117 | | return cuboid.OrientedBox.HalfExtents |
| | 7920 | 118 | | .TryGetMagnitudeCeiling( |
| | 7920 | 119 | | out Fixed64 cuboidRadius) |
| | 7920 | 120 | | ? cuboidRadius |
| | 7920 | 121 | | : Fixed64.MaxValue; |
| | | 122 | | case LSCylinderCollider cylinder: |
| | 578 | 123 | | return GetFiniteAxisLocalRadius( |
| | 578 | 124 | | FixedBoundBox.FromCenteredFiniteCylinderClippedToDomain( |
| | 578 | 125 | | Vector3d.Zero, |
| | 578 | 126 | | Vector3d.Up, |
| | 578 | 127 | | cylinder.Height, |
| | 578 | 128 | | cylinder.ScaledRadius)); |
| | | 129 | | case LSConeCollider cone: |
| | 530 | 130 | | return GetFiniteAxisLocalRadius( |
| | 530 | 131 | | FixedBoundBox.FromCenteredFiniteConeClippedToDomain( |
| | 530 | 132 | | Vector3d.Zero, |
| | 530 | 133 | | Vector3d.Up, |
| | 530 | 134 | | cone.Height, |
| | 530 | 135 | | cone.ScaledRadius)); |
| | | 136 | | case LSMeshCollider mesh: |
| | 284 | 137 | | return mesh.Mesh.ScaledLocalRadius; |
| | | 138 | | case LSCompoundCollider compound: |
| | 165 | 139 | | return GetCompoundCenteredProxyRadius(compound); |
| | | 140 | | default: |
| | 69 | 141 | | return Fixed64.MaxValue; |
| | | 142 | | } |
| | | 143 | | } |
| | | 144 | | |
| | | 145 | | internal static Fixed64 GetGroundProbeRadius( |
| | | 146 | | LSCollider collider) |
| | | 147 | | { |
| | 31878 | 148 | | if (collider is LSSphereCollider sphere) |
| | 21759 | 149 | | return sphere.ScaledRadius; |
| | 10119 | 150 | | if (collider is LSCapsuleCollider capsule) |
| | 575 | 151 | | return capsule.ScaledRadius; |
| | 9544 | 152 | | if (collider is LSCylinderCollider cylinder) |
| | 578 | 153 | | return cylinder.ScaledRadius; |
| | 8966 | 154 | | if (collider is not (LSCuboidCollider or LSCompoundCollider)) |
| | 882 | 155 | | return Fixed64.Zero; |
| | | 156 | | |
| | 8084 | 157 | | Vector3d extents = GetMaximumAbsoluteExtents( |
| | 8084 | 158 | | collider, |
| | 8084 | 159 | | collider.CanonicalCenter); |
| | 8084 | 160 | | return FixedMath.Min(extents.X, extents.Z); |
| | | 161 | | } |
| | | 162 | | |
| | | 163 | | internal static Vector3d GetMaximumAbsoluteExtents( |
| | | 164 | | LSCollider collider, |
| | | 165 | | Vector3d referenceOrigin) |
| | | 166 | | { |
| | 8085 | 167 | | FixedBoundBox bounds = GetRelativeBounds( |
| | 8085 | 168 | | collider, |
| | 8085 | 169 | | referenceOrigin); |
| | 8085 | 170 | | return new Vector3d( |
| | 8085 | 171 | | FixedMath.Max(bounds.Min.X.Abs(), bounds.Max.X.Abs()), |
| | 8085 | 172 | | FixedMath.Max(bounds.Min.Y.Abs(), bounds.Max.Y.Abs()), |
| | 8085 | 173 | | FixedMath.Max(bounds.Min.Z.Abs(), bounds.Max.Z.Abs())); |
| | | 174 | | } |
| | | 175 | | |
| | | 176 | | private static FixedBoundBox GetCompoundRelativeBounds( |
| | | 177 | | LSCompoundCollider compound, |
| | | 178 | | Vector3d referenceOrigin, |
| | | 179 | | FixedQuaternion referenceRotation) |
| | | 180 | | { |
| | 165 | 181 | | FixedBoundBox bounds = GetRelativeBounds( |
| | 165 | 182 | | compound.GetPartCollider(0), |
| | 165 | 183 | | referenceOrigin, |
| | 165 | 184 | | referenceRotation); |
| | 165 | 185 | | Vector3d minimum = bounds.Min; |
| | 165 | 186 | | Vector3d maximum = bounds.Max; |
| | 614 | 187 | | for (int index = 1; index < compound.PartCount; index++) |
| | | 188 | | { |
| | 142 | 189 | | bounds = GetRelativeBounds( |
| | 142 | 190 | | compound.GetPartCollider(index), |
| | 142 | 191 | | referenceOrigin, |
| | 142 | 192 | | referenceRotation); |
| | 142 | 193 | | minimum = Vector3d.Min(minimum, bounds.Min); |
| | 142 | 194 | | maximum = Vector3d.Max(maximum, bounds.Max); |
| | | 195 | | } |
| | | 196 | | |
| | 165 | 197 | | return FixedBoundBox.FromMinMax(minimum, maximum); |
| | | 198 | | } |
| | | 199 | | |
| | | 200 | | private static bool TryGetLocalBounds( |
| | | 201 | | LSCollider collider, |
| | | 202 | | out Vector3d minimum, |
| | | 203 | | out Vector3d maximum) |
| | | 204 | | { |
| | | 205 | | Vector3d halfExtents; |
| | | 206 | | switch (collider) |
| | | 207 | | { |
| | | 208 | | case LSSphereCollider sphere: |
| | 162 | 209 | | halfExtents = Vector3d.One * sphere.ScaledRadius; |
| | 162 | 210 | | break; |
| | | 211 | | case LSCapsuleCollider capsule: |
| | | 212 | | { |
| | 23 | 213 | | FixedBoundBox bounds = |
| | 23 | 214 | | FixedBoundBox.FromCenteredCapsuleClippedToDomain( |
| | 23 | 215 | | Vector3d.Zero, |
| | 23 | 216 | | Vector3d.Up, |
| | 23 | 217 | | capsule.AxisLength, |
| | 23 | 218 | | capsule.ScaledRadius); |
| | 23 | 219 | | minimum = bounds.Min; |
| | 23 | 220 | | maximum = bounds.Max; |
| | 23 | 221 | | return true; |
| | | 222 | | } |
| | | 223 | | case LSCuboidCollider cuboid: |
| | 7965 | 224 | | halfExtents = cuboid.OrientedBox.HalfExtents; |
| | 7965 | 225 | | break; |
| | | 226 | | case LSCylinderCollider cylinder: |
| | | 227 | | { |
| | 23 | 228 | | FixedBoundBox bounds = |
| | 23 | 229 | | FixedBoundBox.FromCenteredFiniteCylinderClippedToDomain( |
| | 23 | 230 | | Vector3d.Zero, |
| | 23 | 231 | | Vector3d.Up, |
| | 23 | 232 | | cylinder.Height, |
| | 23 | 233 | | cylinder.ScaledRadius); |
| | 23 | 234 | | minimum = bounds.Min; |
| | 23 | 235 | | maximum = bounds.Max; |
| | 23 | 236 | | return true; |
| | | 237 | | } |
| | | 238 | | case LSConeCollider cone: |
| | | 239 | | { |
| | 27 | 240 | | FixedBoundBox bounds = |
| | 27 | 241 | | FixedBoundBox.FromCenteredFiniteConeClippedToDomain( |
| | 27 | 242 | | Vector3d.Zero, |
| | 27 | 243 | | Vector3d.Up, |
| | 27 | 244 | | cone.Height, |
| | 27 | 245 | | cone.ScaledRadius); |
| | 27 | 246 | | minimum = bounds.Min; |
| | 27 | 247 | | maximum = bounds.Max; |
| | 27 | 248 | | return true; |
| | | 249 | | } |
| | | 250 | | case LSMeshCollider mesh: |
| | 39 | 251 | | minimum = mesh.Mesh.ScaledLocalBounds.Min; |
| | 39 | 252 | | maximum = mesh.Mesh.ScaledLocalBounds.Max; |
| | 39 | 253 | | return true; |
| | | 254 | | default: |
| | 1 | 255 | | minimum = default; |
| | 1 | 256 | | maximum = default; |
| | 1 | 257 | | return false; |
| | | 258 | | } |
| | | 259 | | |
| | 8127 | 260 | | minimum = -halfExtents; |
| | 8127 | 261 | | maximum = halfExtents; |
| | 8127 | 262 | | return true; |
| | | 263 | | } |
| | | 264 | | |
| | | 265 | | private static Fixed64 GetFiniteAxisLocalRadius( |
| | | 266 | | FixedBoundBox localBounds) |
| | | 267 | | { |
| | 1111 | 268 | | Vector3d extents = new( |
| | 1111 | 269 | | FixedMath.Max(localBounds.Min.X.Abs(), localBounds.Max.X.Abs()), |
| | 1111 | 270 | | FixedMath.Max(localBounds.Min.Y.Abs(), localBounds.Max.Y.Abs()), |
| | 1111 | 271 | | FixedMath.Max(localBounds.Min.Z.Abs(), localBounds.Max.Z.Abs())); |
| | 1111 | 272 | | return extents.TryGetMagnitudeCeiling( |
| | 1111 | 273 | | out Fixed64 radius) |
| | 1111 | 274 | | ? radius |
| | 1111 | 275 | | : Fixed64.MaxValue; |
| | | 276 | | } |
| | | 277 | | |
| | | 278 | | private static Fixed64 GetCurrentCuboidCenteredProxyRadius( |
| | | 279 | | LSCuboidCollider cuboid) |
| | | 280 | | { |
| | 3 | 281 | | cuboid.GetCurrentShapeScales( |
| | 3 | 282 | | out Vector3d ownerScale, |
| | 3 | 283 | | out Vector3d partScale); |
| | 3 | 284 | | Vector3d halfExtents = |
| | 3 | 285 | | ColliderScalePolicy.ScalePositive( |
| | 3 | 286 | | cuboid.Size, |
| | 3 | 287 | | ownerScale, |
| | 3 | 288 | | partScale, |
| | 3 | 289 | | Fixed64.Two); |
| | 3 | 290 | | return halfExtents.TryGetMagnitudeCeiling( |
| | 3 | 291 | | out Fixed64 radius) |
| | 3 | 292 | | ? radius |
| | 3 | 293 | | : Fixed64.MaxValue; |
| | | 294 | | } |
| | | 295 | | |
| | | 296 | | private static Fixed64 GetCurrentCapsuleCenteredProxyRadius( |
| | | 297 | | LSCapsuleCollider capsule) |
| | | 298 | | { |
| | 2 | 299 | | capsule.GetCurrentShapeScales( |
| | 2 | 300 | | out Vector3d ownerScale, |
| | 2 | 301 | | out Vector3d partScale); |
| | 2 | 302 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 2 | 303 | | capsule.Radius, |
| | 2 | 304 | | ownerScale.X, |
| | 2 | 305 | | partScale.X); |
| | 2 | 306 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 2 | 307 | | capsule.Radius, |
| | 2 | 308 | | ownerScale.Z, |
| | 2 | 309 | | partScale.Z); |
| | 2 | 310 | | if (!Fixed64.TryMultiplySubtractClamped( |
| | 2 | 311 | | capsule.Size.Y, |
| | 2 | 312 | | ownerScale.Y, |
| | 2 | 313 | | partScale.Y, |
| | 2 | 314 | | Fixed64.One, |
| | 2 | 315 | | capsule.Radius, |
| | 2 | 316 | | Fixed64.Two, |
| | 2 | 317 | | ownerScale.X, |
| | 2 | 318 | | partScale.X, |
| | 2 | 319 | | out Fixed64 axisLengthX) |
| | 2 | 320 | | || !Fixed64.TryMultiplySubtractClamped( |
| | 2 | 321 | | capsule.Size.Y, |
| | 2 | 322 | | ownerScale.Y, |
| | 2 | 323 | | partScale.Y, |
| | 2 | 324 | | Fixed64.One, |
| | 2 | 325 | | capsule.Radius, |
| | 2 | 326 | | Fixed64.Two, |
| | 2 | 327 | | ownerScale.Z, |
| | 2 | 328 | | partScale.Z, |
| | 2 | 329 | | out Fixed64 axisLengthZ)) |
| | | 330 | | { |
| | 1 | 331 | | return Fixed64.MaxValue; |
| | | 332 | | } |
| | | 333 | | |
| | 1 | 334 | | FixedBoundBox localBounds = |
| | 1 | 335 | | FixedBoundBox.FromCenteredCapsuleClippedToDomain( |
| | 1 | 336 | | Vector3d.Zero, |
| | 1 | 337 | | Vector3d.Up, |
| | 1 | 338 | | FixedMath.Min(axisLengthX, axisLengthZ), |
| | 1 | 339 | | FixedMath.Max(radiusX, radiusZ)); |
| | 1 | 340 | | return localBounds.Max.Y; |
| | | 341 | | } |
| | | 342 | | |
| | | 343 | | private static Fixed64 GetCurrentFiniteAxisCenteredProxyRadius( |
| | | 344 | | LSCollider collider, |
| | | 345 | | Fixed64 authoredHeight) |
| | | 346 | | { |
| | 3 | 347 | | collider.GetCurrentShapeScales( |
| | 3 | 348 | | out Vector3d ownerScale, |
| | 3 | 349 | | out Vector3d partScale); |
| | 3 | 350 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 3 | 351 | | collider.Radius, |
| | 3 | 352 | | ownerScale.X, |
| | 3 | 353 | | partScale.X); |
| | 3 | 354 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 3 | 355 | | collider.Radius, |
| | 3 | 356 | | ownerScale.Z, |
| | 3 | 357 | | partScale.Z); |
| | 3 | 358 | | Fixed64 height = ColliderScalePolicy.ScalePositive( |
| | 3 | 359 | | authoredHeight, |
| | 3 | 360 | | ownerScale.Y, |
| | 3 | 361 | | partScale.Y); |
| | 3 | 362 | | return GetFiniteAxisLocalRadius( |
| | 3 | 363 | | FixedBoundBox.FromCenteredFiniteCylinderClippedToDomain( |
| | 3 | 364 | | Vector3d.Zero, |
| | 3 | 365 | | Vector3d.Up, |
| | 3 | 366 | | height, |
| | 3 | 367 | | FixedMath.Max(radiusX, radiusZ))); |
| | | 368 | | } |
| | | 369 | | |
| | | 370 | | private static Fixed64 GetCompoundCenteredProxyRadius( |
| | | 371 | | LSCompoundCollider compound) |
| | | 372 | | { |
| | 165 | 373 | | Fixed64 bestRadius = Fixed64.Zero; |
| | 916 | 374 | | for (int index = 0; index < compound.PartCount; index++) |
| | | 375 | | { |
| | 307 | 376 | | LSCollider part = compound.GetPartCollider(index); |
| | 307 | 377 | | bool offsetResolved = Vector3d.TrySubtract( |
| | 307 | 378 | | part.CanonicalCenter, |
| | 307 | 379 | | compound.CanonicalCenter, |
| | 307 | 380 | | out Vector3d offset); |
| | 307 | 381 | | bool distanceResolved = |
| | 307 | 382 | | offset.TryGetMagnitudeCeiling( |
| | 307 | 383 | | out Fixed64 distance); |
| | 307 | 384 | | bool radiusResolved = Fixed64.TryAdd( |
| | 307 | 385 | | distance, |
| | 307 | 386 | | part.CanonicalCenteredProxyRadius, |
| | 307 | 387 | | out Fixed64 radius); |
| | 307 | 388 | | if (!(offsetResolved & distanceResolved & radiusResolved)) |
| | 14 | 389 | | return Fixed64.MaxValue; |
| | | 390 | | |
| | 293 | 391 | | bestRadius = FixedMath.Max(bestRadius, radius); |
| | | 392 | | } |
| | | 393 | | |
| | 151 | 394 | | return bestRadius; |
| | | 395 | | } |
| | | 396 | | |
| | | 397 | | private static Fixed64 GetCurrentCompoundCenteredProxyRadius( |
| | | 398 | | LSCompoundCollider compound) |
| | | 399 | | { |
| | 6 | 400 | | Fixed64 bestRadius = Fixed64.Zero; |
| | 30 | 401 | | for (int index = 0; index < compound.PartCount; index++) |
| | | 402 | | { |
| | 10 | 403 | | LSCollider part = compound.GetPartCollider(index); |
| | 10 | 404 | | bool distanceResolved = |
| | 10 | 405 | | part.TryGetCurrentScaledOffset( |
| | 10 | 406 | | out Vector3d centerOffset) |
| | 10 | 407 | | & centerOffset.TryGetMagnitudeCeiling( |
| | 10 | 408 | | out Fixed64 distance); |
| | 10 | 409 | | bool radiusResolved = Fixed64.TryAdd( |
| | 10 | 410 | | distance, |
| | 10 | 411 | | GetCurrentCenteredProxyRadius(part), |
| | 10 | 412 | | out Fixed64 radius); |
| | 10 | 413 | | if (!(distanceResolved |
| | 10 | 414 | | & radiusResolved)) |
| | | 415 | | { |
| | 1 | 416 | | return Fixed64.MaxValue; |
| | | 417 | | } |
| | | 418 | | |
| | 9 | 419 | | bestRadius = FixedMath.Max(bestRadius, radius); |
| | | 420 | | } |
| | | 421 | | |
| | 5 | 422 | | return bestRadius; |
| | | 423 | | } |
| | | 424 | | } |