< Summary

Information
Class: Trailblazer.Pathing.AStarSurveyor
Assembly: Trailblazer
File(s): /home/runner/work/Trailblazer/Trailblazer/src/Trailblazer/Pathing/Search/AStar/AStarSurveyor.cs
Line coverage
100%
Covered lines: 192
Uncovered lines: 0
Coverable lines: 192
Total lines: 492
Line coverage: 100%
Branch coverage
90%
Covered branches: 87
Total branches: 96
Branch coverage: 90.6%
Method coverage

Feature is only available for sponsors

Upgrade to PRO version

Metrics

MethodBranch coverage Crap Score Cyclomatic complexity Line coverage
.cctor()100%11100%
get_Shared()100%11100%
.ctor()100%11100%
FindPath(...)100%88100%
TracePath()100%88100%
ProcessNeighbors(...)100%66100%
TryProcessDirection(...)94.44%1818100%
HasValidDiagonalLegs(...)100%1212100%
IsLegClear(...)83.33%66100%
ProcessNeighbor(...)90%1010100%
SetPathPartitionData(...)100%11100%
BuildRawPath()57.14%1414100%
BuildWaypoints()100%44100%
CalculatePathCost(...)100%11100%
CalculatePathCost(...)100%11100%
GetPathCost(...)100%11100%
ClearWorkingState()100%11100%
CalculateHeuristic(...)100%44100%
CatmullSmooth(...)100%66100%
CatmullRom(...)100%11100%

File(s)

/home/runner/work/Trailblazer/Trailblazer/src/Trailblazer/Pathing/Search/AStar/AStarSurveyor.cs

