| | | 1 | | //======================================================================= |
| | | 2 | | // NavigationMapInstance.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 GridForge.Grids; |
| | | 10 | | using GridForge.Spatial; |
| | | 11 | | |
| | | 12 | | namespace Trailblazer.Pathing; |
| | | 13 | | |
| | | 14 | | /// <summary>Stores one immutable composed map instance without retaining GridForge voxels.</summary> |
| | | 15 | | internal sealed partial class NavigationMapInstance |
| | | 16 | | { |
| | | 17 | | private const int DynamicSlotBase = 1_000_000_000; |
| | | 18 | | private readonly PersistentVoxelIndexMap<NavigationDynamicCellSlot> _dynamicSlots; |
| | | 19 | | private readonly PersistentVoxelIndexMap<byte> _dynamicAddresses; |
| | | 20 | | private readonly PersistentIntMap<VoxelIndex> _dynamicSlotIndexes; |
| | | 21 | | private readonly int _nextDynamicSlot; |
| | | 22 | | private readonly PersistentIntMap<NavigationSemanticPage> _semanticPages; |
| | | 23 | | private readonly PersistentIntMap<NavigationPhysicalPage> _physicalPages; |
| | | 24 | | private readonly PersistentIntMap<ulong> _dynamicBaselineCapturedChangeSequences; |
| | | 25 | | private readonly NavigationBakedCellLookup _bakedLookup; |
| | | 26 | | private readonly long _preparedMapRetainedBytes; |
| | | 27 | | |
| | 3973 | 28 | | private NavigationMapInstance( |
| | 3973 | 29 | | NavigationMap map, |
| | 3973 | 30 | | long bakeVersion, |
| | 3973 | 31 | | NavigationMapOverlayState overlay, |
| | 3973 | 32 | | long dynamicSlotGeneration, |
| | 3973 | 33 | | PersistentVoxelIndexMap<NavigationDynamicCellSlot> dynamicSlots, |
| | 3973 | 34 | | PersistentIntMap<VoxelIndex> dynamicSlotIndexes, |
| | 3973 | 35 | | int nextDynamicSlot, |
| | 3973 | 36 | | PersistentIntMap<NavigationSemanticPage> semanticPages, |
| | 3973 | 37 | | PersistentIntMap<NavigationPhysicalPage> physicalPages, |
| | 3973 | 38 | | PersistentIntMap<ulong> dynamicBaselineCapturedChangeSequences, |
| | 3973 | 39 | | NavigationBakedCellLookup bakedLookup, |
| | 3973 | 40 | | long preparedMapRetainedBytes, |
| | 3973 | 41 | | NavigationGridGenerationIdentity gridIdentity, |
| | 3973 | 42 | | ulong baselineCapturedChangeSequence, |
| | 3973 | 43 | | ulong gridLastChangeSequence, |
| | 3973 | 44 | | long instanceVersion, |
| | 3973 | 45 | | long semanticVersion, |
| | 3973 | 46 | | long physicalVersion, |
| | 3973 | 47 | | int lastBaselineAddressCount, |
| | 3973 | 48 | | int lastCopiedSemanticPages, |
| | 3973 | 49 | | int lastCopiedPhysicalPages, |
| | 3973 | 50 | | PersistentVoxelIndexMap<byte> dynamicAddresses) |
| | | 51 | | { |
| | 3973 | 52 | | Map = map; |
| | 3973 | 53 | | BakeVersion = bakeVersion; |
| | 3973 | 54 | | Overlay = overlay; |
| | 3973 | 55 | | DynamicSlotGeneration = dynamicSlotGeneration; |
| | 3973 | 56 | | _dynamicSlots = dynamicSlots; |
| | 3973 | 57 | | _dynamicAddresses = dynamicAddresses; |
| | 3973 | 58 | | _dynamicSlotIndexes = dynamicSlotIndexes; |
| | 3973 | 59 | | _nextDynamicSlot = nextDynamicSlot; |
| | 3973 | 60 | | _semanticPages = semanticPages; |
| | 3973 | 61 | | _physicalPages = physicalPages; |
| | 3973 | 62 | | _dynamicBaselineCapturedChangeSequences = dynamicBaselineCapturedChangeSequences; |
| | 3973 | 63 | | _bakedLookup = bakedLookup; |
| | 3973 | 64 | | _preparedMapRetainedBytes = preparedMapRetainedBytes; |
| | 3973 | 65 | | GridIdentity = gridIdentity; |
| | 3973 | 66 | | BaselineCapturedChangeSequence = baselineCapturedChangeSequence; |
| | 3973 | 67 | | GridLastChangeSequence = gridLastChangeSequence; |
| | 3973 | 68 | | InstanceVersion = instanceVersion; |
| | 3973 | 69 | | SemanticVersion = semanticVersion; |
| | 3973 | 70 | | PhysicalVersion = physicalVersion; |
| | 3973 | 71 | | LastBaselineAddressCount = lastBaselineAddressCount; |
| | 3973 | 72 | | LastCopiedSemanticPages = lastCopiedSemanticPages; |
| | 3973 | 73 | | LastCopiedPhysicalPages = lastCopiedPhysicalPages; |
| | 3973 | 74 | | RetainedBytes = EstimateRetainedBytes(); |
| | 3973 | 75 | | } |
| | | 76 | | |
| | | 77 | | internal NavigationMap Map { get; } |
| | | 78 | | |
| | 5337047 | 79 | | internal string MapId => Map.MapId; |
| | | 80 | | |
| | | 81 | | internal long BakeVersion { get; } |
| | | 82 | | |
| | | 83 | | internal NavigationMapOverlayState Overlay { get; } |
| | | 84 | | |
| | | 85 | | internal long DynamicSlotGeneration { get; } |
| | | 86 | | |
| | | 87 | | internal NavigationGridGenerationIdentity GridIdentity { get; } |
| | | 88 | | |
| | | 89 | | internal ulong BaselineCapturedChangeSequence { get; } |
| | | 90 | | |
| | | 91 | | internal ulong GridLastChangeSequence { get; } |
| | | 92 | | |
| | | 93 | | internal long InstanceVersion { get; } |
| | | 94 | | |
| | | 95 | | internal long SemanticVersion { get; } |
| | | 96 | | |
| | | 97 | | internal long PhysicalVersion { get; } |
| | | 98 | | |
| | | 99 | | internal int LastBaselineAddressCount { get; } |
| | | 100 | | |
| | | 101 | | internal int LastCopiedSemanticPages { get; } |
| | | 102 | | |
| | | 103 | | internal int LastCopiedPhysicalPages { get; } |
| | | 104 | | |
| | 20549 | 105 | | internal bool IsMaterialized => GridIdentity.IsValid; |
| | | 106 | | |
| | 8679314 | 107 | | internal int BakedSlotCount => Map.CellSpan.Length; |
| | | 108 | | |
| | 2271621 | 109 | | internal int DynamicSlotCount => _dynamicSlots.Count; |
| | | 110 | | |
| | 23 | 111 | | internal PersistentVoxelIndexMap<byte> DynamicAddresses => _dynamicAddresses; |
| | | 112 | | |
| | 2191071 | 113 | | internal int AddressCount => BakedSlotCount + DynamicSlotCount; |
| | | 114 | | |
| | | 115 | | internal long RetainedBytes { get; } |
| | | 116 | | |
| | 1 | 117 | | internal long PreparedMapRetainedBytes => _preparedMapRetainedBytes; |
| | | 118 | | |
| | 23 | 119 | | internal NavigationBakedCellLookup BakedCellLookup => _bakedLookup; |
| | | 120 | | |
| | 14635 | 121 | | internal NavigationCellLookupKind LookupKind => _bakedLookup.Kind; |
| | | 122 | | |
| | 4434 | 123 | | internal int PersistentPageCount => 6 |
| | 4434 | 124 | | + Overlay.PersistentNodeCount |
| | 4434 | 125 | | + _dynamicSlots.PersistentNodeCount |
| | 4434 | 126 | | + _dynamicAddresses.PersistentNodeCount |
| | 4434 | 127 | | + _dynamicSlotIndexes.PersistentNodeCount |
| | 4434 | 128 | | + (_semanticPages.PersistentNodeCount * 2) |
| | 4434 | 129 | | + (_physicalPages.PersistentNodeCount * 2) |
| | 4434 | 130 | | + _dynamicBaselineCapturedChangeSequences.PersistentNodeCount; |
| | | 131 | | |
| | | 132 | | internal bool TryGetSlot(VoxelIndex index, out int slot) |
| | | 133 | | { |
| | 4515871 | 134 | | int baked = _bakedLookup.Find(index); |
| | 4515871 | 135 | | if (baked >= 0) |
| | | 136 | | { |
| | 1157711 | 137 | | slot = baked; |
| | 1157711 | 138 | | return true; |
| | | 139 | | } |
| | | 140 | | |
| | 3358160 | 141 | | if (_dynamicSlots.TryGetValue(index, out NavigationDynamicCellSlot dynamicSlot)) |
| | | 142 | | { |
| | 3125613 | 143 | | slot = dynamicSlot.Slot; |
| | 3125613 | 144 | | return true; |
| | | 145 | | } |
| | | 146 | | |
| | 232547 | 147 | | slot = -1; |
| | 232547 | 148 | | return false; |
| | | 149 | | } |
| | | 150 | | |
| | | 151 | | internal VoxelIndex GetSlotIndex(int slot) |
| | | 152 | | { |
| | 2145039 | 153 | | bool found = TryGetSlotIndex(slot, out VoxelIndex index); |
| | | 154 | | System.Diagnostics.Debug.Assert(found, "Diagnostics enumerate only addressed slots."); |
| | 2145039 | 155 | | return index; |
| | | 156 | | } |
| | | 157 | | |
| | | 158 | | internal bool TryGetEffectiveCell(int slot, out NavigationCell cell) |
| | | 159 | | { |
| | 7072314 | 160 | | NavigationSemanticPage? page = FindSemanticPage(slot / NavigationSemanticPage.SlotCount); |
| | 7072314 | 161 | | int offset = slot % NavigationSemanticPage.SlotCount; |
| | 7072314 | 162 | | if (page != null && page.IsSuppressed[offset]) |
| | | 163 | | { |
| | 299 | 164 | | cell = default; |
| | 299 | 165 | | return false; |
| | | 166 | | } |
| | 7072015 | 167 | | if (page != null && page.HasOverride[offset]) |
| | | 168 | | { |
| | 5359595 | 169 | | cell = page.Cells[offset]; |
| | 5359595 | 170 | | return true; |
| | | 171 | | } |
| | 1712420 | 172 | | if ((uint)slot < (uint)Map.CellSpan.Length) |
| | | 173 | | { |
| | 1709609 | 174 | | cell = Map.CellSpan[slot].Cell; |
| | 1709609 | 175 | | return true; |
| | | 176 | | } |
| | | 177 | | |
| | 2811 | 178 | | if (Map.DefaultCell.HasValue) |
| | | 179 | | { |
| | 2515 | 180 | | cell = Map.DefaultCell.Value; |
| | 2515 | 181 | | return true; |
| | | 182 | | } |
| | | 183 | | |
| | 296 | 184 | | cell = default; |
| | 296 | 185 | | return false; |
| | | 186 | | } |
| | | 187 | | |
| | | 188 | | internal bool TryGetSemanticState( |
| | | 189 | | VoxelIndex index, |
| | | 190 | | out NavigationCellSemanticSource source, |
| | | 191 | | out bool hasCell, |
| | | 192 | | out NavigationCell cell) |
| | | 193 | | { |
| | 113972 | 194 | | if (!TryGetSlot(index, out int slot)) |
| | | 195 | | { |
| | 98775 | 196 | | source = default; |
| | 98775 | 197 | | hasCell = false; |
| | 98775 | 198 | | cell = default; |
| | 98775 | 199 | | return false; |
| | | 200 | | } |
| | 15197 | 201 | | source = GetSemanticSource(slot); |
| | 15197 | 202 | | hasCell = TryGetEffectiveCell(slot, out cell); |
| | 15197 | 203 | | return true; |
| | | 204 | | } |
| | | 205 | | |
| | | 206 | | internal bool TryGetPhysicalState(int slot, out bool isPresent, out byte obstacleCount) |
| | | 207 | | { |
| | 3110148 | 208 | | NavigationPhysicalPage? page = FindPhysicalPage(slot / NavigationPhysicalPage.SlotCount); |
| | 3110148 | 209 | | if (page == null) |
| | | 210 | | { |
| | 2131032 | 211 | | isPresent = false; |
| | 2131032 | 212 | | obstacleCount = 0; |
| | 2131032 | 213 | | return false; |
| | | 214 | | } |
| | | 215 | | |
| | 979116 | 216 | | int offset = slot % NavigationPhysicalPage.SlotCount; |
| | 979116 | 217 | | isPresent = page.IsPresent[offset]; |
| | 979116 | 218 | | obstacleCount = page.ObstacleCounts[offset]; |
| | 979116 | 219 | | return true; |
| | | 220 | | } |
| | | 221 | | |
| | | 222 | | internal TraversalMedia GetEffectiveMedia(VoxelIndex index) => |
| | 5023 | 223 | | TryGetSlot(index, out int slot) |
| | 5023 | 224 | | && TryGetEffectiveCell(slot, out NavigationCell cell) |
| | 5023 | 225 | | ? cell.Media |
| | 5023 | 226 | | : TraversalMedia.None; |
| | | 227 | | |
| | | 228 | | internal bool IsPhysicallyPresent(VoxelIndex index) => |
| | 840 | 229 | | TryGetSlot(index, out int slot) |
| | 840 | 230 | | && TryGetPhysicalState(slot, out bool isPresent, out _) |
| | 840 | 231 | | && isPresent; |
| | | 232 | | |
| | | 233 | | internal GraphPageDependency GetPageDependency(int pageIndex, long transitionVersion = 0) |
| | | 234 | | { |
| | 32909 | 235 | | _semanticPages.TryGetValue(pageIndex, out NavigationSemanticPage? semantic); |
| | 32909 | 236 | | _physicalPages.TryGetValue(pageIndex, out NavigationPhysicalPage? physical); |
| | 32909 | 237 | | return new GraphPageDependency( |
| | 32909 | 238 | | MapId, |
| | 32909 | 239 | | BakeVersion, |
| | 32909 | 240 | | DynamicSlotGeneration, |
| | 32909 | 241 | | pageIndex, |
| | 32909 | 242 | | semantic?.Version ?? 0, |
| | 32909 | 243 | | physical?.Version ?? 0, |
| | 32909 | 244 | | transitionVersion); |
| | | 245 | | } |
| | | 246 | | |
| | | 247 | | internal NavigationMapInstance Apply( |
| | | 248 | | long worldSpawnToken, |
| | | 249 | | in GridEventInfo eventInfo, |
| | | 250 | | long instanceVersion) |
| | | 251 | | { |
| | 33 | 252 | | if (eventInfo.WorldSpawnToken != worldSpawnToken) |
| | 1 | 253 | | return this; |
| | | 254 | | |
| | 32 | 255 | | if (eventInfo.ChangeKind == GridEventKind.WorldReset) |
| | 1 | 256 | | return MakeDormant(instanceVersion); |
| | | 257 | | |
| | 31 | 258 | | if (eventInfo.ChangeKind == GridEventKind.GridAdded |
| | 31 | 259 | | || eventInfo.ChangeKind == GridEventKind.GridChanged) |
| | | 260 | | { |
| | 3 | 261 | | if (!eventInfo.Configuration.ToGridKey().Equals(Map.GridBinding.Key)) |
| | 1 | 262 | | return this; |
| | 2 | 263 | | return MakeDormant(instanceVersion); |
| | | 264 | | } |
| | | 265 | | |
| | 28 | 266 | | if (!GridIdentity.Matches( |
| | 28 | 267 | | eventInfo.WorldSpawnToken, |
| | 28 | 268 | | eventInfo.GridIndex, |
| | 28 | 269 | | eventInfo.GridSpawnToken)) |
| | | 270 | | { |
| | 10 | 271 | | return this; |
| | | 272 | | } |
| | | 273 | | |
| | 18 | 274 | | if (eventInfo.ChangeKind == GridEventKind.GridRemoved) |
| | 11 | 275 | | return MakeDormant(instanceVersion); |
| | 7 | 276 | | if (!eventInfo.HasVoxelState |
| | 7 | 277 | | || !TryGetSlot(eventInfo.VoxelIndex, out int slot) |
| | 7 | 278 | | || eventInfo.ChangeSequence <= GetBaselineCapturedChangeSequence(slot)) |
| | | 279 | | { |
| | 4 | 280 | | return WithGridLastChangeSequence(eventInfo.ChangeSequence, instanceVersion); |
| | | 281 | | } |
| | | 282 | | |
| | 3 | 283 | | return WithPhysicalState( |
| | 3 | 284 | | slot, |
| | 3 | 285 | | eventInfo.IsVoxelPresent, |
| | 3 | 286 | | eventInfo.ObstacleCount, |
| | 3 | 287 | | instanceVersion, |
| | 3 | 288 | | eventInfo.ChangeSequence); |
| | | 289 | | } |
| | | 290 | | |
| | | 291 | | internal NavigationMapInstance ApplyBatch( |
| | | 292 | | long worldSpawnToken, |
| | | 293 | | ReadOnlySpan<GridEventInfo> events, |
| | | 294 | | bool resnapshotAll, |
| | | 295 | | long instanceVersion) |
| | | 296 | | { |
| | 263 | 297 | | NavigationMapInstance next = resnapshotAll ? MakeDormant(instanceVersion) : this; |
| | 263 | 298 | | int segmentStart = 0; |
| | 740 | 299 | | for (int i = 0; i < events.Length; i++) |
| | | 300 | | { |
| | 107 | 301 | | if (!IsBroad(events[i].ChangeKind)) |
| | | 302 | | continue; |
| | 20 | 303 | | next = next.ApplyPhysicalSegment(events.Slice(segmentStart, i - segmentStart), instanceVersion); |
| | 20 | 304 | | next = next.Apply(worldSpawnToken, events[i], instanceVersion); |
| | 20 | 305 | | segmentStart = i + 1; |
| | | 306 | | } |
| | 263 | 307 | | return next.ApplyPhysicalSegment(events.Slice(segmentStart), instanceVersion); |
| | | 308 | | } |
| | | 309 | | |
| | | 310 | | internal NavigationMapInstance Materialize( |
| | | 311 | | in NavigationGridBaselineCapture capture, |
| | | 312 | | long instanceVersion) |
| | | 313 | | { |
| | 1833 | 314 | | GridNavigationBaseline? baseline = capture.Baseline; |
| | 1833 | 315 | | if (!capture.IsRequested |
| | 1833 | 316 | | || !capture.HasBaseline) |
| | | 317 | | { |
| | 226 | 318 | | return MakeDormant(instanceVersion); |
| | | 319 | | } |
| | | 320 | | |
| | 1607 | 321 | | NavigationMapInstance materializing = capture.PreparedInstance ?? this; |
| | 1607 | 322 | | PersistentIntMap<NavigationPhysicalPage> pages = capture.PreparedPages |
| | 1607 | 323 | | ?? (baseline == null |
| | 1607 | 324 | | ? materializing._physicalPages |
| | 1607 | 325 | | : materializing.BuildPhysicalPages(baseline.VoxelStates)); |
| | 1607 | 326 | | return materializing.CreateMaterialized( |
| | 1607 | 327 | | capture, |
| | 1607 | 328 | | pages, |
| | 1607 | 329 | | materializing.BakedSlotCount + materializing.DynamicSlotCount, |
| | 1607 | 330 | | instanceVersion); |
| | | 331 | | } |
| | | 332 | | |
| | | 333 | | internal NavigationMapInstance MaterializeDelta( |
| | | 334 | | NavigationMapInstance previous, |
| | | 335 | | in NavigationGridBaselineCapture capture, |
| | | 336 | | long instanceVersion) |
| | | 337 | | { |
| | 122 | 338 | | GridNavigationBaseline? baseline = capture.Baseline; |
| | 122 | 339 | | if (!capture.IsRequested |
| | 122 | 340 | | || !capture.IsDelta) |
| | | 341 | | { |
| | 86 | 342 | | return Materialize(capture, instanceVersion); |
| | | 343 | | } |
| | 36 | 344 | | if (baseline == null) |
| | | 345 | | { |
| | 1 | 346 | | return MakeDormant(instanceVersion); |
| | | 347 | | } |
| | | 348 | | |
| | 35 | 349 | | PersistentIntMap<NavigationPhysicalPage> pages = previous._physicalPages; |
| | 35 | 350 | | PersistentIntMap<ulong> dynamicCapturedChangeSequences = |
| | 35 | 351 | | _dynamicBaselineCapturedChangeSequences; |
| | 35 | 352 | | var capturedAddresses = new VoxelIndex[capture.AddressCount]; |
| | 142 | 353 | | for (int i = 0; i < capture.AddressCount; i++) |
| | | 354 | | { |
| | 36 | 355 | | VoxelIndex address = baseline.VoxelStates[i].VoxelIndex; |
| | | 356 | | System.Diagnostics.Debug.Assert(!previous.TryGetSlot(address, out _)); |
| | 36 | 357 | | bool found = TryGetSlot(address, out int slot); |
| | | 358 | | System.Diagnostics.Debug.Assert(found); |
| | | 359 | | System.Diagnostics.Debug.Assert(slot >= DynamicSlotBase); |
| | 36 | 360 | | NavigationBaselineVoxelState physical = baseline.VoxelStates[i]; |
| | 36 | 361 | | pages = ApplyPhysicalState( |
| | 36 | 362 | | pages, |
| | 36 | 363 | | slot, |
| | 36 | 364 | | physical.IsPresent, |
| | 36 | 365 | | physical.ObstacleCount, |
| | 36 | 366 | | instanceVersion); |
| | 36 | 367 | | dynamicCapturedChangeSequences = dynamicCapturedChangeSequences.Set( |
| | 36 | 368 | | slot, |
| | 36 | 369 | | baseline.CapturedChangeSequence); |
| | 36 | 370 | | capturedAddresses[i] = address; |
| | | 371 | | } |
| | | 372 | | System.Diagnostics.Debug.Assert(capture.AddressCount > 0); |
| | | 373 | | |
| | 35 | 374 | | return new NavigationMapInstance( |
| | 35 | 375 | | Map, |
| | 35 | 376 | | BakeVersion, |
| | 35 | 377 | | Overlay, |
| | 35 | 378 | | DynamicSlotGeneration, |
| | 35 | 379 | | _dynamicSlots, |
| | 35 | 380 | | _dynamicSlotIndexes, |
| | 35 | 381 | | _nextDynamicSlot, |
| | 35 | 382 | | _semanticPages, |
| | 35 | 383 | | pages, |
| | 35 | 384 | | dynamicCapturedChangeSequences, |
| | 35 | 385 | | _bakedLookup, |
| | 35 | 386 | | _preparedMapRetainedBytes, |
| | 35 | 387 | | new NavigationGridGenerationIdentity( |
| | 35 | 388 | | baseline.WorldSpawnToken, |
| | 35 | 389 | | baseline.GridIndex, |
| | 35 | 390 | | baseline.GridSpawnToken, |
| | 35 | 391 | | baseline.ConfigurationKey), |
| | 35 | 392 | | previous.BaselineCapturedChangeSequence, |
| | 35 | 393 | | baseline.GridLastChangeSequence, |
| | 35 | 394 | | instanceVersion, |
| | 35 | 395 | | SemanticVersion, |
| | 35 | 396 | | physicalVersion: instanceVersion, |
| | 35 | 397 | | lastBaselineAddressCount: capture.AddressCount, |
| | 35 | 398 | | lastCopiedSemanticPages: LastCopiedSemanticPages, |
| | 35 | 399 | | lastCopiedPhysicalPages: CountTouchedPages( |
| | 35 | 400 | | capturedAddresses, |
| | 35 | 401 | | capture.AddressCount, |
| | 35 | 402 | | _dynamicSlots), |
| | 35 | 403 | | dynamicAddresses: _dynamicAddresses); |
| | | 404 | | } |
| | | 405 | | |
| | | 406 | | internal NavigationMapInstance FailClosed(long instanceVersion) => |
| | 44 | 407 | | MakeDormant(instanceVersion); |
| | | 408 | | |
| | | 409 | | internal NavigationGraphMapDiagnostic CreateDiagnostic( |
| | | 410 | | int maximumCells, |
| | | 411 | | int componentId, |
| | | 412 | | long componentVersion, |
| | | 413 | | int incidentEdgeCount, |
| | | 414 | | out bool truncated) |
| | | 415 | | { |
| | 14635 | 416 | | int addressedCount = BakedSlotCount + DynamicSlotCount; |
| | 14635 | 417 | | int count = Math.Min(addressedCount, maximumCells); |
| | 14635 | 418 | | truncated = count < addressedCount; |
| | 14635 | 419 | | var cells = new NavigationGraphCellDiagnostic[count]; |
| | 4319348 | 420 | | for (int ordinal = 0; ordinal < count; ordinal++) |
| | | 421 | | { |
| | 2145039 | 422 | | int slot = ordinal < BakedSlotCount |
| | 2145039 | 423 | | ? ordinal |
| | 2145039 | 424 | | : _dynamicSlots.GetValueAt(ordinal - BakedSlotCount).Slot; |
| | 2145039 | 425 | | bool hasCell = TryGetEffectiveCell(slot, out NavigationCell cell); |
| | 2145039 | 426 | | TryGetPhysicalState(slot, out bool isPresent, out byte obstacleCount); |
| | 2145039 | 427 | | cells[ordinal] = new NavigationGraphCellDiagnostic( |
| | 2145039 | 428 | | GetSlotIndex(slot), |
| | 2145039 | 429 | | slot, |
| | 2145039 | 430 | | GetSemanticSource(slot), |
| | 2145039 | 431 | | hasCell, |
| | 2145039 | 432 | | cell, |
| | 2145039 | 433 | | isPresent, |
| | 2145039 | 434 | | obstacleCount); |
| | | 435 | | } |
| | | 436 | | |
| | 14635 | 437 | | return new NavigationGraphMapDiagnostic( |
| | 14635 | 438 | | MapId, |
| | 14635 | 439 | | BakeVersion, |
| | 14635 | 440 | | InstanceVersion, |
| | 14635 | 441 | | Overlay.HighWaterSequence, |
| | 14635 | 442 | | PhysicalVersion, |
| | 14635 | 443 | | componentId, |
| | 14635 | 444 | | componentVersion, |
| | 14635 | 445 | | incidentEdgeCount, |
| | 14635 | 446 | | IsMaterialized, |
| | 14635 | 447 | | GridIdentity.WorldSpawnToken, |
| | 14635 | 448 | | GridIdentity.GridSpawnToken, |
| | 14635 | 449 | | Map.GridBinding.Key, |
| | 14635 | 450 | | Map.GridBinding.Configuration.TopologyKind, |
| | 14635 | 451 | | Map.GridBinding.Configuration.StorageKind, |
| | 14635 | 452 | | LookupKind, |
| | 14635 | 453 | | BakedSlotCount, |
| | 14635 | 454 | | DynamicSlotCount, |
| | 14635 | 455 | | RetainedBytes, |
| | 14635 | 456 | | LastBaselineAddressCount, |
| | 14635 | 457 | | LastCopiedSemanticPages, |
| | 14635 | 458 | | LastCopiedPhysicalPages, |
| | 14635 | 459 | | cells); |
| | | 460 | | } |
| | | 461 | | |
| | | 462 | | private NavigationMapInstance CreateMaterialized( |
| | | 463 | | in NavigationGridBaselineCapture capture, |
| | | 464 | | PersistentIntMap<NavigationPhysicalPage> pages, |
| | | 465 | | int addressCount, |
| | | 466 | | long instanceVersion) => |
| | 1607 | 467 | | new NavigationMapInstance( |
| | 1607 | 468 | | Map, |
| | 1607 | 469 | | BakeVersion, |
| | 1607 | 470 | | Overlay, |
| | 1607 | 471 | | DynamicSlotGeneration, |
| | 1607 | 472 | | _dynamicSlots, |
| | 1607 | 473 | | _dynamicSlotIndexes, |
| | 1607 | 474 | | _nextDynamicSlot, |
| | 1607 | 475 | | _semanticPages, |
| | 1607 | 476 | | pages, |
| | 1607 | 477 | | FillDynamicBaselineCapturedChangeSequences(capture.CapturedChangeSequence), |
| | 1607 | 478 | | _bakedLookup, |
| | 1607 | 479 | | _preparedMapRetainedBytes, |
| | 1607 | 480 | | new NavigationGridGenerationIdentity( |
| | 1607 | 481 | | capture.WorldSpawnToken, |
| | 1607 | 482 | | capture.GridIndex, |
| | 1607 | 483 | | capture.GridSpawnToken, |
| | 1607 | 484 | | capture.ConfigurationKey), |
| | 1607 | 485 | | capture.CapturedChangeSequence, |
| | 1607 | 486 | | capture.GridLastChangeSequence, |
| | 1607 | 487 | | instanceVersion, |
| | 1607 | 488 | | SemanticVersion, |
| | 1607 | 489 | | physicalVersion: instanceVersion, |
| | 1607 | 490 | | lastBaselineAddressCount: addressCount, |
| | 1607 | 491 | | lastCopiedSemanticPages: 0, |
| | 1607 | 492 | | lastCopiedPhysicalPages: pages.Count, |
| | 1607 | 493 | | dynamicAddresses: _dynamicAddresses); |
| | | 494 | | |
| | | 495 | | internal PersistentIntMap<NavigationPhysicalPage> AppendPhysicalBaselinePages( |
| | | 496 | | PersistentIntMap<NavigationPhysicalPage> pages, |
| | | 497 | | ReadOnlySpan<NavigationBaselineVoxelState> states, |
| | | 498 | | long pageVersion) |
| | | 499 | | { |
| | 28094 | 500 | | for (int i = 0; i < states.Length; i++) |
| | | 501 | | { |
| | 12472 | 502 | | if (!states[i].IsPresent && states[i].ObstacleCount == 0) |
| | | 503 | | continue; |
| | 11947 | 504 | | bool found = TryGetSlot(states[i].VoxelIndex, out int slot); |
| | | 505 | | System.Diagnostics.Debug.Assert(found); |
| | 11947 | 506 | | int pageIndex = slot / NavigationPhysicalPage.SlotCount; |
| | 11947 | 507 | | if (!pages.TryGetValue(pageIndex, out NavigationPhysicalPage? page)) |
| | | 508 | | { |
| | 1611 | 509 | | page = new NavigationPhysicalPage(pageIndex, pageVersion); |
| | 1611 | 510 | | pages = pages.Set(pageIndex, page); |
| | | 511 | | } |
| | 11947 | 512 | | int offset = slot % NavigationPhysicalPage.SlotCount; |
| | 11947 | 513 | | page!.IsPresent[offset] = states[i].IsPresent; |
| | 11947 | 514 | | page.ObstacleCounts[offset] = states[i].ObstacleCount; |
| | | 515 | | } |
| | 1575 | 516 | | return pages; |
| | | 517 | | } |
| | | 518 | | |
| | | 519 | | internal void CopyCanonicalAddressChunk( |
| | | 520 | | ref int bakedCursor, |
| | | 521 | | ref int dynamicCursor, |
| | | 522 | | Span<VoxelIndex> destination) |
| | | 523 | | { |
| | 2168765 | 524 | | ReadOnlySpan<NavigationCellEntry> baked = Map.CellSpan; |
| | 8797682 | 525 | | for (int i = 0; i < destination.Length; i++) |
| | | 526 | | { |
| | 2230076 | 527 | | bool hasBaked = bakedCursor < baked.Length; |
| | 2230076 | 528 | | bool hasDynamic = dynamicCursor < _dynamicSlots.Count; |
| | 2230076 | 529 | | if (!hasDynamic |
| | 2230076 | 530 | | || (hasBaked |
| | 2230076 | 531 | | && baked[bakedCursor].Index.CompareTo( |
| | 2230076 | 532 | | _dynamicSlots.GetKeyAt(dynamicCursor)) < 0)) |
| | | 533 | | { |
| | 49729 | 534 | | destination[i] = baked[bakedCursor++].Index; |
| | | 535 | | } |
| | | 536 | | else |
| | | 537 | | { |
| | 2180347 | 538 | | destination[i] = _dynamicSlots.GetKeyAt(dynamicCursor++); |
| | | 539 | | } |
| | | 540 | | } |
| | 2168765 | 541 | | } |
| | | 542 | | |
| | | 543 | | internal int CopyCanonicalAddresses(Span<VoxelIndex> destination) |
| | | 544 | | { |
| | 1028 | 545 | | int count = AddressCount; |
| | 1028 | 546 | | if (count > destination.Length) |
| | 1 | 547 | | return 0; |
| | 1027 | 548 | | int bakedCursor = 0; |
| | 1027 | 549 | | int dynamicCursor = 0; |
| | 1027 | 550 | | CopyCanonicalAddressChunk( |
| | 1027 | 551 | | ref bakedCursor, |
| | 1027 | 552 | | ref dynamicCursor, |
| | 1027 | 553 | | destination.Slice(0, count)); |
| | 1027 | 554 | | return count; |
| | | 555 | | } |
| | | 556 | | |
| | | 557 | | internal bool TryCopyNewCanonicalAddresses( |
| | | 558 | | NavigationMapInstance previous, |
| | | 559 | | Span<VoxelIndex> destination, |
| | | 560 | | out int count) |
| | | 561 | | { |
| | 36 | 562 | | int bakedCursor = 0; |
| | 36 | 563 | | int dynamicCursor = 0; |
| | 36 | 564 | | count = 0; |
| | 36 | 565 | | Span<VoxelIndex> slot = stackalloc VoxelIndex[1]; |
| | 496 | 566 | | for (int i = 0; i < AddressCount; i++) |
| | | 567 | | { |
| | 213 | 568 | | CopyCanonicalAddressChunk(ref bakedCursor, ref dynamicCursor, slot); |
| | 213 | 569 | | if (previous.TryGetSlot(slot[0], out _)) |
| | | 570 | | continue; |
| | 101 | 571 | | if (count == destination.Length) |
| | 1 | 572 | | return false; |
| | 100 | 573 | | destination[count++] = slot[0]; |
| | | 574 | | } |
| | 35 | 575 | | return true; |
| | | 576 | | } |
| | | 577 | | |
| | | 578 | | private NavigationMapInstance MakeDormant(long instanceVersion) |
| | | 579 | | { |
| | 286 | 580 | | if (!IsMaterialized && _physicalPages.Count == 0) |
| | 235 | 581 | | return this; |
| | 51 | 582 | | return new NavigationMapInstance( |
| | 51 | 583 | | Map, |
| | 51 | 584 | | BakeVersion, |
| | 51 | 585 | | Overlay, |
| | 51 | 586 | | DynamicSlotGeneration, |
| | 51 | 587 | | _dynamicSlots, |
| | 51 | 588 | | _dynamicSlotIndexes, |
| | 51 | 589 | | _nextDynamicSlot, |
| | 51 | 590 | | _semanticPages, |
| | 51 | 591 | | PersistentIntMap<NavigationPhysicalPage>.Empty, |
| | 51 | 592 | | _dynamicBaselineCapturedChangeSequences, |
| | 51 | 593 | | _bakedLookup, |
| | 51 | 594 | | _preparedMapRetainedBytes, |
| | 51 | 595 | | default, |
| | 51 | 596 | | baselineCapturedChangeSequence: 0, |
| | 51 | 597 | | gridLastChangeSequence: 0, |
| | 51 | 598 | | instanceVersion, |
| | 51 | 599 | | SemanticVersion, |
| | 51 | 600 | | physicalVersion: instanceVersion, |
| | 51 | 601 | | lastBaselineAddressCount: 0, |
| | 51 | 602 | | lastCopiedSemanticPages: 0, |
| | 51 | 603 | | lastCopiedPhysicalPages: _physicalPages.Count, |
| | 51 | 604 | | dynamicAddresses: _dynamicAddresses); |
| | | 605 | | } |
| | | 606 | | |
| | | 607 | | internal bool TryGetDefaultBaselineSeedSlot( |
| | | 608 | | int ordinal, |
| | | 609 | | out NavigationDynamicCellSlot slot, |
| | | 610 | | out bool retain) |
| | | 611 | | { |
| | 103 | 612 | | if ((uint)ordinal >= (uint)_dynamicSlots.Count) |
| | | 613 | | { |
| | 2 | 614 | | slot = default; |
| | 2 | 615 | | retain = false; |
| | 2 | 616 | | return false; |
| | | 617 | | } |
| | 101 | 618 | | VoxelIndex index = _dynamicSlots.GetKeyAt(ordinal); |
| | 101 | 619 | | slot = _dynamicSlots.GetValueAt(ordinal); |
| | 101 | 620 | | retain = _dynamicAddresses.TryGetValue(index, out _); |
| | 101 | 621 | | return true; |
| | | 622 | | } |
| | | 623 | | |
| | | 624 | | internal bool TryGetDynamicSlot( |
| | | 625 | | VoxelIndex index, |
| | | 626 | | out NavigationDynamicCellSlot slot) => |
| | 753 | 627 | | _dynamicSlots.TryGetValue(index, out slot); |
| | | 628 | | |
| | | 629 | | internal NavigationMapInstance CreateDefaultBaselineSeed( |
| | | 630 | | long instanceVersion, |
| | | 631 | | PersistentVoxelIndexMap<NavigationDynamicCellSlot> dynamicSlots, |
| | | 632 | | PersistentIntMap<VoxelIndex> dynamicSlotIndexes) |
| | | 633 | | { |
| | 78 | 634 | | if (!Map.DefaultCell.HasValue) |
| | 1 | 635 | | return this; |
| | | 636 | | |
| | 77 | 637 | | return new NavigationMapInstance( |
| | 77 | 638 | | Map, |
| | 77 | 639 | | BakeVersion, |
| | 77 | 640 | | Overlay, |
| | 77 | 641 | | DynamicSlotGeneration, |
| | 77 | 642 | | dynamicSlots, |
| | 77 | 643 | | dynamicSlotIndexes, |
| | 77 | 644 | | _nextDynamicSlot, |
| | 77 | 645 | | _semanticPages, |
| | 77 | 646 | | PersistentIntMap<NavigationPhysicalPage>.Empty, |
| | 77 | 647 | | PersistentIntMap<ulong>.Empty, |
| | 77 | 648 | | _bakedLookup, |
| | 77 | 649 | | _preparedMapRetainedBytes, |
| | 77 | 650 | | default, |
| | 77 | 651 | | baselineCapturedChangeSequence: 0, |
| | 77 | 652 | | gridLastChangeSequence: 0, |
| | 77 | 653 | | instanceVersion, |
| | 77 | 654 | | SemanticVersion, |
| | 77 | 655 | | physicalVersion: instanceVersion, |
| | 77 | 656 | | lastBaselineAddressCount: 0, |
| | 77 | 657 | | lastCopiedSemanticPages: LastCopiedSemanticPages, |
| | 77 | 658 | | lastCopiedPhysicalPages: 0, |
| | 77 | 659 | | dynamicAddresses: _dynamicAddresses); |
| | | 660 | | } |
| | | 661 | | |
| | | 662 | | internal NavigationMapInstance AppendDefaultBaselineStates( |
| | | 663 | | NavigationMapInstance source, |
| | | 664 | | ReadOnlySpan<NavigationBaselineVoxelState> states, |
| | | 665 | | ulong capturedChangeSequence, |
| | | 666 | | long instanceVersion) |
| | | 667 | | { |
| | 78 | 668 | | PersistentVoxelIndexMap<NavigationDynamicCellSlot> dynamicSlots = _dynamicSlots; |
| | 78 | 669 | | PersistentIntMap<VoxelIndex> dynamicSlotIndexes = _dynamicSlotIndexes; |
| | 78 | 670 | | PersistentIntMap<NavigationPhysicalPage> physicalPages = _physicalPages; |
| | 78 | 671 | | PersistentIntMap<ulong> dynamicCapturedChangeSequences = |
| | 78 | 672 | | _dynamicBaselineCapturedChangeSequences; |
| | 78 | 673 | | int nextDynamicSlot = _nextDynamicSlot; |
| | 908 | 674 | | for (int i = 0; i < states.Length; i++) |
| | | 675 | | { |
| | 376 | 676 | | NavigationBaselineVoxelState state = states[i]; |
| | 376 | 677 | | int slot = _bakedLookup.Find(state.VoxelIndex); |
| | 376 | 678 | | if (slot < 0 |
| | 376 | 679 | | && dynamicSlots.TryGetValue( |
| | 376 | 680 | | state.VoxelIndex, |
| | 376 | 681 | | out NavigationDynamicCellSlot existing)) |
| | | 682 | | { |
| | 21 | 683 | | slot = existing.Slot; |
| | | 684 | | } |
| | 376 | 685 | | if (slot < 0 && state.IsPresent) |
| | | 686 | | { |
| | 176 | 687 | | if (source._dynamicSlots.TryGetValue( |
| | 176 | 688 | | state.VoxelIndex, |
| | 176 | 689 | | out NavigationDynamicCellSlot prior)) |
| | | 690 | | { |
| | 51 | 691 | | slot = prior.Slot; |
| | | 692 | | } |
| | | 693 | | else |
| | | 694 | | { |
| | 125 | 695 | | slot = nextDynamicSlot++; |
| | | 696 | | } |
| | 176 | 697 | | var added = new NavigationDynamicCellSlot(state.VoxelIndex, slot); |
| | 176 | 698 | | dynamicSlots = dynamicSlots.Set(state.VoxelIndex, added); |
| | 176 | 699 | | dynamicSlotIndexes = dynamicSlotIndexes.Set(slot, state.VoxelIndex); |
| | | 700 | | } |
| | 376 | 701 | | if (slot < 0) |
| | | 702 | | continue; |
| | 327 | 703 | | physicalPages = ApplyPhysicalState( |
| | 327 | 704 | | physicalPages, |
| | 327 | 705 | | slot, |
| | 327 | 706 | | state.IsPresent, |
| | 327 | 707 | | state.ObstacleCount, |
| | 327 | 708 | | instanceVersion); |
| | 327 | 709 | | if (slot >= DynamicSlotBase) |
| | 197 | 710 | | dynamicCapturedChangeSequences = dynamicCapturedChangeSequences.Set( |
| | 197 | 711 | | slot, |
| | 197 | 712 | | capturedChangeSequence); |
| | | 713 | | } |
| | | 714 | | |
| | 78 | 715 | | return new NavigationMapInstance( |
| | 78 | 716 | | Map, |
| | 78 | 717 | | BakeVersion, |
| | 78 | 718 | | Overlay, |
| | 78 | 719 | | DynamicSlotGeneration, |
| | 78 | 720 | | dynamicSlots, |
| | 78 | 721 | | dynamicSlotIndexes, |
| | 78 | 722 | | nextDynamicSlot, |
| | 78 | 723 | | _semanticPages, |
| | 78 | 724 | | physicalPages, |
| | 78 | 725 | | dynamicCapturedChangeSequences, |
| | 78 | 726 | | _bakedLookup, |
| | 78 | 727 | | _preparedMapRetainedBytes, |
| | 78 | 728 | | default, |
| | 78 | 729 | | baselineCapturedChangeSequence: 0, |
| | 78 | 730 | | gridLastChangeSequence: 0, |
| | 78 | 731 | | instanceVersion, |
| | 78 | 732 | | SemanticVersion, |
| | 78 | 733 | | physicalVersion: instanceVersion, |
| | 78 | 734 | | lastBaselineAddressCount: 0, |
| | 78 | 735 | | lastCopiedSemanticPages: LastCopiedSemanticPages, |
| | 78 | 736 | | lastCopiedPhysicalPages: physicalPages.Count, |
| | 78 | 737 | | dynamicAddresses: _dynamicAddresses); |
| | | 738 | | } |
| | | 739 | | |
| | | 740 | | internal NavigationMapInstance CompleteDefaultBaseline( |
| | | 741 | | long instanceVersion, |
| | | 742 | | bool hasSameDynamicSlots) |
| | | 743 | | { |
| | 70 | 744 | | long dynamicSlotGeneration = hasSameDynamicSlots |
| | 70 | 745 | | ? DynamicSlotGeneration |
| | 70 | 746 | | : instanceVersion; |
| | 70 | 747 | | return new NavigationMapInstance( |
| | 70 | 748 | | Map, |
| | 70 | 749 | | BakeVersion, |
| | 70 | 750 | | Overlay, |
| | 70 | 751 | | dynamicSlotGeneration, |
| | 70 | 752 | | _dynamicSlots, |
| | 70 | 753 | | _dynamicSlotIndexes, |
| | 70 | 754 | | _nextDynamicSlot, |
| | 70 | 755 | | _semanticPages, |
| | 70 | 756 | | _physicalPages, |
| | 70 | 757 | | _dynamicBaselineCapturedChangeSequences, |
| | 70 | 758 | | _bakedLookup, |
| | 70 | 759 | | _preparedMapRetainedBytes, |
| | 70 | 760 | | default, |
| | 70 | 761 | | baselineCapturedChangeSequence: 0, |
| | 70 | 762 | | gridLastChangeSequence: 0, |
| | 70 | 763 | | instanceVersion, |
| | 70 | 764 | | SemanticVersion, |
| | 70 | 765 | | physicalVersion: instanceVersion, |
| | 70 | 766 | | lastBaselineAddressCount: AddressCount, |
| | 70 | 767 | | lastCopiedSemanticPages: LastCopiedSemanticPages, |
| | 70 | 768 | | lastCopiedPhysicalPages: _physicalPages.Count, |
| | 70 | 769 | | dynamicAddresses: _dynamicAddresses); |
| | | 770 | | } |
| | | 771 | | |
| | 620 | 772 | | internal long DefaultBaselineRetainedBytes => checked( |
| | 620 | 773 | | 200L |
| | 620 | 774 | | + _dynamicSlots.RetainedBytes |
| | 620 | 775 | | + _dynamicSlotIndexes.RetainedBytes |
| | 620 | 776 | | + _physicalPages.RetainedBytes |
| | 620 | 777 | | + ((long)_physicalPages.Count * 320L) |
| | 620 | 778 | | + _dynamicBaselineCapturedChangeSequences.RetainedBytes); |
| | | 779 | | |
| | 618 | 780 | | internal int DefaultBaselinePersistentPages => checked( |
| | 618 | 781 | | 4 |
| | 618 | 782 | | + _dynamicSlots.PersistentNodeCount |
| | 618 | 783 | | + _dynamicSlotIndexes.PersistentNodeCount |
| | 618 | 784 | | + (_physicalPages.PersistentNodeCount * 2) |
| | 618 | 785 | | + _dynamicBaselineCapturedChangeSequences.PersistentNodeCount); |
| | | 786 | | |
| | | 787 | | private NavigationMapInstance WithPhysicalState( |
| | | 788 | | int slot, |
| | | 789 | | bool isPresent, |
| | | 790 | | byte obstacleCount, |
| | | 791 | | long instanceVersion, |
| | | 792 | | ulong gridLastChangeSequence) |
| | | 793 | | { |
| | 3 | 794 | | PersistentIntMap<NavigationPhysicalPage> pages = ApplyPhysicalState( |
| | 3 | 795 | | _physicalPages, |
| | 3 | 796 | | slot, |
| | 3 | 797 | | isPresent, |
| | 3 | 798 | | obstacleCount, |
| | 3 | 799 | | instanceVersion); |
| | 3 | 800 | | if (ReferenceEquals(pages, _physicalPages)) |
| | 1 | 801 | | return WithGridLastChangeSequence(gridLastChangeSequence, instanceVersion); |
| | | 802 | | |
| | 2 | 803 | | return new NavigationMapInstance( |
| | 2 | 804 | | Map, |
| | 2 | 805 | | BakeVersion, |
| | 2 | 806 | | Overlay, |
| | 2 | 807 | | DynamicSlotGeneration, |
| | 2 | 808 | | _dynamicSlots, |
| | 2 | 809 | | _dynamicSlotIndexes, |
| | 2 | 810 | | _nextDynamicSlot, |
| | 2 | 811 | | _semanticPages, |
| | 2 | 812 | | pages, |
| | 2 | 813 | | _dynamicBaselineCapturedChangeSequences, |
| | 2 | 814 | | _bakedLookup, |
| | 2 | 815 | | _preparedMapRetainedBytes, |
| | 2 | 816 | | GridIdentity, |
| | 2 | 817 | | BaselineCapturedChangeSequence, |
| | 2 | 818 | | gridLastChangeSequence, |
| | 2 | 819 | | instanceVersion, |
| | 2 | 820 | | SemanticVersion, |
| | 2 | 821 | | physicalVersion: instanceVersion, |
| | 2 | 822 | | lastBaselineAddressCount: 0, |
| | 2 | 823 | | lastCopiedSemanticPages: 0, |
| | 2 | 824 | | lastCopiedPhysicalPages: 1, |
| | 2 | 825 | | dynamicAddresses: _dynamicAddresses); |
| | | 826 | | } |
| | | 827 | | |
| | | 828 | | private static PersistentIntMap<NavigationPhysicalPage> ApplyPhysicalState( |
| | | 829 | | PersistentIntMap<NavigationPhysicalPage> pages, |
| | | 830 | | int slot, |
| | | 831 | | bool isPresent, |
| | | 832 | | byte obstacleCount, |
| | | 833 | | long version) |
| | | 834 | | { |
| | 366 | 835 | | int pageIndex = slot / NavigationPhysicalPage.SlotCount; |
| | 366 | 836 | | int offset = slot % NavigationPhysicalPage.SlotCount; |
| | 366 | 837 | | pages.TryGetValue(pageIndex, out NavigationPhysicalPage? current); |
| | 366 | 838 | | if (current != null |
| | 366 | 839 | | && current.IsPresent[offset] == isPresent |
| | 366 | 840 | | && current.ObstacleCounts[offset] == obstacleCount) |
| | | 841 | | { |
| | 23 | 842 | | return pages; |
| | | 843 | | } |
| | | 844 | | |
| | 343 | 845 | | NavigationPhysicalPage page = current?.Clone(version) |
| | 343 | 846 | | ?? new NavigationPhysicalPage(pageIndex, version); |
| | 343 | 847 | | page.IsPresent[offset] = isPresent; |
| | 343 | 848 | | page.ObstacleCounts[offset] = isPresent ? obstacleCount : (byte)0; |
| | 343 | 849 | | return pages.Set(pageIndex, page); |
| | | 850 | | } |
| | | 851 | | |
| | | 852 | | private NavigationMapInstance ApplyPhysicalSegment( |
| | | 853 | | ReadOnlySpan<GridEventInfo> events, |
| | | 854 | | long instanceVersion) |
| | | 855 | | { |
| | 283 | 856 | | if (!IsMaterialized || events.IsEmpty) |
| | 223 | 857 | | return this; |
| | | 858 | | |
| | 60 | 859 | | PersistentIntMap<NavigationPhysicalPage>? pages = null; |
| | 60 | 860 | | ulong gridLastChangeSequence = GridLastChangeSequence; |
| | 60 | 861 | | int copiedPhysicalPages = 0; |
| | 294 | 862 | | for (int i = 0; i < events.Length; i++) |
| | | 863 | | { |
| | 87 | 864 | | GridEventInfo eventInfo = events[i]; |
| | 87 | 865 | | if (!GridIdentity.Matches( |
| | 87 | 866 | | eventInfo.WorldSpawnToken, |
| | 87 | 867 | | eventInfo.GridIndex, |
| | 87 | 868 | | eventInfo.GridSpawnToken)) |
| | | 869 | | { |
| | | 870 | | continue; |
| | | 871 | | } |
| | 67 | 872 | | if (eventInfo.ChangeSequence > gridLastChangeSequence) |
| | 51 | 873 | | gridLastChangeSequence = eventInfo.ChangeSequence; |
| | 67 | 874 | | if (!eventInfo.HasVoxelState |
| | 67 | 875 | | || !TryGetSlot(eventInfo.VoxelIndex, out int slot) |
| | 67 | 876 | | || eventInfo.ChangeSequence <= GetBaselineCapturedChangeSequence(slot)) |
| | | 877 | | { |
| | | 878 | | continue; |
| | | 879 | | } |
| | | 880 | | |
| | 63 | 881 | | int pageIndex = slot / NavigationPhysicalPage.SlotCount; |
| | 63 | 882 | | pages ??= _physicalPages; |
| | 63 | 883 | | pages.TryGetValue(pageIndex, out NavigationPhysicalPage? page); |
| | 63 | 884 | | if (page == null) |
| | | 885 | | { |
| | 2 | 886 | | page = new NavigationPhysicalPage(pageIndex, instanceVersion); |
| | 2 | 887 | | pages = pages.Set(pageIndex, page); |
| | 2 | 888 | | copiedPhysicalPages++; |
| | | 889 | | } |
| | 61 | 890 | | else if (_physicalPages.TryGetValue(pageIndex, out NavigationPhysicalPage? original) |
| | 61 | 891 | | && ReferenceEquals(page, original)) |
| | | 892 | | { |
| | 51 | 893 | | page = page.Clone(instanceVersion); |
| | 51 | 894 | | pages = pages.Set(pageIndex, page); |
| | 51 | 895 | | copiedPhysicalPages++; |
| | | 896 | | } |
| | | 897 | | |
| | 63 | 898 | | int offset = slot % NavigationPhysicalPage.SlotCount; |
| | 63 | 899 | | page.IsPresent[offset] = eventInfo.IsVoxelPresent; |
| | 63 | 900 | | page.ObstacleCounts[offset] = eventInfo.IsVoxelPresent |
| | 63 | 901 | | ? eventInfo.ObstacleCount |
| | 63 | 902 | | : (byte)0; |
| | | 903 | | } |
| | | 904 | | |
| | 60 | 905 | | return pages == null && gridLastChangeSequence == GridLastChangeSequence |
| | 60 | 906 | | ? this |
| | 60 | 907 | | : new NavigationMapInstance( |
| | 60 | 908 | | Map, |
| | 60 | 909 | | BakeVersion, |
| | 60 | 910 | | Overlay, |
| | 60 | 911 | | DynamicSlotGeneration, |
| | 60 | 912 | | _dynamicSlots, |
| | 60 | 913 | | _dynamicSlotIndexes, |
| | 60 | 914 | | _nextDynamicSlot, |
| | 60 | 915 | | _semanticPages, |
| | 60 | 916 | | pages ?? _physicalPages, |
| | 60 | 917 | | _dynamicBaselineCapturedChangeSequences, |
| | 60 | 918 | | _bakedLookup, |
| | 60 | 919 | | _preparedMapRetainedBytes, |
| | 60 | 920 | | GridIdentity, |
| | 60 | 921 | | BaselineCapturedChangeSequence, |
| | 60 | 922 | | gridLastChangeSequence, |
| | 60 | 923 | | instanceVersion, |
| | 60 | 924 | | SemanticVersion, |
| | 60 | 925 | | physicalVersion: pages == null ? PhysicalVersion : instanceVersion, |
| | 60 | 926 | | lastBaselineAddressCount: 0, |
| | 60 | 927 | | lastCopiedSemanticPages: 0, |
| | 60 | 928 | | lastCopiedPhysicalPages: copiedPhysicalPages, |
| | 60 | 929 | | dynamicAddresses: _dynamicAddresses); |
| | | 930 | | } |
| | | 931 | | |
| | | 932 | | private static int CountTouchedPages( |
| | | 933 | | VoxelIndex[] addresses, |
| | | 934 | | int count, |
| | | 935 | | PersistentVoxelIndexMap<NavigationDynamicCellSlot> slots) |
| | | 936 | | { |
| | 35 | 937 | | int pages = 0; |
| | 35 | 938 | | int priorPage = -1; |
| | 142 | 939 | | for (int i = 0; i < count; i++) |
| | | 940 | | { |
| | 36 | 941 | | bool found = slots.TryGetValue(addresses[i], out NavigationDynamicCellSlot slot); |
| | | 942 | | System.Diagnostics.Debug.Assert(found); |
| | 36 | 943 | | int page = slot.Slot / NavigationPhysicalPage.SlotCount; |
| | 36 | 944 | | if (page != priorPage) |
| | | 945 | | { |
| | 35 | 946 | | pages++; |
| | 35 | 947 | | priorPage = page; |
| | | 948 | | } |
| | | 949 | | } |
| | 35 | 950 | | return pages; |
| | | 951 | | } |
| | | 952 | | |
| | | 953 | | private PersistentIntMap<ulong> FillDynamicBaselineCapturedChangeSequences( |
| | | 954 | | ulong capturedChangeSequence) |
| | | 955 | | { |
| | 1607 | 956 | | PersistentIntMap<ulong> result = PersistentIntMap<ulong>.Empty; |
| | 3608 | 957 | | for (int i = 0; i < _dynamicSlots.Count; i++) |
| | 197 | 958 | | result = result.Set( |
| | 197 | 959 | | _dynamicSlots.GetValueAt(i).Slot, |
| | 197 | 960 | | capturedChangeSequence); |
| | 1607 | 961 | | return result; |
| | | 962 | | } |
| | | 963 | | |
| | | 964 | | private ulong GetBaselineCapturedChangeSequence(int slot) |
| | | 965 | | { |
| | 67 | 966 | | if (slot < BakedSlotCount) |
| | 59 | 967 | | return BaselineCapturedChangeSequence; |
| | 8 | 968 | | bool found = _dynamicBaselineCapturedChangeSequences.TryGetValue( |
| | 8 | 969 | | slot, |
| | 8 | 970 | | out ulong capturedChangeSequence); |
| | | 971 | | System.Diagnostics.Debug.Assert(found); |
| | 8 | 972 | | return capturedChangeSequence; |
| | | 973 | | } |
| | | 974 | | |
| | | 975 | | private static PersistentIntMap<NavigationSemanticPage> ApplySemanticOperation( |
| | | 976 | | PersistentIntMap<NavigationSemanticPage> pages, |
| | | 977 | | int slot, |
| | | 978 | | NavigationCellOverlayOperation operation, |
| | | 979 | | long version, |
| | | 980 | | out int copiedNodeCount) |
| | | 981 | | { |
| | 16674 | 982 | | int pageIndex = slot / NavigationSemanticPage.SlotCount; |
| | | 983 | | NavigationSemanticPage page; |
| | 16674 | 984 | | if (pages.TryGetValue(pageIndex, out NavigationSemanticPage? existing)) |
| | | 985 | | { |
| | 16256 | 986 | | page = existing!.Clone(version); |
| | | 987 | | } |
| | | 988 | | else |
| | | 989 | | { |
| | 418 | 990 | | page = new NavigationSemanticPage(pageIndex, version); |
| | | 991 | | } |
| | | 992 | | |
| | 16674 | 993 | | int offset = slot % NavigationSemanticPage.SlotCount; |
| | 16674 | 994 | | page.HasOverride[offset] = operation.Kind == NavigationCellOverlayOperationKind.Set; |
| | 16674 | 995 | | page.IsSuppressed[offset] = operation.Kind == NavigationCellOverlayOperationKind.Suppress; |
| | 16674 | 996 | | page.Cells[offset] = operation.Cell; |
| | 16674 | 997 | | return page.IsEmpty() |
| | 16674 | 998 | | ? pages.Remove(pageIndex, out copiedNodeCount) |
| | 16674 | 999 | | : pages.Set(pageIndex, page, out copiedNodeCount); |
| | | 1000 | | } |
| | | 1001 | | |
| | | 1002 | | private PersistentIntMap<NavigationPhysicalPage> BuildPhysicalPages( |
| | | 1003 | | ReadOnlySpan<NavigationBaselineVoxelState> states) => |
| | 912 | 1004 | | AppendPhysicalBaselinePages( |
| | 912 | 1005 | | PersistentIntMap<NavigationPhysicalPage>.Empty, |
| | 912 | 1006 | | states, |
| | 912 | 1007 | | InstanceVersion); |
| | | 1008 | | |
| | | 1009 | | private NavigationSemanticPage? FindSemanticPage(int pageIndex) |
| | | 1010 | | { |
| | 9232550 | 1011 | | _semanticPages.TryGetValue(pageIndex, out NavigationSemanticPage? page); |
| | 9232550 | 1012 | | return page; |
| | | 1013 | | } |
| | | 1014 | | |
| | | 1015 | | private NavigationCellSemanticSource GetSemanticSource(int slot) |
| | | 1016 | | { |
| | 2160236 | 1017 | | NavigationSemanticPage? page = FindSemanticPage(slot / NavigationSemanticPage.SlotCount); |
| | 2160236 | 1018 | | int offset = slot % NavigationSemanticPage.SlotCount; |
| | 2160236 | 1019 | | if (page != null && page.IsSuppressed[offset]) |
| | 42 | 1020 | | return NavigationCellSemanticSource.OverlaySuppressed; |
| | 2160194 | 1021 | | if (page != null && page.HasOverride[offset]) |
| | | 1022 | | { |
| | 2138402 | 1023 | | return slot >= BakedSlotCount |
| | 2138402 | 1024 | | ? NavigationCellSemanticSource.DynamicOverlaySet |
| | 2138402 | 1025 | | : NavigationCellSemanticSource.OverlaySet; |
| | | 1026 | | } |
| | 21792 | 1027 | | if (slot >= BakedSlotCount && Map.DefaultCell.HasValue) |
| | 25 | 1028 | | return NavigationCellSemanticSource.Baked; |
| | 21767 | 1029 | | return slot >= BakedSlotCount |
| | 21767 | 1030 | | ? NavigationCellSemanticSource.DynamicInactive |
| | 21767 | 1031 | | : NavigationCellSemanticSource.Baked; |
| | | 1032 | | } |
| | | 1033 | | |
| | | 1034 | | private NavigationPhysicalPage? FindPhysicalPage(int pageIndex) |
| | | 1035 | | { |
| | 3110148 | 1036 | | _physicalPages.TryGetValue(pageIndex, out NavigationPhysicalPage? page); |
| | 3110148 | 1037 | | return page; |
| | | 1038 | | } |
| | | 1039 | | |
| | | 1040 | | private static bool IsBroad(GridEventKind kind) => |
| | 107 | 1041 | | kind == GridEventKind.WorldReset |
| | 107 | 1042 | | || kind == GridEventKind.GridAdded |
| | 107 | 1043 | | || kind == GridEventKind.GridRemoved |
| | 107 | 1044 | | || kind == GridEventKind.GridChanged; |
| | | 1045 | | |
| | 3973 | 1046 | | private long EstimateRetainedBytes() => checked( |
| | 3973 | 1047 | | 200L |
| | 3973 | 1048 | | + Overlay.RetainedBytes |
| | 3973 | 1049 | | + _dynamicSlots.RetainedBytes |
| | 3973 | 1050 | | + _dynamicAddresses.RetainedBytes |
| | 3973 | 1051 | | + _dynamicSlotIndexes.RetainedBytes |
| | 3973 | 1052 | | + _semanticPages.RetainedBytes |
| | 3973 | 1053 | | + ((long)_semanticPages.Count * 4_400L) |
| | 3973 | 1054 | | + _physicalPages.RetainedBytes |
| | 3973 | 1055 | | + ((long)_physicalPages.Count * 320L) |
| | 3973 | 1056 | | + _dynamicBaselineCapturedChangeSequences.RetainedBytes |
| | 3973 | 1057 | | + _preparedMapRetainedBytes); |
| | | 1058 | | |
| | | 1059 | | private NavigationMapInstance WithGridLastChangeSequence( |
| | | 1060 | | ulong gridLastChangeSequence, |
| | | 1061 | | long instanceVersion) |
| | | 1062 | | { |
| | 5 | 1063 | | if (gridLastChangeSequence <= GridLastChangeSequence) |
| | 1 | 1064 | | return this; |
| | 4 | 1065 | | return new NavigationMapInstance( |
| | 4 | 1066 | | Map, |
| | 4 | 1067 | | BakeVersion, |
| | 4 | 1068 | | Overlay, |
| | 4 | 1069 | | DynamicSlotGeneration, |
| | 4 | 1070 | | _dynamicSlots, |
| | 4 | 1071 | | _dynamicSlotIndexes, |
| | 4 | 1072 | | _nextDynamicSlot, |
| | 4 | 1073 | | _semanticPages, |
| | 4 | 1074 | | _physicalPages, |
| | 4 | 1075 | | _dynamicBaselineCapturedChangeSequences, |
| | 4 | 1076 | | _bakedLookup, |
| | 4 | 1077 | | _preparedMapRetainedBytes, |
| | 4 | 1078 | | GridIdentity, |
| | 4 | 1079 | | BaselineCapturedChangeSequence, |
| | 4 | 1080 | | gridLastChangeSequence, |
| | 4 | 1081 | | instanceVersion, |
| | 4 | 1082 | | SemanticVersion, |
| | 4 | 1083 | | PhysicalVersion, |
| | 4 | 1084 | | lastBaselineAddressCount: 0, |
| | 4 | 1085 | | lastCopiedSemanticPages: 0, |
| | 4 | 1086 | | lastCopiedPhysicalPages: 0, |
| | 4 | 1087 | | dynamicAddresses: _dynamicAddresses); |
| | | 1088 | | } |
| | | 1089 | | } |