diff --git a/src/Pathfinding/BaseGridNetwork.cs b/src/Pathfinding/BaseGridNetwork.cs index f1b8312..82b8558 100644 --- a/src/Pathfinding/BaseGridNetwork.cs +++ b/src/Pathfinding/BaseGridNetwork.cs @@ -24,17 +24,8 @@ public abstract class BaseGridNetwork : INetwork /// private const byte CostBitMask = 0b0111_1111; - private static readonly sbyte[,] DirectionOffsets = - { - { 0, -1 }, - { 1, 0 }, - { 0, 1 }, - { -1, 0 }, - { 1, -1 }, - { 1, 1 }, - { -1, 1 }, - { -1, -1 }, - }; + private static readonly sbyte[] DirectionOffsetsX = { 0, 1, 0, -1, 1, 1, -1, -1 }; + private static readonly sbyte[] DirectionOffsetsY = { -1, 0, 1, 0, -1, 1, 1, -1 }; private readonly int _numberOfDirections; private ushort _gridWidth; @@ -79,24 +70,31 @@ public abstract class BaseGridNetwork : INetwork { var grid = this._grid ?? throw new InvalidOperationException("Call Prepare before"); - // ReSharper disable once TooWideLocalVariableScope performance improvement - byte newX; - - // ReSharper disable once TooWideLocalVariableScope performance improvement - byte newY; - for (int i = 0; i < this._numberOfDirections; i++) { - newX = (byte)(node.X + DirectionOffsets[i, 0]); - newY = (byte)(node.Y + DirectionOffsets[i, 1]); - - if (!this._includeSafezone && (grid[newX, newY] & SafezoneBitFlag) > 0) + var calculatedX = node.X + DirectionOffsetsX[i]; + var calculatedY = node.Y + DirectionOffsetsY[i]; + if ((uint)calculatedX > byte.MaxValue || (uint)calculatedY > byte.MaxValue) { continue; } - var costToNode = grid[newX, newY] & CostBitMask; - if (!this.IsWithinBounds(newX, newY) || costToNode == UnreachableGridNodeValue) + var newX = (byte)calculatedX; + var newY = (byte)calculatedY; + + if (!this.IsWithinBounds(newX, newY)) + { + continue; + } + + var gridValue = grid[newX, newY]; + if (!this._includeSafezone && (gridValue & SafezoneBitFlag) > 0) + { + continue; + } + + var costToNode = gridValue & CostBitMask; + if (costToNode == UnreachableGridNodeValue) { continue; } @@ -108,7 +106,7 @@ public abstract class BaseGridNetwork : INetwork continue; } - var newG = node.CostUntilNow + this._grid[newNode.X, newNode.Y]; + var newG = node.CostUntilNow + costToNode; if (newNode.Status == NodeStatus.Open && newNode.CostUntilNow <= newG) { diff --git a/src/Pathfinding/ScopedGridNetwork.cs b/src/Pathfinding/ScopedGridNetwork.cs index ee330e5..25f80d5 100644 --- a/src/Pathfinding/ScopedGridNetwork.cs +++ b/src/Pathfinding/ScopedGridNetwork.cs @@ -82,16 +82,22 @@ public sealed class ScopedGridNetwork : BaseGridNetwork var maxX = offsetX + this._actualSegmentSideLength; var maxY = offsetY + this._actualSegmentSideLength; - for (byte x = offsetX; x < maxX; ++x) + // Must use int; a byte loop counter wraps at 256 and would not terminate. + for (int x = offsetX; x < maxX; ++x) { - for (byte y = offsetY; y < maxY; ++y) + for (int y = offsetY; y < maxY; ++y) { var i = this.GetIndexOfPoint(x, y); + if (i < 0 || i >= this._gridNodes.Length) + { + continue; + } + var node = this._gridNodes[i]; if (node is not null) { node.Status = NodeStatus.Undefined; - node.Position = new(x, y); + node.Position = new((byte)x, (byte)y); } } } @@ -100,9 +106,9 @@ public sealed class ScopedGridNetwork : BaseGridNetwork byte GetOffset(byte avgValue, int gridSize) { - var offset = (byte)Math.Max(avgValue - (this._actualSegmentSideLength / 2), 0); - offset = (byte)Math.Min(offset, gridSize - this._actualSegmentSideLength); - return offset; + var offset = Math.Max(avgValue - (this._actualSegmentSideLength / 2), 0); + offset = Math.Min(offset, Math.Max(gridSize - this._actualSegmentSideLength, 0)); + return (byte)offset; } }