| | | 1 | | //======================================================================= |
| | | 2 | | // LSCapsuleCollider.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 | | using Gravitas.Queries; |
| | | 11 | | using SwiftCollections; |
| | | 12 | | using System; |
| | | 13 | | |
| | | 14 | | namespace Gravitas.Colliders; |
| | | 15 | | |
| | | 16 | | /// <summary>Represents a runtime 3D capsule collider whose local axis follows Y.</summary> |
| | | 17 | | public sealed class LSCapsuleCollider : LSCollider |
| | | 18 | | { |
| | 564 | 19 | | private Fixed64 _scaledRadius = Fixed64.Half; |
| | | 20 | | private Fixed64 _preparedRadius; |
| | | 21 | | private Fixed64 _preparedAxisLength; |
| | | 22 | | private Vector3d _preparedAxis; |
| | | 23 | | private Fixed64 _preparedArea; |
| | | 24 | | |
| | | 25 | | /// <summary>Creates a capsule collider with default dimensions.</summary> |
| | 1072 | 26 | | public LSCapsuleCollider() { } |
| | | 27 | | |
| | | 28 | | /// <summary>Creates a runtime capsule from an authored shape definition.</summary> |
| | 28 | 29 | | public LSCapsuleCollider(ColliderShapeDefinition definition) |
| | | 30 | | { |
| | 28 | 31 | | definition.EnsureKind(ColliderShapeDefinitionKind.Capsule); |
| | 28 | 32 | | Material = definition.Material; |
| | 28 | 33 | | Radius = definition.Radius; |
| | 28 | 34 | | Size = definition.Size; |
| | 28 | 35 | | } |
| | | 36 | | |
| | | 37 | | /// <inheritdoc/> |
| | 881 | 38 | | public override ColliderType Shape => ColliderType.Capsule; |
| | | 39 | | /// <inheritdoc/> |
| | 145 | 40 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 41 | | |
| | | 42 | | /// <inheritdoc/> |
| | 10968 | 43 | | public override Fixed64 ScaledRadius => _scaledRadius; |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Gets the full physical distance between the capsule's hemisphere centers. |
| | | 47 | | /// </summary> |
| | | 48 | | public Fixed64 AxisLength { get; private set; } |
| | | 49 | | |
| | | 50 | | /// <summary> |
| | | 51 | | /// Gets the derived normalized world-space direction of the capsule's |
| | | 52 | | /// conceptual center axis. Exact geometry remains authoritative in the |
| | | 53 | | /// collider's rigid frame. |
| | | 54 | | /// </summary> |
| | | 55 | | public Vector3d WorldAxis { get; private set; } = Vector3d.Up; |
| | | 56 | | |
| | | 57 | | /// <inheritdoc/> |
| | | 58 | | protected override void OnRadiusChanged() |
| | | 59 | | { |
| | 37 | 60 | | Fixed64 diameter = _radius * 2; |
| | 37 | 61 | | _size = new Vector3d(diameter, _size.Y, diameter); |
| | 37 | 62 | | } |
| | | 63 | | |
| | | 64 | | /// <inheritdoc/> |
| | | 65 | | protected override Vector3d NormalizeSize(Vector3d value) => |
| | 519 | 66 | | new(_radius * 2, value.Y, _radius * 2); |
| | | 67 | | |
| | | 68 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 69 | | { |
| | 577 | 70 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 577 | 71 | | snapshot.Radius, |
| | 577 | 72 | | snapshot.OwnerScale.X, |
| | 577 | 73 | | snapshot.PartScale.X); |
| | 576 | 74 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 576 | 75 | | snapshot.Radius, |
| | 576 | 76 | | snapshot.OwnerScale.Z, |
| | 576 | 77 | | snapshot.PartScale.Z); |
| | 576 | 78 | | _preparedRadius = FixedMath.Max(radiusX, radiusZ); |
| | 576 | 79 | | SwiftThrowHelper.ThrowIfArgument( |
| | 576 | 80 | | !HasValidScaledDimensions(snapshot), |
| | 576 | 81 | | nameof(snapshot), |
| | 576 | 82 | | "Scaled capsule height must be at least the capsule diameter."); |
| | 575 | 83 | | SwiftThrowHelper.ThrowIfArgument( |
| | 575 | 84 | | !Fixed64.TryMultiplySubtractClamped( |
| | 575 | 85 | | snapshot.Size.Y, |
| | 575 | 86 | | snapshot.OwnerScale.Y, |
| | 575 | 87 | | snapshot.PartScale.Y, |
| | 575 | 88 | | Fixed64.One, |
| | 575 | 89 | | snapshot.Radius, |
| | 575 | 90 | | Fixed64.Two, |
| | 575 | 91 | | snapshot.OwnerScale.X, |
| | 575 | 92 | | snapshot.PartScale.X, |
| | 575 | 93 | | out Fixed64 axisLengthX) |
| | 575 | 94 | | | !Fixed64.TryMultiplySubtractClamped( |
| | 575 | 95 | | snapshot.Size.Y, |
| | 575 | 96 | | snapshot.OwnerScale.Y, |
| | 575 | 97 | | snapshot.PartScale.Y, |
| | 575 | 98 | | Fixed64.One, |
| | 575 | 99 | | snapshot.Radius, |
| | 575 | 100 | | Fixed64.Two, |
| | 575 | 101 | | snapshot.OwnerScale.Z, |
| | 575 | 102 | | snapshot.PartScale.Z, |
| | 575 | 103 | | out Fixed64 axisLengthZ), |
| | 575 | 104 | | nameof(snapshot), |
| | 575 | 105 | | "Scaled capsule center-axis length must be representable."); |
| | 575 | 106 | | _preparedAxisLength = FixedMath.Min(axisLengthX, axisLengthZ); |
| | 575 | 107 | | _preparedAxis = (snapshot.Rotation * Vector3d.Up).Normalized; |
| | 575 | 108 | | _preparedArea = Fixed64.Two * Fixed64.Pi * _preparedRadius * _preparedAxisLength |
| | 575 | 109 | | + Fixed64.Two * Fixed64.Pi * _preparedRadius * _preparedRadius; |
| | 575 | 110 | | SetPreparedBounds(FixedBoundBox.FromCenteredCapsuleClippedToDomain( |
| | 575 | 111 | | snapshot.Center, |
| | 575 | 112 | | snapshot.Rotation, |
| | 575 | 113 | | Vector3d.Up, |
| | 575 | 114 | | _preparedAxisLength, |
| | 575 | 115 | | _preparedRadius)); |
| | 575 | 116 | | } |
| | | 117 | | |
| | | 118 | | private static bool HasValidScaledDimensions( |
| | | 119 | | in ColliderShapeSnapshot snapshot) => |
| | 576 | 120 | | Fixed64.CompareProducts( |
| | 576 | 121 | | snapshot.Size.Y, |
| | 576 | 122 | | snapshot.OwnerScale.Y, |
| | 576 | 123 | | snapshot.PartScale.Y, |
| | 576 | 124 | | Fixed64.One, |
| | 576 | 125 | | snapshot.Radius, |
| | 576 | 126 | | Fixed64.Two, |
| | 576 | 127 | | snapshot.OwnerScale.X, |
| | 576 | 128 | | snapshot.PartScale.X) >= 0 |
| | 576 | 129 | | & Fixed64.CompareProducts( |
| | 576 | 130 | | snapshot.Size.Y, |
| | 576 | 131 | | snapshot.OwnerScale.Y, |
| | 576 | 132 | | snapshot.PartScale.Y, |
| | 576 | 133 | | Fixed64.One, |
| | 576 | 134 | | snapshot.Radius, |
| | 576 | 135 | | Fixed64.Two, |
| | 576 | 136 | | snapshot.OwnerScale.Z, |
| | 576 | 137 | | snapshot.PartScale.Z) >= 0; |
| | | 138 | | |
| | | 139 | | private protected override void PublishShape() |
| | | 140 | | { |
| | 575 | 141 | | _scaledRadius = _preparedRadius; |
| | 575 | 142 | | AxisLength = _preparedAxisLength; |
| | 575 | 143 | | WorldAxis = _preparedAxis; |
| | 575 | 144 | | Area = _preparedArea; |
| | 575 | 145 | | } |
| | | 146 | | |
| | | 147 | | internal override ExactMassWeight CalculateMassPropertyWeight() => |
| | 48 | 148 | | GetCylinderWeight(ScaledRadius, AxisLength) |
| | 48 | 149 | | .Add(GetCapWeight(ScaledRadius)); |
| | | 150 | | |
| | | 151 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 59 | 152 | | GetCylinderWeight(_preparedRadius, _preparedAxisLength) |
| | 59 | 153 | | .Add(GetCapWeight(_preparedRadius)); |
| | | 154 | | |
| | | 155 | | // The capsule is split into a cylinder and a pair of solid hemispheres with masses |
| | | 156 | | // proportional to their volumes. Each hemisphere's centroid lies 3r/8 outward from |
| | | 157 | | // its sphere center, which contributes the 3dr/4 cross term to transverse cap inertia. |
| | | 158 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 159 | | { |
| | 523 | 160 | | if (AxisLength <= Fixed64.Epsilon) |
| | | 161 | | { |
| | 63 | 162 | | Fixed64 sphereDiagonal = Fixed64.FromFraction(2, 5) * mass * ScaledRadiusSqr; |
| | 63 | 163 | | Fixed3x3 sphereTensor = new( |
| | 63 | 164 | | sphereDiagonal, Fixed64.Zero, Fixed64.Zero, |
| | 63 | 165 | | Fixed64.Zero, sphereDiagonal, Fixed64.Zero, |
| | 63 | 166 | | Fixed64.Zero, Fixed64.Zero, sphereDiagonal |
| | 63 | 167 | | ); |
| | 63 | 168 | | return sphereTensor; |
| | | 169 | | } |
| | | 170 | | |
| | | 171 | | // Masses of the cylinder and spheres (proportional to their volumes) |
| | 460 | 172 | | ExactMassWeight cylinderWeight = |
| | 460 | 173 | | GetCylinderWeight(ScaledRadius, AxisLength); |
| | 460 | 174 | | ExactMassWeight capWeight = GetCapWeight(ScaledRadius); |
| | 460 | 175 | | ExactMassWeight totalWeight = cylinderWeight.Add(capWeight); |
| | | 176 | | |
| | 460 | 177 | | _ = cylinderWeight.TryGetProportionalShare( |
| | 460 | 178 | | mass, |
| | 460 | 179 | | totalWeight, |
| | 460 | 180 | | out Fixed64 cylinderMass); |
| | 460 | 181 | | Fixed64 sphereMass = mass - cylinderMass; |
| | | 182 | | |
| | | 183 | | // Distance from the center of the hemisphere to the center of the capsule |
| | 460 | 184 | | Fixed64 d = AxisLength / 2; |
| | | 185 | | |
| | | 186 | | // Calculating the inertia tensors for the cylinder and the spheres |
| | 460 | 187 | | Fixed64 cylinderInertiaY = Fixed64.FromFraction(1, 2) * cylinderMass * ScaledRadiusSqr; |
| | 460 | 188 | | Fixed64 cylinderInertiaXZ = Fixed64.FromFraction(1, 12) * cylinderMass * ((3 * ScaledRadiusSqr) + (AxisLength * |
| | 460 | 189 | | Fixed64 sphereInertiaXZ = sphereMass |
| | 460 | 190 | | * (Fixed64.FromFraction(2, 5) * ScaledRadiusSqr |
| | 460 | 191 | | + d * d |
| | 460 | 192 | | + Fixed64.FromFraction(3, 4) * d * ScaledRadius); |
| | 460 | 193 | | Fixed64 sphereInertiaY = Fixed64.FromFraction(2, 5) * sphereMass * ScaledRadiusSqr; |
| | | 194 | | |
| | | 195 | | // The total inertia tensor for the capsule |
| | 460 | 196 | | Fixed64 totalInertia_xz = cylinderInertiaXZ + sphereInertiaXZ; |
| | 460 | 197 | | Fixed64 totalInertia_y = cylinderInertiaY + sphereInertiaY; |
| | | 198 | | |
| | 460 | 199 | | Fixed3x3 tensor = new( |
| | 460 | 200 | | totalInertia_xz, Fixed64.Zero, Fixed64.Zero, |
| | 460 | 201 | | Fixed64.Zero, totalInertia_y, Fixed64.Zero, |
| | 460 | 202 | | Fixed64.Zero, Fixed64.Zero, totalInertia_xz |
| | 460 | 203 | | ); |
| | 460 | 204 | | return tensor; |
| | | 205 | | } |
| | | 206 | | |
| | | 207 | | private static ExactMassWeight GetCylinderWeight( |
| | | 208 | | Fixed64 radius, |
| | | 209 | | Fixed64 axisLength) => |
| | 567 | 210 | | ExactMassWeight.FromProduct( |
| | 567 | 211 | | Fixed64.Pi, |
| | 567 | 212 | | radius, |
| | 567 | 213 | | radius, |
| | 567 | 214 | | axisLength); |
| | | 215 | | |
| | | 216 | | private static ExactMassWeight GetCapWeight(Fixed64 radius) => |
| | 567 | 217 | | ExactMassWeight.FromProduct( |
| | 567 | 218 | | Fixed64.FromFraction(4, 3) * Fixed64.Pi, |
| | 567 | 219 | | radius, |
| | 567 | 220 | | radius, |
| | 567 | 221 | | radius); |
| | | 222 | | |
| | | 223 | | // If the capsule is moving in the direction of its main axis, |
| | | 224 | | // the frontal area would be a circle (the end cap of the capsule). |
| | | 225 | | // Therefore, the frontal area would be πr^2, where r is the radius of the capsule. |
| | | 226 | | // If it's moving perpendicular to its main axis, |
| | | 227 | | // then the frontal area would be a rectangle with a semicircle on either end, |
| | | 228 | | // which would be (2r)*h + πr^2, where h is the height of the cylindrical part of the capsule. |
| | | 229 | | /// <inheritdoc/> |
| | | 230 | | public override Fixed64 GetFrontalArea(Vector3d direction) |
| | | 231 | | { |
| | 11 | 232 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 11 | 233 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 2 | 234 | | return Area; |
| | | 235 | | |
| | 9 | 236 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 9 | 237 | | Fixed64 axial = Rotation.Inverse() |
| | 9 | 238 | | .Rotate(normalizedDirection).Y.Abs(); |
| | 9 | 239 | | Fixed64 radialFactorSqr = Fixed64.One - axial * axial; |
| | 9 | 240 | | Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero |
| | 9 | 241 | | ? Fixed64.Zero |
| | 9 | 242 | | : FixedMath.Sqrt(radialFactorSqr); |
| | | 243 | | |
| | 9 | 244 | | Fixed64 capArea = Fixed64.Pi * ScaledRadiusSqr; |
| | 9 | 245 | | Fixed64 sideProfile = 2 * ScaledRadius * AxisLength; |
| | 9 | 246 | | return capArea + radialFactor * sideProfile; |
| | | 247 | | } |
| | | 248 | | |
| | | 249 | | /// <inheritdoc/> |
| | | 250 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 251 | | { |
| | 87 | 252 | | FixedPointAnchor surfaceAnchor = |
| | 87 | 253 | | GetClosestSurfaceAnchor(other, out _); |
| | 87 | 254 | | if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint)) |
| | | 255 | | { |
| | 1 | 256 | | throw new InvalidOperationException( |
| | 1 | 257 | | "The closest capsule surface point is outside the representable coordinate domain."); |
| | | 258 | | } |
| | | 259 | | |
| | 86 | 260 | | return surfacePoint; |
| | | 261 | | } |
| | | 262 | | |
| | | 263 | | /// <inheritdoc/> |
| | | 264 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 265 | | { |
| | 44 | 266 | | _ = GetClosestSurfaceAnchor( |
| | 44 | 267 | | point, |
| | 44 | 268 | | out Vector3d outwardNormal); |
| | 44 | 269 | | return outwardNormal; |
| | | 270 | | } |
| | | 271 | | |
| | | 272 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 273 | | Vector3d point, |
| | | 274 | | out Vector3d normal) => |
| | 341 | 275 | | WideFiniteAxisIntersection |
| | 341 | 276 | | .GetClosestCenteredCapsuleSurfaceAnchor( |
| | 341 | 277 | | point, |
| | 341 | 278 | | Center, |
| | 341 | 279 | | Rotation, |
| | 341 | 280 | | Vector3d.Up, |
| | 341 | 281 | | AxisLength, |
| | 341 | 282 | | ScaledRadius, |
| | 341 | 283 | | Vector3d.Right, |
| | 341 | 284 | | out normal, |
| | 341 | 285 | | out _); |
| | | 286 | | |
| | | 287 | | /// <inheritdoc/> |
| | | 288 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | 8 | 289 | | worker.CheckCapsuleOverlaps(this, ref outputIntersectionPoints); |
| | | 290 | | } |