< Summary

Information
Class: Gravitas.CollisionHandling.MeshTriangleContactGenerator
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/3D/Mesh/MeshTriangleContactGenerator.cs
Line coverage
100%
Covered lines: 296
Uncovered lines: 0
Coverable lines: 296
Total lines: 503
Line coverage: 100%
Branch coverage
100%
Covered branches: 44
Total branches: 44
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/CollisionHandling/Detection/3D/Mesh/MeshTriangleContactGenerator.cs

#LineLine coverage
 1//=======================================================================
 2// MeshTriangleContactGenerator.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;
 11using SwiftCollections;
 12using SwiftCollections.Query;
 13using System;
 14using System.Runtime.CompilerServices;
 15
 16namespace Gravitas.CollisionHandling;
 17
 18/// <summary>
 19/// Builds deterministic triangle-level manifolds for mesh paths that cannot use
 20/// whole-shape convex assumptions.
 21/// </summary>
 22internal static class MeshTriangleContactGenerator
 23{
 24    public static bool TryBuildMeshSphereManifold(
 25        CollisionWorkItem pair,
 26        LSMeshCollider mesh,
 27        LSSphereCollider sphere,
 28        SwiftList<int> triangleBuffer)
 29    {
 1230        mesh.GetTrianglesInBounds(CreateBounds(sphere.BoundsMin, sphere.BoundsMax), triangleBuffer);
 9031        for (int i = 0; i < triangleBuffer.Count; i++)
 3332            TryAddSphereTriangleContact(pair, mesh, triangleBuffer[i], sphere);
 33
 1234        return pair.Manifold.HasContact;
 35    }
 36
 37    public static bool TryBuildMeshCapsuleManifold(
 38        CollisionWorkItem pair,
 39        LSMeshCollider mesh,
 40        LSCapsuleCollider capsule,
 41        SwiftList<int> triangleBuffer)
 42    {
 543        mesh.GetTrianglesInBounds(CreateBounds(capsule.BoundsMin, capsule.BoundsMax), triangleBuffer);
 44
 545        bool overlaps = false;
 3246        for (int i = 0; i < triangleBuffer.Count; i++)
 1147            overlaps |= TryAddCapsuleTriangleContact(pair, mesh, triangleBuffer[i], capsule);
 48
 549        return overlaps;
 50    }
 51
 52    public static bool TryBuildMeshCuboidManifold(
 53        CollisionWorkItem pair,
 54        LSMeshCollider mesh,
 55        LSCuboidCollider cuboid,
 56        SwiftList<int> triangleBuffer)
 57    {
 68958        mesh.GetTrianglesInBounds(CreateBounds(cuboid.BoundsMin, cuboid.BoundsMax), triangleBuffer);
 364059        for (int i = 0; i < triangleBuffer.Count; i++)
 113160            TryAddCuboidTriangleContact(pair, mesh, triangleBuffer[i], cuboid);
 61
 68962        return pair.Manifold.HasContact;
 63    }
 64
 65    public static bool TryBuildMeshCylinderManifold(
 66        CollisionWorkItem pair,
 67        LSMeshCollider mesh,
 68        LSCylinderCollider cylinder,
 69        SwiftList<int> triangleBuffer)
 70    {
 42871        mesh.GetTrianglesInBounds(CreateBounds(cylinder.BoundsMin, cylinder.BoundsMax), triangleBuffer);
 42872        bool overlaps = false;
 256473        for (int i = 0; i < triangleBuffer.Count; i++)
 85474            overlaps |= TryAddCylinderTriangleContact(pair, mesh, triangleBuffer[i], cylinder);
 75
 42876        return overlaps;
 77    }
 78
 79    public static bool TryBuildMeshMeshManifold(
 80        CollisionWorkItem pair,
 81        LSMeshCollider meshA,
 82        LSMeshCollider meshB,
 83        SwiftList<int> triangleBufferA,
 84        SwiftList<int> triangleBufferB)
 85    {
 64586        bool reverseContact = meshA.Id > meshB.Id;
 64587        LSMeshCollider firstMesh = reverseContact ? meshB : meshA;
 64588        LSMeshCollider secondMesh = reverseContact ? meshA : meshB;
 64589        firstMesh.GetTrianglesInBounds(
 64590            CreateBounds(secondMesh.BoundsMin, secondMesh.BoundsMax),
 64591            triangleBufferA);
 1658292        for (int i = 0; i < triangleBufferA.Count; i++)
 93        {
 764694            int triangleA = triangleBufferA[i];
 764695            firstMesh.Mesh.GetLocalTriangleVertices(
 764696                triangleA,
 764697                out Vector3d firstVertex,
 764698                out Vector3d secondVertex,
 764699                out Vector3d thirdVertex);
 7646100            var firstTriangle =
 7646101                new FixedTriangle(firstVertex, secondVertex, thirdVertex);
 7646102            GetTriangleBoundsInFrame(
 7646103                firstTriangle.Bounds,
 7646104                firstMesh.Mesh.Origin,
 7646105                firstMesh.Mesh.Rotation,
 7646106                secondMesh.Mesh.Origin,
 7646107                secondMesh.Mesh.Rotation,
 7646108                out FixedBoundVolume firstInSecondFrameBounds);
 7646109            secondMesh.Mesh.GetTrianglesInLocalBounds(
 7646110                firstInSecondFrameBounds,
 7646111                triangleBufferB);
 112
 76588113            for (int j = 0; j < triangleBufferB.Count; j++)
 114            {
 30648115                int triangleB = triangleBufferB[j];
 30648116                secondMesh.Mesh.GetLocalTriangleVertices(
 30648117                    triangleB,
 30648118                    out Vector3d secondFirstVertex,
 30648119                    out Vector3d secondSecondVertex,
 30648120                    out Vector3d secondThirdVertex);
 30648121                var secondTriangle = new FixedTriangle(
 30648122                    secondFirstVertex,
 30648123                    secondSecondVertex,
 30648124                    secondThirdVertex);
 30648125                if (!firstTriangle.TryGetContact(
 30648126                        firstMesh.Mesh.Origin,
 30648127                        firstMesh.Mesh.Rotation,
 30648128                        secondMesh.Mesh.Origin,
 30648129                        secondMesh.Mesh.Rotation,
 30648130                        secondTriangle,
 30648131                        out FixedContactAnchors contact))
 132                {
 133                    continue;
 134                }
 135
 19583136                if (reverseContact)
 137                {
 85138                    AddContact(
 85139                        pair,
 85140                        new ContactAnchor(contact.SecondAnchor),
 85141                        new ContactAnchor(contact.FirstAnchor),
 85142                        contact.Depth,
 85143                        -contact.Normal,
 85144                        contact.DepthIsClamped);
 145                }
 146                else
 147                {
 19498148                    AddContact(
 19498149                        pair,
 19498150                        new ContactAnchor(contact.FirstAnchor),
 19498151                        new ContactAnchor(contact.SecondAnchor),
 19498152                        contact.Depth,
 19498153                        contact.Normal,
 19498154                        contact.DepthIsClamped);
 155                }
 156            }
 157        }
 158
 645159        return pair.Manifold.HasContact;
 160    }
 161
 162    private static void TryAddSphereTriangleContact(
 163        CollisionWorkItem pair,
 164        LSMeshCollider mesh,
 165        int triangleIndex,
 166        LSSphereCollider sphere)
 167    {
 33168        mesh.Mesh.GetLocalTriangleVertices(
 33169            triangleIndex,
 33170            out Vector3d first,
 33171            out Vector3d second,
 33172            out Vector3d third);
 33173        if (!new FixedTriangle(first, second, third).TryGetSphereContact(
 33174                mesh.Mesh.Origin,
 33175                mesh.Mesh.Rotation,
 33176                sphere.Center,
 33177                sphere.Rotation,
 33178                sphere.ScaledRadius,
 33179                out FixedContactAnchors contact))
 180        {
 19181            return;
 182        }
 183
 14184        AddContact(
 14185            pair,
 14186            new ContactAnchor(contact.FirstAnchor),
 14187            new ContactAnchor(contact.SecondAnchor),
 14188            contact.Depth,
 14189            contact.Normal,
 14190            contact.DepthIsClamped);
 14191    }
 192
 193    private static bool TryAddCapsuleTriangleContact(
 194        CollisionWorkItem pair,
 195        LSMeshCollider mesh,
 196        int triangleIndex,
 197        LSCapsuleCollider capsule)
 198    {
 11199        mesh.Mesh.GetLocalTriangleVertices(
 11200            triangleIndex,
 11201            out Vector3d first,
 11202            out Vector3d second,
 11203            out Vector3d third);
 11204        var triangle = new FixedTriangle(first, second, third);
 11205        Vector3d fallbackNormal = OrientNormal(
 11206            mesh.Mesh.CreatePointAnchor(triangle.Centroid),
 11207            new FixedPointAnchor(
 11208                capsule.Center,
 11209                FixedQuaternion.Identity,
 11210                Vector3d.Zero),
 11211            mesh.Mesh.GetFaceNormalWorld(triangleIndex));
 11212        if (!triangle.TryGetCenteredCapsuleContact(
 11213                mesh.Mesh.Origin,
 11214                mesh.Mesh.Rotation,
 11215                capsule.Center,
 11216                capsule.Rotation,
 11217                capsule.AxisLength,
 11218                capsule.ScaledRadius,
 11219                fallbackNormal,
 11220                out FixedContactAnchors contact))
 221        {
 4222            return false;
 223        }
 224
 7225        AddContact(
 7226            pair,
 7227            new ContactAnchor(contact.FirstAnchor),
 7228            new ContactAnchor(contact.SecondAnchor),
 7229            contact.Depth,
 7230            contact.Normal,
 7231            contact.DepthIsClamped);
 7232        return true;
 233    }
 234
 235    private static void TryAddCuboidTriangleContact(
 236        CollisionWorkItem pair,
 237        LSMeshCollider mesh,
 238        int triangleIndex,
 239        LSCuboidCollider cuboid)
 240    {
 1131241        mesh.Mesh.GetLocalTriangleVertices(
 1131242            triangleIndex,
 1131243            out Vector3d first,
 1131244            out Vector3d second,
 1131245            out Vector3d third);
 1131246        var triangle = new FixedTriangle(first, second, third);
 1131247        Span<FixedContactLocalPoints> faceContacts =
 1131248            stackalloc FixedContactLocalPoints[4];
 1131249        if (!cuboid.OrientedBox.TryGetTriangleContact(
 1131250                mesh.Mesh.Origin,
 1131251                mesh.Mesh.Rotation,
 1131252                triangle,
 1131253                faceContacts,
 1131254                out FixedContactAnchors contact,
 1131255                out int faceContactCount))
 256        {
 237257            return;
 258        }
 259
 894260        if (faceContactCount > 0)
 261        {
 5962262            for (int index = 0; index < faceContactCount; index++)
 263            {
 2130264                AddReversedBoxTriangleContact(
 2130265                    pair,
 2130266                    contact,
 2130267                    faceContacts[index]);
 268            }
 851269            return;
 270        }
 271
 43272        AddReversedBoxTriangleContact(pair, contact);
 43273    }
 274
 275    private static void AddReversedBoxTriangleContact(
 276        CollisionWorkItem pair,
 277        FixedContactAnchors contact)
 278    {
 43279        AddContact(
 43280            pair,
 43281            new ContactAnchor(contact.SecondAnchor),
 43282            new ContactAnchor(contact.FirstAnchor),
 43283            contact.Depth,
 43284            -contact.Normal,
 43285            contact.DepthIsClamped);
 43286    }
 287
 288    private static void AddReversedBoxTriangleContact(
 289        CollisionWorkItem pair,
 290        FixedContactAnchors primary,
 291        FixedContactLocalPoints contact)
 292    {
 2130293        AddContact(
 2130294            pair,
 2130295            new ContactAnchor(
 2130296                primary.SecondAnchor.Origin,
 2130297                primary.SecondAnchor.Rotation,
 2130298                contact.SecondLocalPoint),
 2130299            new ContactAnchor(
 2130300                primary.FirstAnchor.Origin,
 2130301                primary.FirstAnchor.Rotation,
 2130302                contact.FirstLocalPoint),
 2130303            primary.Depth,
 2130304            -primary.Normal,
 2130305            primary.DepthIsClamped);
 2130306    }
 307
 308    private static bool TryAddCylinderTriangleContact(
 309        CollisionWorkItem pair,
 310        LSMeshCollider mesh,
 311        int triangleIndex,
 312        LSCylinderCollider cylinder)
 313    {
 854314        mesh.Mesh.GetLocalTriangleVertices(
 854315            triangleIndex,
 854316            out Vector3d first,
 854317            out Vector3d second,
 854318            out Vector3d third);
 854319        var triangle = new FixedTriangle(first, second, third);
 854320        var cylinderCenter = new FixedPointAnchor(
 854321            cylinder.Center,
 854322            FixedQuaternion.Identity,
 854323            Vector3d.Zero);
 854324        Vector3d normal = OrientNormal(
 854325            mesh.Mesh.CreatePointAnchor(triangle.Centroid),
 854326            cylinderCenter,
 854327            mesh.Mesh.GetFaceNormalWorld(triangleIndex));
 854328        if (TryAddCylinderCapTriangleContacts(
 854329                pair,
 854330                mesh,
 854331                triangle,
 854332                cylinder,
 854333                normal))
 334        {
 428335            return true;
 336        }
 337
 426338        if (!cylinderCenter.TryGetLocalPointIn(
 426339                mesh.Mesh.Origin,
 426340                mesh.Mesh.Rotation,
 426341                out Vector3d localCylinderCenter))
 342        {
 1343            return false;
 344        }
 345
 425346        Vector3d localPointOnMesh =
 425347            triangle.ClosestPoint(localCylinderCenter);
 425348        FixedPointAnchor meshAnchor =
 425349            mesh.Mesh.CreatePointAnchor(localPointOnMesh);
 350        // The closest point belongs to the admitted triangle candidate, so its
 351        // cylinder-frame offset is finite once the center entered the mesh frame.
 425352        _ = meshAnchor.TryGetLocalPointIn(
 425353            cylinder.Center,
 425354            cylinder.Rotation,
 425355            out Vector3d localPointInCylinder);
 425356        if (!FixedSegment.ContainsPointInCenteredFiniteCylinder(
 425357                localPointInCylinder,
 425358                Vector3d.Zero,
 425359                Vector3d.Up,
 425360                cylinder.Height,
 425361                cylinder.ScaledRadius))
 362        {
 5363            return false;
 364        }
 365        // Exact containment proves the centered canonical surface offset
 366        // remains inside the admitted radius and height.
 420367        _ = FixedSegment.TryGetClosestCenteredFiniteCylinderSurfaceOffset(
 420368            localPointInCylinder,
 420369            Vector3d.Zero,
 420370            Vector3d.Up,
 420371            cylinder.Height,
 420372            cylinder.ScaledRadius,
 420373            Vector3d.Right,
 420374            out Vector3d localCylinderPoint,
 420375            out _,
 420376            out Fixed64 signedDistance);
 377
 420378        Fixed64 depth = -signedDistance;
 420379        AddContact(
 420380            pair,
 420381            new ContactAnchor(meshAnchor),
 420382            new ContactAnchor(
 420383                cylinder.Center,
 420384                cylinder.Rotation,
 420385                localCylinderPoint),
 420386            depth,
 420387            normal,
 420388            depthIsClamped: false);
 420389        return true;
 390    }
 391
 392    private static bool TryAddCylinderCapTriangleContacts(
 393        CollisionWorkItem pair,
 394        LSMeshCollider mesh,
 395        FixedTriangle triangle,
 396        LSCylinderCollider cylinder,
 397        Vector3d normal)
 398    {
 854399        if (!CylinderContactGeometry.IsAxisAligned(
 854400                cylinder.Rotation,
 854401                Vector3d.Up,
 854402                normal))
 423403            return false;
 404
 431405        int initialCount = pair.Manifold.Count;
 431406        CylinderContactGeometry.GetCapBasis(cylinder, out Vector3d tangentA, out Vector3d tangentB);
 431407        TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal + tangentA, normal);
 431408        TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal - tangentA, normal);
 431409        TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal + tangentB, normal);
 431410        TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal - tangentB, normal);
 411
 431412        if (pair.Manifold.Count > initialCount)
 428413            return true;
 414
 3415        TryAddCylinderCapTriangleContact(pair, mesh, triangle, cylinder, -normal, normal);
 3416        return pair.Manifold.Count > initialCount;
 417    }
 418
 419    private static void TryAddCylinderCapTriangleContact(
 420        CollisionWorkItem pair,
 421        LSMeshCollider mesh,
 422        FixedTriangle triangle,
 423        LSCylinderCollider cylinder,
 424        Vector3d supportDirection,
 425        Vector3d normal)
 426    {
 1727427        if (!triangle.TryGetCenteredFiniteCylinderSupportContact(
 1727428                mesh.Mesh.Origin,
 1727429                mesh.Mesh.Rotation,
 1727430                cylinder.Center,
 1727431                cylinder.Rotation,
 1727432                cylinder.Height,
 1727433                cylinder.ScaledRadius,
 1727434                supportDirection,
 1727435                normal,
 1727436                out FixedContactAnchors contact))
 437        {
 868438            return;
 439        }
 440
 859441        AddContact(
 859442            pair,
 859443            new ContactAnchor(contact.FirstAnchor),
 859444            new ContactAnchor(contact.SecondAnchor),
 859445            contact.Depth,
 859446            contact.Normal,
 859447            contact.DepthIsClamped);
 859448    }
 449
 450    private static void GetTriangleBoundsInFrame(
 451        FixedBoundBox triangleBounds,
 452        Vector3d sourceOrigin,
 453        FixedQuaternion sourceRotation,
 454        Vector3d frameOrigin,
 455        FixedQuaternion frameRotation,
 456        out FixedBoundVolume bounds)
 457    {
 7646458        FixedBoundBox reframedBounds =
 7646459            FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain(
 7646460                sourceOrigin,
 7646461                sourceRotation,
 7646462                triangleBounds.Min,
 7646463                triangleBounds.Max,
 7646464                frameOrigin,
 7646465                frameRotation);
 7646466        bounds = CreateBounds(reframedBounds.Min, reframedBounds.Max);
 7646467    }
 468
 469    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 470    private static FixedBoundVolume CreateBounds(Vector3d min, Vector3d max) =>
 9425471        new(min, max);
 472
 473    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 474    private static Vector3d OrientNormal(
 475        in FixedPointAnchor source,
 476        in FixedPointAnchor target,
 477        Vector3d normal)
 478    {
 865479        Vector3d resolved = normal.Normalized;
 865480        return source.ProjectNonNegativeOffsetFrom(target, resolved)
 865481                > Fixed64.Zero
 865482            ? -resolved
 865483            : resolved;
 484    }
 485
 486    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 487    private static void AddContact(
 488        CollisionWorkItem pair,
 489        ContactAnchor anchorOnFirst,
 490        ContactAnchor anchorOnSecond,
 491        Fixed64 depth,
 492        Vector3d normalFirstToSecond,
 493        bool depthIsClamped)
 494    {
 23056495        pair.Manifold.AddContact(
 23056496            anchorOnFirst,
 23056497            anchorOnSecond,
 23056498            depth,
 23056499            normalFirstToSecond,
 23056500            depthIsClamped);
 23056501    }
 502
 503}

