< Summary

Line coverage
100%
Covered lines: 486
Uncovered lines: 0
Coverable lines: 486
Total lines: 821
Line coverage: 100%
Branch coverage
100%
Covered branches: 206
Total branches: 206
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: .ctor(...)100%22100%
File 1: .ctor(...)100%11100%
File 1: .ctor(...)100%11100%
File 1: get_ContainsCenter()100%11100%
File 1: get_IsConvexPatch()100%11100%
File 1: GetCenterAnchor()100%66100%
File 1: CreateCircleSlab(...)100%11100%
File 1: CreateSphere(...)100%11100%
File 1: GetSourceBounds(...)100%11100%
File 1: GetBounds(...)100%88100%
File 1: CanTranslateCenter(...)100%66100%
File 1: TryGetBoundsRelativeTo(...)100%1212100%
File 1: WithSourceOffset(...)100%44100%
File 1: GetSupportAnchor(...)100%1010100%
File 1: GetConvexPatchSupportAnchor(...)100%44100%
File 1: GetFallbackSurfaceAnchor(...)100%1616100%
File 1: TryGetClosestPointOnSurface(...)100%2222100%
File 1: TryGetPlanarSurfaceNormal(...)100%88100%
File 1: GetTriangleNormalAxis()100%11100%
File 1: TryGetCapsuleTriangleNormalAxis(...)100%44100%
File 1: GetCircleSlabSupportLocalPoint(...)100%44100%
File 1: GetSphereSupportLocalPoint(...)100%22100%
File 1: GetTriangleSupportLocalPoint(...)100%1010100%
File 1: CompareSupportProjection(...)100%11100%
File 2: TryGetInitialMeshContactNormal(...)100%2020100%
File 2: TryGetInitialMeshFaceContactNormal(...)100%88100%
File 2: TryGetInitialSurfaceProjectionTriangle(...)100%1818100%
File 2: ContainsInitialProjection(...)100%22100%
File 2: TryGetInitialTriangleContact(...)100%2020100%
File 2: TryGetInitialScaledTriangleContact(...)100%1414100%
File 2: GetInitialExitNormal(...)100%22100%
File 2: GetInitialHullFaceExit(...)100%44100%
File 2: TryGetHullCapsuleContact(...)100%11100%

