< Summary

Information
Class: Gravitas.Colliders.LSCompoundCollider
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Colliders/Compound/LSCompoundCollider.cs
Line coverage
100%
Covered lines: 190
Uncovered lines: 0
Coverable lines: 190
Total lines: 386
Line coverage: 100%
Branch coverage
100%
Covered branches: 74
Total branches: 74
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/Compound/LSCompoundCollider.cs

#LineLine coverage
 1//=======================================================================
 2// LSCompoundCollider.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 System.Runtime.CompilerServices;
 10using FixedMathSharp;
 11using FixedMathSharp.Geometry;
 12using Gravitas.Materials;
 13using Gravitas.Queries;
 14using SwiftCollections;
 15
 16namespace Gravitas.Colliders;
 17
 18/// <summary>
 19/// Represents one collider identity whose collision shape is composed from
 20/// deterministic internal primitive and convex-mesh parts.
 21/// </summary>
 22public sealed class LSCompoundCollider : LSCollider
 23{
 24    private readonly CompoundColliderPart[] _parts;
 25    private readonly LSCollider[] _partColliders;
 26    private readonly ExactMassPoint3D[] _massPointScratch;
 27    private readonly ExactMassWeight[] _massWeightScratch;
 28    private bool _hasMassPropertyCenter, _massPropertyCommitted;
 29    private Vector3d _massPropertyCenter, _massPropertyOwnerScale, _massPropertyOffset;
 30    private bool _inertiaGeometryValid, _inertiaGeometryCommitted;
 31    private Vector3d _inertiaGeometryScale, _inertiaGeometryOffset, _inertiaGeometryReference;
 32    private ExactMassWeight _inertiaGeometryWeight;
 33    private CompoundInertiaTensor _inertiaGeometry;
 34
 35
 36    /// <summary>Creates a runtime compound collider from authored 3D parts.</summary>
 50537    public LSCompoundCollider(params CompoundColliderPart[] parts)
 38    {
 50539        ChronicleGuard.ThrowIfNull(parts, nameof(parts));
 50540        ChronicleGuard.ThrowIfArgument(parts.Length == 0, nameof(parts), "Compound collider must contain at least one pa
 41
 260642        for (int i = 0; i < parts.Length; i++)
 79943            ValidatePart(parts[i]);
 44
 50445        _parts = new CompoundColliderPart[parts.Length];
 50446        _partColliders = new LSCollider[parts.Length];
 50447        _massPointScratch = new ExactMassPoint3D[parts.Length];
 50448        _massWeightScratch = new ExactMassWeight[parts.Length];
 260449        for (int i = 0; i < parts.Length; i++)
 50        {
 79851            _parts[i] = parts[i];
 79852            _partColliders[i] = MaterializePartCollider(parts[i]);
 79853            _partColliders[i].ReserveCompoundPart(
 79854                this,
 79855                parts[i].LocalRotation,
 79856                parts[i].LocalScale);
 57        }
 50458    }
 59
 60    /// <inheritdoc/>
 49140761    public override ColliderType Shape => ColliderType.Compound;
 62
 63    /// <inheritdoc/>
 46964    public override int Priority => ColliderSettings.GetPriority(Shape);
 65
 66    /// <inheritdoc/>
 67    public override Fixed64 ScaledRadius
 68    {
 69        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 3570        get => HasCommittedShape
 3571            ? CanonicalCenteredProxyRadius
 3572            : ColliderCanonicalBounds
 3573                .GetCurrentCenteredProxyRadius(this);
 74    }
 75
 76    /// <summary>Gets the authored parts in stable source order.</summary>
 13277    public ReadOnlySpan<CompoundColliderPart> Parts => _parts;
 78
 79    /// <summary>Gets the number of authored compound parts.</summary>
 80    public int PartCount
 81    {
 82        [MethodImpl(MethodImplOptions.AggressiveInlining)]
 9188883        get => _parts.Length;
 84    }
 85
 86    /// <summary>Gets the stable runtime part identifier for a source-order index.</summary>
 87    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 88    public int GetPartId(int index)
 89    {
 290        ChronicleGuard.ThrowIfArrayIndexInvalid(index, _parts.Length, nameof(index));
 291        return index;
 92    }
 93
 94    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 95    internal LSCollider GetPartCollider(int index)
 96    {
 5732597        ChronicleGuard.ThrowIfArrayIndexInvalid(index, _parts.Length, nameof(index));
 5732598        return _partColliders[index];
 99    }
 100
 101    internal Fixed64 GetMutationGeometryReach(LSCollider changed, in ColliderShapeSnapshot candidate)
 102    {
 3103        Fixed64 reach = Fixed64.Zero;
 12104        for (int i = 0; i < _parts.Length; i++)
 105        {
 3106            LSCollider part = _partColliders[i];
 3107            ColliderShapeSnapshot snapshot = ReferenceEquals(part, changed) ? candidate
 3108                : new ColliderShapeSnapshot(default, default, candidate.OwnerScale, _parts[i].LocalScale,
 3109                    part.LocalOffset, part.Size, part.Radius);
 3110            reach = FixedMath.Max(reach, ColliderGeometryPolicy.Add(
 3111                ColliderGeometryPolicy.Attachment(snapshot.LocalOffset, snapshot.OwnerScale),
 3112                part.GetCandidateGeometryReach(snapshot)));
 113        }
 3114        return reach;
 115    }
 116
 117    private protected override void PrepareShape(in ColliderShapeSnapshot snapshot)
 118    {
 20173119        Vector3d min = Vector3d.Zero;
 20173120        Vector3d max = Vector3d.Zero;
 121
 20173122        Fixed64 reach = Fixed64.Zero;
 81812123        for (int i = 0; i < _parts.Length; i++)
 124        {
 20738125            CompoundColliderPart part = _parts[i];
 20738126            LSCollider partCollider = _partColliders[i];
 20738127            partCollider.PrepareCompoundPart(
 20738128                snapshot,
 20738129                part.LocalRotation,
 20738130                part.LocalScale,
 20738131                PreparedContext);
 20733132            reach = FixedMath.Max(reach, ColliderGeometryPolicy.Add(
 20733133                partCollider.PreparedAttachmentReach,
 20733134                partCollider.PreparedGeometryReach));
 135
 20733136            if (i == 0)
 137            {
 20168138                min = partCollider.PreparedShapeBounds.Min;
 20168139                max = partCollider.PreparedShapeBounds.Max;
 140            }
 141            else
 142            {
 565143                min = Vector3d.Min(min, partCollider.PreparedShapeBounds.Min);
 565144                max = Vector3d.Max(max, partCollider.PreparedShapeBounds.Max);
 145            }
 146        }
 147
 20168148        _ = CalculatePreparedLocalMassPoint();
 20167149        _ = CalculatePreparedMassPropertyWeight();
 20167150        SetPreparedGeometryReach(reach);
 20167151        SetPreparedBounds(FixedBoundBox.FromMinMax(min, max));
 20167152    }
 153
 154    private protected override void PublishShape()
 155    {
 20164156        Fixed64 area = Fixed64.Zero;
 81786157        for (int i = 0; i < _parts.Length; i++)
 158        {
 20729159            CompoundColliderPart part = _parts[i];
 20729160            _partColliders[i].PublishCompoundPart(
 20729161                part.LocalRotation,
 20729162                part.LocalScale,
 20729163                PreparedContext);
 20729164            area += _partColliders[i].Area;
 165        }
 166
 20164167        Area = area;
 20164168    }
 169
 170    internal override ExactMassPoint3D CalculateLocalMassPoint()
 171    {
 9953172        GetCurrentShapeScales(out Vector3d scale, out _);
 9953173        return _hasMassPropertyCenter && _massPropertyOwnerScale == scale
 9953174            && _massPropertyOffset == LocalOffset && _massPropertyCommitted == HasCommittedShape
 9953175            ? ExactMassPoint3D.FromPoint(_massPropertyCenter)
 9953176            : CalculateAggregateMassPoint(usePrepared: false);
 177    }
 178
 179    internal override ExactMassPoint3D CalculatePreparedLocalMassPoint() =>
 20168180        CalculateAggregateMassPoint(usePrepared: true);
 181
 182    private ExactMassPoint3D CalculateAggregateMassPoint(bool usePrepared)
 183    {
 20409184        bool hasPositiveWeight = false;
 83152185        for (int i = 0; i < _partColliders.Length; i++)
 186        {
 21167187            LSCollider part = _partColliders[i];
 21167188            ExactMassWeight weight = usePrepared
 21167189                ? part.CalculatePreparedMassPropertyWeight()
 21167190                : part.CalculateMassPropertyWeight();
 21167191            _massPointScratch[i] = usePrepared
 21167192                ? part.CalculatePreparedLocalMassPoint()
 21167193                : part.CalculateLocalMassPoint();
 21167194            _massWeightScratch[i] = weight;
 21167195            hasPositiveWeight |= !weight.IsZero;
 196        }
 197
 20409198        if (!hasPositiveWeight)
 199        {
 200            // Admitted solids and shells have positive exact weights. Only
 201            // unsupported closed-mesh parts need this equal-point fallback.
 78202            for (int i = 0; i < _massWeightScratch.Length; i++)
 22203                _massWeightScratch[i] = ExactMassWeight.One;
 204        }
 205
 20409206        if (!ExactMassPoint3D.TryGetWeightedAverage(
 20409207                _massPointScratch,
 20409208                _massWeightScratch,
 20409209                out Vector3d center))
 210        {
 2211            throw new InvalidOperationException(
 2212                usePrepared
 2213                    ? "Prepared compound mass-property point is outside the Fixed64 coordinate domain."
 2214                    : "The compound collider's center of mass is outside the Fixed64 coordinate domain.");
 215        }
 20407216        if (!usePrepared)
 217        {
 240218            GetCurrentShapeScales(out _massPropertyOwnerScale, out _);
 240219            _massPropertyOffset = LocalOffset;
 240220            _massPropertyCommitted = HasCommittedShape;
 240221            _massPropertyCenter = center;
 240222            _hasMassPropertyCenter = true;
 223        }
 20407224        return ExactMassPoint3D.FromPoint(center);
 225    }
 226
 227    internal override ExactMassWeight CalculateMassPropertyWeight() =>
 1228        CalculateAggregateMassWeight(usePrepared: false);
 229
 230    internal override ExactMassWeight CalculatePreparedMassPropertyWeight() =>
 20167231        CalculateAggregateMassWeight(usePrepared: true);
 232
 233    private ExactMassWeight CalculateAggregateMassWeight(bool usePrepared)
 234    {
 20168235        ExactMassWeight totalWeight = ExactMassWeight.Zero;
 81802236        for (int i = 0; i < _parts.Length; i++)
 237        {
 20733238            ExactMassWeight weight = usePrepared
 20733239                ? _partColliders[i].CalculatePreparedMassPropertyWeight()
 20733240                : _partColliders[i].CalculateMassPropertyWeight();
 20733241            totalWeight = totalWeight.Add(weight);
 242        }
 20168243        return totalWeight;
 244    }
 245
 246    internal override Fixed3x3 CalculateCenterOfMassInertiaTensor(Fixed64 mass) =>
 1247        CalculateInertiaTensor(mass, CalculateLocalCenterOfMassOffset());
 248
 249    /// <inheritdoc/>
 250    public override Fixed3x3 CalculateInertiaTensor(Fixed64 mass, Vector3d localCenterOfMassOffset)
 251    {
 2437252        if (mass <= Fixed64.Zero)
 3253            return Fixed3x3.Zero;
 2434254        GetCurrentShapeScales(out Vector3d scale, out _);
 2434255        if (!_inertiaGeometryValid || _inertiaGeometryCommitted != HasCommittedShape || _inertiaGeometryScale != scale
 2434256            || _inertiaGeometryOffset != LocalOffset || _inertiaGeometryReference != localCenterOfMassOffset)
 257        {
 184258            ExactMassWeight total = ExactMassWeight.Zero;
 184259            CompoundInertiaTensor geometry = default;
 948260            for (int i = 0; i < _partColliders.Length; i++)
 261            {
 294262                LSCollider part = _partColliders[i];
 294263                if (!part.SupportsMassProperties)
 264                    continue;
 286265                ExactMassWeight weight = part.CalculateMassPropertyWeight();
 285266                total = total.Add(weight);
 285267                CompoundInertiaMath3D.Add(ref geometry, part, weight, localCenterOfMassOffset);
 268            }
 180269            if (total.IsZero)
 3270                throw new InvalidOperationException("Compound inertia requires at least one part with valid mass propert
 271
 272            // Cache geometry only after a complete evaluation. Neither a mass
 273            // change nor world pose changes the local intrinsic contribution.
 177274            _inertiaGeometry = geometry;
 177275            _inertiaGeometryWeight = total;
 177276            _inertiaGeometryScale = scale;
 177277            _inertiaGeometryOffset = LocalOffset;
 177278            _inertiaGeometryReference = localCenterOfMassOffset;
 177279            _inertiaGeometryCommitted = HasCommittedShape;
 177280            _inertiaGeometryValid = true;
 281        }
 2427282        return _inertiaGeometry.Materialize(mass, _inertiaGeometryWeight);
 283    }
 284
 285    /// <inheritdoc/>
 286    public override Fixed64 GetFrontalArea(Vector3d direction)
 287    {
 3288        Fixed64 area = Fixed64.Zero;
 18289        for (int i = 0; i < _parts.Length; i++)
 6290            area += _partColliders[i].GetFrontalArea(direction);
 3291        return area;
 292    }
 293
 294    /// <inheritdoc/>
 295    public override Vector3d ClosestPointOnSurface(Vector3d other)
 296    {
 4297        FixedPointAnchor anchor =
 4298            GetClosestSurfaceAnchor(other, out _);
 4299        if (anchor.TryGetPoint(out Vector3d point))
 3300            return point;
 301
 1302        throw new InvalidOperationException(
 1303            "The closest compound surface point is outside the representable coordinate domain.");
 304    }
 305
 306    /// <inheritdoc/>
 307    public override Vector3d GetNormalAtPoint(Vector3d point)
 308    {
 10309        _ = GetClosestSurfaceAnchor(
 10310            point,
 10311            out Vector3d normal);
 10312        return normal;
 313    }
 314
 315    internal override FixedPointAnchor GetClosestSurfaceAnchor(
 316        Vector3d point,
 317        out Vector3d normal) =>
 14318        GetClosestSurfaceAnchor(point, out normal, out _);
 319
 320    internal FixedPointAnchor GetClosestSurfaceAnchor(
 321        Vector3d point,
 322        out Vector3d normal,
 323        out LSCollider surfaceOwner)
 324    {
 1237325        var reference = new FixedPointAnchor(
 1237326            point,
 1237327            FixedQuaternion.Identity,
 1237328            Vector3d.Zero);
 1237329        surfaceOwner = _partColliders[0];
 1237330        FixedPointAnchor closest =
 1237331            surfaceOwner.GetClosestSurfaceAnchor(
 1237332                point,
 1237333                out normal);
 2614334        for (int i = 1; i < _partColliders.Length; i++)
 335        {
 70336            FixedPointAnchor candidate =
 70337                _partColliders[i].GetClosestSurfaceAnchor(
 70338                    point,
 70339                    out Vector3d candidateNormal);
 70340            if (reference.CompareSquaredDistance(
 70341                    candidate,
 70342                    closest) >= 0)
 343            {
 344                continue;
 345            }
 346
 29347            closest = candidate;
 29348            normal = candidateNormal;
 29349            surfaceOwner = _partColliders[i];
 350        }
 351
 1237352        return closest;
 353    }
 354
 355    /// <inheritdoc/>
 356    public override bool ColliderOverlapsRay(RaycastSegmentWorker worker, ref SwiftList<Vector3d> outputIntersectionPoin
 357    {
 10358        bool hit = false;
 56359        for (int i = 0; i < _parts.Length; i++)
 18360            hit |= _partColliders[i].ColliderOverlapsRay(worker, ref outputIntersectionPoints);
 10361        return hit;
 362    }
 363
 364    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 365    internal bool ContainsPartCollider(LSCollider collider) =>
 11380366        ReferenceEquals(collider.CompoundOwner, this);
 367
 368    private static LSCollider MaterializePartCollider(CompoundColliderPart part)
 369    {
 798370        LSCollider collider = part.Shape.CreateRuntimeCollider();
 798371        collider.LocalOffset = part.LocalOffset;
 798372        collider.Material = part.ResolveMaterial(PhysicsMaterial.Default);
 798373        return collider;
 374    }
 375
 376    /// <inheritdoc/>
 377    protected override void OnMaterialChanged()
 378    {
 598379        for (int i = 0; i < _parts.Length; i++)
 168380            _partColliders[i].Material = _parts[i].ResolveMaterial(Material);
 131381    }
 382
 383    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 384    private static void ValidatePart(CompoundColliderPart part) =>
 799385        ChronicleGuard.ThrowIfArgument(part.IsDefault, nameof(part), "Compound collider part cannot be default.");
 386}