< Summary

Line coverage
100%
Covered lines: 922
Uncovered lines: 0
Coverable lines: 922
Total lines: 1683
Line coverage: 100%
Branch coverage
100%
Covered branches: 391
Total branches: 391
Branch coverage: 100%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
File 1: DoCapsuleSphereCheck(...)100%22100%
File 1: DoCapsulesCheck(...)100%22100%
File 1: ResolveCenteredCapsuleFallback(...)100%1212100%
File 2: DoCompoundCheck(...)100%22100%
File 2: DoCompoundOtherCheck(...)100%22100%
File 2: DoCompoundCompoundCheck(...)100%44100%
File 2: TryBuildCompoundPartContact(...)100%44100%
File 2: AddCompoundPartContactsInOwnerOrder(...)100%1414100%
File 2: OrderPartPairForDetection(...)100%22100%
File 2: BoundsOverlapInclusive(...)100%1010100%
File 3: DoConeSphereCheck(...)100%44100%
File 3: DoConeConvexCheck(...)100%22100%
File 3: DoMeshConeCheck(...)100%2222100%
File 3: IntersectsMeshConeSurface(...)100%66100%
File 3: BuildMeshConeSurfaceContacts(...)100%2222100%
File 3: GetConeConvexColliders(...)100%44100%
File 4: DoCollisionCheck(...)100%11100%
File 4: DoCollisionCheck(...)100%2121100%
File 5: DoCuboidSphereCheck(...)100%22100%
File 5: DoCuboidCapsuleCheck(...)100%22100%
File 5: DoCuboidsCheck(...)100%3636100%
File 5: CanBuildAxisAlignedManifold(...)100%44100%
File 5: TryBuildAxisAlignedCuboidManifold(...)100%2020100%
File 5: AddAxisAlignedCuboidContacts(...)100%1010100%
File 5: AddCuboidContact(...)100%11100%
File 6: .ctor(...)100%11100%
File 6: TryBuildCuboidFaceContacts(...)100%5454100%
File 6: GetCuboidFaceAxis(...)100%22100%
File 6: IsCuboidFaceNormal(...)100%11100%
File 6: ClipCuboidFace(...)100%1414100%
File 6: InterpolateCuboidFace(...)100%11100%
File 6: InterpolateCuboidFaceCoordinate(...)100%11100%
File 7: DoCylinderSphereCheck(...)100%44100%
File 7: DoCylinderCapsuleCheck(...)100%44100%
File 7: DoCylindersCheck(...)100%44100%
File 7: DoCuboidCylinderCheck(...)100%66100%
File 7: TryAddCylinderCylinderCapContacts(...)100%44100%
File 7: TryAddCylinderCylinderCapContact(...)100%11100%
File 7: ResolveNormal(...)100%22100%
File 7: SetContactInPairOrder(...)100%22100%
File 8: DoMeshSphereCheck(...)100%44100%
File 8: DoMeshCapsuleCheck(...)100%22100%
File 8: TrySetConvexMeshRoundedContact(...)100%22100%
File 8: DoMeshCuboidCheck(...)100%66100%
File 8: DoMeshCylinderCheck(...)100%66100%
File 8: DoMeshesCheck(...)100%1414100%
File 9: DoesPostureCandidatePenetrate(...)100%88100%
File 9: DoesPostureLeafPenetrate(...)100%44100%
File 9: DoesPostureCapsulePenetrate(...)100%1414100%
File 9: DoesPostureCylinderPenetrate(...)100%1010100%
File 9: DoesPostureCapsulePenetrateMesh(...)100%88100%
File 9: DoesPostureCylinderPenetrateMesh(...)100%66100%
File 10: DoSpheresCheck(...)100%22100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Capsule.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Capsule.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 FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.Colliders;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <content>Finds contacts for capsule pairs and capsule-sphere pairs.</content>
 15public static partial class CollisionDetection
 16{
 17    #region Capsule
 18
 19    private static bool DoCapsuleSphereCheck(CollisionWorkItem pair)
 20    {
 22421        var capsule = (LSCapsuleCollider)pair.ColliderA;
 22422        var sphere = (LSSphereCollider)pair.ColliderB;
 23
 22424        if (!FixedSegment.TryGetCenteredCapsulesContact(
 22425                capsule.Center,
 22426                capsule.Rotation,
 22427                Vector3d.Up,
 22428                capsule.AxisLength,
 22429                capsule.ScaledRadius,
 22430                sphere.Center,
 22431                sphere.Rotation,
 22432                Vector3d.Up,
 22433                Fixed64.Zero,
 22434                sphere.ScaledRadius,
 22435                ResolveCenteredCapsuleFallback(capsule.Center, sphere.Center),
 22436                out FixedContactAnchors contact))
 37        {
 138            return false;
 39        }
 40
 22341        pair.Manifold.SetContact(
 22342            new ContactAnchor(contact.FirstAnchor),
 22343            new ContactAnchor(contact.SecondAnchor),
 22344            contact.Depth,
 22345            contact.Normal,
 22346            contact.DepthIsClamped);
 22347        return true;
 48    }
 49
 50    private static bool DoCapsulesCheck(CollisionWorkItem pair)
 51    {
 22152        var capsule1 = (LSCapsuleCollider)pair.ColliderA;
 22153        var capsule2 = (LSCapsuleCollider)pair.ColliderB;
 54
 22155        if (!FixedSegment.TryGetCenteredCapsulesContact(
 22156                capsule1.Center,
 22157                capsule1.Rotation,
 22158                Vector3d.Up,
 22159                capsule1.AxisLength,
 22160                capsule1.ScaledRadius,
 22161                capsule2.Center,
 22162                capsule2.Rotation,
 22163                Vector3d.Up,
 22164                capsule2.AxisLength,
 22165                capsule2.ScaledRadius,
 22166                ResolveCenteredCapsuleFallback(capsule1.Center, capsule2.Center),
 22167                out FixedContactAnchors contact))
 68        {
 169            return false;
 70        }
 71
 22072        pair.Manifold.SetContact(
 22073            new ContactAnchor(contact.FirstAnchor),
 22074            new ContactAnchor(contact.SecondAnchor),
 22075            contact.Depth,
 22076            contact.Normal,
 22077            contact.DepthIsClamped);
 22078        return true;
 79    }
 80
 81    private static Vector3d ResolveCenteredCapsuleFallback(
 82        Vector3d firstCenter,
 83        Vector3d secondCenter)
 84    {
 169485        if (secondCenter.X != firstCenter.X)
 162786            return secondCenter.X > firstCenter.X ? Vector3d.Right : Vector3d.Left;
 6787        if (secondCenter.Y != firstCenter.Y)
 3088            return secondCenter.Y > firstCenter.Y ? Vector3d.Up : Vector3d.Down;
 3789        if (secondCenter.Z != firstCenter.Z)
 290            return secondCenter.Z > firstCenter.Z ? Vector3d.Forward : Vector3d.Backward;
 3591        return Vector3d.Right;
 92    }
 93
 94    #endregion
 95
 96}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Compound.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Compound.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.Runtime.CompilerServices;
 9using Gravitas.Colliders;
 10
 11namespace Gravitas.CollisionHandling;
 12
 13/// <content>
 14/// Dispatches compound collider part pairs and assembles contacts in owner order.
 15/// </content>
 16public static partial class CollisionDetection
 17{
 18    #region Compound
 19
 20    private static bool DoCompoundCheck(CollisionWorkItem pair)
 21    {
 1230222        var compoundA = (LSCompoundCollider)pair.ColliderA;
 1230223        if (pair.ColliderB is LSCompoundCollider compoundB)
 24        {
 625            return DoCompoundCompoundCheck(pair, compoundA, compoundB);
 26        }
 27
 1229628        return DoCompoundOtherCheck(pair, compoundA, pair.ColliderB);
 29    }
 30
 31    private static bool DoCompoundOtherCheck(
 32        CollisionWorkItem pair,
 33        LSCompoundCollider compound,
 34        LSCollider other)
 35    {
 1229636        bool collided = false;
 4934637        for (int i = 0; i < compound.PartCount; i++)
 38        {
 1237739            LSCollider part = compound.GetPartCollider(i);
 1237740            collided |= TryBuildCompoundPartContact(
 1237741                pair,
 1237742                part,
 1237743                featureNamespaceA: i + 1,
 1237744                other,
 1237745                featureNamespaceB: 0);
 46        }
 47
 1229648        return collided;
 49    }
 50
 51    private static bool DoCompoundCompoundCheck(
 52        CollisionWorkItem pair,
 53        LSCompoundCollider compoundA,
 54        LSCompoundCollider compoundB)
 55    {
 656        bool collided = false;
 7657        for (int i = 0; i < compoundA.PartCount; i++)
 58        {
 3259            LSCollider partA = compoundA.GetPartCollider(i);
 56860            for (int j = 0; j < compoundB.PartCount; j++)
 61            {
 25262                collided |= TryBuildCompoundPartContact(
 25263                    pair,
 25264                    partA,
 25265                    featureNamespaceA: i + 1,
 25266                    compoundB.GetPartCollider(j),
 25267                    featureNamespaceB: -(j + 1));
 68            }
 69        }
 70
 671        return collided;
 72    }
 73
 74    private static bool TryBuildCompoundPartContact(
 75        CollisionWorkItem ownerPair,
 76        LSCollider first,
 77        int featureNamespaceA,
 78        LSCollider second,
 79        int featureNamespaceB)
 80    {
 1262981        if (!BoundsOverlapInclusive(first, second))
 28482            return false;
 83
 1234584        OrderPartPairForDetection(
 1234585            first,
 1234586            featureNamespaceA,
 1234587            second,
 1234588            featureNamespaceB,
 1234589            out LSCollider colliderA,
 1234590            out int orderedNamespaceA,
 1234591            out LSCollider colliderB,
 1234592            out int orderedNamespaceB);
 1234593        CollisionType collisionType = ColliderSettings.GetCollisionType(colliderA.Shape, colliderB.Shape);
 1234594        ContactManifold scratch = ownerPair.Context.CollisionScratch.CompoundPartManifold;
 1234595        scratch.BeginUpdate(ownerPair.Context.FrameCount);
 1234596        var partPair = new CollisionWorkItem(ownerPair.Context, colliderA, colliderB, collisionType, scratch);
 1234597        if (!DoCollisionCheck(partPair))
 96598            return false;
 99
 11380100        AddCompoundPartContactsInOwnerOrder(
 11380101            ownerPair,
 11380102            partPair,
 11380103            orderedNamespaceA,
 11380104            orderedNamespaceB);
 11380105        return true;
 106    }
 107
 108    private static void AddCompoundPartContactsInOwnerOrder(
 109        CollisionWorkItem ownerPair,
 110        CollisionWorkItem partPair,
 111        int featureNamespaceA,
 112        int featureNamespaceB)
 113    {
 11380114        bool addInPartOrder = ((LSCompoundCollider)ownerPair.ColliderA).ContainsPartCollider(partPair.ColliderA);
 115
 11380116        ContactManifold scratch = partPair.Manifold;
 45012117        for (int groupIndex = 0; groupIndex < scratch.GroupCount; groupIndex++)
 118        {
 11126119            ref ContactGroup group = ref scratch.GetGroup(groupIndex);
 73712120            for (int i = 0; i < group.Count; i++)
 121            {
 25730122                ManifoldContact contact = group[i];
 25730123                if (addInPartOrder)
 124                {
 5504125                    ownerPair.Manifold.AddContact(
 5504126                        contact.AnchorA,
 5504127                        contact.AnchorB,
 5504128                        contact.Depth,
 5504129                        contact.Normal,
 5504130                        contact.HasMaterialOverride ? contact.MaterialA : partPair.ColliderA.Material,
 5504131                        contact.HasMaterialOverride ? contact.MaterialB : partPair.ColliderB.Material,
 5504132                        contact.DepthIsClamped,
 5504133                        featureNamespaceA,
 5504134                        featureNamespaceB,
 5504135                        group.Key.Remap(featureNamespaceA, featureNamespaceB, reverse: false),
 5504136                        contactIdentity: ContactManifold.RemapContactIdentity(contact.ContactId, featureNamespaceA, feat
 5504137                    continue;
 138                }
 139
 20226140                ownerPair.Manifold.AddContact(
 20226141                    contact.AnchorB,
 20226142                    contact.AnchorA,
 20226143                    contact.Depth,
 20226144                    -contact.Normal,
 20226145                    contact.HasMaterialOverride ? contact.MaterialB : partPair.ColliderB.Material,
 20226146                    contact.HasMaterialOverride ? contact.MaterialA : partPair.ColliderA.Material,
 20226147                    contact.DepthIsClamped,
 20226148                    featureNamespaceB,
 20226149                    featureNamespaceA,
 20226150                    group.Key.Remap(featureNamespaceB, featureNamespaceA, reverse: true),
 20226151                    contactIdentity: ContactManifold.RemapContactIdentity(contact.ContactId, featureNamespaceA, featureN
 152            }
 153        }
 11380154    }
 155
 156    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 157    private static void OrderPartPairForDetection(
 158        LSCollider first,
 159        int firstFeatureNamespace,
 160        LSCollider second,
 161        int secondFeatureNamespace,
 162        out LSCollider colliderA,
 163        out int featureNamespaceA,
 164        out LSCollider colliderB,
 165        out int featureNamespaceB)
 166    {
 12345167        if (first.Priority >= second.Priority)
 168        {
 1794169            colliderA = first;
 1794170            featureNamespaceA = firstFeatureNamespace;
 1794171            colliderB = second;
 1794172            featureNamespaceB = secondFeatureNamespace;
 1794173            return;
 174        }
 175
 10551176        colliderA = second;
 10551177        featureNamespaceA = secondFeatureNamespace;
 10551178        colliderB = first;
 10551179        featureNamespaceB = firstFeatureNamespace;
 10551180    }
 181
 182    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 183    private static bool BoundsOverlapInclusive(LSCollider colliderA, LSCollider colliderB)
 184    {
 12629185        return colliderA.BoundsMin.X <= colliderB.BoundsMax.X
 12629186            && colliderA.BoundsMax.X >= colliderB.BoundsMin.X
 12629187            && colliderA.BoundsMin.Y <= colliderB.BoundsMax.Y
 12629188            && colliderA.BoundsMax.Y >= colliderB.BoundsMin.Y
 12629189            && colliderA.BoundsMin.Z <= colliderB.BoundsMax.Z
 12629190            && colliderA.BoundsMax.Z >= colliderB.BoundsMin.Z;
 191    }
 192
 193    #endregion
 194
 195}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Cone.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Cone.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.Runtime.CompilerServices;
 9using FixedMathSharp;
 10using FixedMathSharp.Geometry;
 11using Gravitas.Colliders;
 12using SwiftCollections.Query;
 13
 14namespace Gravitas.CollisionHandling;
 15
 16/// <content>Finds finite cone contacts against spheres, convex shapes, and mesh surfaces.</content>
 17public static partial class CollisionDetection
 18{
 19    private static bool DoConeSphereCheck(CollisionWorkItem pair)
 20    {
 9821        var cone = (LSConeCollider)pair.ColliderA;
 9822        var sphere = (LSSphereCollider)pair.ColliderB;
 23
 9824        if (!FixedSegment.TryGetClosestCenteredFiniteConeSurfaceAnchor(
 9825                sphere.Center,
 9826                cone.Center,
 9827                cone.Rotation,
 9828                Vector3d.Up,
 9829                cone.Height,
 9830                cone.ScaledRadius,
 9831                Vector3d.Right,
 9832                out FixedPointAnchor coneAnchor,
 9833                out Vector3d outwardNormal,
 9834                out Fixed64 signedDistance))
 35        {
 136            return false;
 37        }
 9738        if (signedDistance > sphere.ScaledRadius)
 4939            return false;
 40
 41        // Containment increases penetration depth; the sphere still escapes
 42        // along the solid surface's outward normal, not toward the cone center.
 4843        Vector3d normal = outwardNormal;
 44        // Admitted cone and sphere reaches bound containment depth by their
 45        // sum, so no scalar clamping is needed for this complete difference.
 4846        Fixed64 penetrationDepth = sphere.ScaledRadius - signedDistance;
 4847        pair.Manifold.SetContact(
 4848            new ContactAnchor(coneAnchor),
 4849            new ContactAnchor(
 4850                FixedSegment.GetCenteredCapsuleSupportAnchor(
 4851                    sphere.Center,
 4852                    sphere.Rotation,
 4853                    Fixed64.Zero,
 4854                    sphere.ScaledRadius,
 4855                    -normal)),
 4856            penetrationDepth,
 4857            normal,
 4858            depthIsClamped: false);
 59
 4860        return true;
 61    }
 62
 63    private static bool DoConeConvexCheck(CollisionWorkItem pair)
 64    {
 29965        GetConeConvexColliders(pair, out LSConeCollider cone, out LSCollider convex);
 66
 29967        if (!ConvexColliderSupport.Intersects(cone, convex))
 19368            return false;
 69
 10670        Vector3d normalConeToConvex = ResolveNormal(convex.Center - cone.Center);
 10671        normalConeToConvex = normalConeToConvex.Normalized;
 10672        FixedPointAnchor pointOnCone = ConvexColliderSupport.GetSupportAnchor(
 10673            cone,
 10674            normalConeToConvex,
 10675            Vector3d.Zero);
 10676        FixedPointAnchor pointOnConvex = ConvexColliderSupport.GetSupportAnchor(
 10677            convex,
 10678            -normalConeToConvex,
 10679            Vector3d.Zero);
 10680        Fixed64 depth = pointOnCone.ProjectNonNegativeOffsetFrom(
 10681            pointOnConvex,
 10682            normalConeToConvex);
 83
 10684        SetContactInPairOrder(
 10685            pair,
 10686            cone,
 10687            new ContactAnchor(pointOnCone),
 10688            convex,
 10689            new ContactAnchor(pointOnConvex),
 10690            depth,
 10691            normalConeToConvex,
 10692            depthIsClamped: false);
 10693        return true;
 94    }
 95
 96    private static bool DoMeshConeCheck(CollisionWorkItem pair)
 97    {
 1209498        var mesh = (LSMeshCollider)pair.ColliderA;
 1209499        var cone = (LSConeCollider)pair.ColliderB;
 100
 12094101        if (mesh.StaticSurface is { } surface && !mesh.IsTrigger && !cone.IsTrigger
 12094102            && surface.Gather(cone, new FixedBoundVolume(cone.BoundsMin, cone.BoundsMax)) > 1)
 103        {
 6381104            surface.BuildCone(cone);
 6381105            var faces = surface.ConeFaces;
 25598106            for (int region = 0; region < faces.RegionCount; region++)
 107            {
 6418108                if (!faces.IsResponseCarrier(mesh, region)) continue;
 31690109                for (int sample = 0; sample < faces.GetSampleCount(region); sample++)
 110                {
 12674111                    FixedContactAnchors contact = faces.GetJoinedSample(region, sample, out LSMeshCollider supplier, out
 12674112                    pair.Manifold.AddContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnc
 12674113                        contact.Depth, contact.Normal, supplier.Material, cone.Material, contact.DepthIsClamped,
 12674114                        group: new ContactGroupKey(0, 0, owner + 1, 0, region, sourceIdA: supplier.Id + 1,
 12674115                            sourceOrderA: ((IPhysicsColliderRegistryItem)supplier).ReplayOrder + 1),
 12674116                        contactIdentity: faces.GetSampleIdentity(region, sample));
 117                }
 118            }
 6381119            return faces.AppendJoinedBoundaries(mesh, cone, pair.Context.CollisionScratch.MeshConeBoundary, pair.Manifol
 120        }
 5713121        if (BuildMeshConeSurfaceContacts(mesh, cone, pair.Context.CollisionScratch, pair.Manifold))
 5541122            return true;
 123
 124        // An open sheet has no solid interior, even when its bounds were
 125        // authored in convex mode. Only a closed convex volume owns containment.
 172126        if (mesh.Mode == MeshColliderMode.Concave || !mesh.IsClosedSurface
 172127            || !ConvexColliderSupport.Intersects(mesh, cone))
 169128            return false;
 129
 3130        Vector3d normalMeshToCone = ResolveNormal(cone.Center - mesh.Center);
 3131        FixedPointAnchor pointOnMesh = ConvexColliderSupport.GetSupportAnchor(
 3132            mesh,
 3133            normalMeshToCone,
 3134            Vector3d.Zero);
 3135        FixedPointAnchor pointOnCone = ConvexColliderSupport.GetSupportAnchor(
 3136            cone,
 3137            -normalMeshToCone,
 3138            Vector3d.Zero);
 3139        Fixed64 depth = pointOnMesh.ProjectNonNegativeOffsetFrom(
 3140            pointOnCone,
 3141            normalMeshToCone);
 142
 3143        pair.Manifold.SetContact(
 3144            new ContactAnchor(pointOnMesh),
 3145            new ContactAnchor(pointOnCone),
 3146            depth,
 3147            normalMeshToCone);
 3148        return true;
 149    }
 150
 151    /// <summary>Tests finite triangle surfaces without containment, joined response ownership or contact materializatio
 152    internal static bool IntersectsMeshConeSurface(LSMeshCollider meshCollider, LSConeCollider cone,
 153        CollisionSatScratch scratch)
 154    {
 273155        var candidates = scratch.MeshTriangleCandidatesA;
 273156        meshCollider.GetTrianglesInBounds(new FixedBoundVolume(cone.BoundsMin, cone.BoundsMax), candidates);
 273157        PhysicsMesh mesh = meshCollider.Mesh;
 273158        int preparedOwner = -1;
 273159        ConePlaneRayFrame frame = default;
 933160        foreach (int triangle in candidates.AsReadOnlySpan())
 161        {
 269162            mesh.GetGeometryTriangleVertices(triangle, out Vector3d a, out Vector3d b, out Vector3d c);
 269163            var face = new FixedTriangle(a, b, c);
 269164            int owner = mesh.GetManifoldSurfaceOwner(triangle);
 269165            if (owner != preparedOwner)
 166            {
 257167                frame = new ConePlaneRayFrame(face, mesh.GeometryFrame,
 257168                    cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius);
 257169                preparedOwner = owner;
 170            }
 420171            if (ConePlaneRayEvents.IntersectsTriangle(frame, face)) return true;
 172        }
 122173        return false;
 174    }
 175
 176    private static bool BuildMeshConeSurfaceContacts(
 177        LSMeshCollider meshCollider, LSConeCollider cone, CollisionSatScratch scratch, ContactManifold manifold)
 178    {
 5713179        PhysicsMesh mesh = meshCollider.Mesh;
 5713180        var candidates = scratch.MeshTriangleCandidatesA;
 5713181        var admitted = scratch.MeshTriangleCandidatesB;
 5713182        meshCollider.GetTrianglesInBounds(new FixedBoundVolume(cone.BoundsMin, cone.BoundsMax), candidates);
 5713183        scratch.MeshSurfaces.Clear();
 5713184        ContactManifold staged = scratch.MeshSurfaceManifold;
 5713185        staged.Reset();
 52028186        foreach (int triangleIndex in candidates.AsReadOnlySpan())
 187        {
 20301188            int owner = mesh.GetManifoldSurfaceOwner(triangleIndex);
 20301189            if (!scratch.MeshSurfaces.Add(owner)) continue;
 5991190            mesh.GetGeometryTriangleVertices(triangleIndex, out Vector3d a, out Vector3d b, out Vector3d c);
 5991191            var frame = new ConePlaneRayFrame(new FixedTriangle(a, b, c), mesh.GeometryFrame,
 5991192                cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius);
 5991193            admitted.FastClear();
 194            // Reuse one exact plane frame per surface rather than rebuilding
 195            // its reduced normal and rigid basis for each subdivided triangle.
 196            // ponytail: O(candidate count * surface count) owner comparisons;
 197            // bucket candidates if many distinct planes become a measured cost.
 60072198            foreach (int candidate in candidates.AsReadOnlySpan())
 199            {
 24045200                if (mesh.GetManifoldSurfaceOwner(candidate) != owner) continue;
 20301201                mesh.GetGeometryTriangleVertices(candidate, out a, out b, out c);
 33651202                if (ConePlaneRayEvents.IntersectsTriangle(frame, new FixedTriangle(a, b, c))) admitted.Add(candidate);
 203            }
 5991204            if (admitted.Count == 0) continue;
 5727205            var connectivity = scratch.MeshConnectivity;
 5727206            connectivity.Build(mesh, triangleIndex, frame, admitted.AsReadOnlySpan());
 5727207            var faces = scratch.MeshConeFaces;
 208            // Admission bounds radius + half-height to a few thousand units,
 209            // so centered sampling fits even when the cone touches a scalar face.
 5727210            Vector3d samplingOrigin = cone.Center;
 5727211            faces.Build(mesh, triangleIndex, frame, connectivity, admitted.AsReadOnlySpan(), samplingOrigin);
 5727212            int surface = mesh.GetCanonicalSurfaceOrdinal(triangleIndex);
 22910213            for (int region = 0; region < faces.RegionCount; region++)
 214            {
 5728215                var key = new ContactGroupKey(0, 0, surface, 0, region);
 5728216                Vector3d normal = faces.GetNormal(region);
 5728217                int count = faces.GetSampleCount(region);
 56852218                for (int sample = 0; sample < count; sample++)
 219                {
 22698220                    bool found = faces.TryGetSample(region, sample, out Vector3d p, out Vector3d q, out Fixed64 depth);
 221                    System.Diagnostics.Debug.Assert(found);
 222                    // Preserve the sampled world-lattice point without narrowing
 223                    // its offset from a collider origin. Existing additive anchor
 224                    // terms retain any large cancellation exactly.
 22698225                    var meshAnchor = new ContactAnchor(FixedPointAnchor.FromTranslatedPoint(meshCollider.Center, samplin
 22698226                    var coneAnchor = new ContactAnchor(FixedPointAnchor.FromTranslatedPoint(cone.Center, samplingOrigin,
 22698227                    staged.AddContact(meshAnchor, coneAnchor,
 22698228                        depth, normal, meshCollider.Material, cone.Material, group: key,
 22698229                        contactIdentity: faces.GetSampleIdentity(region, sample));
 230                }
 231            }
 232            // Finite exposed-feature witnesses lie in the cone. Reuse the
 233            // face owner's exact boundary admission rather than constructing
 234            // support candidates for an entirely disjoint perimeter.
 5727235            if (faces.HasBoundaryIntersection)
 309236                scratch.MeshConeBoundary.BuildSurfaceContacts(meshCollider, cone, triangleIndex, connectivity, faces, st
 237        }
 238        // Geometry/range failures leave the pair unpublished. Reserve the
 239        // actual grouped result before copying; both owners retain high water.
 5713240        manifold.ReserveGroups(staged.GroupCount);
 24400241        for (int group = 0; group < staged.GroupCount; group++)
 242        {
 6487243            ref ContactGroup source = ref staged.GetGroup(group);
 59946244            for (int sample = 0; sample < source.Count; sample++)
 23486245                manifold.AddContact(source.Key, source[sample]);
 246        }
 5713247        return staged.HasContact;
 248    }
 249
 250    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 251    private static void GetConeConvexColliders(
 252        CollisionWorkItem pair,
 253        out LSConeCollider cone,
 254        out LSCollider convex)
 255    {
 299256        if (pair.ColliderA is LSConeCollider coneA && ConvexColliderSupport.IsSupported(pair.ColliderB))
 257        {
 173258            cone = coneA;
 173259            convex = pair.ColliderB;
 173260            return;
 261        }
 262
 126263        cone = (LSConeCollider)pair.ColliderB;
 126264        convex = pair.ColliderA;
 126265    }
 266
 267}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.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 Gravitas.Colliders;
 9
 10namespace Gravitas.CollisionHandling;
 11
 12/// <summary>Provides deterministic 3D narrow-phase collision checks.</summary>
 13/// <content>Owns 3D pair manifold refresh and narrow-phase shape dispatch.</content>
 14public static partial class CollisionDetection
 15{
 16    /// <summary>Rebuilds and tests the contact manifold for a collision pair.</summary>
 17    public static bool DoCollisionCheck(CollisionPair pair)
 18    {
 6739419        var previous = pair.BeginCollisionLayerUpdate();
 6739420        pair.Manifold.BeginUpdate(pair.Context.FrameCount);
 6739421        bool collided = DoCollisionCheck(CollisionWorkItem.Create(pair));
 6739422        pair.CompleteCollisionLayerUpdate(previous);
 23        // Regeneration retires vanished features even if sleeping/frozen bodies
 24        // skip response. A later reappearing feature must not revive an old load.
 6739425        pair.RetainWarmStarts();
 6739426        pair.ResponseSnapshot = default;
 6739427        return collided;
 28    }
 29
 30    internal static bool DoCollisionCheck(CollisionWorkItem pair)
 31    {
 8383432        return pair.CollisionType switch
 8383433        {
 124934            CollisionType.Sphere_Sphere => DoSpheresCheck(pair),
 22435            CollisionType.Capsule_Sphere => DoCapsuleSphereCheck(pair),
 22136            CollisionType.Capsule_Capsule => DoCapsulesCheck(pair),
 1042437            CollisionType.Cuboid_Sphere => DoCuboidSphereCheck(pair),
 87338            CollisionType.AABox_Capsule => DoCuboidCapsuleCheck(pair),
 22039            CollisionType.OBBox_Capsule => DoCuboidCapsuleCheck(pair),
 260740            CollisionType.Cuboid_Cuboid => DoCuboidsCheck(pair),
 23541            CollisionType.Cylinder_Sphere => DoCylinderSphereCheck(pair),
 21442            CollisionType.Cylinder_Capsule => DoCylinderCapsuleCheck(pair),
 21643            CollisionType.Cylinder_Cylinder => DoCylindersCheck(pair),
 944            CollisionType.Cuboid_Cylinder => DoCuboidCylinderCheck(pair),
 9845            CollisionType.Cone_Sphere => DoConeSphereCheck(pair),
 29946            CollisionType.Cone_Convex => DoConeConvexCheck(pair),
 1372547            CollisionType.Mesh_Sphere => DoMeshSphereCheck(pair),
 1306448            CollisionType.Mesh_Capsule => DoMeshCapsuleCheck(pair),
 472349            CollisionType.Mesh_Cuboid => DoMeshCuboidCheck(pair),
 426650            CollisionType.Mesh_Cylinder => DoMeshCylinderCheck(pair),
 1209451            CollisionType.Mesh_Cone => DoMeshConeCheck(pair),
 677052            CollisionType.Mesh_Mesh => DoMeshesCheck(pair),
 1230253            CollisionType.Compound => DoCompoundCheck(pair),
 154            _ => false,
 8383455        };
 56    }
 57
 58}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Cuboid.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Cuboid.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 FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.Colliders;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <content>Finds cuboid contacts against spheres, capsules, and other cuboids.</content>
 15public static partial class CollisionDetection
 16{
 17    #region Cuboid
 18
 19    private static bool DoCuboidSphereCheck(CollisionWorkItem pair)
 20    {
 1042421        var cuboid = (LSCuboidCollider)pair.ColliderA;
 1042422        if (!cuboid.OrientedBox.TryGetSphereContact(
 1042423                pair.ColliderB.Center,
 1042424                pair.ColliderB.Rotation,
 1042425                pair.ColliderB.ScaledRadius,
 1042426                out FixedContactAnchors contact))
 27        {
 211528            return false;
 29        }
 30
 830931        pair.Manifold.SetContact(
 830932            new ContactAnchor(contact.FirstAnchor),
 830933            new ContactAnchor(contact.SecondAnchor),
 830934            contact.Depth,
 830935            contact.Normal,
 830936            contact.DepthIsClamped);
 830937        return true;
 38    }
 39
 40    private static bool DoCuboidCapsuleCheck(CollisionWorkItem pair)
 41    {
 109342        var cuboid = (LSCuboidCollider)pair.ColliderA;
 109343        var capsule = (LSCapsuleCollider)pair.ColliderB;
 44
 109345        if (!cuboid.OrientedBox.TryGetCenteredCapsuleContact(
 109346                capsule.Center,
 109347                capsule.Rotation,
 109348                Vector3d.Up,
 109349                capsule.AxisLength,
 109350                capsule.ScaledRadius,
 109351                out FixedContactAnchors contact))
 52        {
 1353            return false;
 54        }
 55
 108056        pair.Manifold.SetContact(
 108057            new ContactAnchor(contact.FirstAnchor),
 108058            new ContactAnchor(contact.SecondAnchor),
 108059            contact.Depth,
 108060            contact.Normal,
 108061            contact.DepthIsClamped);
 108062        return true;
 63    }
 64
 65    /// <summary>
 66    /// Checks for collisions between two poly-poly colliders.
 67    /// </summary>
 68    /// <returns>true if a collision is detected, false otherwise.</returns>
 69    private static bool DoCuboidsCheck(CollisionWorkItem pair)
 70    {
 260771        var cuboidA = (LSCuboidCollider)pair.ColliderA;
 260772        var cuboidB = (LSCuboidCollider)pair.ColliderB;
 73
 260774        if (cuboidA.Shape == ColliderType.AABox
 260775            && cuboidB.Shape == ColliderType.AABox
 260776            && CanBuildAxisAlignedManifold(cuboidA)
 260777            && CanBuildAxisAlignedManifold(cuboidB))
 78        {
 175479            return TryBuildAxisAlignedCuboidManifold(pair, cuboidA, cuboidB);
 80        }
 81
 82        // Use one SAT order for standalone pairs, including equal-depth feature ties.
 85383        bool facePatch = cuboidA.Id >= 0 && cuboidB.Id >= 0 && cuboidA.Id != cuboidB.Id
 85384            && (cuboidA.Shape == ColliderType.OBBox || cuboidB.Shape == ColliderType.OBBox);
 85385        bool reversed = facePatch && cuboidB.Id < cuboidA.Id;
 85386        FixedOrientedBox first = (reversed ? cuboidB : cuboidA).OrientedBox;
 85387        FixedOrientedBox second = (reversed ? cuboidA : cuboidB).OrientedBox;
 85388        if (!first.TryGetContact(second, out FixedContactAnchors contact))
 89        {
 590            return false;
 91        }
 92
 84893        if (facePatch && !contact.DepthIsClamped
 84894            && TryBuildCuboidFaceContacts(pair.Manifold, first, second, contact.Normal, reversed))
 95        {
 84396            return true;
 97        }
 98
 599        pair.Manifold.SetContact(
 5100            new ContactAnchor(reversed ? contact.SecondAnchor : contact.FirstAnchor),
 5101            new ContactAnchor(reversed ? contact.FirstAnchor : contact.SecondAnchor),
 5102            contact.Depth,
 5103            reversed ? -contact.Normal : contact.Normal,
 5104            contact.DepthIsClamped);
 5105        return true;
 106    }
 107
 108    private static bool CanBuildAxisAlignedManifold(LSCuboidCollider cuboid)
 109    {
 3509110        Vector3d center = cuboid.Center;
 3509111        Vector3d halfExtents = cuboid.OrientedBox.HalfExtents;
 112        // The four-point fast path materializes both bounds and subtracts their
 113        // overlap widths. Wider conceptual boxes use the exact relative path.
 3509114        return Vector3d.TryAdd(center, halfExtents, out _)
 3509115            && Vector3d.TrySubtract(center, halfExtents, out _)
 3509116            && Vector3d.TryAdd(halfExtents, halfExtents, out _);
 117    }
 118
 119    private static bool TryBuildAxisAlignedCuboidManifold(
 120        CollisionWorkItem pair,
 121        LSCuboidCollider cuboidA,
 122        LSCuboidCollider cuboidB)
 123    {
 1754124        Fixed64 overlapX = FixedMath.Min(cuboidA.BoundsMax.X, cuboidB.BoundsMax.X) - FixedMath.Max(cuboidA.BoundsMin.X, 
 1754125        Fixed64 overlapY = FixedMath.Min(cuboidA.BoundsMax.Y, cuboidB.BoundsMax.Y) - FixedMath.Max(cuboidA.BoundsMin.Y, 
 1754126        Fixed64 overlapZ = FixedMath.Min(cuboidA.BoundsMax.Z, cuboidB.BoundsMax.Z) - FixedMath.Max(cuboidA.BoundsMin.Z, 
 127
 1754128        if (overlapX < Fixed64.Zero || overlapY < Fixed64.Zero || overlapZ < Fixed64.Zero)
 367129            return false;
 130
 1387131        Vector3d centerDelta = cuboidB.Center - cuboidA.Center;
 1387132        int axis = 0;
 1387133        Fixed64 depth = overlapX;
 1387134        if (overlapY < depth)
 135        {
 1169136            axis = 1;
 1169137            depth = overlapY;
 138        }
 139
 1387140        if (overlapZ < depth)
 141        {
 2142            axis = 2;
 2143            depth = overlapZ;
 144        }
 145
 1387146        Vector3d normal = axis switch
 1387147        {
 216148            0 => new Vector3d(centerDelta.X < Fixed64.Zero ? -Fixed64.One : Fixed64.One, Fixed64.Zero, Fixed64.Zero),
 1169149            1 => new Vector3d(Fixed64.Zero, centerDelta.Y < Fixed64.Zero ? -Fixed64.One : Fixed64.One, Fixed64.Zero),
 2150            _ => new Vector3d(Fixed64.Zero, Fixed64.Zero, centerDelta.Z < Fixed64.Zero ? -Fixed64.One : Fixed64.One)
 1387151        };
 152
 1387153        AddAxisAlignedCuboidContacts(pair.Manifold, cuboidA, cuboidB, axis, depth, normal);
 1387154        return pair.Manifold.HasContact;
 155    }
 156
 157    private static void AddAxisAlignedCuboidContacts(
 158        ContactManifold manifold,
 159        LSCuboidCollider cuboidA,
 160        LSCuboidCollider cuboidB,
 161        int axis,
 162        Fixed64 depth,
 163        Vector3d normal)
 164    {
 1387165        Fixed64 minX = FixedMath.Max(cuboidA.BoundsMin.X, cuboidB.BoundsMin.X);
 1387166        Fixed64 maxX = FixedMath.Min(cuboidA.BoundsMax.X, cuboidB.BoundsMax.X);
 1387167        Fixed64 minY = FixedMath.Max(cuboidA.BoundsMin.Y, cuboidB.BoundsMin.Y);
 1387168        Fixed64 maxY = FixedMath.Min(cuboidA.BoundsMax.Y, cuboidB.BoundsMax.Y);
 1387169        Fixed64 minZ = FixedMath.Max(cuboidA.BoundsMin.Z, cuboidB.BoundsMin.Z);
 1387170        Fixed64 maxZ = FixedMath.Min(cuboidA.BoundsMax.Z, cuboidB.BoundsMax.Z);
 171
 172        switch (axis)
 173        {
 174            case 0:
 175                {
 216176                    Fixed64 x = normal.X > Fixed64.Zero ? cuboidA.BoundsMax.X : cuboidA.BoundsMin.X;
 216177                    AddCuboidContact(manifold, new Vector3d(x, minY, minZ), normal, depth);
 216178                    AddCuboidContact(manifold, new Vector3d(x, minY, maxZ), normal, depth);
 216179                    AddCuboidContact(manifold, new Vector3d(x, maxY, minZ), normal, depth);
 216180                    AddCuboidContact(manifold, new Vector3d(x, maxY, maxZ), normal, depth);
 216181                    break;
 182                }
 183            case 1:
 184                {
 1169185                    Fixed64 y = normal.Y > Fixed64.Zero ? cuboidA.BoundsMax.Y : cuboidA.BoundsMin.Y;
 1169186                    AddCuboidContact(manifold, new Vector3d(minX, y, minZ), normal, depth);
 1169187                    AddCuboidContact(manifold, new Vector3d(minX, y, maxZ), normal, depth);
 1169188                    AddCuboidContact(manifold, new Vector3d(maxX, y, minZ), normal, depth);
 1169189                    AddCuboidContact(manifold, new Vector3d(maxX, y, maxZ), normal, depth);
 1169190                    break;
 191                }
 192            default:
 193                {
 2194                    Fixed64 z = normal.Z > Fixed64.Zero ? cuboidA.BoundsMax.Z : cuboidA.BoundsMin.Z;
 2195                    AddCuboidContact(manifold, new Vector3d(minX, minY, z), normal, depth);
 2196                    AddCuboidContact(manifold, new Vector3d(minX, maxY, z), normal, depth);
 2197                    AddCuboidContact(manifold, new Vector3d(maxX, minY, z), normal, depth);
 2198                    AddCuboidContact(manifold, new Vector3d(maxX, maxY, z), normal, depth);
 199                    break;
 200                }
 201        }
 2202    }
 203
 204    private static void AddCuboidContact(ContactManifold manifold, Vector3d pointA, Vector3d normal, Fixed64 depth)
 205    {
 5548206        Vector3d pointB = pointA - normal * depth;
 5548207        manifold.AddContact(pointA, pointB, depth, normal);
 5548208    }
 209
 210    #endregion
 211
 212}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.CuboidFaces.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.CuboidFaces.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;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <content>Builds finite witness patches for cuboid face contacts.</content>
 15public static partial class CollisionDetection
 16{
 17    private readonly struct CuboidFaceVertex
 18    {
 19        internal CuboidFaceVertex(Vector3d reference, Vector3d incident, int boundaries)
 20        {
 505421            Reference = reference;
 505422            Incident = incident;
 505423            Boundaries = boundaries;
 505424        }
 25
 26        internal readonly Vector3d Reference, Incident;
 27        internal readonly int Boundaries;
 28    }
 29
 30    private static bool TryBuildCuboidFaceContacts(
 31        ContactManifold manifold,
 32        FixedOrientedBox first,
 33        FixedOrientedBox second,
 34        Vector3d normal,
 35        bool reversed)
 36    {
 84637        Vector3d localFirst = first.Orientation.Conjugate().Rotate(normal);
 84638        Vector3d localSecond = second.Orientation.Conjugate().Rotate(-normal);
 84639        int firstAxis = GetCuboidFaceAxis(localFirst);
 84640        int secondAxis = GetCuboidFaceAxis(localSecond);
 84641        Fixed64 firstSign = localFirst[firstAxis] < Fixed64.Zero ? -Fixed64.One : Fixed64.One;
 84642        Fixed64 secondSign = localSecond[secondAxis] < Fixed64.Zero ? -Fixed64.One : Fixed64.One;
 43
 44        // Compare with the same materialized basis direction as the SAT, without
 45        // accepting a near-parallel edge axis as a face normal.
 84646        bool secondIsReference = !IsCuboidFaceNormal(first.Orientation, firstAxis, firstSign, normal);
 84647        if (secondIsReference && !IsCuboidFaceNormal(second.Orientation, secondAxis, secondSign, -normal))
 248            return false;
 49
 84450        FixedOrientedBox reference = secondIsReference ? second : first;
 84451        FixedOrientedBox incident = secondIsReference ? first : second;
 84452        int referenceAxis = secondIsReference ? secondAxis : firstAxis;
 84453        int incidentAxis = secondIsReference ? firstAxis : secondAxis;
 84454        Fixed64 referenceSign = secondIsReference ? secondSign : firstSign;
 84455        Fixed64 incidentSign = secondIsReference ? firstSign : secondSign;
 84456        Vector3d referenceNormal = secondIsReference ? -normal : normal;
 84457        int referenceU = (referenceAxis + 1) % 3;
 84458        int referenceV = (referenceAxis + 2) % 3;
 84459        int incidentU = (incidentAxis + 1) % 3;
 84460        int incidentV = (incidentAxis + 2) % 3;
 84461        Vector3d referenceHalf = reference.HalfExtents;
 84462        Vector3d incidentHalf = incident.HalfExtents;
 63
 64        // Rounded crossings need not preserve exact convexity. Any sequence of n vertices
 65        // emits at most n + floor(n/2) samples on one plane, so five clips grow 4 to at most 28.
 84466        Span<CuboidFaceVertex> vertices = stackalloc CuboidFaceVertex[28];
 84467        Span<CuboidFaceVertex> scratch = stackalloc CuboidFaceVertex[28];
 844068        for (int i = 0; i < 4; i++)
 69        {
 337670            Vector3d incidentPoint = default;
 337671            incidentPoint[incidentAxis] = incidentSign * incidentHalf[incidentAxis];
 337672            incidentPoint[incidentU] = i is 0 or 3 ? -incidentHalf[incidentU] : incidentHalf[incidentU];
 337673            incidentPoint[incidentV] = i < 2 ? -incidentHalf[incidentV] : incidentHalf[incidentV];
 337674            var anchor = new FixedPointAnchor(incident.Center, incident.Orientation, incidentPoint);
 75            // SAT overlap bounds center separation by two admitted radii. Exact
 76            // rigid rotations preserve length, so a corner fits within 3 * 4096.
 337677            _ = anchor.TryGetLocalPointIn(reference.Center, reference.Orientation, out Vector3d referencePoint);
 337678            vertices[i] = new CuboidFaceVertex(referencePoint, incidentPoint, (1 << i) | (1 << ((i + 3) % 4)));
 79        }
 80
 84481        int count = 4;
 1012282        for (int plane = 0; plane < 5; plane++)
 83        {
 421884            int axis = plane < 2 ? referenceU : plane < 4 ? referenceV : referenceAxis;
 421885            Fixed64 sign = plane == 4 ? referenceSign : (plane & 1) == 0 ? Fixed64.One : -Fixed64.One;
 421886            count = ClipCuboidFace(vertices[..count], scratch, axis, referenceHalf[axis], sign, 1 << (4 + plane));
 421887            if (count == 0)
 188                return false;
 421789            Span<CuboidFaceVertex> temporary = vertices;
 421790            vertices = scratch;
 421791            scratch = temporary;
 92        }
 93
 84394        bool referenceIsPairFirst = secondIsReference == reversed;
 84395        Vector3d pairNormal = reversed ? -normal : normal;
 96        // Face and clipping-boundary identities survive changes in penetration.
 84397        int faces = secondIsReference ? 1 : 0;
 84398        faces = faces * 6 + referenceAxis * 2 + (referenceSign < Fixed64.Zero ? 1 : 0);
 84399        faces = faces * 6 + incidentAxis * 2 + (incidentSign < Fixed64.Zero ? 1 : 0);
 8430100        foreach (CuboidFaceVertex sample in vertices[..count])
 101        {
 3372102            Vector3d referencePoint = sample.Reference;
 3372103            referencePoint[referenceAxis] = referenceHalf[referenceAxis] * referenceSign;
 3372104            var referenceAnchor = new ContactAnchor(reference.Center, reference.Orientation, referencePoint);
 3372105            var incidentAnchor = new ContactAnchor(incident.Center, incident.Orientation, sample.Incident);
 106            // Each witness stays within its box. Their offset is bounded by
 107            // 4 * 4096, and the dot with a unit normal also fits in Fixed64.
 3372108            _ = referenceAnchor.TryGetOffsetFrom(incidentAnchor, out Vector3d difference);
 3372109            _ = Vector3d.TryDot(difference, referenceNormal, out Fixed64 depth);
 3372110            if (depth < Fixed64.Zero)
 111                continue;
 112
 3370113            ulong id = 0xC0B0000000000000UL | ((ulong)faces << 9) | (uint)sample.Boundaries;
 3370114            manifold.AddContact(default, new ManifoldContact(id,
 3370115                referenceIsPairFirst ? referenceAnchor : incidentAnchor,
 3370116                referenceIsPairFirst ? incidentAnchor : referenceAnchor, depth, pairNormal));
 117        }
 843118        return manifold.HasContact;
 119    }
 120
 121    private static int GetCuboidFaceAxis(Vector3d localNormal)
 122    {
 1692123        int axis = localNormal.X.Abs() >= localNormal.Y.Abs() ? 0 : 1;
 1692124        return localNormal[axis].Abs() >= localNormal.Z.Abs() ? axis : 2;
 125    }
 126
 127    private static bool IsCuboidFaceNormal(FixedQuaternion orientation, int axis, Fixed64 sign, Vector3d normal)
 128    {
 1057129        var basis = new WideRationalBasis3d(orientation);
 1057130        WideAxis3 direction = basis.GetAxis(axis);
 1057131        return WideNormalization.GetNormalized(Signed576.ExtendValue(direction.X),
 1057132            Signed576.ExtendValue(direction.Y), Signed576.ExtendValue(direction.Z)) * sign == normal;
 133    }
 134
 135    private static int ClipCuboidFace(
 136        ReadOnlySpan<CuboidFaceVertex> input,
 137        Span<CuboidFaceVertex> output,
 138        int axis,
 139        Fixed64 limit,
 140        Fixed64 sign,
 141        int boundary)
 142    {
 4218143        int count = 0;
 4218144        CuboidFaceVertex previous = input[^1];
 4218145        Fixed64 previousDistance = limit - previous.Reference[axis] * sign;
 42176146        foreach (CuboidFaceVertex current in input)
 147        {
 16870148            Fixed64 distance = limit - current.Reference[axis] * sign;
 16870149            if ((previousDistance < Fixed64.Zero && distance > Fixed64.Zero)
 16870150                || (previousDistance > Fixed64.Zero && distance < Fixed64.Zero))
 151            {
 842152                Fixed64 firstWeight = distance.Abs();
 842153                Fixed64 secondWeight = previousDistance.Abs();
 842154                Vector3d clipped = InterpolateCuboidFace(previous.Reference, current.Reference, firstWeight, secondWeigh
 842155                clipped[axis] = limit * sign;
 842156                output[count++] = new CuboidFaceVertex(clipped,
 842157                    InterpolateCuboidFace(previous.Incident, current.Incident, firstWeight, secondWeight),
 842158                    (previous.Boundaries & current.Boundaries) | boundary);
 159            }
 16870160            if (distance >= Fixed64.Zero)
 161            {
 16024162                output[count++] = distance == Fixed64.Zero
 16024163                    ? new CuboidFaceVertex(current.Reference, current.Incident, current.Boundaries | boundary)
 16024164                    : current;
 165            }
 16870166            previous = current;
 16870167            previousDistance = distance;
 168        }
 4218169        return count;
 170    }
 171
 172    private static Vector3d InterpolateCuboidFace(
 173        Vector3d first, Vector3d second, Fixed64 firstWeight, Fixed64 secondWeight)
 174    {
 1684175        Signed192 denominator = Signed192.Raw(firstWeight + secondWeight);
 1684176        return new Vector3d(
 1684177            InterpolateCuboidFaceCoordinate(first.X, second.X, firstWeight, secondWeight, denominator),
 1684178            InterpolateCuboidFaceCoordinate(first.Y, second.Y, firstWeight, secondWeight, denominator),
 1684179            InterpolateCuboidFaceCoordinate(first.Z, second.Z, firstWeight, secondWeight, denominator));
 180    }
 181
 182    private static Fixed64 InterpolateCuboidFaceCoordinate(
 183        Fixed64 first, Fixed64 second, Fixed64 firstWeight, Fixed64 secondWeight, Signed192 denominator)
 184    {
 185        // Both weights are positive. The result stays in the bounded endpoint interval.
 5052186        Signed192 numerator = WideArithmetic.AddSigned192(Fixed64.GetExactRawProduct(first, firstWeight),
 5052187            Fixed64.GetExactRawProduct(second, secondWeight));
 5052188        _ = Fixed64.TryGetSignedRawRatio(numerator, denominator, out Fixed64 coordinate);
 5052189        return coordinate;
 190    }
 191}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Cylinder.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Cylinder.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.Colliders;
 13
 14namespace Gravitas.CollisionHandling;
 15
 16/// <content>Finds finite cylinder contacts and builds cap contacts for cylinder pairs.</content>
 17public static partial class CollisionDetection
 18{
 19    #region Cylinder
 20
 21    private static bool DoCylinderSphereCheck(CollisionWorkItem pair)
 22    {
 23523        var cylinder = (LSCylinderCollider)pair.ColliderA;
 23524        var sphere = (LSSphereCollider)pair.ColliderB;
 25
 23526        if (!FixedSegment.TryGetClosestCenteredFiniteCylinderSurfaceAnchor(
 23527                sphere.Center,
 23528                cylinder.Center,
 23529                cylinder.Rotation,
 23530                Vector3d.Up,
 23531                cylinder.Height,
 23532                cylinder.ScaledRadius,
 23533                Vector3d.Right,
 23534                out FixedPointAnchor cylinderAnchor,
 23535                out Vector3d outwardNormal,
 23536                out Fixed64 signedDistance))
 37        {
 138            return false;
 39        }
 23440        if (signedDistance > sphere.ScaledRadius)
 141            return false;
 42
 43        // Containment increases penetration depth; the sphere still escapes
 44        // along the solid surface's outward normal, not toward the cylinder center.
 23345        Vector3d normal = outwardNormal;
 46        // Both admitted reaches are at most 4,096: containment depth is
 47        // bounded by their sum, so the complete difference is representable.
 23348        Fixed64 penetrationDepth = sphere.ScaledRadius - signedDistance;
 23349        pair.Manifold.SetContact(
 23350            new ContactAnchor(cylinderAnchor),
 23351            new ContactAnchor(
 23352                FixedSegment.GetCenteredCapsuleSupportAnchor(
 23353                    sphere.Center,
 23354                    sphere.Rotation,
 23355                    Fixed64.Zero,
 23356                    sphere.ScaledRadius,
 23357                    -normal)),
 23358            penetrationDepth,
 23359            normal,
 23360            depthIsClamped: false);
 61
 23362        return true;
 63    }
 64
 65    private static bool DoCylinderCapsuleCheck(CollisionWorkItem pair)
 66    {
 67        LSCylinderCollider cylinder;
 68        LSCapsuleCollider capsule;
 21469        if (pair.ColliderA is LSCylinderCollider cylinderA)
 70        {
 21371            cylinder = cylinderA;
 21372            capsule = (LSCapsuleCollider)pair.ColliderB;
 73        }
 74        else
 75        {
 176            cylinder = (LSCylinderCollider)pair.ColliderB;
 177            capsule = (LSCapsuleCollider)pair.ColliderA;
 78        }
 79
 21480        if (!FixedSegment.TryGetCenteredFiniteCylinderCapsuleContact(
 21481                cylinder.Center,
 21482                cylinder.Rotation,
 21483                Vector3d.Up,
 21484                cylinder.Height,
 21485                cylinder.ScaledRadius,
 21486                capsule.Center,
 21487                capsule.Rotation,
 21488                Vector3d.Up,
 21489                capsule.AxisLength,
 21490                capsule.ScaledRadius,
 21491                out FixedContactAnchors contact))
 92        {
 493            return false;
 94        }
 95
 21096        SetContactInPairOrder(
 21097            pair,
 21098            cylinder,
 21099            new ContactAnchor(contact.FirstAnchor),
 210100            capsule,
 210101            new ContactAnchor(contact.SecondAnchor),
 210102            contact.Depth,
 210103            contact.Normal,
 210104            contact.DepthIsClamped);
 105
 210106        return true;
 107    }
 108
 109    private static bool DoCylindersCheck(CollisionWorkItem pair)
 110    {
 216111        var cylinderA = (LSCylinderCollider)pair.ColliderA;
 216112        var cylinderB = (LSCylinderCollider)pair.ColliderB;
 113
 216114        if (!FixedSegment.TryGetCenteredFiniteCylindersContact(
 216115                cylinderA.Center,
 216116                cylinderA.Rotation,
 216117                Vector3d.Up,
 216118                cylinderA.Height,
 216119                cylinderA.ScaledRadius,
 216120                cylinderB.Center,
 216121                cylinderB.Rotation,
 216122                Vector3d.Up,
 216123                cylinderB.Height,
 216124                cylinderB.ScaledRadius,
 216125                out FixedContactAnchors contact))
 126        {
 4127            return false;
 128        }
 129
 212130        if (TryAddCylinderCylinderCapContacts(
 212131                pair,
 212132                cylinderA,
 212133                cylinderB,
 212134                contact))
 135        {
 3136            return true;
 137        }
 138
 209139        pair.Manifold.SetContact(
 209140            new ContactAnchor(contact.FirstAnchor),
 209141            new ContactAnchor(contact.SecondAnchor),
 209142            contact.Depth,
 209143            contact.Normal,
 209144            contact.DepthIsClamped);
 145
 209146        return true;
 147    }
 148
 149    private static bool DoCuboidCylinderCheck(CollisionWorkItem pair)
 150    {
 9151        var cuboid = (LSCuboidCollider)pair.ColliderA;
 9152        var cylinder = (LSCylinderCollider)pair.ColliderB;
 153
 9154        Span<FixedContactLocalPoints> capFaceContacts =
 9155            stackalloc FixedContactLocalPoints[ContactManifold.MaxContactsPerGroup];
 9156        if (!cuboid.OrientedBox.TryGetCenteredCylinderContact(
 9157                cylinder.Center,
 9158                cylinder.Rotation,
 9159                Vector3d.Up,
 9160                cylinder.Height,
 9161                cylinder.ScaledRadius,
 9162                capFaceContacts,
 9163                out FixedContactAnchors contact,
 9164                out int capFaceContactCount))
 165        {
 4166            return false;
 167        }
 168
 5169        if (capFaceContactCount > 0)
 170        {
 10171            for (int index = 0; index < capFaceContactCount; index++)
 172            {
 4173                FixedContactLocalPoints capContact = capFaceContacts[index];
 4174                pair.Manifold.AddContact(
 4175                    new ContactAnchor(
 4176                        contact.FirstAnchor.Origin,
 4177                        contact.FirstAnchor.Rotation,
 4178                        capContact.FirstLocalPoint),
 4179                    new ContactAnchor(
 4180                        contact.SecondAnchor.Origin,
 4181                        contact.SecondAnchor.Rotation,
 4182                        capContact.SecondLocalPoint),
 4183                    contact.Depth,
 4184                    contact.Normal,
 4185                    contact.DepthIsClamped);
 186            }
 1187            return true;
 188        }
 189
 4190        pair.Manifold.SetContact(
 4191            new ContactAnchor(contact.FirstAnchor),
 4192            new ContactAnchor(contact.SecondAnchor),
 4193            contact.Depth,
 4194            contact.Normal,
 4195            contact.DepthIsClamped);
 4196        return true;
 197    }
 198
 199    private static bool TryAddCylinderCylinderCapContacts(
 200        CollisionWorkItem pair,
 201        LSCylinderCollider cylinderA,
 202        LSCylinderCollider cylinderB,
 203        FixedContactAnchors contact)
 204    {
 212205        if (contact.FirstAnchor.LocalDisplacement != Vector3d.Zero
 212206            || contact.SecondAnchor.LocalDisplacement != Vector3d.Zero)
 207        {
 209208            return false;
 209        }
 210
 3211        CylinderContactGeometry.GetCapBasis(cylinderA, out Vector3d tangentA, out Vector3d tangentB);
 3212        int initialCount = pair.Manifold.Count;
 3213        TryAddCylinderCylinderCapContact(pair, cylinderA, cylinderB, tangentA, contact);
 3214        TryAddCylinderCylinderCapContact(pair, cylinderA, cylinderB, -tangentA, contact);
 3215        TryAddCylinderCylinderCapContact(pair, cylinderA, cylinderB, tangentB, contact);
 3216        TryAddCylinderCylinderCapContact(pair, cylinderA, cylinderB, -tangentB, contact);
 3217        return pair.Manifold.Count > initialCount;
 218    }
 219
 220    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 221    private static void TryAddCylinderCylinderCapContact(
 222        CollisionWorkItem pair,
 223        LSCylinderCollider cylinderA,
 224        LSCylinderCollider cylinderB,
 225        Vector3d radialDirection,
 226        FixedContactAnchors contact)
 227    {
 12228        pair.Manifold.AddContact(
 12229            new ContactAnchor(ConvexColliderSupport.GetSupportAnchor(
 12230                cylinderA,
 12231                contact.Normal + radialDirection,
 12232                Vector3d.Zero)),
 12233            new ContactAnchor(ConvexColliderSupport.GetSupportAnchor(
 12234                cylinderB,
 12235                -contact.Normal + radialDirection,
 12236                Vector3d.Zero)),
 12237            contact.Depth,
 12238            contact.Normal,
 12239            contact.DepthIsClamped);
 12240    }
 241
 242    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 243    private static Vector3d ResolveNormal(Vector3d delta) =>
 109244        delta.MagnitudeSquared > Fixed64.Epsilon
 109245            ? delta.Normalized
 109246            : Vector3d.Right;
 247
 248    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 249    private static void SetContactInPairOrder(
 250        CollisionWorkItem pair,
 251        LSCollider first,
 252        ContactAnchor anchorOnFirst,
 253        LSCollider second,
 254        ContactAnchor anchorOnSecond,
 255        Fixed64 depth,
 256        Vector3d normalFirstToSecond,
 257        bool depthIsClamped)
 258    {
 316259        if (ReferenceEquals(pair.ColliderA, first))
 260        {
 262261            pair.Manifold.SetContact(
 262262                anchorOnFirst,
 262263                anchorOnSecond,
 262264                depth,
 262265                normalFirstToSecond,
 262266                depthIsClamped);
 262267            return;
 268        }
 269
 54270        pair.Manifold.SetContact(
 54271            anchorOnSecond,
 54272            anchorOnFirst,
 54273            depth,
 54274            -normalFirstToSecond,
 54275            depthIsClamped);
 54276    }
 277
 278    #endregion
 279
 280}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Mesh.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Mesh.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 FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10using Gravitas.Colliders;
 11
 12namespace Gravitas.CollisionHandling;
 13
 14/// <content>Dispatches whole-hull volume and finite-surface mesh contacts.</content>
 15public static partial class CollisionDetection
 16{
 17    #region Mesh
 18
 19    private static bool DoMeshSphereCheck(CollisionWorkItem pair)
 20    {
 1372521        var meshCollider = (LSMeshCollider)pair.ColliderA;
 1372522        var sphere = (LSSphereCollider)pair.ColliderB;
 23
 24        // Only closed convex meshes fill their volume; open meshes stay surfaces.
 1372525        if (meshCollider.Mode == MeshColliderMode.Convex && meshCollider.Mesh.IsClosedSurface)
 21426            return TrySetConvexMeshRoundedContact(pair, meshCollider.Mesh, sphere.Center,
 21427                sphere.Rotation, Fixed64.Zero, sphere.ScaledRadius);
 28
 1351129        return MeshTriangleContactGenerator.TryBuildMeshSphereManifold(
 1351130            pair,
 1351131            meshCollider,
 1351132            sphere,
 1351133            pair.Context.CollisionScratch.MeshTriangleCandidatesA);
 34    }
 35
 36    private static bool DoMeshCapsuleCheck(CollisionWorkItem pair)
 37    {
 1306438        var mesh = (LSMeshCollider)pair.ColliderA;
 1306439        var capsule = (LSCapsuleCollider)pair.ColliderB;
 40
 1306441        if (mesh.Mode == MeshColliderMode.Convex)
 26142            return TrySetConvexMeshRoundedContact(pair, mesh.Mesh, capsule.Center,
 26143                capsule.Rotation, capsule.AxisLength, capsule.ScaledRadius);
 44
 1280345        return MeshTriangleContactGenerator.TryBuildMeshCapsuleManifold(
 1280346            pair,
 1280347            mesh,
 1280348            capsule,
 1280349            pair.Context.CollisionScratch.MeshTriangleCandidatesA);
 50    }
 51
 52    private static bool TrySetConvexMeshRoundedContact(CollisionWorkItem pair, PhysicsMesh mesh,
 53        Vector3d center, FixedQuaternion rotation, Fixed64 axisLength, Fixed64 radius)
 54    {
 47555        if (!WideOrientedBox.TryGetConvexHullCenteredCapsuleContact(mesh.GeometryFrame, mesh.Origin,
 47556            mesh.GeometryVertices, mesh.Triangles, mesh.ConvexSatEdgeVertexPairs,
 47557            center, rotation, Vector3d.Up, axisLength, radius, out FixedContactAnchors contact))
 2858            return false;
 44759        pair.Manifold.SetContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor),
 44760            contact.Depth, contact.Normal, contact.DepthIsClamped);
 44761        return true;
 62    }
 63
 64    private static bool DoMeshCuboidCheck(CollisionWorkItem pair)
 65    {
 472366        var mesh = (LSMeshCollider)pair.ColliderA;
 472367        var cuboid = (LSCuboidCollider)pair.ColliderB;
 68
 472369        if (MeshTriangleContactGenerator.TryBuildMeshCuboidManifold(
 472370            pair,
 472371            mesh,
 472372            cuboid,
 472373            pair.Context.CollisionScratch.MeshTriangleCandidatesA))
 74        {
 439975            return true;
 76        }
 77
 32478        if (mesh.Mode == MeshColliderMode.Concave)
 7079            return false;
 80
 25481        PhysicsMesh physicsMesh = mesh.Mesh;
 25482        if (!WideOrientedBox.TryGetConvexHullContact(
 25483                cuboid.Center, cuboid.Rotation, cuboid.OrientedBox.HalfExtents,
 25484                physicsMesh.GeometryFrame, physicsMesh.Origin,
 25485                physicsMesh.GeometryVertices,
 25486                physicsMesh.Triangles,
 25487                physicsMesh.ConvexSatEdgeVertexPairs,
 25488                out FixedContactAnchors contact))
 89        {
 4390            return false;
 91        }
 92
 21193        pair.Manifold.SetContact(
 21194            new ContactAnchor(contact.SecondAnchor),
 21195            new ContactAnchor(contact.FirstAnchor),
 21196            contact.Depth,
 21197            -contact.Normal,
 21198            contact.DepthIsClamped);
 99
 211100        return true;
 101    }
 102
 103    private static bool DoMeshCylinderCheck(CollisionWorkItem pair)
 104    {
 4266105        var mesh = (LSMeshCollider)pair.ColliderA;
 4266106        var cylinder = (LSCylinderCollider)pair.ColliderB;
 107
 108        // An interior/boundary center needs the filled hull's exit, even when
 109        // it already reaches a wall whose two-sided surface would push inward.
 4266110        if (mesh.Mode == MeshColliderMode.Convex && mesh.Mesh.IsClosedSurface
 4266111            && ConvexHullCylinderContact.TryGetContactWithContainedCenter(mesh.Mesh.GeometryFrame,
 4266112                mesh.Mesh.GeometryVertices, mesh.Mesh.Triangles, cylinder.Center, cylinder.Rotation,
 4266113                cylinder.Height, cylinder.ScaledRadius, out FixedContactAnchors contact))
 114        {
 147115            pair.Manifold.SetContact(new ContactAnchor(contact.FirstAnchor), new ContactAnchor(contact.SecondAnchor),
 147116                contact.Depth, contact.Normal, contact.DepthIsClamped);
 147117            return true;
 118        }
 4119119        return MeshTriangleContactGenerator.TryBuildMeshCylinderManifold(pair, mesh, cylinder,
 4119120            pair.Context.CollisionScratch.MeshTriangleCandidatesA);
 121    }
 122
 123    /// <summary>
 124    /// Checks if two mesh colliders intersect and sets the contact point if they do.
 125    /// </summary>
 126    /// <param name="pair">The pair of colliders to test for collision.</param>
 127    /// <returns>true if the colliders intersect; otherwise, false.</returns>
 128    private static bool DoMeshesCheck(CollisionWorkItem pair)
 129    {
 6770130        var meshA = (LSMeshCollider)pair.ColliderA;
 6770131        var meshB = (LSMeshCollider)pair.ColliderB;
 6770132        if (meshA.Mesh.IsClosedSurface != meshB.Mesh.IsClosedSurface
 6770133            || meshA.Mode == MeshColliderMode.Concave
 6770134            || meshB.Mode == MeshColliderMode.Concave)
 135        {
 6758136            return MeshTriangleContactGenerator.TryBuildMeshMeshManifold(
 6758137                pair,
 6758138                meshA,
 6758139                meshB,
 6758140                pair.Context.CollisionScratch.MeshTriangleCandidatesA,
 6758141                pair.Context.CollisionScratch.MeshTriangleCandidatesB);
 142        }
 143
 12144        PhysicsMesh physicsMeshA = meshA.Mesh;
 12145        PhysicsMesh physicsMeshB = meshB.Mesh;
 12146        bool hasContact = physicsMeshA.GeometryFrame.IsRigid && physicsMeshB.GeometryFrame.IsRigid
 12147            ? FixedConvexHullRelations.TryGetContact(physicsMeshA.Origin, physicsMeshA.Rotation,
 12148                physicsMeshA.GeometryVertices, physicsMeshA.Triangles, physicsMeshA.ConvexSatEdgeVertexPairs,
 12149                physicsMeshB.Origin, physicsMeshB.Rotation, physicsMeshB.GeometryVertices,
 12150                physicsMeshB.Triangles, physicsMeshB.ConvexSatEdgeVertexPairs, out FixedContactAnchors contact)
 12151            : WideOrientedBox.TryGetConvexHullContact(physicsMeshA.GeometryFrame, physicsMeshA.Origin,
 12152                physicsMeshA.GeometryVertices, physicsMeshA.Triangles, physicsMeshA.ConvexSatEdgeVertexPairs,
 12153                physicsMeshB.GeometryFrame, physicsMeshB.Origin, physicsMeshB.GeometryVertices,
 12154                physicsMeshB.Triangles, physicsMeshB.ConvexSatEdgeVertexPairs, out contact);
 12155        if (!hasContact)
 4156            return false;
 8157        if (contact.Depth <= Fixed64.Zero)
 1158            return false;
 159
 7160        pair.Manifold.SetContact(
 7161            new ContactAnchor(contact.FirstAnchor),
 7162            new ContactAnchor(contact.SecondAnchor),
 7163            contact.Depth,
 7164            contact.Normal,
 7165            contact.DepthIsClamped);
 166
 7167        return true;
 168    }
 169
 170    #endregion
 171}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.PostureClearance.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.PostureClearance.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.Colliders;
 12
 13namespace Gravitas.CollisionHandling;
 14
 15/// <content>Classifies posture clearance before solver contact rounding.</content>
 16public static partial class CollisionDetection
 17{
 18    // The transaction admits sphere, capsule and finite-cylinder candidates.
 19    // Pair policy, supported collision types, root bounds and root ordering are
 20    // checked by SolidBody. Compound parts are geometry only and must retain
 21    // their parent's blocker identity.
 22    internal static bool DoesPostureCandidatePenetrate(LSCollider candidate, LSCollider other)
 23    {
 13324        if (other is LSCompoundCollider compound)
 25        {
 14426            for (int i = 0; i < compound.PartCount; i++)
 27            {
 4828                LSCollider part = compound.GetPartCollider(i);
 4829                if (candidate.Bounds.Intersects(part.Bounds)
 4830                    && DoesPostureLeafPenetrate(candidate, part))
 2431                    return true;
 32            }
 2433            return false;
 34        }
 8535        return DoesPostureLeafPenetrate(candidate, other);
 36    }
 37
 38    private static bool DoesPostureLeafPenetrate(LSCollider candidate, LSCollider other)
 39    {
 13340        if (candidate is LSCylinderCollider cylinder)
 4441            return DoesPostureCylinderPenetrate(cylinder, other);
 8942        Fixed64 length = candidate is LSCapsuleCollider capsule ? capsule.AxisLength : Fixed64.Zero;
 8943        return DoesPostureCapsulePenetrate(candidate, length, other);
 44    }
 45
 46    private static bool DoesPostureCapsulePenetrate(LSCollider candidate, Fixed64 length, LSCollider other)
 47    {
 48        switch (other)
 49        {
 50            case LSSphereCollider sphere:
 951                return FixedSegment.DoCenteredCapsulesOverlapStrict(
 952                    candidate.Center, candidate.Rotation, Vector3d.Up, length, candidate.ScaledRadius,
 953                    sphere.Center, sphere.Rotation, Vector3d.Up, Fixed64.Zero, sphere.ScaledRadius);
 54            case LSCapsuleCollider capsule:
 855                return FixedSegment.DoCenteredCapsulesOverlapStrict(
 856                    candidate.Center, candidate.Rotation, Vector3d.Up, length, candidate.ScaledRadius,
 857                    capsule.Center, capsule.Rotation, Vector3d.Up, capsule.AxisLength, capsule.ScaledRadius);
 58            case LSCuboidCollider box:
 1759                return WideOrientedBox.DoesCenteredCapsulePenetrate(
 1760                    box.Center, box.Rotation, box.OrientedBox.HalfExtents,
 1761                    candidate.Center, candidate.Rotation, Vector3d.Up, length, candidate.ScaledRadius);
 62            case LSCylinderCollider cylinder:
 1163                return length == Fixed64.Zero
 1164                    ? WideFiniteAxisIntersection.DoesCenteredFiniteCylinderPenetrateSphere(
 1165                        cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 1166                        candidate.Center, candidate.ScaledRadius)
 1167                    : WideFiniteAxisIntersection.DoesCenteredFiniteCylinderPenetrateCapsule(
 1168                        cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 1169                        candidate.Center, candidate.Rotation, Vector3d.Up, length, candidate.ScaledRadius);
 70            case LSConeCollider cone:
 871                return length == Fixed64.Zero
 872                    ? WideFiniteAxisIntersection.DoesCenteredFiniteConePenetrateSphere(
 873                        cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius,
 874                        candidate.Center, candidate.ScaledRadius)
 875                    : WideFiniteAxisIntersection.DoesCenteredFiniteConePenetrateCapsule(
 876                        cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius,
 877                        candidate.Center, candidate.Rotation, Vector3d.Up, length, candidate.ScaledRadius);
 78            default:
 3679                return DoesPostureCapsulePenetrateMesh(candidate, length, (LSMeshCollider)other);
 80        }
 81    }
 82
 83    private static bool DoesPostureCylinderPenetrate(LSCylinderCollider cylinder, LSCollider other)
 84    {
 85        switch (other)
 86        {
 87            case LSSphereCollider sphere:
 488                return WideFiniteAxisIntersection.DoesCenteredFiniteCylinderPenetrateSphere(
 489                    cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 490                    sphere.Center, sphere.ScaledRadius);
 91            case LSCapsuleCollider capsule:
 492                return WideFiniteAxisIntersection.DoesCenteredFiniteCylinderPenetrateCapsule(
 493                    cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 494                    capsule.Center, capsule.Rotation, Vector3d.Up, capsule.AxisLength, capsule.ScaledRadius);
 95            case LSCuboidCollider box:
 696                return WideOrientedBox.DoesCenteredCylinderPenetrateBox(
 697                    cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 698                    box.Center, box.Rotation, box.OrientedBox.HalfExtents);
 99            case LSCylinderCollider otherCylinder:
 7100                return WideFiniteAxisIntersection.DoesCenteredFiniteCylindersPenetrate(
 7101                    cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 7102                    otherCylinder.Center, otherCylinder.Rotation, otherCylinder.Height, otherCylinder.ScaledRadius);
 103            case LSConeCollider cone:
 4104                return WideFiniteAxisIntersection.DoesCenteredFiniteCylinderPenetrateCone(
 4105                    cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 4106                    cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius);
 107            default:
 19108                return DoesPostureCylinderPenetrateMesh(cylinder, (LSMeshCollider)other);
 109        }
 110    }
 111
 112    private static bool DoesPostureCapsulePenetrateMesh(LSCollider candidate, Fixed64 length, LSMeshCollider collider)
 113    {
 36114        PhysicsMesh mesh = collider.Mesh;
 36115        if (collider.Mode == MeshColliderMode.Convex && collider.IsClosedSurface)
 116        {
 14117            return WideOrientedBox.DoesConvexHullCenteredCapsuleStrictlyOverlap(
 14118                mesh.GeometryFrame, mesh.GeometryVertices, mesh.Triangles,
 14119                mesh.ConvexSatEdgeVertexPairs, candidate.Center, candidate.Rotation,
 14120                Vector3d.Up, length, candidate.ScaledRadius);
 121        }
 122
 123        // Open convex meshes and concave meshes are surfaces, not filled hulls.
 124        // Pass authored local vertices plus the exact rigid frame: transforming
 125        // triangle points into rounded world coordinates changes boundary truth.
 22126        ReadOnlySpan<int> indices = mesh.Triangles;
 22127        ReadOnlySpan<Vector3d> points = mesh.GeometryVertices;
 22128        Span<Vector3d> triangle = stackalloc Vector3d[3];
 22129        ReadOnlySpan<int> face = stackalloc int[] { 0, 1, 2 };
 22130        ReadOnlySpan<int> edges = stackalloc int[] { 0, 1, 1, 2, 2, 0 };
 176131        for (int i = 0; i < indices.Length; i += 3)
 132        {
 76133            triangle[0] = points[indices[i]];
 76134            triangle[1] = points[indices[i + 1]];
 76135            triangle[2] = points[indices[i + 2]];
 76136            if (WideOrientedBox.DoesConvexHullCenteredCapsuleStrictlyOverlap(
 76137                mesh.GeometryFrame, triangle, face, edges,
 76138                candidate.Center, candidate.Rotation, Vector3d.Up, length, candidate.ScaledRadius))
 10139                return true;
 140        }
 12141        return false;
 142    }
 143
 144    private static bool DoesPostureCylinderPenetrateMesh(LSCylinderCollider cylinder, LSMeshCollider collider)
 145    {
 19146        PhysicsMesh mesh = collider.Mesh;
 19147        if (collider.Mode == MeshColliderMode.Convex)
 148        {
 11149            return WideOrientedBox.DoesCenteredCylinderPenetrateConvexHull(
 11150                cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 11151                mesh.GeometryFrame, mesh.GeometryVertices, mesh.Triangles, collider.IsClosedSurface);
 152        }
 8153        ReadOnlySpan<int> indices = mesh.Triangles;
 8154        ReadOnlySpan<Vector3d> points = mesh.GeometryVertices;
 78155        for (int i = 0; i < indices.Length; i += 3)
 156        {
 35157            var triangle = new FixedTriangle(points[indices[i]], points[indices[i + 1]], points[indices[i + 2]]);
 35158            if (WideOrientedBox.DoesCenteredCylinderPenetrateTriangle(
 35159                cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius,
 35160                triangle, mesh.GeometryFrame))
 4161                return true;
 162        }
 4163        return false;
 164    }
 165}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/CollisionDetection.Sphere.cs

#LineLine coverage
 1//=======================================================================
 2// CollisionDetection.Sphere.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 FixedMathSharp;
 9using FixedMathSharp.Geometry;
 10
 11namespace Gravitas.CollisionHandling;
 12
 13/// <content>Finds contacts between sphere pairs.</content>
 14public static partial class CollisionDetection
 15{
 16    #region Sphere
 17
 18    private static bool DoSpheresCheck(CollisionWorkItem pair)
 19    {
 124920        if (!FixedSegment.TryGetCenteredCapsulesContact(
 124921                pair.ColliderA.Center,
 124922                pair.ColliderA.Rotation,
 124923                Vector3d.Up,
 124924                Fixed64.Zero,
 124925                pair.ColliderA.ScaledRadius,
 124926                pair.ColliderB.Center,
 124927                pair.ColliderB.Rotation,
 124928                Vector3d.Up,
 124929                Fixed64.Zero,
 124930                pair.ColliderB.ScaledRadius,
 124931                ResolveCenteredCapsuleFallback(pair.ColliderA.Center, pair.ColliderB.Center),
 124932                out FixedContactAnchors contact))
 33        {
 48034            return false;
 35        }
 36
 76937        pair.Manifold.SetContact(
 76938            new ContactAnchor(contact.FirstAnchor),
 76939            new ContactAnchor(contact.SecondAnchor),
 76940            contact.Depth,
 76941            contact.Normal,
 76942            contact.DepthIsClamped);
 76943        return true;
 44    }
 45
 46    #endregion
 47
 48}

Methods/Properties

DoCapsuleSphereCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCapsulesCheck(Gravitas.CollisionHandling.CollisionWorkItem)
ResolveCenteredCapsuleFallback(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
DoCompoundCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCompoundOtherCheck(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCompoundCollider,Gravitas.Colliders.LSCollider)
DoCompoundCompoundCheck(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCompoundCollider,Gravitas.Colliders.LSCompoundCollider)
TryBuildCompoundPartContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCollider,System.Int32,Gravitas.Colliders.LSCollider,System.Int32)
AddCompoundPartContactsInOwnerOrder(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.CollisionHandling.CollisionWorkItem,System.Int32,System.Int32)
OrderPartPairForDetection(Gravitas.Colliders.LSCollider,System.Int32,Gravitas.Colliders.LSCollider,System.Int32,Gravitas.Colliders.LSCollider&,System.Int32&,Gravitas.Colliders.LSCollider&,System.Int32&)
BoundsOverlapInclusive(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider)
DoConeSphereCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoConeConvexCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoMeshConeCheck(Gravitas.CollisionHandling.CollisionWorkItem)
IntersectsMeshConeSurface(Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSConeCollider,Gravitas.CollisionHandling.CollisionSatScratch)
BuildMeshConeSurfaceContacts(Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSConeCollider,Gravitas.CollisionHandling.CollisionSatScratch,Gravitas.CollisionHandling.ContactManifold)
GetConeConvexColliders(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSConeCollider&,Gravitas.Colliders.LSCollider&)
DoCollisionCheck(Gravitas.CollisionHandling.CollisionPair)
DoCollisionCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCuboidSphereCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCuboidCapsuleCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCuboidsCheck(Gravitas.CollisionHandling.CollisionWorkItem)
CanBuildAxisAlignedManifold(Gravitas.Colliders.LSCuboidCollider)
TryBuildAxisAlignedCuboidManifold(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCuboidCollider,Gravitas.Colliders.LSCuboidCollider)
AddAxisAlignedCuboidContacts(Gravitas.CollisionHandling.ContactManifold,Gravitas.Colliders.LSCuboidCollider,Gravitas.Colliders.LSCuboidCollider,System.Int32,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d)
AddCuboidContact(Gravitas.CollisionHandling.ContactManifold,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
.ctor(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Int32)
TryBuildCuboidFaceContacts(Gravitas.CollisionHandling.ContactManifold,FixedMathSharp.Geometry.FixedOrientedBox,FixedMathSharp.Geometry.FixedOrientedBox,FixedMathSharp.Vector3d,System.Boolean)
GetCuboidFaceAxis(FixedMathSharp.Vector3d)
IsCuboidFaceNormal(FixedMathSharp.FixedQuaternion,System.Int32,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d)
ClipCuboidFace(System.ReadOnlySpan`1<Gravitas.CollisionHandling.CollisionDetection/CuboidFaceVertex>,System.Span`1<Gravitas.CollisionHandling.CollisionDetection/CuboidFaceVertex>,System.Int32,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Int32)
InterpolateCuboidFace(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
InterpolateCuboidFaceCoordinate(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Signed192)
DoCylinderSphereCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCylinderCapsuleCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCylindersCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoCuboidCylinderCheck(Gravitas.CollisionHandling.CollisionWorkItem)
TryAddCylinderCylinderCapContacts(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCylinderCollider,Gravitas.Colliders.LSCylinderCollider,FixedMathSharp.Geometry.FixedContactAnchors)
TryAddCylinderCylinderCapContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCylinderCollider,Gravitas.Colliders.LSCylinderCollider,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.FixedContactAnchors)
ResolveNormal(FixedMathSharp.Vector3d)
SetContactInPairOrder(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSCollider,Gravitas.CollisionHandling.ContactAnchor,Gravitas.Colliders.LSCollider,Gravitas.CollisionHandling.ContactAnchor,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,System.Boolean)
DoMeshSphereCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoMeshCapsuleCheck(Gravitas.CollisionHandling.CollisionWorkItem)
TrySetConvexMeshRoundedContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
DoMeshCuboidCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoMeshCylinderCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoMeshesCheck(Gravitas.CollisionHandling.CollisionWorkItem)
DoesPostureCandidatePenetrate(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider)
DoesPostureLeafPenetrate(Gravitas.Colliders.LSCollider,Gravitas.Colliders.LSCollider)
DoesPostureCapsulePenetrate(Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64,Gravitas.Colliders.LSCollider)
DoesPostureCylinderPenetrate(Gravitas.Colliders.LSCylinderCollider,Gravitas.Colliders.LSCollider)
DoesPostureCapsulePenetrateMesh(Gravitas.Colliders.LSCollider,FixedMathSharp.Fixed64,Gravitas.Colliders.LSMeshCollider)
DoesPostureCylinderPenetrateMesh(Gravitas.Colliders.LSCylinderCollider,Gravitas.Colliders.LSMeshCollider)
DoSpheresCheck(Gravitas.CollisionHandling.CollisionWorkItem)