File(s)

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexShape.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexShape.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;
 12using Gravitas.CollisionHandling;
 13
 14namespace Gravitas.Queries;
 15
 16/// <summary>Retains canonical convex geometry and exact support anchors for prepared sweeps.</summary>
 17/// <content>Owns shape views, source bounds and support feature selection.</content>
 18internal readonly partial struct ConvexShape
 19{
 20    private readonly ConvexShapeKind _kind;
 21    private readonly LSCollider? _collider;
 22    private readonly LSMeshCollider? _triangleOwner;
 23    private readonly int _triangleIndex;
 24    private readonly Vector3d _offset;
 25    private readonly Vector3d _triangleA;
 26    private readonly Vector3d _triangleB;
 27    private readonly Vector3d _triangleC;
 28    private readonly Vector3d _center;
 29    private readonly Fixed64 _radius;
 30    private readonly Fixed64 _halfHeight;
 31
 32    public ConvexShape(LSCollider collider, Vector3d offset, bool convexPatch = false)
 33    {
 845434        _kind = convexPatch ? ConvexShapeKind.ConvexPatch : ConvexShapeKind.Collider;
 845435        _collider = collider;
 845436        _triangleOwner = null;
 845437        _triangleIndex = -1;
 845438        _offset = offset;
 845439        _triangleA = Vector3d.Zero;
 845440        _triangleB = Vector3d.Zero;
 845441        _triangleC = Vector3d.Zero;
 845442        _center = Vector3d.Zero;
 845443        _radius = Fixed64.Zero;
 845444        _halfHeight = Fixed64.Zero;
 845445    }
 46
 47    public ConvexShape(
 48        LSMeshCollider triangleOwner,
 49        int triangleIndex,
 50        Vector3d triangleA,
 51        Vector3d triangleB,
 52        Vector3d triangleC)
 53    {
 348654        _kind = ConvexShapeKind.Triangle;
 348655        _collider = null;
 348656        _triangleOwner = triangleOwner;
 348657        _triangleIndex = triangleIndex;
 348658        _offset = Vector3d.Zero;
 348659        _triangleA = triangleA;
 348660        _triangleB = triangleB;
 348661        _triangleC = triangleC;
 348662        _center = Vector3d.Zero;
 348663        _radius = Fixed64.Zero;
 348664        _halfHeight = Fixed64.Zero;
 348665    }
 66
 67    private ConvexShape(
 68        ConvexShapeKind kind,
 69        Vector3d center,
 70        Fixed64 radius,
 71        Fixed64 halfHeight,
 72        Vector3d offset)
 73    {
 2376374        _kind = kind;
 2376375        _collider = null;
 2376376        _triangleOwner = null;
 2376377        _triangleIndex = -1;
 2376378        _offset = offset;
 2376379        _triangleA = Vector3d.Zero;
 2376380        _triangleB = Vector3d.Zero;
 2376381        _triangleC = Vector3d.Zero;
 2376382        _center = center;
 2376383        _radius = radius;
 2376384        _halfHeight = halfHeight;
 2376385    }
 86
 3487    public bool ContainsCenter => _collider is not LSMeshCollider;
 88
 371989    public bool IsConvexPatch => _kind == ConvexShapeKind.ConvexPatch;
 90
 91    public FixedPointAnchor GetCenterAnchor()
 92    {
 1699493        if (_kind == ConvexShapeKind.Triangle)
 94        {
 247395            return _triangleOwner!.Mesh.CreatePointAnchor(new Vector3d(
 247396                    FixedMath.Average(
 247397                        _triangleA.X,
 247398                        _triangleB.X,
 247399                        _triangleC.X),
 2473100                    FixedMath.Average(
 2473101                        _triangleA.Y,
 2473102                        _triangleB.Y,
 2473103                        _triangleC.Y),
 2473104                    FixedMath.Average(
 2473105                        _triangleA.Z,
 2473106                        _triangleB.Z,
 2473107                        _triangleC.Z)));
 108        }
 109
 14521110        return new FixedPointAnchor(
 14521111            _kind is ConvexShapeKind.CircleSlab or ConvexShapeKind.Sphere
 14521112                ? _center
 14521113                : _collider!.Center,
 14521114            FixedQuaternion.Identity,
 14521115            Vector3d.Zero,
 14521116            _offset);
 117    }
 118
 119    public static ConvexShape CreateCircleSlab(Vector3d center, Fixed64 radius, Fixed64 halfHeight) =>
 32120        new(ConvexShapeKind.CircleSlab, center, radius, halfHeight, Vector3d.Zero);
 121
 122    public static ConvexShape CreateSphere(Vector3d center, Fixed64 radius) =>
 22099123        new(ConvexShapeKind.Sphere, center, radius, Fixed64.Zero, Vector3d.Zero);
 124
 125    public void GetSourceBounds(out Vector3d min, out Vector3d max) =>
 30863126        GetBounds(out min, out max);
 127
 128    public void GetBounds(out Vector3d min, out Vector3d max)
 129    {
 32350130        if (_kind is ConvexShapeKind.CircleSlab or ConvexShapeKind.Sphere)
 131        {
 23625132            Vector3d center = _center + _offset;
 23625133            Vector3d extents = _kind == ConvexShapeKind.Sphere
 23625134                ? Vector3d.One * _radius
 23625135                : new Vector3d(_radius, _halfHeight, _radius);
 23625136            min = center - extents;
 23625137            max = center + extents;
 23625138            return;
 139        }
 140
 8725141        if (_kind == ConvexShapeKind.Triangle)
 142        {
 1487143            Vector3d localMin = Vector3d.Min(
 1487144                _triangleA,
 1487145                Vector3d.Min(_triangleB, _triangleC));
 1487146            Vector3d localMax = Vector3d.Max(
 1487147                _triangleA,
 1487148                Vector3d.Max(_triangleB, _triangleC));
 1487149            FixedBoundBox bounds = _triangleOwner!.Mesh.GeometryFrame.GetWorldBounds(localMin, localMax);
 1487150            min = bounds.Min;
 1487151            max = bounds.Max;
 1487152            return;
 153        }
 154
 7238155        min = _collider!.Bounds.Min + _offset;
 7238156        max = _collider.Bounds.Max + _offset;
 7238157    }
 158
 159    public bool CanTranslateCenter(Vector3d displacement)
 160    {
 28897161        Vector3d canonicalCenter =
 28897162            _kind is ConvexShapeKind.CircleSlab or ConvexShapeKind.Sphere
 28897163                ? _center
 28897164                : _collider!.Center;
 28897165        return Vector3d.TryAdd(
 28897166                canonicalCenter,
 28897167                _offset,
 28897168                out Vector3d currentCenter)
 28897169            && Vector3d.TryAdd(
 28897170                currentCenter,
 28897171                displacement,
 28897172                out _);
 173    }
 174
 175    public bool TryGetBoundsRelativeTo(
 176        Vector3d referenceOrigin,
 177        FixedQuaternion referenceRotation,
 178        out Vector3d min,
 179        out Vector3d max)
 180    {
 181        FixedBoundBox relativeBounds;
 1136182        if (_kind is ConvexShapeKind.CircleSlab or ConvexShapeKind.Sphere)
 183        {
 26184            Vector3d extents = _kind == ConvexShapeKind.Sphere
 26185                ? Vector3d.One * _radius
 26186                : new Vector3d(_radius, _halfHeight, _radius);
 26187            relativeBounds =
 26188                FixedBoundBox.FromRelativeRotatedBoundsClippedToDomain(
 26189                    _center,
 26190                    FixedQuaternion.Identity,
 26191                    -extents,
 26192                    extents,
 26193                    referenceOrigin,
 26194                    referenceRotation);
 195        }
 196        else
 197        {
 1110198            relativeBounds = ColliderCanonicalBounds.GetRelativeBounds(
 1110199                _collider!,
 1110200                referenceOrigin,
 1110201                referenceRotation);
 202        }
 203
 1136204        if (_offset == Vector3d.Zero)
 205        {
 1133206            min = relativeBounds.Min;
 1133207            max = relativeBounds.Max;
 1133208            return true;
 209        }
 210
 211        // Source offsets come from a chord whose magnitude was admitted by
 212        // Prepare, so a unit rotation cannot move them outside Fixed64.
 3213        _ = referenceRotation.Inverse().TryRotate(
 3214            _offset,
 3215            out Vector3d localOffset);
 3216        if (!Vector3d.TryAdd(relativeBounds.Min, localOffset, out min)
 3217            || !Vector3d.TryAdd(relativeBounds.Max, localOffset, out max))
 218        {
 2219            min = default;
 2220            max = default;
 2221            return false;
 222        }
 223
 1224        return true;
 225    }
 226
 227    public ConvexShape WithSourceOffset(Vector3d additionalOffset) =>
 4693228        _kind is ConvexShapeKind.CircleSlab or ConvexShapeKind.Sphere
 4693229            ? new ConvexShape(
 4693230                _kind,
 4693231                _center,
 4693232                _radius,
 4693233                _halfHeight,
 4693234                _offset + additionalOffset)
 4693235            : new ConvexShape(_collider!, _offset + additionalOffset);
 236
 237    public FixedPointAnchor GetSupportAnchor(Vector3d direction, bool preferAxialCenter = false)
 238    {
 64260239        if (_kind == ConvexShapeKind.ConvexPatch)
 4040240            return GetConvexPatchSupportAnchor(direction);
 60220241        if (_kind == ConvexShapeKind.Triangle)
 242        {
 14909243            return _triangleOwner!.Mesh.CreatePointAnchor(GetTriangleSupportLocalPoint(direction));
 244        }
 245
 45311246        if (_kind is ConvexShapeKind.CircleSlab or ConvexShapeKind.Sphere)
 247        {
 5662248            if (_kind == ConvexShapeKind.Sphere)
 249            {
 5158250                return FixedSegment.GetCenteredCapsuleSupportAnchor(
 5158251                        _center,
 5158252                        FixedQuaternion.Identity,
 5158253                        Fixed64.Zero,
 5158254                        _radius,
 5158255                        direction)
 5158256                    .WithLocalTranslation(_offset);
 257            }
 258
 504259            return new FixedPointAnchor(
 504260                _center,
 504261                FixedQuaternion.Identity,
 504262                GetCircleSlabSupportLocalPoint(direction),
 504263                _offset);
 264        }
 265
 39649266        return ConvexColliderSupport.GetSupportAnchor(
 39649267            _collider!,
 39649268            direction,
 39649269            _offset,
 39649270            preferAxialCenter);
 271    }
 272
 273    private FixedPointAnchor GetConvexPatchSupportAnchor(Vector3d direction)
 274    {
 4040275        PhysicsMesh mesh = ((LSMeshCollider)_collider!).Mesh;
 4040276        ReadOnlySpan<int> corners = mesh.GetConvexCoplanarPatchCornerVertexIndices(0);
 4040277        ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices;
 4040278        WideAxis3 axis = mesh.GeometryFrame.GetSourceDirectionNumerators(direction);
 4040279        Vector3d best = vertices[corners[0]];
 32320280        for (int index = 1; index < corners.Length; index++)
 12120281            if (CompareSupportProjection(vertices[corners[index]], best, axis) > 0)
 3888282                best = vertices[corners[index]];
 4040283        return mesh.CreatePointAnchor(best);
 284    }
 285
 286    public FixedPointAnchor GetFallbackSurfaceAnchor(Vector3d direction)
 287    {
 20288        if (_kind == ConvexShapeKind.Collider
 20289            && _collider is LSCuboidCollider cuboid)
 290        {
 12291            Vector3d localDirection =
 12292                cuboid.Rotation.Inverse().Rotate(direction);
 12293            Vector3d absoluteDirection = Vector3d.Abs(localDirection);
 12294            Vector3d halfExtents = cuboid.OrientedBox.HalfExtents;
 295            Vector3d localPoint;
 12296            if (absoluteDirection.X >= absoluteDirection.Y
 12297                && absoluteDirection.X >= absoluteDirection.Z)
 298            {
 6299                localPoint = new Vector3d(
 6300                    localDirection.X >= Fixed64.Zero
 6301                        ? halfExtents.X
 6302                        : -halfExtents.X,
 6303                    Fixed64.Zero,
 6304                    Fixed64.Zero);
 305            }
 6306            else if (absoluteDirection.Y >= absoluteDirection.Z)
 307            {
 2308                localPoint = new Vector3d(
 2309                    Fixed64.Zero,
 2310                    localDirection.Y >= Fixed64.Zero
 2311                        ? halfExtents.Y
 2312                        : -halfExtents.Y,
 2313                    Fixed64.Zero);
 314            }
 315            else
 316            {
 4317                localPoint = new Vector3d(
 4318                    Fixed64.Zero,
 4319                    Fixed64.Zero,
 4320                    localDirection.Z >= Fixed64.Zero
 4321                        ? halfExtents.Z
 4322                        : -halfExtents.Z);
 323            }
 324
 12325            return new FixedPointAnchor(
 12326                cuboid.Center,
 12327                cuboid.Rotation,
 12328                localPoint,
 12329                ConvexColliderSupport.GetLocalDisplacement(
 12330                    cuboid.Rotation,
 12331                    _offset));
 332        }
 333
 8334        return GetSupportAnchor(direction);
 335    }
 336
 337    public bool TryGetClosestPointOnSurface(Vector3d point, out Vector3d closest)
 338    {
 72339        ChronicleGuard.ThrowIfTrue(
 72340            _kind == ConvexShapeKind.Triangle,
 72341            nameof(ConvexShape),
 72342            "Triangle shapes are stationary mesh targets and cannot be sweep sources.");
 343
 71344        if (_kind == ConvexShapeKind.Collider)
 345        {
 55346            if (_collider is LSMeshCollider)
 347            {
 10348                closest = Vector3d.Zero;
 10349                return false;
 350            }
 45351            if (_collider is LSCuboidCollider cuboid)
 352            {
 21353                if (!Vector3d.TrySubtract(
 21354                        point,
 21355                        _offset,
 21356                        out Vector3d untranslatedPoint))
 357                {
 1358                    closest = Vector3d.Zero;
 1359                    return false;
 360                }
 361
 362                // Sweep offsets come from a chord whose magnitude was admitted
 363                // by Prepare, so a unit rotation preserves representability.
 20364                _ = cuboid.Rotation.Inverse().TryRotate(
 20365                    _offset,
 20366                    out Vector3d localTranslation);
 20367                return cuboid.OrientedBox
 20368                    .GetClosestPointAnchor(untranslatedPoint)
 20369                    .WithLocalTranslation(localTranslation)
 20370                    .TryGetPoint(out closest);
 371            }
 24372            closest = _collider!.ClosestPointOnSurface(point - _offset) + _offset;
 24373            return true;
 374        }
 375
 16376        Vector3d center = _center + _offset;
 16377        if (_kind == ConvexShapeKind.Sphere)
 378        {
 2379            Vector3d direction = point - center;
 2380            closest = center + GetSphereSupportLocalPoint(direction);
 2381            return true;
 382        }
 383
 14384        Vector3d local = point - center;
 14385        Vector3d radial = new(local.X, Fixed64.Zero, local.Z);
 14386        Fixed64 radialDistance = radial.Magnitude;
 14387        Vector3d radialDirection = radialDistance > Fixed64.Epsilon
 14388            ? radial / radialDistance
 14389            : Vector3d.Right;
 14390        Fixed64 clampedY = FixedMath.Clamp(local.Y, -_halfHeight, _halfHeight);
 391
 14392        if (radialDistance <= _radius && local.Y >= -_halfHeight && local.Y <= _halfHeight)
 393        {
 6394            Fixed64 sideDistance = _radius - radialDistance;
 6395            Fixed64 capDistance = _halfHeight - local.Y.Abs();
 6396            closest = sideDistance <= capDistance
 6397                ? center + new Vector3d(radialDirection.X * _radius, local.Y, radialDirection.Z * _radius)
 6398                : center + new Vector3d(local.X, local.Y.Sign() * _halfHeight, local.Z);
 6399            return true;
 400        }
 401
 8402        Fixed64 surfaceRadius = radialDistance > _radius ? _radius : radialDistance;
 8403        closest = center + new Vector3d(
 8404            radialDirection.X * surfaceRadius,
 8405            clampedY,
 8406            radialDirection.Z * surfaceRadius);
 8407        return true;
 408    }
 409
 410    public bool TryGetPlanarSurfaceNormal(Vector3d point, out Vector3d normal)
 411    {
 5812412        if (_kind == ConvexShapeKind.ConvexPatch)
 413        {
 173414            normal = ((LSMeshCollider)_collider!).Mesh.GetFaceNormalWorld(0);
 173415            return true;
 416        }
 5639417        if (_kind == ConvexShapeKind.CircleSlab)
 418        {
 1419            throw new InvalidOperationException(
 1420                "Circle slabs are sweep sources and cannot be target shapes.");
 421        }
 5638422        if (_kind == ConvexShapeKind.Sphere)
 423        {
 1424            throw new InvalidOperationException(
 1425                "Sphere query sources cannot be target shapes.");
 426        }
 427
 5637428        if (_kind == ConvexShapeKind.Triangle)
 429        {
 1925430            normal = _triangleOwner!.Mesh.GetFaceNormalWorld(_triangleIndex);
 1925431            return true;
 432        }
 433
 3712434        return _collider!.TryGetPlanarSurfaceNormal(point, out normal);
 435    }
 436
 437    internal WideDirection3d GetTriangleNormalAxis()
 438    {
 439        // The CCD admission hook only receives admitted concave-mesh triangles;
 440        // mesh construction and scale admission reject degenerate triangles.
 441        System.Diagnostics.Debug.Assert(_kind == ConvexShapeKind.Triangle);
 153442        var frame = _triangleOwner!.Mesh.GeometryFrame;
 153443        return WideTriangleRelations.GetScaledTriangleLocalNormal(new(_triangleA, _triangleB, _triangleC), frame)
 153444            .Transform(frame.GeometryBasis);
 445    }
 446
 447    internal bool TryGetCapsuleTriangleNormalAxis(in ConvexShape target, in FixedPointAnchor point, out WideDirection3d 
 448    {
 154449        axis = default;
 164450        if (_collider is not LSCapsuleCollider capsule) return false;
 451        // CanTranslateCenter validates both chord endpoints for every source
 452        // leaf before the worker samples any intermediate translation.
 144453        Vector3d center = capsule.Center + _offset;
 144454        axis = WideFiniteAxisIntersection.GetPointToCenteredCapsuleCoreDirection(point, center,
 144455            capsule.Rotation, Vector3d.Up, capsule.AxisLength);
 456        // A materialized target witness may no longer be the closest point.
 457        // Replace the selected face normal only when both finite features
 458        // support this exact core-to-triangle direction.
 459        System.Diagnostics.Debug.Assert(target._kind == ConvexShapeKind.Triangle);
 144460        return !axis.IsZero && MeshSurfaceBoundaryFan.SupportsTriangle(target._triangleOwner!.Mesh,
 144461            target._triangleIndex, RationalPoint3d.FromAnchor(point), axis);
 462    }
 463
 464    private Vector3d GetCircleSlabSupportLocalPoint(Vector3d direction)
 465    {
 504466        Vector3d radial = new(direction.X, Fixed64.Zero, direction.Z);
 504467        Fixed64 radialMagnitude = radial.Magnitude;
 504468        Vector3d radialSupport = radialMagnitude > Fixed64.Epsilon
 504469            ? radial / radialMagnitude * _radius
 504470            : Vector3d.Right * _radius;
 504471        Fixed64 y = direction.Y >= Fixed64.Zero ? _halfHeight : -_halfHeight;
 504472        return new Vector3d(radialSupport.X, y, radialSupport.Z);
 473    }
 474
 475    private Vector3d GetSphereSupportLocalPoint(Vector3d direction)
 476    {
 2477        Vector3d normal = direction.IsZero
 2478            ? Vector3d.Right
 2479            : direction.Normalized;
 2480        return normal * _radius;
 481    }
 482
 483    private Vector3d GetTriangleSupportLocalPoint(Vector3d direction)
 484    {
 14909485        if (!_triangleOwner!.Mesh.GeometryFrame.IsRigid)
 486        {
 98487            WideAxis3 axis = _triangleOwner.Mesh.GeometryFrame.GetSourceDirectionNumerators(direction);
 98488            Vector3d selected = _triangleA;
 108489            if (CompareSupportProjection(_triangleB, selected, axis) > 0) selected = _triangleB;
 110490            if (CompareSupportProjection(_triangleC, selected, axis) > 0) selected = _triangleC;
 98491            return selected;
 492        }
 14811493        Vector3d localDirection =
 14811494            _triangleOwner!.Mesh.Rotation.Inverse().Rotate(direction);
 14811495        Vector3d best = _triangleA;
 14811496        if (Vector3d.CompareProjection(_triangleB, best, localDirection) > 0)
 4339497            best = _triangleB;
 498
 14811499        if (Vector3d.CompareProjection(_triangleC, best, localDirection) > 0)
 3028500            best = _triangleC;
 501
 14811502        return best;
 503    }
 504
 12316505    private static int CompareSupportProjection(Vector3d point, Vector3d current, in WideAxis3 axis) => WideAxis3.Dot(ax
 12316506            Signed320.ExtendValue(WideArithmetic.Difference(point.X, current.X)),
 12316507            Signed320.ExtendValue(WideArithmetic.Difference(point.Y, current.Y)),
 12316508            Signed320.ExtendValue(WideArithmetic.Difference(point.Z, current.Z)))).Sign;
 509}

