< Summary

Information
Class: Gravitas.Queries.RaycastSegmentWorker
Assembly: Gravitas
File(s): /home/runner/work/Gravitas/Gravitas/src/Gravitas/Queries/3D/RaycastSegmentWorker.cs
Line coverage
100%
Covered lines: 261
Uncovered lines: 0
Coverable lines: 261
Total lines: 506
Line coverage: 100%
Branch coverage
100%
Covered branches: 130
Total branches: 130
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/Queries/3D/RaycastSegmentWorker.cs

#LineLine coverage
 1//=======================================================================
 2// RaycastSegmentWorker.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;
 13
 14namespace Gravitas.Queries;
 15
 16/// <summary>
 17/// Stores segment data used by one context-owned raycast service while checking collider overlaps.
 18/// </summary>
 19public sealed class RaycastSegmentWorker
 20{
 21    private Vector3d _cachedOrigin;
 22    private Vector3d _cachedEnd;
 23    private Vector3d _cachedSegment;
 24    private Vector3d _segmentDirection;
 25    private Fixed64 _segmentLength;
 26    private bool _segmentIsValid;
 27    private bool _calculateIntersections;
 1031328    private readonly SwiftList<int> _meshTriangleBuffer = new();
 29
 130    internal Vector3d SegmentDirection => _segmentDirection;
 31
 32    /// <summary>
 33    /// Prepares this worker for overlap checks against the line segment between two points.
 34    /// </summary>
 35    public void PrepareSegmentCheck(Vector3d p1, Vector3d p2, bool calculateIntersectionPoints = true)
 36    {
 393837        _cachedOrigin = p1;
 393838        _cachedEnd = p2;
 39
 393840        if (!Vector3d.TrySubtract(p2, p1, out Vector3d segment)
 393841            || !Vector3d.TryGetMagnitude(segment, out _segmentLength))
 42        {
 243            _segmentLength = Fixed64.Zero;
 244            _cachedSegment = Vector3d.Zero;
 245            _segmentDirection = Vector3d.Zero;
 246            _segmentIsValid = false;
 247            _calculateIntersections = calculateIntersectionPoints;
 248            return;
 49        }
 50
 393651        _cachedSegment = segment;
 393652        _segmentDirection = _segmentLength == Fixed64.Zero ? Vector3d.Zero : segment.Normalized;
 393653        _segmentIsValid = true;
 393654        _calculateIntersections = calculateIntersectionPoints;
 393655    }
 56
 57    /// <summary>
 58    /// Checks whether a sphere collider overlaps the prepared segment.
 59    /// </summary>
 60    public bool CheckSphereOverlaps(LSSphereCollider sphereCollider, ref SwiftList<Vector3d> outputIntersectionPoints) =
 138661        CheckSphereOverlaps(
 138662            new FixedBoundSphere(sphereCollider.Center, sphereCollider.ScaledRadius),
 138663            ref outputIntersectionPoints);
 64
 65    /// <summary>
 66    /// Checks whether a sphere overlaps this worker's prepared ray segment.
 67    /// </summary>
 68    /// <param name="sphere">The sphere bound.</param>
 69    /// <param name="outputIntersectionPoints">
 70    /// Receives segment points reconstructed from exact physical intersection distances.
 71    /// </param>
 72    /// <returns><see langword="true"/> when the prepared segment overlaps the sphere.</returns>
 73    public bool CheckSphereOverlaps(
 74        FixedBoundSphere sphere,
 75        ref SwiftList<Vector3d> outputIntersectionPoints)
 76    {
 139977        if (!_segmentIsValid)
 278            return false;
 79
 139780        if (_cachedSegment.IsZero)
 381            return CheckPointInsideSphere(sphere, ref outputIntersectionPoints);
 82
 139483        var query = new FixedSegment(_cachedOrigin, _cachedEnd);
 139484        if (!query.TryGetSphereIntersectionDistanceInterval(
 139485                sphere,
 139486                Fixed64.Zero,
 139487                _segmentLength,
 139488                out Fixed64 entry,
 139489                out Fixed64 exit,
 139490                out bool startContained,
 139491                out bool endContainedStrict))
 92        {
 2393            return false;
 94        }
 95
 137196        if (!_calculateIntersections)
 197            return true;
 98
 137099        AddFiniteAxisIntersectionInterval(
 1370100            entry,
 1370101            exit,
 1370102            startContained,
 1370103            endContainedStrict,
 1370104            ref outputIntersectionPoints);
 1370105        return true;
 106    }
 107
 108    /// <summary>
 109    /// Checks whether a capsule collider overlaps the prepared segment.
 110    /// </summary>
 111    public bool CheckCapsuleOverlaps(LSCapsuleCollider capsuleCollider, ref SwiftList<Vector3d> outputIntersectionPoints
 112    {
 448113        if (!_segmentIsValid)
 1114            return false;
 115
 447116        var query = new FixedSegment(_cachedOrigin, _cachedEnd);
 447117        if (!query.TryGetCapsuleIntersectionDistanceInterval(
 447118                capsuleCollider.Center,
 447119                capsuleCollider.Rotation,
 447120                capsuleCollider.AxisLength,
 447121                capsuleCollider.ScaledRadius,
 447122                Fixed64.Zero,
 447123                _segmentLength,
 447124                out Fixed64 entry,
 447125                out Fixed64 exit,
 447126                out bool startContained,
 447127                out bool endContainedStrict))
 128        {
 4129            return false;
 130        }
 131
 443132        if (!_calculateIntersections)
 1133            return true;
 134
 442135        AddFiniteAxisIntersectionInterval(
 442136            entry,
 442137            exit,
 442138            startContained,
 442139            endContainedStrict,
 442140            ref outputIntersectionPoints);
 442141        return true;
 142    }
 143
 144    /// <summary>
 145    /// Checks whether a cylinder collider overlaps the prepared segment.
 146    /// </summary>
 147    public bool CheckCylinderOverlaps(LSCylinderCollider cylinderCollider, ref SwiftList<Vector3d> outputIntersectionPoi
 148    {
 30149        if (!_segmentIsValid)
 1150            return false;
 151
 29152        var query = new FixedSegment(_cachedOrigin, _cachedEnd);
 29153        if (!query.TryGetFiniteCylinderIntersectionDistanceInterval(
 29154                cylinderCollider.Center,
 29155                cylinderCollider.Rotation,
 29156                cylinderCollider.Height,
 29157                cylinderCollider.ScaledRadius,
 29158                Fixed64.Zero,
 29159                Fixed64.Zero,
 29160                _segmentLength,
 29161                out Fixed64 entry,
 29162                out Fixed64 exit,
 29163                out bool startContained,
 29164                out bool endContainedStrict))
 165        {
 7166            return false;
 167        }
 168
 22169        if (!_calculateIntersections)
 1170            return true;
 171
 21172        AddFiniteAxisIntersectionInterval(
 21173            entry,
 21174            exit,
 21175            startContained,
 21176            endContainedStrict,
 21177            ref outputIntersectionPoints);
 21178        return true;
 179    }
 180
 181    /// <summary>
 182    /// Checks whether a cone collider overlaps the prepared segment.
 183    /// </summary>
 184    public bool CheckConeOverlaps(LSConeCollider coneCollider, ref SwiftList<Vector3d> outputIntersectionPoints)
 185    {
 851186        if (!_segmentIsValid)
 1187            return false;
 188
 850189        var query = new FixedSegment(_cachedOrigin, _cachedEnd);
 850190        if (!query.TryGetCenteredFiniteConeIntersectionDistanceInterval(
 850191                coneCollider.Center,
 850192                coneCollider.Rotation,
 850193                coneCollider.Height,
 850194                coneCollider.ScaledRadius,
 850195                _segmentLength,
 850196                out Fixed64 entry,
 850197                out Fixed64 exit,
 850198                out bool startContained,
 850199                out bool endContainedStrict))
 200        {
 11201            return false;
 202        }
 203
 839204        if (!_calculateIntersections)
 3205            return true;
 206
 836207        AddFiniteAxisIntersectionInterval(
 836208            entry,
 836209            exit,
 836210            startContained,
 836211            endContainedStrict,
 836212            ref outputIntersectionPoints);
 213
 836214        return true;
 215    }
 216
 217    /// <summary>
 218    /// Checks whether a mesh collider overlaps the prepared segment.
 219    /// </summary>
 220    public bool CheckMeshOverlaps(LSMeshCollider meshCollider, ref SwiftList<Vector3d> outputIntersectionPoints)
 221    {
 1371222        if (!_segmentIsValid)
 1223            return false;
 224
 1370225        if (!meshCollider.Mesh.GeometryFrame.IsRigid)
 226        {
 276227            PhysicsMesh mesh = meshCollider.Mesh;
 276228            mesh.GetTrianglesInWorldBounds(CreateSegmentBounds(_cachedOrigin, _cachedEnd), _meshTriangleBuffer);
 276229            bool found = false;
 1053230            foreach (int index in _meshTriangleBuffer.AsReadOnlySpan())
 231            {
 252232                mesh.GetGeometryTriangleVertices(index, out Vector3d a, out Vector3d b, out Vector3d c);
 252233                if (!WideTriangleRelations.TryGetSegmentIntersection(new FixedTriangle(a, b, c), mesh.GeometryFrame,
 252234                    new FixedSegment(_cachedOrigin, _cachedEnd), out Vector3d entry, out Vector3d exit)) continue;
 235235                if (!_calculateIntersections) return true;
 229236                AddIntersectionPoint(entry, ref outputIntersectionPoints);
 237237                if (exit != entry) AddIntersectionPoint(exit, ref outputIntersectionPoints);
 229238                found = true;
 239            }
 273240            return found;
 241        }
 242
 1094243        if (!meshCollider.Mesh.TryConvertWorldToRigidGeometry(
 1094244                _cachedOrigin,
 1094245                out Vector3d localOrigin)
 1094246            || !meshCollider.Mesh.TryConvertWorldToRigidGeometry(
 1094247                _cachedEnd,
 1094248                out Vector3d localEnd))
 249        {
 2250            return false;
 251        }
 1092252        Vector3d localSegment = localEnd - localOrigin;
 1092253        Fixed64 localSegmentLengthSqr = localSegment.MagnitudeSquared;
 1092254        Fixed64 localSegmentLength = localSegmentLengthSqr == Fixed64.Zero ? Fixed64.Zero : localSegment.Magnitude;
 1092255        Vector3d localSegmentDirection = localSegmentLength == Fixed64.Zero ? Vector3d.Zero : localSegment.Normalized;
 256
 1092257        _meshTriangleBuffer.FastClear();
 1092258        meshCollider.Mesh.GetTrianglesInGeometryBounds(CreateSegmentBounds(localOrigin, localEnd), _meshTriangleBuffer);
 1092259        bool intersects = false;
 5280260        for (int i = 0; i < _meshTriangleBuffer.Count; i++)
 261        {
 1552262            int triangleIndex = _meshTriangleBuffer[i];
 1552263            meshCollider.Mesh.GetGeometryTriangleVertices(triangleIndex, out Vector3d first, out Vector3d second, out Ve
 1552264            if (!TryAddLocalTriangleIntersection(
 1552265                    meshCollider.Mesh,
 1552266                    first,
 1552267                    second,
 1552268                    third,
 1552269                    meshCollider.Mesh.GetScaledLocalFaceNormal(triangleIndex),
 1552270                    localOrigin,
 1552271                    localEnd,
 1552272                    localSegmentDirection,
 1552273                    localSegmentLength,
 1552274                    localSegmentLengthSqr,
 1552275                    ref outputIntersectionPoints))
 276            {
 277                continue;
 278            }
 279
 1249280            intersects = true;
 1249281            if (!_calculateIntersections)
 4282                return true;
 283        }
 284
 1088285        return intersects;
 286    }
 287
 288    /// <summary>
 289    /// Checks whether a cuboid's axis-aligned bounds overlap the prepared segment.
 290    /// </summary>
 291    public bool CheckAABBoxOverlaps(LSCuboidCollider aabox, ref SwiftList<Vector3d> outputIntersectionPoints) =>
 439292         CheckAABBoxOverlaps(aabox.BoundsMin, aabox.BoundsMax, ref outputIntersectionPoints);
 293
 294    /// <summary>
 295    /// Checks whether an axis-aligned bounding box overlaps this worker's prepared ray segment.
 296    /// </summary>
 297    public bool CheckAABBoxOverlaps(Vector3d min, Vector3d max, ref SwiftList<Vector3d> outputIntersectionPoints)
 298    {
 457299        if (!_segmentIsValid)
 1300            return false;
 301
 456302        if (_cachedSegment.IsZero)
 11303            return CheckPointInsideBox(min, max, ref outputIntersectionPoints);
 304
 445305        if (!SweepBoundsUtility.TryClipSegment(
 445306            _cachedOrigin,
 445307            _segmentDirection,
 445308            _segmentLength,
 445309            min,
 445310            max,
 445311            out Fixed64 entry,
 445312            out Fixed64 exit))
 313        {
 17314            return false;
 315        }
 316
 428317        if (_calculateIntersections)
 318        {
 427319            outputIntersectionPoints.Add(_cachedOrigin + _segmentDirection * entry);
 427320            if (exit != entry)
 422321                outputIntersectionPoints.Add(_cachedOrigin + _segmentDirection * exit);
 322        }
 323
 428324        return true;
 325    }
 326
 327    /// <summary>
 328    /// Checks whether an oriented bounding box overlaps this worker's prepared ray segment.
 329    /// </summary>
 330    public bool CheckOBBoxOverlaps(LSCuboidCollider oobox, ref SwiftList<Vector3d> outputIntersectionPoints)
 331    {
 245332        if (!_segmentIsValid)
 1333            return false;
 334
 244335        var ray = new FixedRay(_cachedOrigin, _cachedSegment);
 244336        if (!oobox.OrientedBox.TryGetRayIntersectionInterval(
 244337                ray,
 244338                Fixed64.One,
 244339                out Fixed64 entry,
 244340                out Fixed64 exit))
 18341            return false;
 342
 226343        if (_calculateIntersections)
 344        {
 345            // The interval is clipped to this segment, so both parameters are
 346            // convex combinations of its representable endpoints.
 225347            _ = ray.TryGetPoint(entry, out Vector3d entryPoint);
 225348            Vector3d exitPoint = default;
 225349            if (exit != entry)
 16350                _ = ray.TryGetPoint(exit, out exitPoint);
 351
 225352            AddIntersectionPoint(entryPoint, ref outputIntersectionPoints);
 225353            if (exit != entry)
 16354                AddIntersectionPoint(exitPoint, ref outputIntersectionPoints);
 355        }
 356
 226357        return true;
 358    }
 359
 360    private bool TryAddLocalTriangleIntersection(
 361        PhysicsMesh mesh,
 362        Vector3d first,
 363        Vector3d second,
 364        Vector3d third,
 365        Vector3d normal,
 366        Vector3d localOrigin,
 367        Vector3d localEnd,
 368        Vector3d localSegmentDirection,
 369        Fixed64 localSegmentLength,
 370        Fixed64 localSegmentLengthSqr,
 371        ref SwiftList<Vector3d> outputIntersectionPoints)
 372    {
 1552373        var triangle = new FixedTriangle(first, second, third);
 1552374        if (localSegmentLengthSqr == Fixed64.Zero)
 375        {
 8376            if (Vector3d.Dot(localOrigin - first, normal).Abs() > Fixed64.Epsilon
 8377                || !triangle.ContainsProjection(localOrigin))
 378            {
 5379                return false;
 380            }
 381
 382            // localOrigin was obtained from this representable world endpoint.
 3383            _ = mesh.GeometryFrame.TryGetPoint(localOrigin, out Vector3d worldOrigin);
 384
 3385            AddIntersectionPoint(worldOrigin, ref outputIntersectionPoints);
 3386            return true;
 387        }
 388
 1544389        Fixed64 denominator = Vector3d.Dot(normal, localSegmentDirection);
 1544390        if (denominator.Abs() <= Fixed64.Epsilon)
 391        {
 377392            if (Vector3d.Dot(localOrigin - first, normal).Abs() > Fixed64.Epsilon)
 42393                return false;
 394
 335395            bool found = false;
 335396            if (triangle.ContainsProjection(localOrigin))
 397            {
 160398                _ = mesh.GeometryFrame.TryGetPoint(localOrigin, out Vector3d worldOrigin);
 399
 160400                AddIntersectionPoint(worldOrigin, ref outputIntersectionPoints);
 160401                found = true;
 402            }
 403
 335404            if (triangle.ContainsProjection(localEnd))
 405            {
 228406                _ = mesh.GeometryFrame.TryGetPoint(localEnd, out Vector3d worldEnd);
 407
 228408                AddIntersectionPoint(worldEnd, ref outputIntersectionPoints);
 228409                found = true;
 410            }
 411
 335412            return found;
 413        }
 414
 1167415        Fixed64 distance = Vector3d.Dot(first - localOrigin, normal) / denominator;
 1167416        if (distance < Fixed64.Zero || distance > localSegmentLength)
 26417            return false;
 418
 1141419        Vector3d localPoint = localOrigin + localSegmentDirection * distance;
 1141420        if (!triangle.ContainsProjection(localPoint))
 143421            return false;
 422
 423        // A point on the local segment maps to the same convex combination of
 424        // the already-representable world endpoints.
 998425        _ = mesh.GeometryFrame.TryGetPoint(localPoint, out Vector3d worldPoint);
 426
 998427        AddIntersectionPoint(worldPoint, ref outputIntersectionPoints);
 998428        return true;
 429    }
 430
 431    private void AddIntersectionPoint(Vector3d point, ref SwiftList<Vector3d> outputIntersectionPoints)
 432    {
 1867433        if (!_calculateIntersections)
 5434            return;
 435
 4174436        for (int i = 0; i < outputIntersectionPoints.Count; i++)
 437        {
 845438            if (Vector3d.DistanceSquared(outputIntersectionPoints[i], point) <= Fixed64.Epsilon)
 620439                return;
 440        }
 441
 1242442        outputIntersectionPoints.Add(point);
 1242443    }
 444
 445    private static FixedBoundVolume CreateSegmentBounds(Vector3d origin, Vector3d end)
 446    {
 1368447        Vector3d min = Vector3d.Min(origin, end);
 1368448        Vector3d max = Vector3d.Max(origin, end);
 1368449        return new FixedBoundVolume(min, max);
 450    }
 451
 452    private bool CheckPointInsideSphere(
 453        FixedBoundSphere sphere,
 454        ref SwiftList<Vector3d> outputIntersectionPoints)
 455    {
 3456        if (!sphere.Contains(_cachedOrigin))
 1457            return false;
 458
 2459        if (_calculateIntersections)
 1460            outputIntersectionPoints.Add(_cachedOrigin);
 461
 2462        return true;
 463    }
 464
 465    private void AddFiniteAxisIntersectionInterval(
 466        Fixed64 entry,
 467        Fixed64 exit,
 468        bool startContained,
 469        bool endInsideStrict,
 470        ref SwiftList<Vector3d> outputIntersectionPoints)
 471    {
 2669472        AddSegmentIntersectionPoint(entry, ref outputIntersectionPoints);
 2669473        if (!startContained
 2669474            && exit != entry
 2669475            && (exit < _segmentLength || !endInsideStrict))
 476        {
 2011477            AddSegmentIntersectionPoint(exit, ref outputIntersectionPoints);
 478        }
 479
 2669480    }
 481
 482    private void AddSegmentIntersectionPoint(
 483        Fixed64 distance,
 484        ref SwiftList<Vector3d> outputIntersectionPoints) =>
 4680485        outputIntersectionPoints.Add(new FixedSegment(_cachedOrigin, _cachedEnd)
 4680486            .GetPointAtDistance(distance, _segmentLength));
 487
 488    private bool CheckPointInsideBox(
 489        Vector3d min,
 490        Vector3d max,
 491        ref SwiftList<Vector3d> outputIntersectionPoints)
 492    {
 11493        if (_cachedOrigin.X < min.X || _cachedOrigin.X > max.X
 11494            || _cachedOrigin.Y < min.Y || _cachedOrigin.Y > max.Y
 11495            || _cachedOrigin.Z < min.Z || _cachedOrigin.Z > max.Z)
 496        {
 6497            return false;
 498        }
 499
 5500        if (_calculateIntersections)
 4501            outputIntersectionPoints.Add(_cachedOrigin);
 502
 5503        return true;
 504    }
 505
 506}

Methods/Properties

.ctor()
get_SegmentDirection()
PrepareSegmentCheck(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,System.Boolean)
CheckSphereOverlaps(Gravitas.Colliders.LSSphereCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckSphereOverlaps(FixedMathSharp.Geometry.FixedBoundSphere,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckCapsuleOverlaps(Gravitas.Colliders.LSCapsuleCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckCylinderOverlaps(Gravitas.Colliders.LSCylinderCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckConeOverlaps(Gravitas.Colliders.LSConeCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckMeshOverlaps(Gravitas.Colliders.LSMeshCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckAABBoxOverlaps(Gravitas.Colliders.LSCuboidCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckAABBoxOverlaps(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckOBBoxOverlaps(Gravitas.Colliders.LSCuboidCollider,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
TryAddLocalTriangleIntersection(Gravitas.Colliders.PhysicsMesh,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
AddIntersectionPoint(FixedMathSharp.Vector3d,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CreateSegmentBounds(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d)
CheckPointInsideSphere(FixedMathSharp.Geometry.FixedBoundSphere,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
AddFiniteAxisIntersectionInterval(FixedMathSharp.Fixed64,FixedMathSharp.Fixed64,System.Boolean,System.Boolean,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
AddSegmentIntersectionPoint(FixedMathSharp.Fixed64,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)
CheckPointInsideBox(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,SwiftCollections.SwiftList`1<FixedMathSharp.Vector3d>&)