| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationMapBuilder.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 System.Collections.Generic; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | using GridForge.Configuration; |
| | | 12 | | using GridForge.Grids.Topology; |
| | | 13 | | using GridForge.Spatial; |
| | | 14 | | using SwiftCollections; |
| | | 15 | | |
| | | 16 | | namespace Trailblazer.Pathing; |
| | | 17 | | |
| | | 18 | | /// <summary> |
| | | 19 | | /// Collects one-grid authoring input and builds a validated, canonically ordered map. |
| | | 20 | | /// </summary> |
| | | 21 | | public sealed class NavigationMapBuilder |
| | | 22 | | { |
| | 2074 | 23 | | private readonly SwiftList<NavigationCellEntry> _cells = new(); |
| | 2074 | 24 | | private readonly SwiftList<NavigationConnection> _connections = new(); |
| | 2074 | 25 | | private readonly SwiftList<TraversalTransitionDefinition> _transitions = new(); |
| | 2074 | 26 | | private readonly SwiftList<TraversalTransitionRule> _transitionRules = new(); |
| | | 27 | | private NavigationCell? _defaultCell; |
| | | 28 | | |
| | | 29 | | /// <summary> |
| | | 30 | | /// The stable host-owned identifier assigned to the resulting map. |
| | | 31 | | /// </summary> |
| | | 32 | | public string MapId { get; } |
| | | 33 | | |
| | | 34 | | /// <summary> |
| | | 35 | | /// The normalized storage-neutral GridForge configuration binding. |
| | | 36 | | /// </summary> |
| | | 37 | | public NormalizedGridConfiguration GridBinding { get; } |
| | | 38 | | |
| | | 39 | | /// <summary> |
| | | 40 | | /// Creates a builder for an already normalized grid descriptor. |
| | | 41 | | /// </summary> |
| | 2074 | 42 | | public NavigationMapBuilder( |
| | 2074 | 43 | | string mapId, |
| | 2074 | 44 | | NormalizedGridConfiguration gridBinding) |
| | | 45 | | { |
| | 2074 | 46 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2074 | 47 | | string.IsNullOrWhiteSpace(mapId), |
| | 2074 | 48 | | nameof(mapId), |
| | 2074 | 49 | | "Map ID cannot be null, empty, or whitespace."); |
| | 2074 | 50 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2074 | 51 | | !gridBinding.IsValid, |
| | 2074 | 52 | | nameof(gridBinding), |
| | 2074 | 53 | | "Grid binding must contain a valid normalized address space."); |
| | | 54 | | |
| | 2074 | 55 | | MapId = mapId; |
| | 2074 | 56 | | GridBinding = gridBinding; |
| | 2074 | 57 | | } |
| | | 58 | | |
| | | 59 | | /// <summary> |
| | | 60 | | /// Creates a builder by normalizing an offline GridForge configuration. |
| | | 61 | | /// </summary> |
| | | 62 | | public NavigationMapBuilder( |
| | | 63 | | string mapId, |
| | | 64 | | GridConfiguration gridConfiguration) |
| | 46 | 65 | | : this(mapId, Normalize(gridConfiguration)) |
| | | 66 | | { |
| | 46 | 67 | | } |
| | | 68 | | |
| | | 69 | | /// <summary> |
| | | 70 | | /// Adds one authored cell. Duplicate indices reject when <see cref="Build"/> normalizes the bake. |
| | | 71 | | /// </summary> |
| | | 72 | | public NavigationMapBuilder AddCell(VoxelIndex index, NavigationCell cell) |
| | | 73 | | { |
| | 23139 | 74 | | _cells.Add(new NavigationCellEntry(index, cell)); |
| | 23139 | 75 | | return this; |
| | | 76 | | } |
| | | 77 | | |
| | | 78 | | /// <summary> |
| | | 79 | | /// Adds one authored cell entry. |
| | | 80 | | /// </summary> |
| | | 81 | | public NavigationMapBuilder AddCell(NavigationCellEntry entry) |
| | | 82 | | { |
| | 4 | 83 | | _cells.Add(entry); |
| | 4 | 84 | | return this; |
| | | 85 | | } |
| | | 86 | | |
| | | 87 | | /// <summary> |
| | | 88 | | /// Sets the optional complete fallback payload used when no explicit cell wins. |
| | | 89 | | /// </summary> |
| | | 90 | | public NavigationMapBuilder SetDefaultCell(NavigationCell? defaultCell) |
| | | 91 | | { |
| | 65 | 92 | | _defaultCell = defaultCell; |
| | 65 | 93 | | return this; |
| | | 94 | | } |
| | | 95 | | |
| | | 96 | | /// <summary> |
| | | 97 | | /// Adds one directed source-owned physical connection. |
| | | 98 | | /// </summary> |
| | | 99 | | public NavigationMapBuilder AddConnection(NavigationConnection connection) |
| | | 100 | | { |
| | 303 | 101 | | SwiftThrowHelper.ThrowIfNull(connection, nameof(connection)); |
| | 303 | 102 | | _connections.Add(connection); |
| | 303 | 103 | | return this; |
| | | 104 | | } |
| | | 105 | | |
| | | 106 | | /// <summary> |
| | | 107 | | /// Adds one directed source-owned semantic transition. |
| | | 108 | | /// </summary> |
| | | 109 | | public NavigationMapBuilder AddTransition(TraversalTransitionDefinition transition) |
| | | 110 | | { |
| | 177 | 111 | | _transitions.Add(transition); |
| | 177 | 112 | | return this; |
| | | 113 | | } |
| | | 114 | | |
| | | 115 | | /// <summary>Adds one bounded procedural semantic transition rule.</summary> |
| | | 116 | | public NavigationMapBuilder AddTransitionRule(TraversalTransitionRule rule) |
| | | 117 | | { |
| | 80 | 118 | | _transitionRules.Add(rule); |
| | 80 | 119 | | return this; |
| | | 120 | | } |
| | | 121 | | |
| | | 122 | | /// <summary> |
| | | 123 | | /// Validates and copies all input into an immutable canonical map. |
| | | 124 | | /// </summary> |
| | | 125 | | public NavigationMap Build() |
| | | 126 | | { |
| | 2060 | 127 | | NavigationCellEntry[] cells = _cells.ToArray(); |
| | 2060 | 128 | | NavigationConnection[] connections = _connections.ToArray(); |
| | 2060 | 129 | | TraversalTransitionDefinition[] transitions = _transitions.ToArray(); |
| | 2060 | 130 | | TraversalTransitionRule[] transitionRules = _transitionRules.ToArray(); |
| | | 131 | | |
| | 2060 | 132 | | Array.Sort(cells, NavigationCellEntryComparer.Instance); |
| | 2060 | 133 | | Array.Sort(connections, NavigationConnectionComparer.Instance); |
| | 2060 | 134 | | Array.Sort(transitions, TraversalTransitionDefinitionComparer.Instance); |
| | 2060 | 135 | | Array.Sort(transitionRules, TraversalTransitionRuleComparer.Instance); |
| | | 136 | | |
| | 2060 | 137 | | ValidateGridGeometry(); |
| | 2060 | 138 | | if (_defaultCell.HasValue) |
| | 65 | 139 | | ValidateCellPayload(_defaultCell.Value, nameof(_defaultCell)); |
| | 2059 | 140 | | ValidateCells(cells); |
| | 2056 | 141 | | ValidateConnections(cells, connections); |
| | 2046 | 142 | | ValidateTransitions(cells, transitions); |
| | 2039 | 143 | | ValidateTransitionRules(transitionRules); |
| | | 144 | | |
| | 2037 | 145 | | return new NavigationMap( |
| | 2037 | 146 | | MapId, |
| | 2037 | 147 | | GridBinding, |
| | 2037 | 148 | | cells, |
| | 2037 | 149 | | connections, |
| | 2037 | 150 | | transitions, |
| | 2037 | 151 | | transitionRules, |
| | 2037 | 152 | | _defaultCell); |
| | | 153 | | } |
| | | 154 | | |
| | | 155 | | /// <summary> |
| | | 156 | | /// Imports an X/Y/Z rectangular dense source into the canonical sparse map representation. |
| | | 157 | | /// Null entries are omitted from the bake. |
| | | 158 | | /// </summary> |
| | | 159 | | public static NavigationMap ImportDenseRectangular( |
| | | 160 | | string mapId, |
| | | 161 | | GridConfiguration gridConfiguration, |
| | | 162 | | NavigationCell?[,,] cells) |
| | | 163 | | { |
| | 4 | 164 | | SwiftThrowHelper.ThrowIfNull(cells, nameof(cells)); |
| | 4 | 165 | | var builder = new NavigationMapBuilder(mapId, gridConfiguration); |
| | 4 | 166 | | SwiftThrowHelper.ThrowIfArgument( |
| | 4 | 167 | | builder.GridBinding.Configuration.TopologyKind != GridTopologyKind.RectangularPrism, |
| | 4 | 168 | | nameof(gridConfiguration), |
| | 4 | 169 | | "Dense rectangular import requires rectangular-prism topology."); |
| | 4 | 170 | | SwiftThrowHelper.ThrowIfArgument( |
| | 4 | 171 | | cells.GetLength(0) != builder.GridBinding.Width |
| | 4 | 172 | | || cells.GetLength(1) != builder.GridBinding.Height |
| | 4 | 173 | | || cells.GetLength(2) != builder.GridBinding.Length, |
| | 4 | 174 | | nameof(cells), |
| | 4 | 175 | | "Dense input dimensions must match the normalized grid address space in X/Y/Z order."); |
| | | 176 | | |
| | 10 | 177 | | for (int x = 0; x < cells.GetLength(0); x++) |
| | | 178 | | { |
| | 32 | 179 | | for (int y = 0; y < cells.GetLength(1); y++) |
| | | 180 | | { |
| | 96 | 181 | | for (int z = 0; z < cells.GetLength(2); z++) |
| | | 182 | | { |
| | 36 | 183 | | NavigationCell? cell = cells[x, y, z]; |
| | 36 | 184 | | if (cell.HasValue) |
| | 2 | 185 | | builder.AddCell(new VoxelIndex(x, y, z), cell.Value); |
| | | 186 | | } |
| | | 187 | | } |
| | | 188 | | } |
| | | 189 | | |
| | 1 | 190 | | return builder.Build(); |
| | | 191 | | } |
| | | 192 | | |
| | | 193 | | /// <summary> |
| | | 194 | | /// Imports explicit axial Q/layer/R hex cells into the canonical sparse map representation. |
| | | 195 | | /// </summary> |
| | | 196 | | public static NavigationMap ImportAxialHex( |
| | | 197 | | string mapId, |
| | | 198 | | GridConfiguration gridConfiguration, |
| | | 199 | | IEnumerable<NavigationCellEntry> cells) |
| | | 200 | | { |
| | 2 | 201 | | SwiftThrowHelper.ThrowIfNull(cells, nameof(cells)); |
| | 2 | 202 | | var builder = new NavigationMapBuilder(mapId, gridConfiguration); |
| | 2 | 203 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2 | 204 | | builder.GridBinding.Configuration.TopologyKind != GridTopologyKind.HexPrism, |
| | 2 | 205 | | nameof(gridConfiguration), |
| | 2 | 206 | | "Axial hex import requires hex-prism topology."); |
| | | 207 | | |
| | 12 | 208 | | foreach (NavigationCellEntry cell in cells) |
| | 4 | 209 | | builder.AddCell(cell); |
| | | 210 | | |
| | 2 | 211 | | return builder.Build(); |
| | | 212 | | } |
| | | 213 | | |
| | | 214 | | private static NormalizedGridConfiguration Normalize(GridConfiguration configuration) |
| | | 215 | | { |
| | 46 | 216 | | bool normalized = configuration.TryNormalize(out NormalizedGridConfiguration descriptor); |
| | 46 | 217 | | SwiftThrowHelper.ThrowIfArgument( |
| | 46 | 218 | | !normalized, |
| | 46 | 219 | | nameof(configuration), |
| | 46 | 220 | | "Grid configuration could not be normalized into a supported address space."); |
| | 46 | 221 | | return descriptor; |
| | | 222 | | } |
| | | 223 | | |
| | | 224 | | private void ValidateCells(NavigationCellEntry[] cells) |
| | | 225 | | { |
| | 50398 | 226 | | for (int i = 0; i < cells.Length; i++) |
| | | 227 | | { |
| | 23143 | 228 | | NavigationCellEntry entry = cells[i]; |
| | 23143 | 229 | | SwiftThrowHelper.ThrowIfArgument( |
| | 23143 | 230 | | !GridBinding.IsValidIndex(entry.Index), |
| | 23143 | 231 | | nameof(cells), |
| | 23143 | 232 | | $"Cell index {entry.Index} is outside the normalized grid address space."); |
| | 23142 | 233 | | ValidateCellPayload(entry.Cell, nameof(cells)); |
| | | 234 | | |
| | 23141 | 235 | | SwiftThrowHelper.ThrowIfArgument( |
| | 23141 | 236 | | i > 0 && cells[i - 1].Index.Equals(entry.Index), |
| | 23141 | 237 | | nameof(cells), |
| | 23141 | 238 | | $"Duplicate authored cell index {entry.Index}."); |
| | | 239 | | } |
| | 2056 | 240 | | } |
| | | 241 | | |
| | | 242 | | private void ValidateConnections( |
| | | 243 | | NavigationCellEntry[] cells, |
| | | 244 | | NavigationConnection[] connections) |
| | | 245 | | { |
| | 2056 | 246 | | int maximumPrismCount = GetMaximumLocalPrismCount(connections); |
| | 2056 | 247 | | var orderedPrisms = maximumPrismCount == 0 |
| | 2056 | 248 | | ? Array.Empty<GridCellPrism>() |
| | 2056 | 249 | | : new GridCellPrism[maximumPrismCount]; |
| | 2056 | 250 | | var portalWaypoints = maximumPrismCount <= 1 |
| | 2056 | 251 | | ? Array.Empty<Vector3d>() |
| | 2056 | 252 | | : new Vector3d[(maximumPrismCount - 1) * 2]; |
| | 2056 | 253 | | int maximumWitnessCount = GetMaximumWitnessCount(connections); |
| | 2056 | 254 | | var witnessSet = new SwiftHashSet<NavigationCellAddress>(maximumWitnessCount); |
| | | 255 | | |
| | 4698 | 256 | | for (int i = 0; i < connections.Length; i++) |
| | | 257 | | { |
| | 303 | 258 | | NavigationConnection connection = connections[i]; |
| | 303 | 259 | | SwiftThrowHelper.ThrowIfArgument( |
| | 303 | 260 | | i > 0 && string.Equals(connections[i - 1].Id, connection.Id, StringComparison.Ordinal), |
| | 303 | 261 | | nameof(connections), |
| | 303 | 262 | | $"Duplicate map-local connection ID '{connection.Id}'."); |
| | | 263 | | |
| | 302 | 264 | | SwiftThrowHelper.ThrowIfArgument( |
| | 302 | 265 | | !TryGetAuthoredCell(cells, connection.SourceIndex, out NavigationCell sourceCell), |
| | 302 | 266 | | nameof(connections), |
| | 302 | 267 | | $"Connection '{connection.Id}' references missing local source {connection.SourceIndex}."); |
| | 301 | 268 | | ValidateLocalAnchor( |
| | 301 | 269 | | connection.EntryAnchor, |
| | 301 | 270 | | connection.SourceIndex, |
| | 301 | 271 | | nameof(connections), |
| | 301 | 272 | | "Connection entry anchor is outside its source prism."); |
| | | 273 | | |
| | 300 | 274 | | ValidateClearance( |
| | 300 | 275 | | sourceCell, |
| | 300 | 276 | | connection.PortalRadiusClearance, |
| | 300 | 277 | | connection.PortalHeightClearance, |
| | 300 | 278 | | nameof(connections), |
| | 300 | 279 | | "Connection exceeds source-cell clearance."); |
| | | 280 | | |
| | 299 | 281 | | if (string.Equals(connection.Destination.MapId, MapId, StringComparison.Ordinal)) |
| | | 282 | | { |
| | 206 | 283 | | SwiftThrowHelper.ThrowIfArgument( |
| | 206 | 284 | | !TryGetAuthoredCell( |
| | 206 | 285 | | cells, |
| | 206 | 286 | | connection.Destination.Index, |
| | 206 | 287 | | out NavigationCell destinationCell), |
| | 206 | 288 | | nameof(connections), |
| | 206 | 289 | | $"Connection '{connection.Id}' references missing local destination {connection.Destination.Index}." |
| | 205 | 290 | | ValidateLocalAnchor( |
| | 205 | 291 | | connection.ExitAnchor, |
| | 205 | 292 | | connection.Destination.Index, |
| | 205 | 293 | | nameof(connections), |
| | 205 | 294 | | "Connection exit anchor is outside its destination prism."); |
| | 205 | 295 | | ValidateClearance( |
| | 205 | 296 | | destinationCell, |
| | 205 | 297 | | connection.PortalRadiusClearance, |
| | 205 | 298 | | connection.PortalHeightClearance, |
| | 205 | 299 | | nameof(connections), |
| | 205 | 300 | | "Connection exceeds destination-cell clearance."); |
| | | 301 | | } |
| | | 302 | | |
| | 298 | 303 | | ValidateWitnesses(cells, connection, witnessSet); |
| | 296 | 304 | | ValidateConnectionGeometry(cells, connection, orderedPrisms, portalWaypoints); |
| | | 305 | | } |
| | 2046 | 306 | | } |
| | | 307 | | |
| | | 308 | | private void ValidateTransitions( |
| | | 309 | | NavigationCellEntry[] cells, |
| | | 310 | | TraversalTransitionDefinition[] transitions) |
| | | 311 | | { |
| | 4432 | 312 | | for (int i = 0; i < transitions.Length; i++) |
| | | 313 | | { |
| | 177 | 314 | | TraversalTransitionDefinition transition = transitions[i]; |
| | 177 | 315 | | SwiftThrowHelper.ThrowIfArgument( |
| | 177 | 316 | | i > 0 && string.Equals(transitions[i - 1].Id, transition.Id, StringComparison.Ordinal), |
| | 177 | 317 | | nameof(transitions), |
| | 177 | 318 | | $"Duplicate map-local transition ID '{transition.Id}'."); |
| | | 319 | | |
| | 176 | 320 | | SwiftThrowHelper.ThrowIfArgument( |
| | 176 | 321 | | !TryGetAuthoredCell(cells, transition.SourceIndex, out NavigationCell sourceCell), |
| | 176 | 322 | | nameof(transitions), |
| | 176 | 323 | | $"Transition '{transition.Id}' references missing local source {transition.SourceIndex}."); |
| | 175 | 324 | | SwiftThrowHelper.ThrowIfArgument( |
| | 175 | 325 | | !SupportsMedium(sourceCell, transition.SourceMedium), |
| | 175 | 326 | | nameof(transitions), |
| | 175 | 327 | | "Transition source medium is not authored on its source cell."); |
| | | 328 | | |
| | 174 | 329 | | if (transition.HasSourcePointOverride) |
| | | 330 | | { |
| | 8 | 331 | | ValidateLocalAnchor( |
| | 8 | 332 | | transition.SourcePointOverride, |
| | 8 | 333 | | transition.SourceIndex, |
| | 8 | 334 | | nameof(transitions), |
| | 8 | 335 | | "Transition source point is outside its source prism."); |
| | | 336 | | } |
| | | 337 | | |
| | 173 | 338 | | if (!string.Equals(transition.Destination.MapId, MapId, StringComparison.Ordinal)) |
| | | 339 | | continue; |
| | | 340 | | |
| | 158 | 341 | | SwiftThrowHelper.ThrowIfArgument( |
| | 158 | 342 | | !TryGetAuthoredCell( |
| | 158 | 343 | | cells, |
| | 158 | 344 | | transition.Destination.Index, |
| | 158 | 345 | | out NavigationCell destinationCell), |
| | 158 | 346 | | nameof(transitions), |
| | 158 | 347 | | $"Transition '{transition.Id}' references missing local destination {transition.Destination.Index}."); |
| | 157 | 348 | | SwiftThrowHelper.ThrowIfArgument( |
| | 157 | 349 | | !SupportsMedium(destinationCell, transition.DestinationMedium), |
| | 157 | 350 | | nameof(transitions), |
| | 157 | 351 | | "Transition destination medium is not authored on its destination cell."); |
| | | 352 | | |
| | 156 | 353 | | if (transition.HasDestinationPointOverride) |
| | | 354 | | { |
| | 8 | 355 | | ValidateLocalAnchor( |
| | 8 | 356 | | transition.DestinationPointOverride, |
| | 8 | 357 | | transition.Destination.Index, |
| | 8 | 358 | | nameof(transitions), |
| | 8 | 359 | | "Transition destination point is outside its destination prism."); |
| | | 360 | | } |
| | | 361 | | } |
| | 2039 | 362 | | } |
| | | 363 | | |
| | | 364 | | private static void ValidateTransitionRules(TraversalTransitionRule[] rules) |
| | | 365 | | { |
| | 4234 | 366 | | for (int i = 0; i < rules.Length; i++) |
| | | 367 | | { |
| | 80 | 368 | | TraversalTransitionRule rule = rules[i]; |
| | 80 | 369 | | rule.Validate(); |
| | 79 | 370 | | SwiftThrowHelper.ThrowIfArgument( |
| | 79 | 371 | | i > 0 && string.Equals(rules[i - 1].Id, rule.Id, StringComparison.Ordinal), |
| | 79 | 372 | | nameof(rules), |
| | 79 | 373 | | $"Duplicate map-local transition rule ID '{rule.Id}'."); |
| | | 374 | | } |
| | 2037 | 375 | | } |
| | | 376 | | |
| | | 377 | | private void ValidateWitnesses( |
| | | 378 | | NavigationCellEntry[] cells, |
| | | 379 | | NavigationConnection connection, |
| | | 380 | | SwiftHashSet<NavigationCellAddress> witnessSet) |
| | | 381 | | { |
| | 298 | 382 | | witnessSet.Clear(); |
| | 1020 | 383 | | for (int i = 0; i < connection.Witnesses.Count; i++) |
| | | 384 | | { |
| | 214 | 385 | | NavigationCellAddress witness = connection.Witnesses[i]; |
| | 214 | 386 | | SwiftThrowHelper.ThrowIfArgument( |
| | 214 | 387 | | !witnessSet.Add(witness), |
| | 214 | 388 | | nameof(connection), |
| | 214 | 389 | | $"Connection '{connection.Id}' repeats witness {witness}."); |
| | | 390 | | |
| | 213 | 391 | | if (!string.Equals(witness.MapId, MapId, StringComparison.Ordinal)) |
| | | 392 | | continue; |
| | | 393 | | |
| | 82 | 394 | | SwiftThrowHelper.ThrowIfArgument( |
| | 82 | 395 | | !TryGetAuthoredCell(cells, witness.Index, out NavigationCell witnessCell), |
| | 82 | 396 | | nameof(connection), |
| | 82 | 397 | | $"Connection '{connection.Id}' references missing local witness {witness.Index}."); |
| | 81 | 398 | | ValidateClearance( |
| | 81 | 399 | | witnessCell, |
| | 81 | 400 | | connection.PortalRadiusClearance, |
| | 81 | 401 | | connection.PortalHeightClearance, |
| | 81 | 402 | | nameof(connection), |
| | 81 | 403 | | "Connection exceeds witness-cell clearance."); |
| | | 404 | | } |
| | 296 | 405 | | } |
| | | 406 | | |
| | | 407 | | private void ValidateGridGeometry() |
| | | 408 | | { |
| | 2060 | 409 | | int lastX = GridBinding.Width - 1; |
| | 2060 | 410 | | int lastY = GridBinding.Height - 1; |
| | 2060 | 411 | | int lastZ = GridBinding.Length - 1; |
| | 12360 | 412 | | for (int xSelector = 0; xSelector < 2; xSelector++) |
| | | 413 | | { |
| | 4120 | 414 | | int x = xSelector == 0 ? 0 : lastX; |
| | 24720 | 415 | | for (int ySelector = 0; ySelector < 2; ySelector++) |
| | | 416 | | { |
| | 8240 | 417 | | int y = ySelector == 0 ? 0 : lastY; |
| | 49440 | 418 | | for (int zSelector = 0; zSelector < 2; zSelector++) |
| | | 419 | | { |
| | 16480 | 420 | | int z = zSelector == 0 ? 0 : lastZ; |
| | 16480 | 421 | | bool representable = GridBinding.TryGetCellPrism( |
| | 16480 | 422 | | new VoxelIndex(x, y, z), |
| | 16480 | 423 | | out _); |
| | 16480 | 424 | | SwiftThrowHelper.ThrowIfArgument( |
| | 16480 | 425 | | !representable, |
| | 16480 | 426 | | nameof(GridBinding), |
| | 16480 | 427 | | "Grid binding cannot produce exact cell prisms across its address-space extremes."); |
| | | 428 | | } |
| | | 429 | | } |
| | | 430 | | } |
| | 2060 | 431 | | } |
| | | 432 | | |
| | | 433 | | private void ValidateConnectionGeometry( |
| | | 434 | | NavigationCellEntry[] cells, |
| | | 435 | | NavigationConnection connection, |
| | | 436 | | GridCellPrism[] prismScratch, |
| | | 437 | | Vector3d[] waypointScratch) |
| | | 438 | | { |
| | | 439 | | // Full cross-map geometry is validated transactionally when the destination map is present. |
| | 296 | 440 | | if (!IsCompleteLocalChain(connection)) |
| | 94 | 441 | | return; |
| | | 442 | | |
| | 202 | 443 | | int prismCount = connection.Witnesses.Count + 2; |
| | 202 | 444 | | GridBinding.TryGetCellPrism(connection.SourceIndex, out prismScratch[0]); |
| | 562 | 445 | | for (int i = 0; i < connection.Witnesses.Count; i++) |
| | | 446 | | { |
| | 79 | 447 | | GridBinding.TryGetCellPrism( |
| | 79 | 448 | | connection.Witnesses[i].Index, |
| | 79 | 449 | | out prismScratch[i + 1]); |
| | | 450 | | } |
| | 202 | 451 | | GridBinding.TryGetCellPrism(connection.Destination.Index, out prismScratch[prismCount - 1]); |
| | | 452 | | |
| | 202 | 453 | | bool validCorridor = GridCellGeometry.TryValidateNavigationCorridor( |
| | 202 | 454 | | prismScratch.AsSpan(0, prismCount), |
| | 202 | 455 | | connection.EntryAnchor, |
| | 202 | 456 | | connection.ExitAnchor, |
| | 202 | 457 | | connection.PortalRadiusClearance, |
| | 202 | 458 | | connection.PortalHeightClearance, |
| | 202 | 459 | | waypointScratch.AsSpan(0, (prismCount - 1) * 2), |
| | 202 | 460 | | out _, |
| | 202 | 461 | | out Fixed64 corridorCost); |
| | 202 | 462 | | SwiftThrowHelper.ThrowIfArgument( |
| | 202 | 463 | | !validCorridor, |
| | 202 | 464 | | nameof(connection), |
| | 202 | 465 | | $"Connection '{connection.Id}' does not define a valid clearance-bearing prism corridor."); |
| | | 466 | | |
| | 200 | 467 | | if (!connection.IsLowerBoundCertified) |
| | 198 | 468 | | return; |
| | | 469 | | |
| | 2 | 470 | | TryGetAuthoredCell(cells, connection.Destination.Index, out NavigationCell destinationCell); |
| | 2 | 471 | | bool certified = TryProveLowerBound( |
| | 2 | 472 | | prismScratch[0], |
| | 2 | 473 | | prismScratch[prismCount - 1], |
| | 2 | 474 | | connection, |
| | 2 | 475 | | corridorCost, |
| | 2 | 476 | | destinationCell.EnterCost); |
| | 2 | 477 | | SwiftThrowHelper.ThrowIfArgument( |
| | 2 | 478 | | !certified, |
| | 2 | 479 | | nameof(connection), |
| | 2 | 480 | | $"Connection '{connection.Id}' lower-bound declaration cannot be proven by its canonical fixed-point cost.") |
| | 1 | 481 | | } |
| | | 482 | | |
| | | 483 | | private bool IsCompleteLocalChain(NavigationConnection connection) |
| | | 484 | | { |
| | 599 | 485 | | if (!string.Equals(connection.Destination.MapId, MapId, StringComparison.Ordinal)) |
| | 186 | 486 | | return false; |
| | | 487 | | |
| | 1148 | 488 | | for (int i = 0; i < connection.Witnesses.Count; i++) |
| | | 489 | | { |
| | 163 | 490 | | if (!string.Equals(connection.Witnesses[i].MapId, MapId, StringComparison.Ordinal)) |
| | 2 | 491 | | return false; |
| | | 492 | | } |
| | | 493 | | |
| | 411 | 494 | | return true; |
| | | 495 | | } |
| | | 496 | | |
| | | 497 | | private int GetMaximumLocalPrismCount(NavigationConnection[] connections) |
| | | 498 | | { |
| | 2056 | 499 | | int maximumPrismCount = 0; |
| | 4718 | 500 | | for (int i = 0; i < connections.Length; i++) |
| | | 501 | | { |
| | 303 | 502 | | if (!IsCompleteLocalChain(connections[i])) |
| | | 503 | | continue; |
| | | 504 | | |
| | 209 | 505 | | int prismCount = connections[i].Witnesses.Count + 2; |
| | 209 | 506 | | if (prismCount > maximumPrismCount) |
| | 56 | 507 | | maximumPrismCount = prismCount; |
| | | 508 | | } |
| | | 509 | | |
| | 2056 | 510 | | return maximumPrismCount; |
| | | 511 | | } |
| | | 512 | | |
| | | 513 | | private static int GetMaximumWitnessCount(NavigationConnection[] connections) |
| | | 514 | | { |
| | 2056 | 515 | | int maximumWitnessCount = 0; |
| | 4718 | 516 | | for (int i = 0; i < connections.Length; i++) |
| | | 517 | | { |
| | 303 | 518 | | if (connections[i].Witnesses.Count > maximumWitnessCount) |
| | 38 | 519 | | maximumWitnessCount = connections[i].Witnesses.Count; |
| | | 520 | | } |
| | | 521 | | |
| | 2056 | 522 | | return maximumWitnessCount; |
| | | 523 | | } |
| | | 524 | | |
| | | 525 | | private static bool TryProveLowerBound( |
| | | 526 | | in GridCellPrism source, |
| | | 527 | | in GridCellPrism destination, |
| | | 528 | | NavigationConnection connection, |
| | | 529 | | Fixed64 corridorCost, |
| | | 530 | | Fixed64 destinationEnterCost) |
| | | 531 | | { |
| | 2 | 532 | | Vector3d sourceAnchor = GetFootAnchor(source); |
| | 2 | 533 | | Vector3d destinationAnchor = GetFootAnchor(destination); |
| | 2 | 534 | | return Vector3d.TryGetDistance(sourceAnchor, connection.EntryAnchor, out Fixed64 approachCost) |
| | 2 | 535 | | && Vector3d.TryGetDistance(connection.ExitAnchor, destinationAnchor, out Fixed64 departureCost) |
| | 2 | 536 | | && Fixed64.TryAdd(approachCost, corridorCost, out Fixed64 traversalCost) |
| | 2 | 537 | | && Fixed64.TryAdd(traversalCost, departureCost, out traversalCost) |
| | 2 | 538 | | && Fixed64.TryAdd(traversalCost, connection.AdditionalCost, out traversalCost) |
| | 2 | 539 | | && Fixed64.TryAdd(traversalCost, destinationEnterCost, out traversalCost) |
| | 2 | 540 | | && NavigationDistanceMath.TryCeiling( |
| | 2 | 541 | | sourceAnchor, |
| | 2 | 542 | | destinationAnchor, |
| | 2 | 543 | | out Fixed64 directCost) |
| | 2 | 544 | | && traversalCost >= directCost; |
| | | 545 | | } |
| | | 546 | | |
| | | 547 | | private static Vector3d GetFootAnchor(in GridCellPrism prism) => |
| | 4 | 548 | | new(prism.Center.X, prism.VerticalMin, prism.Center.Z); |
| | | 549 | | |
| | | 550 | | private void ValidateLocalAnchor( |
| | | 551 | | Vector3d anchor, |
| | | 552 | | VoxelIndex index, |
| | | 553 | | string parameterName, |
| | | 554 | | string message) |
| | | 555 | | { |
| | 522 | 556 | | bool representable = GridBinding.TryGetCellPrism(index, out GridCellPrism prism); |
| | | 557 | | System.Diagnostics.Debug.Assert(representable); |
| | 522 | 558 | | SwiftThrowHelper.ThrowIfArgument(!prism.Contains(anchor), parameterName, message); |
| | 519 | 559 | | } |
| | | 560 | | |
| | | 561 | | private static void ValidateClearance( |
| | | 562 | | NavigationCell cell, |
| | | 563 | | Fixed64 radius, |
| | | 564 | | Fixed64 height, |
| | | 565 | | string parameterName, |
| | | 566 | | string message) => |
| | 586 | 567 | | SwiftThrowHelper.ThrowIfArgument( |
| | 586 | 568 | | radius > cell.RadiusClearance || height > cell.HeightClearance, |
| | 586 | 569 | | parameterName, |
| | 586 | 570 | | message); |
| | | 571 | | |
| | | 572 | | private static void ValidateCellPayload(NavigationCell cell, string parameterName) |
| | | 573 | | { |
| | 23207 | 574 | | SwiftThrowHelper.ThrowIfArgument( |
| | 23207 | 575 | | cell.Media == TraversalMedia.None, |
| | 23207 | 576 | | parameterName, |
| | 23207 | 577 | | "Cell media must contain at least one known traversal-medium bit."); |
| | | 578 | | |
| | | 579 | | System.Diagnostics.Debug.Assert((cell.Media & ~NavigationCell.KnownMedia) == 0); |
| | | 580 | | System.Diagnostics.Debug.Assert((cell.RequiredCapabilities & ~NavigationCell.KnownCapabilities) == 0); |
| | | 581 | | System.Diagnostics.Debug.Assert(cell.EnterCost >= Fixed64.Zero); |
| | | 582 | | System.Diagnostics.Debug.Assert(cell.RadiusClearance >= Fixed64.Zero); |
| | | 583 | | System.Diagnostics.Debug.Assert(cell.HeightClearance >= Fixed64.Zero); |
| | | 584 | | System.Diagnostics.Debug.Assert((cell.Flags & ~NavigationCell.KnownFlags) == 0); |
| | 23205 | 585 | | } |
| | | 586 | | |
| | | 587 | | private static bool SupportsMedium(NavigationCell cell, TraversalMedium medium) |
| | | 588 | | { |
| | | 589 | | System.Diagnostics.Debug.Assert(NavigationCell.IsKnownMedium(medium)); |
| | 332 | 590 | | TraversalMedia requested = medium == TraversalMedium.Solid |
| | 332 | 591 | | ? TraversalMedia.Solid |
| | 332 | 592 | | : medium == TraversalMedium.Gas |
| | 332 | 593 | | ? TraversalMedia.Gas |
| | 332 | 594 | | : TraversalMedia.Liquid; |
| | 332 | 595 | | return (cell.Media & requested) != 0; |
| | | 596 | | } |
| | | 597 | | |
| | | 598 | | private bool TryGetAuthoredCell( |
| | | 599 | | NavigationCellEntry[] cells, |
| | | 600 | | VoxelIndex index, |
| | | 601 | | out NavigationCell cell) |
| | | 602 | | { |
| | 926 | 603 | | int ordinal = FindCell(cells, index); |
| | 926 | 604 | | if (ordinal >= 0) |
| | | 605 | | { |
| | 913 | 606 | | cell = cells[ordinal].Cell; |
| | 913 | 607 | | return true; |
| | | 608 | | } |
| | 13 | 609 | | if (_defaultCell.HasValue && GridBinding.IsValidIndex(index)) |
| | | 610 | | { |
| | 8 | 611 | | cell = _defaultCell.Value; |
| | 8 | 612 | | return true; |
| | | 613 | | } |
| | | 614 | | |
| | 5 | 615 | | cell = default; |
| | 5 | 616 | | return false; |
| | | 617 | | } |
| | | 618 | | |
| | | 619 | | private static int FindCell(NavigationCellEntry[] cells, VoxelIndex index) |
| | | 620 | | { |
| | 926 | 621 | | int low = 0; |
| | 926 | 622 | | int high = cells.Length - 1; |
| | 1770 | 623 | | while (low <= high) |
| | | 624 | | { |
| | 1757 | 625 | | int middle = low + ((high - low) >> 1); |
| | 1757 | 626 | | int comparison = cells[middle].Index.CompareTo(index); |
| | 1757 | 627 | | if (comparison == 0) |
| | 913 | 628 | | return middle; |
| | 844 | 629 | | if (comparison < 0) |
| | 453 | 630 | | low = middle + 1; |
| | | 631 | | else |
| | 391 | 632 | | high = middle - 1; |
| | | 633 | | } |
| | | 634 | | |
| | 13 | 635 | | return -1; |
| | | 636 | | } |
| | | 637 | | |
| | | 638 | | private sealed class NavigationCellEntryComparer : IComparer<NavigationCellEntry> |
| | | 639 | | { |
| | 1 | 640 | | internal static readonly NavigationCellEntryComparer Instance = new(); |
| | | 641 | | |
| | | 642 | | public int Compare(NavigationCellEntry left, NavigationCellEntry right) => |
| | 65902 | 643 | | left.Index.CompareTo(right.Index); |
| | | 644 | | } |
| | | 645 | | |
| | | 646 | | private sealed class NavigationConnectionComparer : IComparer<NavigationConnection> |
| | | 647 | | { |
| | 1 | 648 | | internal static readonly NavigationConnectionComparer Instance = new(); |
| | | 649 | | |
| | | 650 | | public int Compare(NavigationConnection? left, NavigationConnection? right) => |
| | 359 | 651 | | StringComparer.Ordinal.Compare(left!.Id, right!.Id); |
| | | 652 | | } |
| | | 653 | | |
| | | 654 | | private sealed class TraversalTransitionDefinitionComparer : IComparer<TraversalTransitionDefinition> |
| | | 655 | | { |
| | 1 | 656 | | internal static readonly TraversalTransitionDefinitionComparer Instance = new(); |
| | | 657 | | |
| | | 658 | | public int Compare( |
| | | 659 | | TraversalTransitionDefinition left, |
| | | 660 | | TraversalTransitionDefinition right) => |
| | 135 | 661 | | StringComparer.Ordinal.Compare(left.Id, right.Id); |
| | | 662 | | } |
| | | 663 | | } |