| | | 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 System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | using FixedMathSharp.Geometry; |
| | | 11 | | using Gravitas.Queries; |
| | | 12 | | using SwiftCollections; |
| | | 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 | | { |
| | 1244 | 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> |
| | 2036 | 26 | | public LSCapsuleCollider() { } |
| | | 27 | | |
| | | 28 | | /// <summary>Creates a runtime capsule from an authored shape definition.</summary> |
| | 226 | 29 | | public LSCapsuleCollider(ColliderShapeDefinition definition) |
| | | 30 | | { |
| | 226 | 31 | | definition.EnsureKind(ColliderShapeDefinitionKind.Capsule); |
| | 226 | 32 | | Material = definition.Material; |
| | 226 | 33 | | Radius = definition.Radius; |
| | 226 | 34 | | Size = definition.Size; |
| | 226 | 35 | | } |
| | | 36 | | |
| | | 37 | | /// <inheritdoc/> |
| | 545897 | 38 | | public override ColliderType Shape => ColliderType.Capsule; |
| | | 39 | | /// <inheritdoc/> |
| | 4275 | 40 | | public override int Priority => ColliderSettings.GetPriority(Shape); |
| | | 41 | | |
| | | 42 | | /// <inheritdoc/> |
| | 71558 | 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 | | { |
| | 313 | 60 | | Fixed64 diameter = _radius * 2; |
| | 313 | 61 | | _size = new Vector3d(diameter, _size.Y, diameter); |
| | 313 | 62 | | } |
| | | 63 | | |
| | | 64 | | /// <inheritdoc/> |
| | | 65 | | protected override Vector3d NormalizeSize(Vector3d value) => |
| | 1195 | 66 | | new(_radius * 2, value.Y, _radius * 2); |
| | | 67 | | |
| | | 68 | | private protected override void PrepareShape(in ColliderShapeSnapshot snapshot) |
| | | 69 | | { |
| | 26037 | 70 | | GetScaledGeometry(snapshot, out _preparedRadius, out _preparedAxisLength); |
| | 26035 | 71 | | SetPreparedGeometryReach(ColliderGeometryPolicy.Capsule(_preparedRadius, _preparedAxisLength)); |
| | 26035 | 72 | | _preparedAxis = (snapshot.Rotation * Vector3d.Up).Normalized; |
| | 26035 | 73 | | _preparedArea = Fixed64.Two * Fixed64.Pi * _preparedRadius * _preparedAxisLength |
| | 26035 | 74 | | + Fixed64.Two * Fixed64.Pi * _preparedRadius * _preparedRadius; |
| | 26035 | 75 | | SetPreparedBounds(FixedBoundBox.FromCenteredCapsuleClippedToDomain( |
| | 26035 | 76 | | snapshot.Center, |
| | 26035 | 77 | | snapshot.Rotation, |
| | 26035 | 78 | | Vector3d.Up, |
| | 26035 | 79 | | _preparedAxisLength, |
| | 26035 | 80 | | _preparedRadius)); |
| | 26035 | 81 | | } |
| | | 82 | | |
| | | 83 | | internal static void GetScaledGeometry( |
| | | 84 | | in ColliderShapeSnapshot snapshot, |
| | | 85 | | out Fixed64 radius, |
| | | 86 | | out Fixed64 axisLength) |
| | | 87 | | { |
| | 26056 | 88 | | Fixed64 radiusX = ColliderScalePolicy.ScalePositive( |
| | 26056 | 89 | | snapshot.Radius, |
| | 26056 | 90 | | snapshot.OwnerScale.X, |
| | 26056 | 91 | | snapshot.PartScale.X); |
| | 26055 | 92 | | Fixed64 radiusZ = ColliderScalePolicy.ScalePositive( |
| | 26055 | 93 | | snapshot.Radius, |
| | 26055 | 94 | | snapshot.OwnerScale.Z, |
| | 26055 | 95 | | snapshot.PartScale.Z); |
| | 26055 | 96 | | radius = FixedMath.Max(radiusX, radiusZ); |
| | 26055 | 97 | | ChronicleGuard.ThrowIfArgument( |
| | 26055 | 98 | | !HasValidScaledDimensions(snapshot), |
| | 26055 | 99 | | nameof(snapshot), |
| | 26055 | 100 | | "Scaled capsule height must be at least the capsule diameter."); |
| | 26054 | 101 | | ChronicleGuard.ThrowIfArgument( |
| | 26054 | 102 | | !Fixed64.TryMultiplySubtractClamped( |
| | 26054 | 103 | | snapshot.Size.Y, |
| | 26054 | 104 | | snapshot.OwnerScale.Y, |
| | 26054 | 105 | | snapshot.PartScale.Y, |
| | 26054 | 106 | | Fixed64.One, |
| | 26054 | 107 | | snapshot.Radius, |
| | 26054 | 108 | | Fixed64.Two, |
| | 26054 | 109 | | snapshot.OwnerScale.X, |
| | 26054 | 110 | | snapshot.PartScale.X, |
| | 26054 | 111 | | out Fixed64 axisLengthX) |
| | 26054 | 112 | | | !Fixed64.TryMultiplySubtractClamped( |
| | 26054 | 113 | | snapshot.Size.Y, |
| | 26054 | 114 | | snapshot.OwnerScale.Y, |
| | 26054 | 115 | | snapshot.PartScale.Y, |
| | 26054 | 116 | | Fixed64.One, |
| | 26054 | 117 | | snapshot.Radius, |
| | 26054 | 118 | | Fixed64.Two, |
| | 26054 | 119 | | snapshot.OwnerScale.Z, |
| | 26054 | 120 | | snapshot.PartScale.Z, |
| | 26054 | 121 | | out Fixed64 axisLengthZ), |
| | 26054 | 122 | | nameof(snapshot), |
| | 26054 | 123 | | "Scaled capsule center-axis length must be representable."); |
| | 26054 | 124 | | axisLength = FixedMath.Min(axisLengthX, axisLengthZ); |
| | 26054 | 125 | | } |
| | | 126 | | |
| | | 127 | | private static bool HasValidScaledDimensions( |
| | | 128 | | in ColliderShapeSnapshot snapshot) => |
| | 26055 | 129 | | Fixed64.CompareProducts( |
| | 26055 | 130 | | snapshot.Size.Y, |
| | 26055 | 131 | | snapshot.OwnerScale.Y, |
| | 26055 | 132 | | snapshot.PartScale.Y, |
| | 26055 | 133 | | Fixed64.One, |
| | 26055 | 134 | | snapshot.Radius, |
| | 26055 | 135 | | Fixed64.Two, |
| | 26055 | 136 | | snapshot.OwnerScale.X, |
| | 26055 | 137 | | snapshot.PartScale.X) >= 0 |
| | 26055 | 138 | | & Fixed64.CompareProducts( |
| | 26055 | 139 | | snapshot.Size.Y, |
| | 26055 | 140 | | snapshot.OwnerScale.Y, |
| | 26055 | 141 | | snapshot.PartScale.Y, |
| | 26055 | 142 | | Fixed64.One, |
| | 26055 | 143 | | snapshot.Radius, |
| | 26055 | 144 | | Fixed64.Two, |
| | 26055 | 145 | | snapshot.OwnerScale.Z, |
| | 26055 | 146 | | snapshot.PartScale.Z) >= 0; |
| | | 147 | | |
| | | 148 | | private protected override void PublishShape() |
| | | 149 | | { |
| | 26029 | 150 | | _scaledRadius = _preparedRadius; |
| | 26029 | 151 | | AxisLength = _preparedAxisLength; |
| | 26029 | 152 | | WorldAxis = _preparedAxis; |
| | 26029 | 153 | | Area = _preparedArea; |
| | 26029 | 154 | | } |
| | | 155 | | |
| | | 156 | | internal override ExactMassWeight CalculateMassPropertyWeight() |
| | | 157 | | { |
| | 50 | 158 | | GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axisLength); |
| | 50 | 159 | | return GetCylinderWeight(radius, axisLength).Add(GetCapWeight(radius)); |
| | | 160 | | } |
| | | 161 | | |
| | | 162 | | internal void GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axisLength) |
| | | 163 | | { |
| | 2600 | 164 | | if (HasCommittedShape) |
| | | 165 | | { |
| | 2589 | 166 | | radius = _scaledRadius; |
| | 2589 | 167 | | axisLength = AxisLength; |
| | 2589 | 168 | | return; |
| | | 169 | | } |
| | | 170 | | |
| | 11 | 171 | | GetCurrentShapeScales(out Vector3d ownerScale, out Vector3d partScale); |
| | 11 | 172 | | var snapshot = new ColliderShapeSnapshot( |
| | 11 | 173 | | Vector3d.Zero, |
| | 11 | 174 | | FixedQuaternion.Identity, |
| | 11 | 175 | | ownerScale, |
| | 11 | 176 | | partScale, |
| | 11 | 177 | | LocalOffset, |
| | 11 | 178 | | Size, |
| | 11 | 179 | | Radius); |
| | 11 | 180 | | GetScaledGeometry(snapshot, out radius, out axisLength); |
| | 11 | 181 | | } |
| | | 182 | | |
| | | 183 | | internal override ExactMassWeight CalculatePreparedMassPropertyWeight() => |
| | 8914 | 184 | | GetCylinderWeight(_preparedRadius, _preparedAxisLength) |
| | 8914 | 185 | | .Add(GetCapWeight(_preparedRadius)); |
| | | 186 | | |
| | | 187 | | // The capsule is split into a cylinder and a pair of solid hemispheres with masses |
| | | 188 | | // proportional to their volumes. Each hemisphere's centroid lies 3r/8 outward from |
| | | 189 | | // its sphere center, which contributes the 3dr/4 cross term to transverse cap inertia. |
| | | 190 | | internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) |
| | | 191 | | { |
| | 2529 | 192 | | GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axisLength); |
| | 2529 | 193 | | Signed192 sphereAxial = ExactMassProperties.InertiaGeometryTerm( |
| | 2529 | 194 | | Fixed64.FromFraction(2, 5), radius, radius); |
| | 2529 | 195 | | if (axisLength <= Fixed64.Epsilon) |
| | | 196 | | { |
| | 172 | 197 | | Fixed64 sphereDiagonal = ExactMassProperties.InertiaMoment(mass, sphereAxial); |
| | 172 | 198 | | return new Fixed3x3( |
| | 172 | 199 | | sphereDiagonal, Fixed64.Zero, Fixed64.Zero, |
| | 172 | 200 | | Fixed64.Zero, sphereDiagonal, Fixed64.Zero, |
| | 172 | 201 | | Fixed64.Zero, Fixed64.Zero, sphereDiagonal); |
| | | 202 | | } |
| | | 203 | | |
| | | 204 | | // Cancelling the shared r^2 retains the existing volume coefficients |
| | | 205 | | // while preserving the exact internal cylinder/hemisphere mass ratio. |
| | 2357 | 206 | | Signed192 cylinderWeight = Fixed64.GetExactRawProduct(Fixed64.Pi, axisLength); |
| | 2357 | 207 | | Signed192 capWeight = Fixed64.GetExactRawProduct( |
| | 2357 | 208 | | Fixed64.FromFraction(4, 3) * Fixed64.Pi, radius); |
| | 2357 | 209 | | GetComponentInertiaGeometry(radius, axisLength, sphereAxial, |
| | 2357 | 210 | | out Signed192 cylinderTransverse, out Signed192 sphereTransverse); |
| | 2357 | 211 | | Fixed64 totalInertia_xz = ExactMassProperties.InertiaWeightedMoment( |
| | 2357 | 212 | | mass, cylinderTransverse, cylinderWeight, sphereTransverse, capWeight); |
| | 2357 | 213 | | Fixed64 totalInertia_y = ExactMassProperties.InertiaWeightedMoment( |
| | 2357 | 214 | | mass, ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius), cylinderWeight, |
| | 2357 | 215 | | sphereAxial, capWeight); |
| | | 216 | | |
| | 2357 | 217 | | Fixed3x3 tensor = new( |
| | 2357 | 218 | | totalInertia_xz, Fixed64.Zero, Fixed64.Zero, |
| | 2357 | 219 | | Fixed64.Zero, totalInertia_y, Fixed64.Zero, |
| | 2357 | 220 | | Fixed64.Zero, Fixed64.Zero, totalInertia_xz |
| | 2357 | 221 | | ); |
| | 2357 | 222 | | return tensor; |
| | | 223 | | } |
| | | 224 | | |
| | | 225 | | internal static void GetComponentInertiaGeometry( |
| | | 226 | | Fixed64 radius, Fixed64 axisLength, Signed192 sphereAxial, |
| | | 227 | | out Signed192 cylinderTransverse, out Signed192 sphereTransverse) |
| | | 228 | | { |
| | 2377 | 229 | | Fixed64 twelfth = Fixed64.FromFraction(1, 12); |
| | 2377 | 230 | | cylinderTransverse = WideArithmetic.AddSigned192( |
| | 2377 | 231 | | ExactMassProperties.InertiaGeometryTerm(twelfth * (Fixed64)3, radius, radius), |
| | 2377 | 232 | | ExactMassProperties.InertiaGeometryTerm(twelfth, axisLength, axisLength)); |
| | 2377 | 233 | | sphereTransverse = WideArithmetic.AddSigned192(sphereAxial, |
| | 2377 | 234 | | WideArithmetic.AddSigned192( |
| | 2377 | 235 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(1, 4), axisLength, axisLength), |
| | 2377 | 236 | | ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 8), axisLength, radius))); |
| | 2377 | 237 | | } |
| | | 238 | | |
| | | 239 | | internal static ExactMassWeight GetCylinderWeight( |
| | | 240 | | Fixed64 radius, |
| | | 241 | | Fixed64 axisLength) => |
| | 8984 | 242 | | ExactMassWeight.FromProduct( |
| | 8984 | 243 | | Fixed64.Pi, |
| | 8984 | 244 | | radius, |
| | 8984 | 245 | | radius, |
| | 8984 | 246 | | axisLength); |
| | | 247 | | |
| | | 248 | | internal static ExactMassWeight GetCapWeight(Fixed64 radius) => |
| | 8984 | 249 | | ExactMassWeight.FromProduct( |
| | 8984 | 250 | | Fixed64.FromFraction(4, 3) * Fixed64.Pi, |
| | 8984 | 251 | | radius, |
| | 8984 | 252 | | radius, |
| | 8984 | 253 | | radius); |
| | | 254 | | |
| | | 255 | | // If the capsule is moving in the direction of its main axis, |
| | | 256 | | // the frontal area would be a circle (the end cap of the capsule). |
| | | 257 | | // Therefore, the frontal area would be πr^2, where r is the radius of the capsule. |
| | | 258 | | // If it's moving perpendicular to its main axis, |
| | | 259 | | // then the frontal area would be a rectangle with a semicircle on either end, |
| | | 260 | | // which would be (2r)*h + πr^2, where h is the height of the cylindrical part of the capsule. |
| | | 261 | | /// <inheritdoc/> |
| | | 262 | | public override Fixed64 GetFrontalArea(Vector3d direction) |
| | | 263 | | { |
| | 10 | 264 | | Fixed64 directionMagnitude = direction.Magnitude; |
| | 10 | 265 | | if (directionMagnitude <= Fixed64.Epsilon) |
| | 2 | 266 | | return Area; |
| | | 267 | | |
| | 8 | 268 | | Vector3d normalizedDirection = direction / directionMagnitude; |
| | 8 | 269 | | Fixed64 axial = Rotation.Inverse() |
| | 8 | 270 | | .Rotate(normalizedDirection).Y.Abs(); |
| | 8 | 271 | | Fixed64 radialFactorSqr = Fixed64.One - axial * axial; |
| | 8 | 272 | | Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero |
| | 8 | 273 | | ? Fixed64.Zero |
| | 8 | 274 | | : FixedMath.Sqrt(radialFactorSqr); |
| | | 275 | | |
| | 8 | 276 | | Fixed64 capArea = Fixed64.Pi * ScaledRadiusSqr; |
| | 8 | 277 | | Fixed64 sideProfile = 2 * ScaledRadius * AxisLength; |
| | 8 | 278 | | return capArea + radialFactor * sideProfile; |
| | | 279 | | } |
| | | 280 | | |
| | | 281 | | /// <inheritdoc/> |
| | | 282 | | public override Vector3d ClosestPointOnSurface(Vector3d other) |
| | | 283 | | { |
| | 10 | 284 | | FixedPointAnchor surfaceAnchor = |
| | 10 | 285 | | GetClosestSurfaceAnchor(other, out _); |
| | 10 | 286 | | if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint)) |
| | | 287 | | { |
| | 1 | 288 | | throw new InvalidOperationException( |
| | 1 | 289 | | "The closest capsule surface point is outside the representable coordinate domain."); |
| | | 290 | | } |
| | | 291 | | |
| | 9 | 292 | | return surfacePoint; |
| | | 293 | | } |
| | | 294 | | |
| | | 295 | | /// <inheritdoc/> |
| | | 296 | | public override Vector3d GetNormalAtPoint(Vector3d point) |
| | | 297 | | { |
| | 454 | 298 | | _ = GetClosestSurfaceAnchor( |
| | 454 | 299 | | point, |
| | 454 | 300 | | out Vector3d outwardNormal); |
| | 454 | 301 | | return outwardNormal; |
| | | 302 | | } |
| | | 303 | | |
| | | 304 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 305 | | Vector3d point, |
| | | 306 | | out Vector3d normal) => |
| | 720 | 307 | | GetClosestSurfaceAnchor(point, out normal, out _); |
| | | 308 | | |
| | | 309 | | internal override FixedPointAnchor GetClosestSurfaceAnchor( |
| | | 310 | | Vector3d point, out Vector3d normal, out bool contained) => |
| | 900 | 311 | | WideFiniteAxisIntersection |
| | 900 | 312 | | .GetClosestCenteredCapsuleSurfaceAnchor( |
| | 900 | 313 | | point, |
| | 900 | 314 | | Center, |
| | 900 | 315 | | Rotation, |
| | 900 | 316 | | Vector3d.Up, |
| | 900 | 317 | | AxisLength, |
| | 900 | 318 | | ScaledRadius, |
| | 900 | 319 | | Vector3d.Right, |
| | 900 | 320 | | out normal, |
| | 900 | 321 | | out contained); |
| | | 322 | | |
| | | 323 | | /// <inheritdoc/> |
| | | 324 | | public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin |
| | 440 | 325 | | worker.CheckCapsuleOverlaps(this, ref outputIntersectionPoints); |
| | | 326 | | } |