#LineLine coverage
 1using FixedMathSharp;
 2using GridForge.Grids;
 3using GridForge.Spatial;
 4using SwiftCollections;
 5using System;
 6using System.Linq;
 7using System.Runtime.CompilerServices;
 8using System.Threading;
 9
 10namespace Trailblazer.Pathing;
 11
 12internal struct AStarVoxelMeta
 13{
 14    /// <summary>
 15    /// The movement penalty cost of this voxel.
 16    /// </summary>
 17    public int MovementCost;
 18
 19    /// <summary>
 20    /// The total survey path cost recorded for this voxel.
 21    /// </summary>
 22    public int PathCost;
 23
 24    /// <summary>
 25    /// The next voxel in the trail path.
 26    /// </summary>
 27    public WorldVoxelIndex? NextTrailIndex;
 28}
 29
 30/// <summary>
 31/// Executes A* pathfinding logic using partitioned grids to find viable navigation paths for agents.
 32/// Supports climb height constraints and optional spline smoothing of the final path.
 33/// </summary>
 34public class AStarSurveyor
 35{
 36    #region Constants
 37
 38    /// <summary>
 39    /// Cost applied for straight (orthogonal) pathfinding moves.
 40    /// </summary>
 41    public const int StraightCost = 100;
 42
 43    /// <summary>
 44    /// Cost applied for diagonal pathfinding moves.
 45    /// </summary>
 46    public const int DiagonalCost = 141;
 47
 48    #endregion
 49
 50    #region Singleton Instances
 51
 52    /// <summary>
 53    /// A lazily initialized singleton instance of the pathfinder.
 54    /// </summary>
 155    private static readonly Lazy<AStarSurveyor> _instance =
 156        new(() => new AStarSurveyor(), LazyThreadSafetyMode.ExecutionAndPublication);
 57
 58    /// <summary>
 59    /// Gets the shared instance of the pathfinder.
 60    /// </summary>
 1161    public static AStarSurveyor Shared => _instance.Value;
 62
 63    #endregion
 64
 96465    private readonly SurveyorLock _scratchLock = new();
 66
 96467    private readonly PathHeap<SolidChartPartition> _heap = new();
 68
 69    // Key by stable voxel index so reconstruction is independent of GridForge voxel wrapper identity.
 96470    private readonly SwiftDictionary<WorldVoxelIndex, AStarVoxelMeta> _meta = new();
 71
 96472    private readonly SwiftList<SolidChartPartition> _rawPath = new();
 73
 96474    private readonly SwiftList<AStarWaypoint> _waypoints = new();
 75
 96476    private readonly SwiftHashSet<string> _chartKeys = new();
 77
 78    private AStarPathRequest? _request;
 79
 80    /// <summary>
 81    /// Attempts to find a path between the start and end points provided in the request.
 82    /// Returns true if a valid path was found and outputs the resulting waypoint list.
 83    /// </summary>
 84    /// <param name="request">The pathfinding request containing start/end info and constraints.</param>
 85    /// <returns>The list of path waypoints if successful; otherwise null.</returns>
 86    public AStarSurveyResult FindPath(AStarPathRequest request)
 87    {
 45888        lock (_scratchLock)
 89        {
 45890            if (request == null
 45891                || request.HasZeroDisplacement
 45892                || request.StartNode!.TryGetPartition(out SolidChartPartition? startPartition) != true)
 93            {
 194                return AStarSurveyResult.Empty;
 95            }
 96
 45797            _request = request;
 98
 45799            ClearWorkingState();
 100            // Trace path from the start to the end
 457101            _meta[_request.StartNode.WorldIndex] = new AStarVoxelMeta
 457102            {
 457103                PathCost = 0
 457104            };
 457105            _heap.Add(startPartition!, pathCost: 0);
 106
 457107            if (!TracePath())
 108            {
 307109                ClearWorkingState();
 307110                return AStarSurveyResult.Empty;
 111            }
 112
 150113            BuildRawPath();
 150114            BuildWaypoints();
 115
 150116            AStarWaypoint[] waypoints = _waypoints.ToArray();
 150117            string[] chartKeys = _chartKeys.ToArray();
 150118            AStarSurveyResult result = AStarSurveyResult.Create(request.Context, waypoints, chartKeys, request.RequestCa
 150119            ClearWorkingState();
 150120            return result;
 121        }
 458122    }
 123
 124    /// <summary>
 125    /// Executes the core A* loop to find a valid trail between the start and end voxels.
 126    /// </summary>
 127    /// <returns>True if the path to the target was found; false otherwise.</returns>
 128    private bool TracePath()
 129    {
 457130        int iterations = 0;
 457131        int searchSize = _request!.MaxPathSearchRange;
 1273132        while (_heap.RemoveFirst(out SolidChartPartition? currentPartition)
 1273133            && currentPartition != null
 1273134            && iterations++ < searchSize)
 135        {
 966136            if (ProcessNeighbors(currentPartition))
 150137                return true;
 138
 816139            _heap.SetClosed(currentPartition);
 816140        }
 141
 307142        return false;
 143    }
 144
 145    /// <summary>
 146    /// Indicates whether straight and diagonal neighbor voxels should be processed during pathfinding.
 147    /// </summary>
 148    /// <returns>True if any neighbor is the target destination.</returns>
 149    private bool ProcessNeighbors(SolidChartPartition current)
 150    {
 967151        if (!_meta.TryGetValue(current.WorldIndex, out AStarVoxelMeta data))
 1152            return false;
 153
 966154        if (TryProcessDirection(current, SpatialAwareness.PerpendicularDirections, data.MovementCost + StraightCost))
 128155            return true;
 838156        if (TryProcessDirection(current, SpatialAwareness.DiagonalDirections, data.MovementCost + DiagonalCost, true))
 22157            return true;
 158
 816159        return false;
 160    }
 161
 162    private bool TryProcessDirection(SolidChartPartition current, SpatialDirection[] directions, int cost, bool checkEdg
 163    {
 47106164        foreach (SpatialDirection dir in directions)
 165        {
 21824166            SolidChartPartition? neighbor = current.Neighbors?[(int)dir] ?? null;
 21824167            if (neighbor is null || _heap.IsClosed(neighbor) || neighbor.IsImpassable(_request!.UnitSize))
 168                continue;
 169
 2889170            if (checkEdges && !HasValidDiagonalLegs(current, dir))
 171                continue;
 172
 1585173            if (ProcessNeighbor(current, neighbor, cost))
 150174                return true;
 175        }
 176
 1654177        return false;
 178    }
 179
 180    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 181    private bool HasValidDiagonalLegs(SolidChartPartition current, SpatialDirection diagonal)
 182    {
 1618183        (int dx, int dy, int dz) = SpatialAwareness.DirectionOffsets[(int)diagonal];
 184
 1618185        if (dx != 0 && !IsLegClear(current, DiagonalTraversalLegs.ForXOffset(dx)))
 611186            return false;
 187
 1007188        if (dy != 0 && !IsLegClear(current, DiagonalTraversalLegs.ForYOffset(dy)))
 418189            return false;
 190
 589191        if (dz != 0 && !IsLegClear(current, DiagonalTraversalLegs.ForZOffset(dz)))
 275192            return false;
 193
 314194        return true;
 195    }
 196
 197    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 198    private bool IsLegClear(SolidChartPartition current, SpatialDirection legDir)
 199    {
 2625200        SolidChartPartition? leg = current.Neighbors?[(int)legDir] ?? null;
 201        // Diagonal legality is a topology check, not a search-order check. Requiring closed legs
 202        // makes admissible heuristics flood open planes before diagonal routes can form.
 2625203        return leg != null && !leg.IsImpassable(_request!.UnitSize);
 204    }
 205
 206    /// <summary>
 207    /// Determines whether a given neighbor voxel should be considered for path expansion.
 208    /// </summary>
 209    /// <returns>True if the neighbor is the target destination.</returns>
 210    private bool ProcessNeighbor(
 211        SolidChartPartition current,
 212        SolidChartPartition neighbor,
 213        int cost)
 214    {
 215        // Skip neighbors that have a height difference greater than the allowed maximum
 1586216        Fixed64 heightDifference = (current.VoxelPosition.y - neighbor.VoxelPosition.y).Abs();
 1586217        if (heightDifference > _request!.MaxClimbHeight)
 275218            return false;
 219
 1311220        if (neighbor.Voxel == _request.EndNode)
 221        {
 150222            int endPathCost = CalculatePathCost(neighbor, cost);
 150223            SetPathPartitionData(neighbor, current.WorldIndex, cost, endPathCost);
 150224            return true;
 225        }
 226
 1161227        int pathCost = CalculatePathCost(neighbor, cost, out int heuristicCost);
 1161228        if (!_heap.Contains(neighbor))
 229        {
 939230            SetPathPartitionData(neighbor, current.WorldIndex, cost, pathCost);
 939231            _heap.Add(neighbor, pathCost, heuristicCost);
 232        }
 222233        else if (_meta.TryGetValue(neighbor.WorldIndex, out AStarVoxelMeta neighborData)
 222234            && neighborData.MovementCost > cost)
 235        {
 1236            SetPathPartitionData(neighbor, current.WorldIndex, cost, pathCost);
 1237            _heap.UpdatePathCost(neighbor, pathCost, heuristicCost);
 1238            _heap.SortUp(neighbor);
 239        }
 240
 1161241        return false;
 242    }
 243
 244    /// <summary>
 245    /// Assigns pathfinding data to a path partition, including cost and direction toward the next trail voxel.
 246    /// </summary>
 247    /// <param name="partition">The path partition being updated.</param>
 248    /// <param name="nextTrailCoordinates">The coordinates of the parent partition leading to this one.</param>
 249    /// <param name="movementCost">The cumulative movement cost to this partition.</param>
 250    /// <param name="pathCost">The total survey path cost recorded for this partition.</param>
 251    private void SetPathPartitionData(
 252        SolidChartPartition partition,
 253        WorldVoxelIndex nextTrailCoordinates,
 254        int movementCost,
 255        int pathCost)
 256    {
 1090257        _meta[partition.WorldIndex] = new AStarVoxelMeta
 1090258        {
 1090259            MovementCost = movementCost,
 1090260            NextTrailIndex = nextTrailCoordinates,
 1090261            PathCost = pathCost
 1090262        };
 1090263    }
 264
 265    /// <summary>
 266    /// Reconstructs the raw voxel-based path from the destination to the origin by walking backwards through trail link
 267    /// </summary>
 268    /// <returns>A list of voxels from start to end representing the raw path.</returns>
 269    private void BuildRawPath()
 270    {
 150271        Voxel? current = _request!.EndNode;
 518272        while (current != null && current != _request.StartNode)
 273        {
 368274            SolidChartPartition? currentPartition = current.GetPartitionOrDefault<SolidChartPartition>();
 368275            if (currentPartition == null)
 276                break;
 277
 368278            _rawPath.Insert(0, currentPartition);
 279
 368280            if (!_meta.TryGetValue(current.WorldIndex, out AStarVoxelMeta data) || !data.NextTrailIndex.HasValue)
 281                break; // break in the trail!
 282
 368283            if (!_request.Context.World.TryGetGridAndVoxel(data.NextTrailIndex.Value, out _, out Voxel? nextTrailVoxel))
 284                break; // break in the trail!
 285
 368286            current = nextTrailVoxel;
 287        }
 288
 289        // Ensure start position is included
 150290        SolidChartPartition? startPartition = _request!.StartNode!.GetPartitionOrDefault<SolidChartPartition>();
 150291        if (startPartition != null)
 150292            _rawPath.Insert(0, startPartition);
 150293    }
 294
 295    /// <summary>
 296    /// Constructs a smoothed version of the path using direction changes and optional spline smoothing.
 297    /// </summary>
 298    /// <returns>A smoothed list of world positions.</returns>
 299    private void BuildWaypoints()
 300    {
 150301        _waypoints.EnsureCapacity(_rawPath.Count);
 150302        SolidChartPartition start = _rawPath[0];
 150303        _waypoints.Add(new()
 150304        {
 150305            Position = start.VoxelPosition,
 150306            PathCost = GetPathCost(start.Voxel),
 150307            GlobalIndex = start.WorldIndex
 150308        });
 150309        ChartOwnerUtility.AddOwners(_chartKeys, start.ChartOwners);
 310
 150311        Vector3d lastDirection = Vector3d.Zero;
 312
 313        // add 1 to ensure we preserve unwalkable voxels that are close enough to matter for the unit size
 150314        byte scaledUnitSize = (byte)((_request!.UnitSize / _request.Context.VoxelSize).CeilToInt() + 1);
 736315        for (int i = 1; i < _rawPath.Count - 1; i++)
 316        {
 218317            Vector3d direction = (_rawPath[i + 1].VoxelPosition - _rawPath[i].VoxelPosition).Normalize();
 318
 319
 218320            bool preserveUnwalkable = _rawPath[i].GetNeighborClearance() <= scaledUnitSize;
 218321            bool directionChanged = !lastDirection.FuzzyEqual(direction);
 322
 218323            if (preserveUnwalkable || directionChanged)
 324            {
 162325                _waypoints.Add(new()
 162326                {
 162327                    Position = _rawPath[i].VoxelPosition,
 162328                    PathCost = GetPathCost(_rawPath[i].Voxel),
 162329                    GlobalIndex = _rawPath[i].WorldIndex
 162330                });
 331            }
 332
 218333            lastDirection = direction;
 218334            ChartOwnerUtility.AddOwners(_chartKeys, _rawPath[i].ChartOwners);
 335        }
 336
 150337        SolidChartPartition end = _rawPath.FromEnd(1);
 150338        _waypoints.Add(new()
 150339        {
 150340            Position = end.VoxelPosition,
 150341            PathCost = GetPathCost(end.Voxel),
 150342            GlobalIndex = end.WorldIndex,
 150343            IsGoal = true
 150344        });
 150345        ChartOwnerUtility.AddOwners(_chartKeys, end.ChartOwners);
 150346    }
 347
 348    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 349    private int CalculatePathCost(SolidChartPartition partition, int movementCost) =>
 150350        CalculatePathCost(partition, movementCost, out _);
 351
 352    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 353    private int CalculatePathCost(
 354        SolidChartPartition partition,
 355        int movementCost,
 356        out int heuristicCost)
 357    {
 1311358        heuristicCost = CalculateHeuristic(
 1311359            partition.VoxelPosition,
 1311360            _request!.EndNode!.WorldPosition,
 1311361            _request.Heuristic);
 362
 1311363        return partition.PathCostModifier + movementCost + heuristicCost;
 364    }
 365
 366    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 367    private int GetPathCost(Voxel voxel)
 368    {
 462369        return _meta[voxel.WorldIndex].PathCost;
 370    }
 371
 372    private void ClearWorkingState()
 373    {
 914374        _heap.FastClear();
 914375        _meta.Clear();
 914376        _rawPath.FastClear();
 914377        _waypoints.FastClear();
 914378        _chartKeys.Clear();
 914379    }
 380
 381    /// <summary>
 382    /// Calculates the heuristic cost for the current voxel based on the target voxel and the heuristic method used.
 383    /// This implementation takes into account the X, Y, and Z axes for pathfinding.
 384    /// </summary>
 385    public static int CalculateHeuristic(
 386        Vector3d currentPosition,
 387        Vector3d targetPosition,
 388        HeuristicMethod heuristicMethod)
 389    {
 1915390        Fixed64 heuristicCost = Fixed64.MAX_VALUE;
 391
 392        // Calculate the absolute distance in each axis
 1915393        Vector3d dst = Vector3d.Abs(currentPosition - targetPosition);
 394
 395        switch (heuristicMethod)
 396        {
 397            case HeuristicMethod.Manhattan:
 398                // Sum the distances and multiply by 100 for the heuristic cost
 1044399                heuristicCost = (dst.x + dst.y + dst.z) * StraightCost;
 1044400                break;
 401            case HeuristicMethod.Octile:
 7402                Fixed64 maxXY = FixedMath.Max(dst.x, dst.y);
 7403                Fixed64 max = FixedMath.Max(maxXY, dst.z);
 7404                Fixed64 minXY = FixedMath.Min(dst.x, dst.y);
 7405                Fixed64 min = FixedMath.Min(minXY, dst.z);
 7406                Fixed64 middle = dst.x + dst.y + dst.z - max - min;
 7407                heuristicCost = (middle * DiagonalCost) + ((max - middle) * StraightCost);
 7408                break;
 409            case HeuristicMethod.Euclidean:
 410                // Calculate the squared distance and find the square root
 863411                Fixed64 d = dst.x * dst.x + dst.y * dst.y + dst.z * dst.z;
 863412                d = FixedMath.Sqrt(d);
 413                // Multiply the result by 100 for the heuristic cost
 863414                heuristicCost = d * StraightCost;
 415                break;
 416            default:
 417                break;
 418        }
 419
 1915420        return heuristicCost.CeilToInt();
 421    }
 422
 423    /// <summary>
 424    /// Applies Catmull-Rom spline smoothing to a set of input path points to produce a smoother curve.
 425    /// </summary>
 426    /// <param name="input">The input path of waypoints.</param>
 427    /// <param name="resolutionPerSegment">The number of interpolated points per segment.</param>
 428    /// <returns>A smoothed path using Catmull-Rom spline interpolation.</returns>
 429    public static AStarWaypoint[] CatmullSmooth(AStarWaypoint[] input, int resolutionPerSegment = 3)
 430    {
 6431        if (input.Length < 4) return input;
 432
 433        // size = smoothing points + 2 for start/end points
 4434        AStarWaypoint[] output = new AStarWaypoint[((input.Length - 3) * resolutionPerSegment) + 2];
 435
 436        // Add the starting point
 4437        output[0] = input[0];
 438
 4439        int outputIndex = 1; // Start at 1 because output[0] = input[0]
 26440        for (int i = 0; i < input.Length - 3; i++)
 441        {
 9442            Vector3d p0 = input[i].Position;
 9443            Vector3d p1 = input[i + 1].Position;
 9444            Vector3d p2 = input[i + 2].Position;
 9445            Vector3d p3 = input[i + 3].Position;
 446
 447            // j starts at 1 to skip duplicate of first point
 72448            for (int j = 1; j <= resolutionPerSegment; j++)
 449            {
 27450                Fixed64 t = new(j / (double)resolutionPerSegment);
 451
 452                // You should create a new waypoint here:
 27453                output[outputIndex] = new AStarWaypoint
 27454                {
 27455                    Position = CatmullRom(p0, p1, p2, p3, t),
 27456                    GlobalIndex = input[i + 1].GlobalIndex,
 27457                    PathCost = input[i + 1].PathCost,
 27458                    IsGoal = false
 27459                };
 460
 27461                outputIndex++;
 462            }
 463        }
 464
 465        // Add the final point
 4466        output[outputIndex] = input[^1];
 4467        return output;
 468    }
 469
 470    /// <summary>
 471    /// Computes the interpolated point along a Catmull-Rom spline given four control points.
 472    /// </summary>
 473    /// <param name="p0">The first control point.</param>
 474    /// <param name="p1">The second control point.</param>
 475    /// <param name="p2">The third control point.</param>
 476    /// <param name="p3">The fourth control point.</param>
 477    /// <param name="t">Interpolation factor between 0 and 1.</param>
 478    /// <returns>The interpolated point on the spline.</returns>
 479    [MethodImpl(MethodImplOptions.AggressiveInlining)]
 480    private static Vector3d CatmullRom(Vector3d p0, Vector3d p1, Vector3d p2, Vector3d p3, Fixed64 t)
 481    {
 482        // Classic Catmull-Rom basis matrix
 27483        Fixed64 t2 = t * t;
 27484        Fixed64 t3 = t2 * t;
 485
 27486        return
 27487            ((-t3 + 2 * t2 - t) * p0 +
 27488             (3 * t3 - 5 * t2 + 2) * p1 +
 27489             (-3 * t3 + 4 * t2 + t) * p2 +
 27490             (t3 - t2) * p3) / 2;
 491    }
 492}
Generated by: ReportGenerator 5.5.10.0
05/19/2026 - 21:06:43
GitHub | reportgenerator.io

Methods/Properties

.cctor()
get_Shared()
.ctor()
FindPath(Trailblazer.Pathing.AStarPathRequest)
TracePath()
ProcessNeighbors(Trailblazer.Pathing.SolidChartPartition)
TryProcessDirection(Trailblazer.Pathing.SolidChartPartition,GridForge.Spatial.SpatialDirection[],System.Int32,System.Boolean)
HasValidDiagonalLegs(Trailblazer.Pathing.SolidChartPartition,GridForge.Spatial.SpatialDirection)
IsLegClear(Trailblazer.Pathing.SolidChartPartition,GridForge.Spatial.SpatialDirection)
ProcessNeighbor(Trailblazer.Pathing.SolidChartPartition,Trailblazer.Pathing.SolidChartPartition,System.Int32)
SetPathPartitionData(Trailblazer.Pathing.SolidChartPartition,GridForge.Spatial.WorldVoxelIndex,System.Int32,System.Int32)
BuildRawPath()
BuildWaypoints()
CalculatePathCost(Trailblazer.Pathing.SolidChartPartition,System.Int32)
CalculatePathCost(Trailblazer.Pathing.SolidChartPartition,System.Int32,System.Int32&)
GetPathCost(GridForge.Grids.Voxel)
ClearWorkingState()
CalculateHeuristic(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,Trailblazer.Pathing.HeuristicMethod)
CatmullSmooth(Trailblazer.Pathing.AStarWaypoint[],System.Int32)
CatmullRom(FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Vector3d,FixedMathSharp.Fixed64)