< Summary

Information
Class: Gravitas.CollisionHandling.MixedEmbedded2DGeometry
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/CollisionHandling/Detection/Mixed/MixedEmbedded2DGeometry.cs
Line coverage
100%
Covered lines: 173
Uncovered lines: 0
Coverable lines: 173
Total lines: 313
Line coverage: 100%
Branch coverage
100%
Covered branches: 46
Total branches: 46
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/Mixed/MixedEmbedded2DGeometry.cs

#LineLine coverage
 1//=======================================================================
 2// MixedEmbedded2DGeometry.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/// <summary>
 16/// Shared fixed-point geometry helpers for embedded 2D mixed slabs.
 17/// </summary>
 18internal static class MixedEmbedded2DGeometry
 19{
 20    public static Vector3d GetCenter3D(LSCollider2D embedded) =>
 5039221        new(embedded.Center.X, embedded.MixedSlabCenterY, embedded.Center.Y);
 22
 23    public static ContactAnchor GetSupportAnchor(LSCollider2D embedded, Vector3d worldDirection)
 24    {
 139825        Vector2d planarDirection = new(worldDirection.X, worldDirection.Z);
 139826        if (!TryGetPlanarSupportAnchor(
 139827                embedded,
 139828                planarDirection,
 139829                out ContactAnchor2D planarAnchor))
 30        {
 131            throw new InvalidOperationException(
 132                "Committed embedded 2D geometry has no representable owner-relative support anchor.");
 33        }
 34
 139735        Fixed64 yOffset = worldDirection.Y > Fixed64.Zero
 139736            ? embedded.MixedHalfThickness
 139737            : worldDirection.Y < Fixed64.Zero
 139738                ? -embedded.MixedHalfThickness
 139739                : Fixed64.Zero;
 139740        return EmbedPlanarAnchor(
 139741            planarAnchor,
 139742            embedded.MixedSlabCenterY,
 139743            yOffset);
 44    }
 45
 46    public static ContactAnchor GetClosestAnchorOnEmbeddedVolume(
 47        LSCollider2D embedded,
 48        Vector3d point)
 49    {
 190750        Vector2d planarPoint = new(point.X, point.Z);
 190751        bool planarInside = embedded.ContainsPoint(planarPoint);
 190752        GetClosestSlabOffset(
 190753            point.Y,
 190754            embedded.MixedSlabCenterY,
 190755            embedded.MixedHalfThickness,
 190756            out Fixed64 closestYOffset,
 190757            out bool yInside,
 190758            out Fixed64 signedCenterOffset);
 59
 190760        if (!planarInside || !yInside)
 61        {
 168662            if (planarInside)
 63            {
 52064                return new ContactAnchor(
 52065                    new Vector3d(
 52066                        planarPoint.X,
 52067                        embedded.MixedSlabCenterY,
 52068                        planarPoint.Y),
 52069                    new Vector3d(
 52070                        Fixed64.Zero,
 52071                        closestYOffset,
 52072                        Fixed64.Zero));
 73            }
 74
 116675            if (TryGetPlanarBoundaryAnchor(
 116676                    embedded,
 116677                    planarPoint,
 116678                    out ContactAnchor2D closestPlanar))
 79            {
 116580                return EmbedPlanarAnchor(
 116581                    closestPlanar,
 116582                    embedded.MixedSlabCenterY,
 116583                    closestYOffset);
 84            }
 85
 186            throw new InvalidOperationException(
 187                "Committed embedded 2D geometry has no semantic boundary anchor.");
 88        }
 89
 22190        bool upperYBoundary = signedCenterOffset > Fixed64.Zero;
 22191        Fixed64 yBoundaryOffset = upperYBoundary
 22192            ? embedded.MixedHalfThickness
 22193            : -embedded.MixedHalfThickness;
 22194        Fixed64 bestDistance = upperYBoundary
 22195            ? embedded.MixedHalfThickness - signedCenterOffset
 22196            : embedded.MixedHalfThickness + signedCenterOffset;
 22197        ContactAnchor closest = new(
 22198            new Vector3d(
 22199                planarPoint.X,
 221100                embedded.MixedSlabCenterY,
 221101                planarPoint.Y),
 221102            new Vector3d(
 221103                Fixed64.Zero,
 221104                yBoundaryOffset,
 221105                Fixed64.Zero));
 106
 107        // A point inside an admitted built-in shape is no farther from its
 108        // nearest boundary than that shape's representable dimensions.
 221109        _ = TryGetPlanarBoundaryAnchor(
 221110            embedded,
 221111            planarPoint,
 221112            out ContactAnchor2D planarBoundary,
 221113            out Fixed64 planarDistance);
 221114        if (planarDistance < bestDistance)
 115        {
 110116            closest = EmbedPlanarAnchor(
 110117                planarBoundary,
 110118                embedded.MixedSlabCenterY,
 110119                signedCenterOffset);
 120        }
 121
 221122        return closest;
 123    }
 124
 125    public static bool ContainsPointInSlab(LSCollider2D embedded, Fixed64 y)
 126    {
 1549127        GetClosestSlabOffset(
 1549128            y,
 1549129            embedded.MixedSlabCenterY,
 1549130            embedded.MixedHalfThickness,
 1549131            out _,
 1549132            out bool inside,
 1549133            out _);
 1549134        return inside;
 135    }
 136
 137    public static bool TryGetPlanarBoundaryPoint(
 138        LSCollider2D embedded,
 139        Vector2d point,
 140        out Vector2d boundary,
 141        out Fixed64 distance)
 142    {
 11143        if (TryGetPlanarBoundaryAnchor(
 11144                embedded,
 11145                point,
 11146                out ContactAnchor2D anchor,
 11147                out distance)
 11148            && anchor.TryGetWorldPoint(out boundary))
 149        {
 9150            return true;
 151        }
 152
 2153        boundary = default;
 2154        return false;
 155    }
 156
 157    public static bool TryGetPlanarBoundaryAnchor(
 158        LSCollider2D embedded,
 159        Vector2d point,
 160        out ContactAnchor2D boundary)
 161    {
 1166162        if (embedded.TryGetClosestBoundaryAnchor(
 1166163                point,
 1166164                out FixedPointAnchor2d anchor))
 165        {
 1165166            boundary = new ContactAnchor2D(anchor);
 1165167            return true;
 168        }
 169
 1170        boundary = default;
 1171        return false;
 172    }
 173
 174    public static bool TryGetPlanarBoundaryAnchor(
 175        LSCollider2D embedded,
 176        Vector2d point,
 177        out ContactAnchor2D boundary,
 178        out Fixed64 distance)
 179    {
 238180        if (embedded.TryGetClosestBoundaryAnchor(
 238181                point,
 238182                out FixedPointAnchor2d anchor,
 238183                out distance))
 184        {
 234185            boundary = new ContactAnchor2D(anchor);
 234186            return true;
 187        }
 188
 4189        boundary = default;
 4190        return false;
 191    }
 192
 193    private static bool TryGetPlanarSupportAnchor(
 194        LSCollider2D embedded,
 195        Vector2d direction,
 196        out ContactAnchor2D anchor)
 197    {
 1398198        switch (embedded.Shape)
 199        {
 200            case ColliderType2D.Circle:
 201                {
 660202                    var circle = (LSCircleCollider2D)embedded;
 660203                    return TryGetRoundSupportAnchor(
 660204                        circle.Center,
 660205                        circle.Rotation,
 660206                        Fixed64.Zero,
 660207                        circle.ScaledRadius,
 660208                        direction,
 660209                        out anchor);
 210                }
 211            case ColliderType2D.Capsule:
 212                {
 730213                    var capsule = (LSCapsuleCollider2D)embedded;
 730214                    return TryGetRoundSupportAnchor(
 730215                        capsule.Center,
 730216                        capsule.Rotation,
 730217                        capsule.AxisLength,
 730218                        capsule.ScaledRadius,
 730219                        direction,
 730220                        out anchor);
 221                }
 222            case ColliderType2D.AABox:
 223            case ColliderType2D.ConvexPolygon:
 7224                anchor = new ContactAnchor2D(
 7225                    embedded.GetConvexSupportAnchor(
 7226                        direction == Vector2d.Zero
 7227                            ? Vector2d.Right
 7228                            : direction));
 7229                return true;
 230            default:
 1231                anchor = default;
 1232                return false;
 233        }
 234    }
 235
 236    private static bool TryGetRoundSupportAnchor(
 237        Vector2d center,
 238        Fixed64 rotation,
 239        Fixed64 axisLength,
 240        Fixed64 radius,
 241        Vector2d direction,
 242        out ContactAnchor2D anchor)
 243    {
 1390244        Vector2d localDirection = direction == Vector2d.Zero
 1390245            ? Vector2d.Zero
 1390246            : Vector2d.Rotate(direction.Normalized, -rotation);
 247
 1390248        FixedPointAnchor2d localAnchor =
 1390249            FixedSegment2d.GetCenteredCapsuleSupportAnchor(
 1390250                center,
 1390251                rotation,
 1390252                Vector2d.Forward,
 1390253                axisLength,
 1390254                radius,
 1390255                localDirection);
 1390256        anchor = new ContactAnchor2D(localAnchor);
 1390257        return true;
 258    }
 259
 260    private static ContactAnchor EmbedPlanarAnchor(
 261        ContactAnchor2D planar,
 262        Fixed64 slabCenterY,
 263        Fixed64 localYDisplacement = default)
 264    {
 2672265        return new ContactAnchor(
 2672266            new Vector3d(planar.Origin.X, slabCenterY, planar.Origin.Y),
 2672267            FixedQuaternion.FromAxisAngle(Vector3d.Up, -planar.Rotation),
 2672268            new Vector3d(
 2672269                planar.LocalPoint.X,
 2672270                Fixed64.Zero,
 2672271                planar.LocalPoint.Y),
 2672272            new Vector3d(
 2672273                planar.LocalDisplacement.X,
 2672274                localYDisplacement,
 2672275                planar.LocalDisplacement.Y));
 276    }
 277
 278    private static void GetClosestSlabOffset(
 279        Fixed64 y,
 280        Fixed64 slabCenterY,
 281        Fixed64 halfThickness,
 282        out Fixed64 closestOffset,
 283        out bool inside,
 284        out Fixed64 signedCenterOffset)
 285    {
 3456286        if (!Fixed64.TrySubtract(y, slabCenterY, out signedCenterOffset))
 287        {
 2288            closestOffset = y < slabCenterY
 2289                ? -halfThickness
 2290                : halfThickness;
 2291            inside = false;
 2292            signedCenterOffset = closestOffset;
 2293            return;
 294        }
 295
 3454296        if (signedCenterOffset < -halfThickness)
 297        {
 36298            closestOffset = -halfThickness;
 36299            inside = false;
 36300            return;
 301        }
 302
 3418303        if (signedCenterOffset > halfThickness)
 304        {
 584305            closestOffset = halfThickness;
 584306            inside = false;
 584307            return;
 308        }
 309
 2834310        closestOffset = signedCenterOffset;
 2834311        inside = true;
 2834312    }
 313}