< Summary

Information
Class: Gravitas.Colliders.LSCapsuleCollider
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/3D/Shapes/LSCapsuleCollider.cs
Line coverage
100%
Covered lines: 187
Uncovered lines: 0
Coverable lines: 187
Total lines: 326
Line coverage: 100%
Branch coverage
100%
Covered branches: 10
Total branches: 10
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/3D/Shapes/LSCapsuleCollider.cs

#LineLine coverage
 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
 8using System;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11using Gravitas.Queries;
 12using SwiftCollections;
 13
 14namespace Gravitas.Colliders;
 15
 16/// <summary>Represents a runtime 3D capsule collider whose local axis follows Y.</summary>
 17public sealed class LSCapsuleCollider : LSCollider
 18{
 124419    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>
 203626    public LSCapsuleCollider() { }
 27
 28    /// <summary>Creates a runtime capsule from an authored shape definition.</summary>
 22629    public LSCapsuleCollider(ColliderShapeDefinition definition)
 30    {
 22631        definition.EnsureKind(ColliderShapeDefinitionKind.Capsule);
 22632        Material = definition.Material;
 22633        Radius = definition.Radius;
 22634        Size = definition.Size;
 22635    }
 36
 37    /// <inheritdoc/>
 54589738    public override ColliderType Shape => ColliderType.Capsule;
 39    /// <inheritdoc/>
 427540    public override int Priority => ColliderSettings.GetPriority(Shape);
 41
 42    /// <inheritdoc/>
 7155843    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    {
 31360        Fixed64 diameter = _radius * 2;
 31361        _size = new Vector3d(diameter, _size.Y, diameter);
 31362    }
 63
 64    /// <inheritdoc/>
 65    protected override Vector3d NormalizeSize(Vector3d value) =>
 119566        new(_radius * 2, value.Y, _radius * 2);
 67
 68    private protected override void PrepareShape(in ColliderShapeSnapshot snapshot)
 69    {
 2603770        GetScaledGeometry(snapshot, out _preparedRadius, out _preparedAxisLength);
 2603571        SetPreparedGeometryReach(ColliderGeometryPolicy.Capsule(_preparedRadius, _preparedAxisLength));
 2603572        _preparedAxis = (snapshot.Rotation * Vector3d.Up).Normalized;
 2603573        _preparedArea = Fixed64.Two * Fixed64.Pi * _preparedRadius * _preparedAxisLength
 2603574            + Fixed64.Two * Fixed64.Pi * _preparedRadius * _preparedRadius;
 2603575        SetPreparedBounds(FixedBoundBox.FromCenteredCapsuleClippedToDomain(
 2603576            snapshot.Center,
 2603577            snapshot.Rotation,
 2603578            Vector3d.Up,
 2603579            _preparedAxisLength,
 2603580            _preparedRadius));
 2603581    }
 82
 83    internal static void GetScaledGeometry(
 84        in ColliderShapeSnapshot snapshot,
 85        out Fixed64 radius,
 86        out Fixed64 axisLength)
 87    {
 2605688        Fixed64 radiusX = ColliderScalePolicy.ScalePositive(
 2605689            snapshot.Radius,
 2605690            snapshot.OwnerScale.X,
 2605691            snapshot.PartScale.X);
 2605592        Fixed64 radiusZ = ColliderScalePolicy.ScalePositive(
 2605593            snapshot.Radius,
 2605594            snapshot.OwnerScale.Z,
 2605595            snapshot.PartScale.Z);
 2605596        radius = FixedMath.Max(radiusX, radiusZ);
 2605597        ChronicleGuard.ThrowIfArgument(
 2605598            !HasValidScaledDimensions(snapshot),
 2605599            nameof(snapshot),
 26055100            "Scaled capsule height must be at least the capsule diameter.");
 26054101        ChronicleGuard.ThrowIfArgument(
 26054102            !Fixed64.TryMultiplySubtractClamped(
 26054103                snapshot.Size.Y,
 26054104                snapshot.OwnerScale.Y,
 26054105                snapshot.PartScale.Y,
 26054106                Fixed64.One,
 26054107                snapshot.Radius,
 26054108                Fixed64.Two,
 26054109                snapshot.OwnerScale.X,
 26054110                snapshot.PartScale.X,
 26054111                out Fixed64 axisLengthX)
 26054112            | !Fixed64.TryMultiplySubtractClamped(
 26054113                snapshot.Size.Y,
 26054114                snapshot.OwnerScale.Y,
 26054115                snapshot.PartScale.Y,
 26054116                Fixed64.One,
 26054117                snapshot.Radius,
 26054118                Fixed64.Two,
 26054119                snapshot.OwnerScale.Z,
 26054120                snapshot.PartScale.Z,
 26054121                out Fixed64 axisLengthZ),
 26054122            nameof(snapshot),
 26054123            "Scaled capsule center-axis length must be representable.");
 26054124        axisLength = FixedMath.Min(axisLengthX, axisLengthZ);
 26054125    }
 126
 127    private static bool HasValidScaledDimensions(
 128        in ColliderShapeSnapshot snapshot) =>
 26055129        Fixed64.CompareProducts(
 26055130            snapshot.Size.Y,
 26055131            snapshot.OwnerScale.Y,
 26055132            snapshot.PartScale.Y,
 26055133            Fixed64.One,
 26055134            snapshot.Radius,
 26055135            Fixed64.Two,
 26055136            snapshot.OwnerScale.X,
 26055137            snapshot.PartScale.X) >= 0
 26055138        & Fixed64.CompareProducts(
 26055139            snapshot.Size.Y,
 26055140            snapshot.OwnerScale.Y,
 26055141            snapshot.PartScale.Y,
 26055142            Fixed64.One,
 26055143            snapshot.Radius,
 26055144            Fixed64.Two,
 26055145            snapshot.OwnerScale.Z,
 26055146            snapshot.PartScale.Z) >= 0;
 147
 148    private protected override void PublishShape()
 149    {
 26029150        _scaledRadius = _preparedRadius;
 26029151        AxisLength = _preparedAxisLength;
 26029152        WorldAxis = _preparedAxis;
 26029153        Area = _preparedArea;
 26029154    }
 155
 156    internal override ExactMassWeight CalculateMassPropertyWeight()
 157    {
 50158        GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axisLength);
 50159        return GetCylinderWeight(radius, axisLength).Add(GetCapWeight(radius));
 160    }
 161
 162    internal void GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axisLength)
 163    {
 2600164        if (HasCommittedShape)
 165        {
 2589166            radius = _scaledRadius;
 2589167            axisLength = AxisLength;
 2589168            return;
 169        }
 170
 11171        GetCurrentShapeScales(out Vector3d ownerScale, out Vector3d partScale);
 11172        var snapshot = new ColliderShapeSnapshot(
 11173            Vector3d.Zero,
 11174            FixedQuaternion.Identity,
 11175            ownerScale,
 11176            partScale,
 11177            LocalOffset,
 11178            Size,
 11179            Radius);
 11180        GetScaledGeometry(snapshot, out radius, out axisLength);
 11181    }
 182
 183    internal override ExactMassWeight CalculatePreparedMassPropertyWeight() =>
 8914184        GetCylinderWeight(_preparedRadius, _preparedAxisLength)
 8914185            .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    {
 2529192        GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 axisLength);
 2529193        Signed192 sphereAxial = ExactMassProperties.InertiaGeometryTerm(
 2529194            Fixed64.FromFraction(2, 5), radius, radius);
 2529195        if (axisLength <= Fixed64.Epsilon)
 196        {
 172197            Fixed64 sphereDiagonal = ExactMassProperties.InertiaMoment(mass, sphereAxial);
 172198            return new Fixed3x3(
 172199                sphereDiagonal, Fixed64.Zero, Fixed64.Zero,
 172200                Fixed64.Zero, sphereDiagonal, Fixed64.Zero,
 172201                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.
 2357206        Signed192 cylinderWeight = Fixed64.GetExactRawProduct(Fixed64.Pi, axisLength);
 2357207        Signed192 capWeight = Fixed64.GetExactRawProduct(
 2357208            Fixed64.FromFraction(4, 3) * Fixed64.Pi, radius);
 2357209        GetComponentInertiaGeometry(radius, axisLength, sphereAxial,
 2357210            out Signed192 cylinderTransverse, out Signed192 sphereTransverse);
 2357211        Fixed64 totalInertia_xz = ExactMassProperties.InertiaWeightedMoment(
 2357212            mass, cylinderTransverse, cylinderWeight, sphereTransverse, capWeight);
 2357213        Fixed64 totalInertia_y = ExactMassProperties.InertiaWeightedMoment(
 2357214            mass, ExactMassProperties.InertiaGeometryTerm(Fixed64.Half, radius, radius), cylinderWeight,
 2357215            sphereAxial, capWeight);
 216
 2357217        Fixed3x3 tensor = new(
 2357218            totalInertia_xz, Fixed64.Zero, Fixed64.Zero,
 2357219            Fixed64.Zero, totalInertia_y, Fixed64.Zero,
 2357220            Fixed64.Zero, Fixed64.Zero, totalInertia_xz
 2357221        );
 2357222        return tensor;
 223    }
 224
 225    internal static void GetComponentInertiaGeometry(
 226        Fixed64 radius, Fixed64 axisLength, Signed192 sphereAxial,
 227        out Signed192 cylinderTransverse, out Signed192 sphereTransverse)
 228    {
 2377229        Fixed64 twelfth = Fixed64.FromFraction(1, 12);
 2377230        cylinderTransverse = WideArithmetic.AddSigned192(
 2377231            ExactMassProperties.InertiaGeometryTerm(twelfth * (Fixed64)3, radius, radius),
 2377232            ExactMassProperties.InertiaGeometryTerm(twelfth, axisLength, axisLength));
 2377233        sphereTransverse = WideArithmetic.AddSigned192(sphereAxial,
 2377234            WideArithmetic.AddSigned192(
 2377235                ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(1, 4), axisLength, axisLength),
 2377236                ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 8), axisLength, radius)));
 2377237    }
 238
 239    internal static ExactMassWeight GetCylinderWeight(
 240        Fixed64 radius,
 241        Fixed64 axisLength) =>
 8984242        ExactMassWeight.FromProduct(
 8984243            Fixed64.Pi,
 8984244            radius,
 8984245            radius,
 8984246            axisLength);
 247
 248    internal static ExactMassWeight GetCapWeight(Fixed64 radius) =>
 8984249        ExactMassWeight.FromProduct(
 8984250            Fixed64.FromFraction(4, 3) * Fixed64.Pi,
 8984251            radius,
 8984252            radius,
 8984253            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    {
 10264        Fixed64 directionMagnitude = direction.Magnitude;
 10265        if (directionMagnitude <= Fixed64.Epsilon)
 2266            return Area;
 267
 8268        Vector3d normalizedDirection = direction / directionMagnitude;
 8269        Fixed64 axial = Rotation.Inverse()
 8270            .Rotate(normalizedDirection).Y.Abs();
 8271        Fixed64 radialFactorSqr = Fixed64.One - axial * axial;
 8272        Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero
 8273            ? Fixed64.Zero
 8274            : FixedMath.Sqrt(radialFactorSqr);
 275
 8276        Fixed64 capArea = Fixed64.Pi * ScaledRadiusSqr;
 8277        Fixed64 sideProfile = 2 * ScaledRadius * AxisLength;
 8278        return capArea + radialFactor * sideProfile;
 279    }
 280
 281    /// <inheritdoc/>
 282    public override Vector3d ClosestPointOnSurface(Vector3d other)
 283    {
 10284        FixedPointAnchor surfaceAnchor =
 10285            GetClosestSurfaceAnchor(other, out _);
 10286        if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint))
 287        {
 1288            throw new InvalidOperationException(
 1289                "The closest capsule surface point is outside the representable coordinate domain.");
 290        }
 291
 9292        return surfacePoint;
 293    }
 294
 295    /// <inheritdoc/>
 296    public override Vector3d GetNormalAtPoint(Vector3d point)
 297    {
 454298        _ = GetClosestSurfaceAnchor(
 454299            point,
 454300            out Vector3d outwardNormal);
 454301        return outwardNormal;
 302    }
 303
 304    internal override FixedPointAnchor GetClosestSurfaceAnchor(
 305        Vector3d point,
 306        out Vector3d normal) =>
 720307        GetClosestSurfaceAnchor(point, out normal, out _);
 308
 309    internal override FixedPointAnchor GetClosestSurfaceAnchor(
 310        Vector3d point, out Vector3d normal, out bool contained) =>
 900311        WideFiniteAxisIntersection
 900312            .GetClosestCenteredCapsuleSurfaceAnchor(
 900313                point,
 900314                Center,
 900315                Rotation,
 900316                Vector3d.Up,
 900317                AxisLength,
 900318                ScaledRadius,
 900319                Vector3d.Right,
 900320                out normal,
 900321                out contained);
 322
 323    /// <inheritdoc/>
 324    public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin
 440325        worker.CheckCapsuleOverlaps(this, ref outputIntersectionPoints);
 326}