/home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/Sweeps/ConvexShape.InitialMeshContact.cs

#LineLine coverage
 1//=======================================================================
 2// ConvexShape.InitialMeshContact.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.Diagnostics;
 10using FixedMathSharp;
 11using FixedMathSharp.Geometry;
 12using Gravitas.Colliders;
 13
 14namespace Gravitas.Queries;
 15
 16/// <content>Resolves initial mesh contact witnesses from the selected convex source leaf.</content>
 17internal readonly partial struct ConvexShape
 18{
 19    // Both views describe their initial committed poses. Compound reduction
 20    // must call this while the actual source and target leaves are retained.
 21    internal bool TryGetInitialMeshContactNormal(
 22        in ConvexShape source,
 23        bool allowHullFaceExit,
 24        out FixedPointAnchor targetAnchor,
 25        out Vector3d normal)
 26    {
 27        Debug.Assert(_offset == Vector3d.Zero && source._offset == Vector3d.Zero);
 28        FixedContactAnchors contact;
 182829        if (_kind == ConvexShapeKind.Triangle)
 30        {
 82831            if (TryGetInitialMeshFaceContactNormal(source, out targetAnchor, out normal))
 78632                return true;
 4233            bool found = TryGetInitialTriangleContact(source,
 4234                new FixedTriangle(_triangleA, _triangleB, _triangleC), out contact);
 4235            targetAnchor = contact.FirstAnchor;
 4236            normal = contact.Normal;
 4237            return found;
 38        }
 39
 100040        var mesh = (LSMeshCollider)_collider!;
 100041        PhysicsMesh hull = mesh.Mesh;
 42        bool hasContact;
 100043        switch (source._collider)
 44        {
 45            case LSCuboidCollider box:
 93846                hasContact = WideOrientedBox.TryGetConvexHullContact(
 93847                    box.Center, box.Rotation, box.OrientedBox.HalfExtents,
 93848                    hull.GeometryFrame, hull.Origin, hull.GeometryVertices,
 93849                    hull.Triangles, hull.ConvexSatEdgeVertexPairs, out contact);
 93850                targetAnchor = contact.SecondAnchor;
 93851                normal = -contact.Normal;
 93852                return hasContact;
 53            case LSCapsuleCollider capsule:
 1054                hasContact = TryGetHullCapsuleContact(hull, capsule.Center,
 1055                    capsule.Rotation, capsule.AxisLength, capsule.ScaledRadius, out contact);
 1056                break;
 57            case LSSphereCollider sphere:
 858                hasContact = TryGetHullCapsuleContact(hull, sphere.Center,
 859                    sphere.Rotation, Fixed64.Zero, sphere.ScaledRadius, out contact);
 860                break;
 61            case LSMeshCollider sourceMesh:
 1062                PhysicsMesh sourceHull = sourceMesh.Mesh;
 1063                hasContact = hull.GeometryFrame.IsRigid && sourceHull.GeometryFrame.IsRigid
 1064                    ? FixedConvexHullRelations.TryGetContact(hull.Origin, hull.Rotation, hull.GeometryVertices,
 1065                        hull.Triangles, hull.ConvexSatEdgeVertexPairs, sourceHull.Origin, sourceHull.Rotation,
 1066                        sourceHull.GeometryVertices, sourceHull.Triangles, sourceHull.ConvexSatEdgeVertexPairs, out cont
 1067                    : WideOrientedBox.TryGetConvexHullContact(hull.GeometryFrame, hull.Origin, hull.GeometryVertices,
 1068                        hull.Triangles, hull.ConvexSatEdgeVertexPairs, sourceHull.GeometryFrame, sourceHull.Origin,
 1069                        sourceHull.GeometryVertices, sourceHull.Triangles, sourceHull.ConvexSatEdgeVertexPairs, out cont
 1070                break;
 71            default:
 3472                if (source._kind == ConvexShapeKind.Sphere)
 73                {
 774                    hasContact = TryGetHullCapsuleContact(hull, source._center,
 775                        FixedQuaternion.Identity, Fixed64.Zero, source._radius, out contact);
 776                    break;
 77                }
 78                // A hull face exit alone cannot certify a tolerant, disjoint contact.
 2779                if (!allowHullFaceExit)
 80                {
 281                    targetAnchor = default;
 282                    normal = default;
 283                    return false;
 84                }
 2585                GetInitialHullFaceExit(source, hull, out targetAnchor, out normal);
 2586                return true;
 87        }
 3588        targetAnchor = contact.FirstAnchor;
 3589        normal = contact.Normal;
 3590        return hasContact;
 91    }
 92
 93    /// <summary>Certifies a positive finite face contact before reducing covered triangle witnesses.</summary>
 94    internal bool TryGetInitialMeshFaceContactNormal(in ConvexShape source,
 95        out FixedPointAnchor targetAnchor, out Vector3d normal)
 96    {
 82997        targetAnchor = default;
 82998        normal = default;
 99        Debug.Assert(_kind == ConvexShapeKind.Triangle);
 829100        PhysicsMesh mesh = _triangleOwner!.Mesh;
 829101        Vector3d planeNormal = GetInitialExitNormal(source, mesh.GetFaceNormalWorld(_triangleIndex));
 829102        FixedPointAnchor support = source.GetSupportAnchor(-planeNormal);
 829103        var triangle = new FixedTriangle(_triangleA, _triangleB, _triangleC);
 829104        if (!TryGetInitialSurfaceProjectionTriangle(mesh, support, ref triangle)
 829105            || !TryGetInitialTriangleContact(source, triangle, out FixedContactAnchors contact))
 37106            return false;
 107
 108        // A finite neighboring face must actually contact the source; projection
 109        // alone is insufficient for tilted shapes, and a perimeter touch stays radial.
 792110        var matched = new ConvexShape(_triangleOwner!, _triangleIndex, triangle.A, triangle.B, triangle.C);
 792111        FixedPointAnchor targetSupport = matched.GetSupportAnchor(contact.Normal);
 792112        FixedPointAnchor sourceSupport = source.GetSupportAnchor(-contact.Normal);
 792113        if (WidePointAnchor3d.CompareProjectedOffsets(targetSupport, sourceSupport,
 792114                targetSupport, targetSupport, contact.Normal) <= 0)
 6115            return false;
 786116        if (mesh.GeometryFrame.IsRigid)
 778117            targetAnchor = triangle.GetClosestPointAnchor(mesh.Origin, mesh.Rotation, support);
 118        else
 119        {
 120            // TryGetInitialSurfaceProjectionTriangle already certified this
 121            // finite projection, whose mesh-center offset is reach-bounded.
 8122            bool represented = WideTriangleRelations.TryGetProjectedPointAnchor(triangle, mesh.GeometryFrame,
 8123                RationalPoint3d.FromAnchor(support), mesh.Origin, out targetAnchor);
 124            Debug.Assert(represented);
 125        }
 786126        normal = planeNormal;
 786127        return true;
 128    }
 129
 130    private bool TryGetInitialSurfaceProjectionTriangle(
 131        PhysicsMesh mesh, in FixedPointAnchor support, ref FixedTriangle triangle)
 132    {
 829133        if (ContainsInitialProjection(mesh, triangle, support))
 716134            return true;
 113135        if (mesh.GetCoplanarTriangleNeighbors(_triangleIndex).IsEmpty)
 8136            return false;
 137
 105138        ReadOnlySpan<int> corners = mesh.GetConvexCoplanarPatchCornerVertexIndices(_triangleIndex);
 105139        if (!corners.IsEmpty)
 140        {
 141            // The prepared convex fill removes subdivision cost without filling holes or notches.
 95142            ReadOnlySpan<Vector3d> vertices = mesh.GeometryVertices;
 292143            for (int corner = 1; corner < corners.Length - 1; corner++)
 144            {
 125145                triangle = new FixedTriangle(vertices[corners[0]], vertices[corners[corner]],
 125146                    vertices[corners[corner + 1]]);
 125147                if (ContainsInitialProjection(mesh, triangle, support))
 74148                    return true;
 149            }
 21150            return false;
 151        }
 152
 10153        int owner = mesh.GetManifoldSurfaceOwner(_triangleIndex);
 170154        for (int index = 0; index < mesh.TriangleCount; index++)
 155        {
 78156            if (index == _triangleIndex || mesh.GetManifoldSurfaceOwner(index) != owner)
 157                continue;
 67158            mesh.GetGeometryTriangleVertices(index, out Vector3d a, out Vector3d b, out Vector3d c);
 67159            triangle = new FixedTriangle(a, b, c);
 67160            if (ContainsInitialProjection(mesh, triangle, support))
 3161                return true;
 162        }
 7163        return false;
 164    }
 165
 166    private static bool ContainsInitialProjection(PhysicsMesh mesh, in FixedTriangle triangle,
 1021167        in FixedPointAnchor support) => mesh.GeometryFrame.IsRigid
 1021168        ? WideTriangleRelations.ContainsProjection(triangle, mesh.Origin, mesh.Rotation, support)
 1021169        : WideTriangleRelations.ContainsProjection(triangle, mesh.GeometryFrame, RationalPoint3d.FromAnchor(support));
 170
 171    private bool TryGetInitialTriangleContact(in ConvexShape source, FixedTriangle triangle,
 172        out FixedContactAnchors contact)
 173    {
 835174        PhysicsMesh targetMesh = _triangleOwner!.Mesh;
 835175        if (!targetMesh.GeometryFrame.IsRigid
 835176            || source._collider is LSMeshCollider sourceMesh && !sourceMesh.Mesh.GeometryFrame.IsRigid)
 8177            return TryGetInitialScaledTriangleContact(source, triangle, targetMesh, out contact);
 827178        if (source._kind == ConvexShapeKind.Sphere)
 3179            return triangle.TryGetSphereContact(targetMesh.Origin, targetMesh.Rotation,
 3180                source._center, FixedQuaternion.Identity, source._radius, out contact);
 824181        if (source._kind == ConvexShapeKind.CircleSlab)
 6182            return triangle.TryGetCircleSlabContact(targetMesh.Origin, targetMesh.Rotation,
 6183                source._center, Fixed64.Zero, source._halfHeight, source._radius, out contact);
 184
 818185        switch (source._collider)
 186        {
 187            case LSSphereCollider sphere:
 6188                return triangle.TryGetSphereContact(targetMesh.Origin, targetMesh.Rotation,
 6189                    sphere.Center, sphere.Rotation, sphere.ScaledRadius, out contact);
 190            case LSCapsuleCollider capsule:
 253191                Vector3d fallback = GetInitialExitNormal(source,
 253192                    targetMesh.GetFaceNormalWorld(_triangleIndex));
 253193                return triangle.TryGetCenteredCapsuleContact(targetMesh.Origin, targetMesh.Rotation,
 253194                    capsule.Center, capsule.Rotation, capsule.AxisLength, capsule.ScaledRadius,
 253195                    fallback, out contact);
 196            case LSCuboidCollider box:
 514197                bool found = box.OrientedBox.TryGetTriangleContact(targetMesh.Origin,
 514198                    targetMesh.Rotation, triangle, out FixedContactAnchors reversed);
 514199                contact = new FixedContactAnchors(reversed.SecondAnchor, reversed.FirstAnchor,
 514200                    -reversed.Normal, reversed.Depth, reversed.DepthIsClamped);
 514201                return found;
 202            case LSCylinderCollider cylinder:
 19203                return triangle.TryGetCenteredFiniteCylinderContact(targetMesh.Origin, targetMesh.Rotation,
 19204                    cylinder.Center, cylinder.Rotation, cylinder.Height, cylinder.ScaledRadius, out contact);
 205            case LSConeCollider cone:
 20206                return triangle.TryGetCenteredFiniteConeContact(targetMesh.Origin, targetMesh.Rotation,
 20207                    cone.Center, cone.Rotation, cone.Height, cone.ScaledRadius, out contact);
 208            default:
 6209                var mesh = (LSMeshCollider)source._collider!;
 6210                PhysicsMesh sourceHull = mesh.Mesh;
 6211                ReadOnlySpan<Vector3d> vertices = stackalloc Vector3d[3]
 6212                    { triangle.A, triangle.B, triangle.C };
 6213                ReadOnlySpan<int> triangles = stackalloc int[3] { 0, 1, 2 };
 6214                ReadOnlySpan<int> edges = stackalloc int[6] { 0, 1, 1, 2, 2, 0 };
 6215                return FixedConvexHullRelations.TryGetContact(
 6216                    targetMesh.Origin, targetMesh.Rotation, vertices, triangles, edges,
 6217                    sourceHull.Origin, sourceHull.Rotation, sourceHull.GeometryVertices,
 6218                    sourceHull.Triangles, sourceHull.ConvexSatEdgeVertexPairs, out contact);
 219        }
 220    }
 221
 222    private bool TryGetInitialScaledTriangleContact(in ConvexShape source, in FixedTriangle triangle,
 223        PhysicsMesh mesh, out FixedContactAnchors contact)
 224    {
 8225        Vector3d fallback = GetInitialExitNormal(source, mesh.GetFaceNormalWorld(_triangleIndex));
 8226        if (source._kind == ConvexShapeKind.Sphere)
 1227            return triangle.TryGetCenteredCapsuleContact(mesh.GeometryFrame, source._center,
 1228                FixedQuaternion.Identity, Fixed64.Zero, source._radius, fallback, out contact, out _, out _);
 7229        if (source._kind == ConvexShapeKind.CircleSlab)
 1230            return TriangleCylinderContact.TryGetContact(triangle, mesh.GeometryFrame, source._center,
 1231                FixedQuaternion.Identity, WideArithmetic.AddSigned192(Signed192.Raw(source._halfHeight),
 1232                    Signed192.Raw(source._halfHeight)), source._radius, out contact, out _);
 6233        switch (source._collider)
 234        {
 235            case LSSphereCollider sphere:
 1236                return triangle.TryGetCenteredCapsuleContact(mesh.GeometryFrame, sphere.Center, sphere.Rotation,
 1237                    Fixed64.Zero, sphere.ScaledRadius, fallback, out contact, out _, out _);
 238            case LSCapsuleCollider capsule:
 1239                return triangle.TryGetCenteredCapsuleContact(mesh.GeometryFrame, capsule.Center, capsule.Rotation,
 1240                    capsule.AxisLength, capsule.ScaledRadius, fallback, out contact, out _, out _);
 241            case LSCuboidCollider box:
 1242                ReadOnlySpan<Vector3d> points = stackalloc Vector3d[3] { triangle.A, triangle.B, triangle.C };
 1243                ReadOnlySpan<int> triangles = stackalloc int[3] { 0, 1, 2 };
 1244                ReadOnlySpan<int> edges = stackalloc int[6] { 0, 1, 1, 2, 2, 0 };
 1245                bool found = WideOrientedBox.TryGetConvexHullContact(box.Center, box.Rotation, box.OrientedBox.HalfExten
 1246                    mesh.GeometryFrame, mesh.Origin, points, triangles, edges, out FixedContactAnchors reversed);
 1247                contact = new FixedContactAnchors(reversed.SecondAnchor, reversed.FirstAnchor, -reversed.Normal,
 1248                    reversed.Depth, reversed.DepthIsClamped);
 1249                return found;
 250            case LSCylinderCollider cylinder:
 1251                return TriangleCylinderContact.TryGetContact(triangle, mesh.GeometryFrame, cylinder.Center,
 1252                    cylinder.Rotation, Signed192.Raw(cylinder.Height), cylinder.ScaledRadius, out contact, out _);
 253            case LSConeCollider cone:
 1254                return TriangleConeContact.TryGetContact(triangle, mesh.GeometryFrame, cone.Center,
 1255                    cone.Rotation, cone.Height, cone.ScaledRadius, out contact);
 256            default:
 1257                PhysicsMesh sourceHull = ((LSMeshCollider)source._collider!).Mesh;
 1258                ReadOnlySpan<Vector3d> targetPoints = stackalloc Vector3d[3] { triangle.A, triangle.B, triangle.C };
 1259                ReadOnlySpan<int> targetTriangles = stackalloc int[3] { 0, 1, 2 };
 1260                ReadOnlySpan<int> targetEdges = stackalloc int[6] { 0, 1, 1, 2, 2, 0 };
 1261                return WideOrientedBox.TryGetConvexHullContact(mesh.GeometryFrame, mesh.Origin, targetPoints,
 1262                    targetTriangles, targetEdges, sourceHull.GeometryFrame, sourceHull.Origin,
 1263                    sourceHull.GeometryVertices, sourceHull.Triangles, sourceHull.ConvexSatEdgeVertexPairs, out contact)
 264        }
 265    }
 266
 267    internal Vector3d GetInitialExitNormal(in ConvexShape source, Vector3d axis)
 268    {
 3432269        FixedPointAnchor targetMax = GetSupportAnchor(axis);
 3432270        FixedPointAnchor sourceMin = source.GetSupportAnchor(-axis);
 3432271        FixedPointAnchor sourceMax = source.GetSupportAnchor(axis);
 3432272        FixedPointAnchor targetMin = GetSupportAnchor(-axis);
 3432273        return WidePointAnchor3d.CompareProjectedOffsets(
 3432274            targetMax, sourceMin, sourceMax, targetMin, axis) <= 0 ? axis : -axis;
 275    }
 276
 277    private void GetInitialHullFaceExit(in ConvexShape source, PhysicsMesh hull,
 278        out FixedPointAnchor targetAnchor, out Vector3d normal)
 279    {
 280        // A two-sided triangle MTV does not describe exit from a solid hull.
 281        // Rank full-shape exit projections on the cached unit face axes. This
 282        // preserves hull containment and authored ties without claiming a
 283        // complete curved-shape/hull MTV (edge axes can provide shorter exits).
 284        // Committed mesh topology has at least one nondegenerate triangle;
 285        // scale validation preserves nonzero cached face normals.
 25286        normal = GetInitialExitNormal(source, hull.GetFaceNormalWorld(0));
 25287        targetAnchor = GetSupportAnchor(normal);
 25288        FixedPointAnchor selectedSource = source.GetSupportAnchor(-normal);
 600289        for (int index = 1; index < hull.TriangleCount; index++)
 290        {
 275291            Vector3d axis = hull.GetFaceNormalWorld(index);
 292            Debug.Assert(axis != Vector3d.Zero);
 275293            axis = GetInitialExitNormal(source, axis);
 275294            FixedPointAnchor targetSupport = GetSupportAnchor(axis);
 275295            FixedPointAnchor sourceSupport = source.GetSupportAnchor(-axis);
 275296            if (WidePointAnchor3d.CompareProjectedOffsets(
 275297                    targetSupport, sourceSupport, targetAnchor, selectedSource,
 275298                    axis, normal) >= 0)
 299                continue;
 21300            normal = axis;
 21301            targetAnchor = targetSupport;
 21302            selectedSource = sourceSupport;
 303        }
 25304    }
 305
 306    private static bool TryGetHullCapsuleContact(PhysicsMesh hull, Vector3d center,
 307        FixedQuaternion rotation, Fixed64 axisLength, Fixed64 radius, out FixedContactAnchors contact) =>
 25308        WideOrientedBox.TryGetConvexHullCenteredCapsuleContact(
 25309            hull.GeometryFrame, hull.Origin, hull.GeometryVertices,
 25310            hull.Triangles, hull.ConvexSatEdgeVertexPairs,
 25311            center, rotation, Vector3d.Up, axisLength, radius, out contact);
 312}

Methods/Properties

.ctor(Gravitas.Colliders.LSCollider,FixedMathSharp.Vector3d,System.Boolean)
.ctor(Gravitas.Colliders.LSMeshCollider,System.Int32,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
.ctor(Gravitas.Queries.ConvexShapeKind,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Vector3d)
get_ContainsCenter()
get_IsConvexPatch()
GetCenterAnchor()
CreateCircleSlab(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64)
CreateSphere(FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)
GetSourceBounds(FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
GetBounds(FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
CanTranslateCenter(FixedMathSharp.Vector3d)
TryGetBoundsRelativeTo(FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Vector3d&,FixedMathSharp.Vector3d&)
WithSourceOffset(FixedMathSharp.Vector3d)
GetSupportAnchor(FixedMathSharp.Vector3d,System.Boolean)
GetConvexPatchSupportAnchor(FixedMathSharp.Vector3d)
GetFallbackSurfaceAnchor(FixedMathSharp.Vector3d)
TryGetClosestPointOnSurface(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
TryGetPlanarSurfaceNormal(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d&)
GetTriangleNormalAxis()
TryGetCapsuleTriangleNormalAxis(Gravitas.Queries.ConvexShape&,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Geometry.WideDirection3d&)
GetCircleSlabSupportLocalPoint(FixedMathSharp.Vector3d)
GetSphereSupportLocalPoint(FixedMathSharp.Vector3d)
GetTriangleSupportLocalPoint(FixedMathSharp.Vector3d)
CompareSupportProjection(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Geometry.WideAxis3&)
TryGetInitialMeshContactNormal(Gravitas.Queries.ConvexShape&,System.Boolean,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Vector3d&)
TryGetInitialMeshFaceContactNormal(Gravitas.Queries.ConvexShape&,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Vector3d&)
TryGetInitialSurfaceProjectionTriangle(Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Geometry.FixedTriangle&)
ContainsInitialProjection(Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Geometry.FixedTriangle&,FixedMathSharp.Geometry.FixedPointAnchor&)
TryGetInitialTriangleContact(Gravitas.Queries.ConvexShape&,FixedMathSharp.Geometry.FixedTriangle,FixedMathSharp.Geometry.FixedContactAnchors&)
TryGetInitialScaledTriangleContact(Gravitas.Queries.ConvexShape&,FixedMathSharp.Geometry.FixedTriangle&,Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Geometry.FixedContactAnchors&)
GetInitialExitNormal(Gravitas.Queries.ConvexShape&,FixedMathSharp.Vector3d)
GetInitialHullFaceExit(Gravitas.Queries.ConvexShape&,Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Geometry.FixedPointAnchor&,FixedMathSharp.Vector3d&)
TryGetHullCapsuleContact(Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Vector3d,FixedMathSharp.FixedQuaternion,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,FixedMathSharp.Geometry.FixedContactAnchors&)