Methods/Properties

TryBuildMeshSphereManifold(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSSphereCollider,SwiftCollections.SwiftList`1<System.Int32>)
TryBuildMeshCapsuleManifold(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSCapsuleCollider,SwiftCollections.SwiftList`1<System.Int32>)
TryBuildMeshCuboidManifold(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSCuboidCollider,SwiftCollections.SwiftList`1<System.Int32>)
TryBuildMeshCylinderManifold(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSCylinderCollider,SwiftCollections.SwiftList`1<System.Int32>)
TryBuildMeshMeshManifold(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,Gravitas.Colliders.LSMeshCollider,SwiftCollections.SwiftList`1<System.Int32>,SwiftCollections.SwiftList`1<System.Int32>)
TryAddSphereTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,System.Int32,Gravitas.Colliders.LSSphereCollider)
TryAddCapsuleTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,System.Int32,Gravitas.Colliders.LSCapsuleCollider)
TryAddCuboidTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,System.Int32,Gravitas.Colliders.LSCuboidCollider)
AddReversedBoxTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,FixedMathSharp.Geometry.FixedContactAnchors)
AddReversedBoxTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,FixedMathSharp.Geometry.FixedContactAnchors,FixedMathSharp.Geometry.FixedContactLocalPoints)
TryAddCylinderTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,System.Int32,Gravitas.Colliders.LSCylinderCollider)
TryAddCylinderCapTriangleContacts(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,FixedMathSharp.Geometry.FixedTriangle,Gravitas.Colliders.LSCylinderCollider,FixedMathSharp.Vector3d)
TryAddCylinderCapTriangleContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.Colliders.LSMeshCollider,FixedMathSharp.Geometry.FixedTriangle,Gravitas.Colliders.LSCylinderCollider,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
GetTriangleBoundsInFrame(FixedMathSharp.Geometry.FixedBoundBox,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,SwiftCollections.Query.FixedBoundVolume&)
CreateBounds(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
OrientNormal(FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Vector3d)
AddContact(Gravitas.CollisionHandling.CollisionWorkItem,Gravitas.CollisionHandling.ContactAnchor,Gravitas.CollisionHandling.ContactAnchor,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d,System.Boolean)