< Summary

Information
Class: Gravitas.Colliders.LSConeCollider
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/3D/Shapes/LSConeCollider.cs
Line coverage
100%
Covered lines: 160
Uncovered lines: 0
Coverable lines: 160
Total lines: 303
Line coverage: 100%
Branch coverage
100%
Covered branches: 36
Total branches: 36
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/LSConeCollider.cs

#LineLine coverage
 1//=======================================================================
 2// LSConeCollider.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>
 17/// Represents a finite circular cone whose local Y axis runs from base plane
 18/// to apex and whose local origin is the cone's bounding center.
 19/// </summary>
 20public sealed class LSConeCollider : LSCollider
 21{
 98122    private Fixed64 _scaledRadius = Fixed64.Half;
 23    private Fixed64 _preparedRadius;
 24    private Fixed64 _preparedHeight;
 25    private Vector3d _preparedAxis;
 26    private Fixed64 _preparedVolume;
 27
 28    /// <summary>Creates a cone collider with default dimensions.</summary>
 174429    public LSConeCollider() { }
 30
 31    /// <summary>Creates a runtime cone from an authored shape definition.</summary>
 10932    public LSConeCollider(ColliderShapeDefinition definition)
 33    {
 10934        definition.EnsureKind(ColliderShapeDefinitionKind.Cone);
 10935        Material = definition.Material;
 10936        Radius = definition.Radius;
 10937        Size = definition.Size;
 10938    }
 39
 40    /// <inheritdoc/>
 46663141    public override ColliderType Shape => ColliderType.Cone;
 42
 43    /// <inheritdoc/>
 352344    public override int Priority => ColliderSettings.GetPriority(Shape);
 45
 46    /// <inheritdoc/>
 5548347    public override Fixed64 ScaledRadius => _scaledRadius;
 48
 49    /// <summary>
 50    /// Gets the derived normalized world-space base-to-apex axis. Exact
 51    /// geometry remains authoritative in the collider's rigid frame.
 52    /// </summary>
 53    public Vector3d WorldAxis { get; private set; } = Vector3d.Up;
 54
 55    /// <summary>
 56    /// Gets the full physical distance from the cone's base plane to its apex.
 57    /// </summary>
 58    public Fixed64 Height { get; private set; }
 59
 60    /// <summary>
 61    /// Gets the solid cone volume, rounded once after final division and
 62    /// saturated to <see cref="Fixed64.MaxValue"/> when unrepresentable.
 63    /// Compound mass weighting retains its separate unnarrowed measure.
 64    /// </summary>
 65    public Fixed64 Volume { get; private set; }
 66
 67    /// <inheritdoc/>
 68    protected override void OnRadiusChanged()
 69    {
 19470        Fixed64 diameter = _radius * 2;
 19471        _size = new Vector3d(diameter, _size.Y, diameter);
 19472    }
 73
 74    /// <inheritdoc/>
 75    protected override Vector3d NormalizeSize(Vector3d value) =>
 82976        new(_radius * 2, value.Y, _radius * 2);
 77
 78    private protected override void PrepareShape(in ColliderShapeSnapshot snapshot)
 79    {
 1859680        Fixed64 radiusX = ColliderScalePolicy.ScalePositive(
 1859681            snapshot.Radius,
 1859682            snapshot.OwnerScale.X,
 1859683            snapshot.PartScale.X);
 1859684        Fixed64 radiusZ = ColliderScalePolicy.ScalePositive(
 1859685            snapshot.Radius,
 1859686            snapshot.OwnerScale.Z,
 1859687            snapshot.PartScale.Z);
 1859688        Fixed64 radius = FixedMath.Max(radiusX, radiusZ);
 1859689        Fixed64 height = ColliderScalePolicy.ScalePositive(
 1859690            snapshot.Size.Y,
 1859691            snapshot.OwnerScale.Y,
 1859692            snapshot.PartScale.Y);
 93        Fixed64 volume;
 1859694        if (HasCommittedShape && radius == ScaledRadius && height == Height)
 95        {
 1761096            volume = Volume;
 97        }
 98698        else if (!Fixed64.TryMultiplyDivideBySum(
 98699            Fixed64.Pi, radius, radius, height,
 986100            (Fixed64)3, Fixed64.Zero, Fixed64.Zero, Fixed64.Zero,
 986101            out volume))
 102        {
 103            // Saturate only the final diagnostic measure; valid geometry and
 104            // exact compound mass weights need not fit a scalar volume.
 9105            volume = Fixed64.MaxValue;
 106        }
 18596107        _preparedRadius = radius;
 18596108        _preparedHeight = height;
 18596109        SetPreparedGeometryReach(HasCommittedShape && radius == _scaledRadius && height == Height
 18596110            ? CanonicalCenteredProxyRadius : ColliderGeometryPolicy.FiniteAxis(radius, height));
 18596111        _preparedVolume = volume;
 18596112        _preparedAxis = HasCommittedShape && snapshot.Rotation == CanonicalRotation
 18596113            ? WorldAxis
 18596114            : (snapshot.Rotation * Vector3d.Up).Normalized;
 115        // The complete admitted body-relative reach also bounds this primitive's
 116        // centroid; bodyless attachments retain their independent scalar check.
 18596117        if (CompoundOwner == null
 18596118            && Body == null
 18596119            && !CalculatePreparedLocalMassPoint().TryGetPoint(out _))
 120        {
 2121            throw new InvalidOperationException(
 2122                "Prepared collider mass-property point is outside the Fixed64 coordinate domain.");
 123        }
 18594124        SetPreparedBounds(FixedBoundBox.FromCenteredFiniteConeClippedToDomain(
 18594125            snapshot.Center,
 18594126            snapshot.Rotation,
 18594127            Vector3d.Up,
 18594128            _preparedHeight,
 18594129            _preparedRadius));
 18594130    }
 131
 132    private protected override void PublishShape()
 133    {
 18589134        _scaledRadius = _preparedRadius;
 18589135        Height = _preparedHeight;
 18589136        WorldAxis = _preparedAxis;
 18589137        Volume = _preparedVolume;
 18589138        Area = _preparedVolume;
 18589139    }
 140
 141    internal override ExactMassWeight CalculateMassPropertyWeight()
 142    {
 54143        GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height);
 54144        return ExactMassWeight.FromProduct(
 54145            Fixed64.Pi / (Fixed64)3,
 54146            radius,
 54147            radius,
 54148            height);
 149    }
 150
 151    private void GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height)
 152    {
 909153        if (HasCommittedShape)
 154        {
 899155            radius = _scaledRadius;
 899156            height = Height;
 899157            return;
 158        }
 159
 10160        GetCurrentShapeScales(out Vector3d ownerScale, out Vector3d partScale);
 10161        radius = FixedMath.Max(
 10162            ColliderScalePolicy.ScalePositive(Radius, ownerScale.X, partScale.X),
 10163            ColliderScalePolicy.ScalePositive(Radius, ownerScale.Z, partScale.Z));
 10164        height = ColliderScalePolicy.ScalePositive(Size.Y, ownerScale.Y, partScale.Y);
 10165    }
 166
 167    internal override ExactMassWeight CalculatePreparedMassPropertyWeight() =>
 5343168        ExactMassWeight.FromProduct(
 5343169            Fixed64.Pi / (Fixed64)3,
 5343170            _preparedRadius,
 5343171            _preparedRadius,
 5343172            _preparedHeight);
 173
 174    internal override ExactMassPoint3D CalculateLocalMassPoint() =>
 8894175        TransformRelativeMassPropertyPointExact(
 8894176            GetLocalCenterOfMass(
 8894177                HasCommittedShape
 8894178                    ? Height
 8894179                    : GetCurrentHeight()));
 180
 181    internal override ExactMassPoint3D CalculatePreparedLocalMassPoint() =>
 3661182        TransformPreparedRelativeMassPropertyPointExact(
 3661183            GetLocalCenterOfMass(_preparedHeight));
 184
 185    private Fixed64 GetCurrentHeight()
 186    {
 9187        GetCurrentShapeScales(
 9188            out Vector3d ownerScale,
 9189            out Vector3d partScale);
 9190        return ColliderScalePolicy.ScalePositive(
 9191            Size.Y,
 9192            ownerScale.Y,
 9193            partScale.Y);
 194    }
 195
 196    private static Vector3d GetLocalCenterOfMass(Fixed64 height) =>
 12555197        new(
 12555198            Fixed64.Zero,
 12555199            -height * Fixed64.FromFraction(1, 4),
 12555200            Fixed64.Zero);
 201
 202    internal void GetInertiaGeometry(out Signed192 transverse, out Signed192 axial)
 203    {
 855204        GetMassPropertyGeometry(out Fixed64 radius, out Fixed64 height);
 855205        transverse = WideArithmetic.AddSigned192(
 855206            ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 20), radius, radius),
 855207            ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 80), height, height));
 855208        axial = ExactMassProperties.InertiaGeometryTerm(Fixed64.FromFraction(3, 10), radius, radius);
 855209    }
 210
 211    internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass)
 212    {
 837213        GetInertiaGeometry(out Signed192 transverse, out Signed192 axial);
 837214        Fixed64 inertiaXZ = ExactMassProperties.InertiaMoment(mass, transverse);
 837215        Fixed64 inertiaY = ExactMassProperties.InertiaMoment(mass, axial);
 216
 837217        Fixed3x3 tensor = new(
 837218            inertiaXZ, Fixed64.Zero, Fixed64.Zero,
 837219            Fixed64.Zero, inertiaY, Fixed64.Zero,
 837220            Fixed64.Zero, Fixed64.Zero, inertiaXZ);
 837221        return tensor;
 222    }
 223
 224    /// <inheritdoc/>
 225    public override Fixed64 GetFrontalArea(Vector3d direction)
 226    {
 3227        Fixed64 directionMagnitude = direction.Magnitude;
 3228        if (directionMagnitude <= Fixed64.Epsilon)
 1229            return Area;
 230
 2231        Vector3d normalizedDirection = direction / directionMagnitude;
 2232        Fixed64 axial = Rotation.Inverse()
 2233            .Rotate(normalizedDirection).Y.Abs();
 2234        Fixed64 radialFactorSqr = Fixed64.One - axial * axial;
 2235        Fixed64 radialFactor = radialFactorSqr <= Fixed64.Zero ? Fixed64.Zero : FixedMath.Sqrt(radialFactorSqr);
 236
 2237        Fixed64 baseArea = Fixed64.Pi * ScaledRadiusSqr;
 2238        Fixed64 triangularProfile = ScaledRadius * Height;
 2239        return axial * baseArea + radialFactor * triangularProfile;
 240    }
 241
 242    /// <inheritdoc/>
 243    public override Vector3d ClosestPointOnSurface(Vector3d other)
 244    {
 10245        FixedPointAnchor surfaceAnchor =
 10246            GetClosestSurfaceAnchor(other, out _);
 10247        if (!surfaceAnchor.TryGetPoint(out Vector3d surfacePoint))
 248        {
 1249            throw new InvalidOperationException(
 1250                "The closest cone surface point is outside the representable coordinate domain.");
 251        }
 252
 9253        return surfacePoint;
 254    }
 255
 256    /// <inheritdoc/>
 257    public override Vector3d GetNormalAtPoint(Vector3d point)
 258    {
 1298259        _ = GetClosestSurfaceAnchor(
 1298260            point,
 1298261            out Vector3d outwardNormal);
 1298262        return outwardNormal;
 263    }
 264
 265    internal override FixedPointAnchor GetClosestSurfaceAnchor(
 266        Vector3d point,
 267        out Vector3d normal) =>
 1849268        GetClosestSurfaceAnchor(point, out normal, out _);
 269
 270    internal override FixedPointAnchor GetClosestSurfaceAnchor(
 271        Vector3d point, out Vector3d normal, out bool contained) =>
 2012272        WideFiniteAxisIntersection
 2012273            .GetClosestCenteredFiniteConeSurfaceAnchor(
 2012274                point,
 2012275                Center,
 2012276                Rotation,
 2012277                Vector3d.Up,
 2012278                Height,
 2012279                ScaledRadius,
 2012280                Vector3d.Right,
 2012281                out normal,
 2012282                out contained);
 283
 284    /// <inheritdoc/>
 285    public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin
 622286        worker.CheckConeOverlaps(this, ref outputIntersectionPoints);
 287
 288    internal bool ContainsWorldPoint(
 289        Vector3d point,
 290        Fixed64 tolerance = default) =>
 6291        FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor(
 6292            point,
 6293            Center,
 6294            Rotation,
 6295            Vector3d.Up,
 6296            Height,
 6297            ScaledRadius,
 6298            Vector3d.Right,
 6299            out _,
 6300            out _,
 6301            out Fixed64 signedDistance)
 6302        && signedDistance <= tolerance;
 303}