| | | 1 | | //======================================================================= |
| | | 2 | | // GridNavigationCorridorValidationCursor.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024-present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using FixedMathSharp; |
| | | 10 | | |
| | | 11 | | namespace GridForge.Grids.Topology; |
| | | 12 | | |
| | | 13 | | |
| | | 14 | | |
| | | 15 | | /// <summary> |
| | | 16 | | /// Resumably validates one deterministic navigation corridor into caller-owned storage. |
| | | 17 | | /// </summary> |
| | | 18 | | /// <remarks> |
| | | 19 | | /// The ordered cell and waypoint spans are not retained. Their lengths and contents must remain |
| | | 20 | | /// stable between calls. A successful corridor of N cells consumes exactly 2N+1 work units. |
| | | 21 | | /// Advance one work unit at a time and call <see cref="TryGetCurrentPortal"/> after each call to |
| | | 22 | | /// consume every portal certificate in order. |
| | | 23 | | /// </remarks> |
| | | 24 | | public struct GridNavigationCorridorValidationCursor |
| | | 25 | | { |
| | | 26 | | private enum ValidationStage : byte |
| | | 27 | | { |
| | | 28 | | Cells, |
| | | 29 | | EntryAnchor, |
| | | 30 | | Portals, |
| | | 31 | | ExitAnchor |
| | | 32 | | } |
| | | 33 | | |
| | | 34 | | private readonly int _cellCount; |
| | | 35 | | private readonly Vector3d _entryAnchor; |
| | | 36 | | private readonly Vector3d _exitAnchor; |
| | | 37 | | private readonly Fixed64 _radiusClearance; |
| | | 38 | | private readonly Fixed64 _heightClearance; |
| | | 39 | | private GridNavigationCorridorValidationStatus _status; |
| | | 40 | | private ValidationStage _stage; |
| | | 41 | | private bool _hasCurrentPortal; |
| | | 42 | | private int _cellIndex; |
| | | 43 | | private int _portalIndex; |
| | | 44 | | private int _portalWaypointCount; |
| | | 45 | | private Vector3d _previousPoint; |
| | | 46 | | private Fixed64 _geometricCost; |
| | | 47 | | private GridNavigationPortal _selectedPortal; |
| | | 48 | | |
| | | 49 | | /// <summary>The current validation state.</summary> |
| | 9105 | 50 | | public readonly GridNavigationCorridorValidationStatus Status => _status; |
| | | 51 | | |
| | | 52 | | /// <summary>The number of canonical waypoints written, or zero after failure.</summary> |
| | 24 | 53 | | public readonly int PortalWaypointCount => _portalWaypointCount; |
| | | 54 | | |
| | | 55 | | /// <summary>The checked canonical polyline length accumulated so far.</summary> |
| | 24 | 56 | | public readonly Fixed64 GeometricCost => _geometricCost; |
| | | 57 | | |
| | | 58 | | /// <summary> |
| | | 59 | | /// Attempts to get the portal certificate produced by the last completed work unit. |
| | | 60 | | /// </summary> |
| | | 61 | | /// <remarks> |
| | | 62 | | /// The value is available only when the final work unit completed by the most recent advance |
| | | 63 | | /// was successful portal work. Use a one-unit budget to consume every certificate in order. |
| | | 64 | | /// </remarks> |
| | | 65 | | /// <param name="portal">The produced portal, or <see langword="default"/> when unavailable.</param> |
| | | 66 | | /// <returns><see langword="true"/> when a portal certificate is available.</returns> |
| | | 67 | | public readonly bool TryGetCurrentPortal(out GridNavigationPortal portal) |
| | | 68 | | { |
| | 7058 | 69 | | portal = _hasCurrentPortal ? _selectedPortal : default; |
| | 7058 | 70 | | return _hasCurrentPortal; |
| | | 71 | | } |
| | | 72 | | |
| | | 73 | | /// <summary> |
| | | 74 | | /// Creates a cursor for one ordered source, witness, and destination cell chain. |
| | | 75 | | /// </summary> |
| | | 76 | | /// <param name="cellCount">The stable number of ordered cells supplied to every advance.</param> |
| | | 77 | | /// <param name="entryAnchor">The source-cell foot anchor.</param> |
| | | 78 | | /// <param name="exitAnchor">The destination-cell foot anchor.</param> |
| | | 79 | | /// <param name="radiusClearance">The required nonnegative horizontal body radius.</param> |
| | | 80 | | /// <param name="heightClearance">The required positive body height.</param> |
| | | 81 | | public GridNavigationCorridorValidationCursor( |
| | | 82 | | int cellCount, |
| | | 83 | | Vector3d entryAnchor, |
| | | 84 | | Vector3d exitAnchor, |
| | | 85 | | Fixed64 radiusClearance, |
| | | 86 | | Fixed64 heightClearance) |
| | | 87 | | { |
| | 1057 | 88 | | _cellCount = cellCount; |
| | 1057 | 89 | | _entryAnchor = entryAnchor; |
| | 1057 | 90 | | _exitAnchor = exitAnchor; |
| | 1057 | 91 | | _radiusClearance = radiusClearance; |
| | 1057 | 92 | | _heightClearance = heightClearance; |
| | 1057 | 93 | | _status = cellCount >= 2 |
| | 1057 | 94 | | && radiusClearance >= Fixed64.Zero |
| | 1057 | 95 | | && heightClearance > Fixed64.Zero |
| | 1057 | 96 | | ? GridNavigationCorridorValidationStatus.InProgress |
| | 1057 | 97 | | : GridNavigationCorridorValidationStatus.Invalid; |
| | 1057 | 98 | | _stage = ValidationStage.Cells; |
| | 1057 | 99 | | _cellIndex = 0; |
| | 1057 | 100 | | _portalIndex = 0; |
| | 1057 | 101 | | _portalWaypointCount = 0; |
| | 1057 | 102 | | _previousPoint = entryAnchor; |
| | 1057 | 103 | | _geometricCost = default; |
| | 1057 | 104 | | _selectedPortal = default; |
| | 1057 | 105 | | _hasCurrentPortal = false; |
| | 1057 | 106 | | } |
| | | 107 | | |
| | | 108 | | /// <summary> |
| | | 109 | | /// Performs at most <paramref name="maxWork"/> bounded validation units. |
| | | 110 | | /// </summary> |
| | | 111 | | /// <param name="orderedCells">The unchanged ordered cells supplied for this cursor.</param> |
| | | 112 | | /// <param name="portalWaypoints">Caller-owned storage for twice the portal count.</param> |
| | | 113 | | /// <param name="maxWork">The nonnegative maximum number of work units to perform.</param> |
| | | 114 | | /// <returns>The resulting validation state.</returns> |
| | | 115 | | public GridNavigationCorridorValidationStatus Advance( |
| | | 116 | | ReadOnlySpan<GridCellPrism> orderedCells, |
| | | 117 | | Span<Vector3d> portalWaypoints, |
| | | 118 | | int maxWork) |
| | | 119 | | { |
| | 7113 | 120 | | if (maxWork < 0) |
| | 1 | 121 | | throw new ArgumentOutOfRangeException(nameof(maxWork)); |
| | | 122 | | |
| | 7112 | 123 | | _hasCurrentPortal = false; |
| | 7112 | 124 | | if (_status != GridNavigationCorridorValidationStatus.InProgress) |
| | 2 | 125 | | return _status; |
| | | 126 | | |
| | 7110 | 127 | | if (orderedCells.Length != _cellCount |
| | 7110 | 128 | | || _cellCount - 1 > portalWaypoints.Length / 2) |
| | | 129 | | { |
| | 1 | 130 | | return Fail(GridNavigationCorridorValidationStatus.Invalid); |
| | | 131 | | } |
| | | 132 | | |
| | 14364 | 133 | | while (maxWork-- > 0 && _status == GridNavigationCorridorValidationStatus.InProgress) |
| | 7255 | 134 | | PerformNext(orderedCells, portalWaypoints); |
| | | 135 | | |
| | 7109 | 136 | | return _status; |
| | | 137 | | } |
| | | 138 | | |
| | | 139 | | private void PerformNext( |
| | | 140 | | ReadOnlySpan<GridCellPrism> orderedCells, |
| | | 141 | | Span<Vector3d> portalWaypoints) |
| | | 142 | | { |
| | 7255 | 143 | | _hasCurrentPortal = false; |
| | 7255 | 144 | | switch (_stage) |
| | | 145 | | { |
| | | 146 | | case ValidationStage.Cells: |
| | 3122 | 147 | | GridCellPrism cell = orderedCells[_cellIndex++]; |
| | 3122 | 148 | | if (cell.FootprintVertexCount is not 4 and not 6 |
| | 3122 | 149 | | || cell.VerticalMax < cell.VerticalMin) |
| | | 150 | | { |
| | 2 | 151 | | Fail(GridNavigationCorridorValidationStatus.Invalid); |
| | 2 | 152 | | return; |
| | | 153 | | } |
| | | 154 | | |
| | 3120 | 155 | | if (_cellIndex == _cellCount) |
| | 1049 | 156 | | _stage = ValidationStage.EntryAnchor; |
| | 3120 | 157 | | return; |
| | | 158 | | |
| | | 159 | | case ValidationStage.EntryAnchor: |
| | 1049 | 160 | | GridCellGeometry.TryCreateNavigationPortal( |
| | 1049 | 161 | | orderedCells[0], |
| | 1049 | 162 | | orderedCells[1], |
| | 1049 | 163 | | out _selectedPortal); |
| | 1049 | 164 | | if (!GridCellGeometry.IsNavigationBodyAnchorValid( |
| | 1049 | 165 | | orderedCells[0], |
| | 1049 | 166 | | _entryAnchor, |
| | 1049 | 167 | | _radiusClearance, |
| | 1049 | 168 | | _heightClearance, |
| | 1049 | 169 | | _selectedPortal)) |
| | | 170 | | { |
| | 10 | 171 | | Fail(GridNavigationCorridorValidationStatus.Invalid); |
| | 10 | 172 | | return; |
| | | 173 | | } |
| | | 174 | | |
| | 1039 | 175 | | _stage = ValidationStage.Portals; |
| | 1039 | 176 | | return; |
| | | 177 | | |
| | | 178 | | case ValidationStage.Portals: |
| | 2054 | 179 | | ValidateNextPortal(orderedCells, portalWaypoints); |
| | 2054 | 180 | | return; |
| | | 181 | | |
| | | 182 | | default: |
| | 1030 | 183 | | ValidateExitAnchor(orderedCells); |
| | 1030 | 184 | | return; |
| | | 185 | | } |
| | | 186 | | } |
| | | 187 | | |
| | | 188 | | private void ValidateNextPortal( |
| | | 189 | | ReadOnlySpan<GridCellPrism> orderedCells, |
| | | 190 | | Span<Vector3d> portalWaypoints) |
| | | 191 | | { |
| | 2054 | 192 | | GridCellPrism source = orderedCells[_portalIndex]; |
| | 2054 | 193 | | GridCellPrism target = orderedCells[_portalIndex + 1]; |
| | 2054 | 194 | | GridNavigationPortal incomingPortal = _portalIndex == 0 |
| | 2054 | 195 | | ? default |
| | 2054 | 196 | | : _selectedPortal; |
| | 2054 | 197 | | GridNavigationPortal outgoingPortal = _selectedPortal; |
| | 2054 | 198 | | if ((_portalIndex > 0 |
| | 2054 | 199 | | && !GridCellGeometry.TryCreateNavigationPortal(source, target, out outgoingPortal)) |
| | 2054 | 200 | | || !outgoingPortal.IsValid |
| | 2054 | 201 | | || !outgoingPortal.TryResolveProfile( |
| | 2054 | 202 | | _radiusClearance, |
| | 2054 | 203 | | _heightClearance, |
| | 2054 | 204 | | out Vector3d sourcePoint, |
| | 2054 | 205 | | out Vector3d targetPoint) |
| | 2054 | 206 | | || !GridCellGeometry.IsNavigationBodySegmentValid( |
| | 2054 | 207 | | source, |
| | 2054 | 208 | | _previousPoint, |
| | 2054 | 209 | | sourcePoint, |
| | 2054 | 210 | | _radiusClearance, |
| | 2054 | 211 | | _heightClearance, |
| | 2054 | 212 | | incomingPortal, |
| | 2054 | 213 | | outgoingPortal, |
| | 2054 | 214 | | GridNavigationBodySegmentEndpointAllowance.None)) |
| | | 215 | | { |
| | 7 | 216 | | Fail(GridNavigationCorridorValidationStatus.Invalid); |
| | 7 | 217 | | return; |
| | | 218 | | } |
| | | 219 | | |
| | 2047 | 220 | | portalWaypoints[_portalWaypointCount++] = sourcePoint; |
| | 2047 | 221 | | if (!TryAccumulateDistance(_previousPoint, sourcePoint)) |
| | | 222 | | { |
| | 1 | 223 | | Fail(GridNavigationCorridorValidationStatus.CostOverflow); |
| | 1 | 224 | | return; |
| | | 225 | | } |
| | | 226 | | |
| | 2046 | 227 | | _previousPoint = sourcePoint; |
| | 2046 | 228 | | if (outgoingPortal.FaceKind == VoxelContactFaceKind.Horizontal) |
| | | 229 | | { |
| | 4 | 230 | | portalWaypoints[_portalWaypointCount++] = targetPoint; |
| | 4 | 231 | | if (!TryAccumulateDistance(_previousPoint, targetPoint)) |
| | | 232 | | { |
| | 1 | 233 | | Fail(GridNavigationCorridorValidationStatus.CostOverflow); |
| | 1 | 234 | | return; |
| | | 235 | | } |
| | | 236 | | |
| | 3 | 237 | | _previousPoint = targetPoint; |
| | | 238 | | } |
| | | 239 | | |
| | 2045 | 240 | | _selectedPortal = outgoingPortal; |
| | 2045 | 241 | | _hasCurrentPortal = true; |
| | 2045 | 242 | | _portalIndex++; |
| | 2045 | 243 | | if (_portalIndex == _cellCount - 1) |
| | 1030 | 244 | | _stage = ValidationStage.ExitAnchor; |
| | 2045 | 245 | | } |
| | | 246 | | |
| | | 247 | | private void ValidateExitAnchor(ReadOnlySpan<GridCellPrism> orderedCells) |
| | | 248 | | { |
| | 1030 | 249 | | if (!TryAccumulateDistance(_previousPoint, _exitAnchor)) |
| | | 250 | | { |
| | 1 | 251 | | Fail(GridNavigationCorridorValidationStatus.CostOverflow); |
| | 1 | 252 | | return; |
| | | 253 | | } |
| | | 254 | | |
| | 1029 | 255 | | if (!GridCellGeometry.IsNavigationBodySegmentValid( |
| | 1029 | 256 | | orderedCells[_cellCount - 1], |
| | 1029 | 257 | | _previousPoint, |
| | 1029 | 258 | | _exitAnchor, |
| | 1029 | 259 | | _radiusClearance, |
| | 1029 | 260 | | _heightClearance, |
| | 1029 | 261 | | _selectedPortal, |
| | 1029 | 262 | | default, |
| | 1029 | 263 | | GridNavigationBodySegmentEndpointAllowance.None)) |
| | | 264 | | { |
| | 3 | 265 | | Fail(GridNavigationCorridorValidationStatus.Invalid); |
| | 3 | 266 | | return; |
| | | 267 | | } |
| | | 268 | | |
| | 1026 | 269 | | _status = GridNavigationCorridorValidationStatus.Complete; |
| | 1026 | 270 | | } |
| | | 271 | | |
| | | 272 | | private GridNavigationCorridorValidationStatus Fail( |
| | | 273 | | GridNavigationCorridorValidationStatus status) |
| | | 274 | | { |
| | 26 | 275 | | _portalWaypointCount = 0; |
| | 26 | 276 | | _geometricCost = default; |
| | 26 | 277 | | _hasCurrentPortal = false; |
| | 26 | 278 | | _status = status; |
| | 26 | 279 | | return status; |
| | | 280 | | } |
| | | 281 | | |
| | | 282 | | private bool TryAccumulateDistance(Vector3d start, Vector3d end) |
| | | 283 | | { |
| | 3081 | 284 | | return Vector3d.TryGetDistance(start, end, out Fixed64 distance) |
| | 3081 | 285 | | && Fixed64.TryAdd(_geometricCost, distance, out _geometricCost); |
| | | 286 | | } |
| | | 287 | | |
| | | 288 | | } |