| | | 1 | | //======================================================================= |
| | | 2 | | // GridTracer.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 FixedMathSharp; |
| | | 9 | | using GridForge.Grids; |
| | | 10 | | using GridForge.Grids.Topology; |
| | | 11 | | using GridForge.Spatial; |
| | | 12 | | using SwiftCollections; |
| | | 13 | | using SwiftCollections.Pool; |
| | | 14 | | using SwiftCollections.Utility; |
| | | 15 | | using System.Collections.Generic; |
| | | 16 | | using System.Runtime.CompilerServices; |
| | | 17 | | |
| | | 18 | | namespace GridForge.Utility; |
| | | 19 | | |
| | | 20 | | /// <summary> |
| | | 21 | | /// Provides utilities for tracing lines or bounding areas in a grid, aligning them to grid voxels. |
| | | 22 | | /// Uses fixed-point calculations to ensure deterministic and accurate grid traversal. |
| | | 23 | | /// </summary> |
| | | 24 | | public static class GridTracer |
| | | 25 | | { |
| | | 26 | | private readonly struct TraceLinePlan |
| | | 27 | | { |
| | | 28 | | public readonly Vector3d TraceStart; |
| | | 29 | | public readonly Fixed64 Steps; |
| | | 30 | | public readonly Fixed64 StepX; |
| | | 31 | | public readonly Fixed64 StepY; |
| | | 32 | | public readonly Fixed64 StepZ; |
| | | 33 | | |
| | | 34 | | public TraceLinePlan( |
| | | 35 | | Vector3d traceStart, |
| | | 36 | | Fixed64 steps, |
| | | 37 | | Fixed64 stepX, |
| | | 38 | | Fixed64 stepY, |
| | | 39 | | Fixed64 stepZ) |
| | | 40 | | { |
| | 19 | 41 | | TraceStart = traceStart; |
| | 19 | 42 | | Steps = steps; |
| | 19 | 43 | | StepX = stepX; |
| | 19 | 44 | | StepY = stepY; |
| | 19 | 45 | | StepZ = stepZ; |
| | 19 | 46 | | } |
| | | 47 | | } |
| | | 48 | | |
| | | 49 | | /// <summary> |
| | | 50 | | /// Traces a 3D line between two points in the supplied world. |
| | | 51 | | /// The traced points are returned as grid voxels. |
| | | 52 | | /// </summary> |
| | | 53 | | /// <remarks> |
| | | 54 | | /// Uses a fractional step algorithm inspired by Bresenham’s line algorithm. |
| | | 55 | | /// This implementation leverages fixed-point math to maintain precision across a deterministic grid. |
| | | 56 | | /// </remarks> |
| | | 57 | | /// <param name="world">The world whose grids should be traced.</param> |
| | | 58 | | /// <param name="start">Starting position in world space.</param> |
| | | 59 | | /// <param name="end">Ending position in world space.</param> |
| | | 60 | | /// <param name="padding">Value applied to the start/end positions before snapping.</param> |
| | | 61 | | /// <param name="includeEnd">Whether to include the end voxel in the traced line.</param> |
| | | 62 | | /// <returns>A collection of <see cref="GridVoxelSet"/> objects representing the traced path.</returns> |
| | | 63 | | public static IEnumerable<GridVoxelSet> TraceLine( |
| | | 64 | | GridWorld world, |
| | | 65 | | Vector3d start, |
| | | 66 | | Vector3d end, |
| | | 67 | | Fixed64? padding = null, |
| | | 68 | | bool includeEnd = true) |
| | | 69 | | { |
| | 28 | 70 | | if (world == null || !world.IsActive) |
| | 2 | 71 | | return System.Array.Empty<GridVoxelSet>(); |
| | | 72 | | |
| | 26 | 73 | | return TraceLineIterator(world, start, end, padding, includeEnd); |
| | | 74 | | } |
| | | 75 | | |
| | | 76 | | /// <summary> |
| | | 77 | | /// Traces a 2D XZ-plane line between two points in the supplied world, snapping them to grid coordinates. |
| | | 78 | | /// </summary> |
| | | 79 | | /// <remarks> |
| | | 80 | | /// This method maps <see cref="Vector2d.X"/> to world X, <see cref="Vector2d.Y"/> to world Z, |
| | | 81 | | /// and <paramref name="layerY"/> to world Y. The default layer is world Y = 0. |
| | | 82 | | /// </remarks> |
| | | 83 | | /// <param name="world">The world whose grids should be traced.</param> |
| | | 84 | | /// <param name="start">Starting XZ-plane position in world space.</param> |
| | | 85 | | /// <param name="end">Ending XZ-plane position in world space.</param> |
| | | 86 | | /// <param name="padding">Value applied to the start/end positions before snapping.</param> |
| | | 87 | | /// <param name="includeEnd">Whether to include the end voxel in the traced line.</param> |
| | | 88 | | /// <param name="layerY">The world Y layer to trace. Defaults to zero.</param> |
| | | 89 | | /// <returns>A collection of <see cref="GridVoxelSet"/> objects representing the traced path.</returns> |
| | | 90 | | public static IEnumerable<GridVoxelSet> TraceLine( |
| | | 91 | | GridWorld world, |
| | | 92 | | Vector2d start, |
| | | 93 | | Vector2d end, |
| | | 94 | | Fixed64? padding = null, |
| | | 95 | | bool includeEnd = true, |
| | | 96 | | Fixed64 layerY = default) |
| | | 97 | | { |
| | 3 | 98 | | Vector3d start3D = GridPlane2d.ToWorld(start, layerY); |
| | 3 | 99 | | Vector3d end3D = GridPlane2d.ToWorld(end, layerY); |
| | | 100 | | |
| | 3 | 101 | | return TraceLine(world, start3D, end3D, padding, includeEnd); |
| | | 102 | | } |
| | | 103 | | |
| | | 104 | | /// <summary> |
| | | 105 | | /// Retrieves all grid voxels covered by the given bounding area in the supplied world. |
| | | 106 | | /// </summary> |
| | | 107 | | public static IEnumerable<GridVoxelSet> GetCoveredVoxels( |
| | | 108 | | GridWorld world, |
| | | 109 | | Vector3d boundsMin, |
| | | 110 | | Vector3d boundsMax, |
| | | 111 | | Fixed64? padding = null) |
| | | 112 | | { |
| | 187 | 113 | | if (world == null || !world.IsActive) |
| | 2 | 114 | | return System.Array.Empty<GridVoxelSet>(); |
| | | 115 | | |
| | 185 | 116 | | return GetCoveredVoxelsIterator(world, boundsMin, boundsMax, padding); |
| | | 117 | | } |
| | | 118 | | |
| | | 119 | | /// <summary> |
| | | 120 | | /// Retrieves all grid voxels covered by the given XZ-plane bounding area on the supplied world Y layer. |
| | | 121 | | /// </summary> |
| | | 122 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 123 | | /// <param name="boundsMin">The 2D minimum corner whose X component maps to world X and Y component maps to world Z. |
| | | 124 | | /// <param name="boundsMax">The 2D maximum corner whose X component maps to world X and Y component maps to world Z. |
| | | 125 | | /// <param name="layerY">The world Y layer to cover. Defaults to zero.</param> |
| | | 126 | | /// <param name="padding">Value applied to the min/max bounds before snapping.</param> |
| | | 127 | | /// <returns>A collection of <see cref="GridVoxelSet"/> objects representing the covered voxels.</returns> |
| | | 128 | | public static IEnumerable<GridVoxelSet> GetCoveredVoxels( |
| | | 129 | | GridWorld world, |
| | | 130 | | Vector2d boundsMin, |
| | | 131 | | Vector2d boundsMax, |
| | | 132 | | Fixed64 layerY = default, |
| | | 133 | | Fixed64? padding = null) |
| | | 134 | | { |
| | 3 | 135 | | (Vector3d min, Vector3d max) = GridPlane2d.ToWorldBounds(boundsMin, boundsMax, layerY); |
| | 3 | 136 | | return GetCoveredVoxels(world, min, max, padding); |
| | | 137 | | } |
| | | 138 | | |
| | | 139 | | /// <summary> |
| | | 140 | | /// Clears and fills caller-owned storage with voxels covered by the supplied bounding area. |
| | | 141 | | /// </summary> |
| | | 142 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 143 | | /// <param name="boundsMin">The minimum corner of the bounding area.</param> |
| | | 144 | | /// <param name="boundsMax">The maximum corner of the bounding area.</param> |
| | | 145 | | /// <param name="results">Caller-owned storage that receives covered voxels.</param> |
| | | 146 | | /// <param name="padding">Value applied to the min/max bounds before normalization.</param> |
| | | 147 | | public static void GetCoveredVoxelsInto( |
| | | 148 | | GridWorld world, |
| | | 149 | | Vector3d boundsMin, |
| | | 150 | | Vector3d boundsMax, |
| | | 151 | | SwiftList<Voxel> results, |
| | | 152 | | Fixed64? padding = null) |
| | | 153 | | { |
| | 4 | 154 | | SwiftThrowHelper.ThrowIfNull(results, nameof(results)); |
| | | 155 | | |
| | 3 | 156 | | results.Clear(); |
| | 3 | 157 | | if (world == null || !world.IsActive) |
| | 1 | 158 | | return; |
| | | 159 | | |
| | 2 | 160 | | AddCoveredVoxelsTo(world, boundsMin, boundsMax, results, padding); |
| | 2 | 161 | | } |
| | | 162 | | |
| | | 163 | | /// <summary> |
| | | 164 | | /// Clears and fills caller-owned storage with voxels covered by the supplied XZ-plane bounding area. |
| | | 165 | | /// </summary> |
| | | 166 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 167 | | /// <param name="boundsMin">The 2D minimum corner whose X component maps to world X and Y component maps to world Z. |
| | | 168 | | /// <param name="boundsMax">The 2D maximum corner whose X component maps to world X and Y component maps to world Z. |
| | | 169 | | /// <param name="results">Caller-owned storage that receives covered voxels.</param> |
| | | 170 | | /// <param name="layerY">The world Y layer to cover. Defaults to zero.</param> |
| | | 171 | | /// <param name="padding">Value applied to the min/max bounds before normalization.</param> |
| | | 172 | | public static void GetCoveredVoxelsInto( |
| | | 173 | | GridWorld world, |
| | | 174 | | Vector2d boundsMin, |
| | | 175 | | Vector2d boundsMax, |
| | | 176 | | SwiftList<Voxel> results, |
| | | 177 | | Fixed64 layerY = default, |
| | | 178 | | Fixed64? padding = null) |
| | | 179 | | { |
| | 1 | 180 | | (Vector3d min, Vector3d max) = GridPlane2d.ToWorldBounds(boundsMin, boundsMax, layerY); |
| | 1 | 181 | | GetCoveredVoxelsInto(world, min, max, results, padding); |
| | 1 | 182 | | } |
| | | 183 | | |
| | | 184 | | /// <summary> |
| | | 185 | | /// Clears and fills caller-owned storage using caller-owned scratch collections. |
| | | 186 | | /// </summary> |
| | | 187 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 188 | | /// <param name="boundsMin">The minimum corner of the bounding area.</param> |
| | | 189 | | /// <param name="boundsMax">The maximum corner of the bounding area.</param> |
| | | 190 | | /// <param name="results">Caller-owned storage that receives covered voxels.</param> |
| | | 191 | | /// <param name="scratch">Reusable scratch storage for processed-grid and duplicate-voxel guards.</param> |
| | | 192 | | /// <param name="padding">Value applied to the min/max bounds before normalization.</param> |
| | | 193 | | public static void GetCoveredVoxelsInto( |
| | | 194 | | GridWorld world, |
| | | 195 | | Vector3d boundsMin, |
| | | 196 | | Vector3d boundsMax, |
| | | 197 | | SwiftList<Voxel> results, |
| | | 198 | | GridTraceScratch scratch, |
| | | 199 | | Fixed64? padding = null) |
| | | 200 | | { |
| | 5 | 201 | | SwiftThrowHelper.ThrowIfNull(results, nameof(results)); |
| | 4 | 202 | | SwiftThrowHelper.ThrowIfNull(scratch, nameof(scratch)); |
| | | 203 | | |
| | 3 | 204 | | results.Clear(); |
| | 3 | 205 | | if (world == null || !world.IsActive) |
| | 1 | 206 | | return; |
| | | 207 | | |
| | 2 | 208 | | AddCoveredVoxelsTo(world, boundsMin, boundsMax, results, scratch, padding); |
| | 2 | 209 | | } |
| | | 210 | | |
| | | 211 | | /// <summary> |
| | | 212 | | /// Clears and fills caller-owned storage using caller-owned scratch collections for an XZ-plane bounding area. |
| | | 213 | | /// </summary> |
| | | 214 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 215 | | /// <param name="boundsMin">The 2D minimum corner whose X component maps to world X and Y component maps to world Z. |
| | | 216 | | /// <param name="boundsMax">The 2D maximum corner whose X component maps to world X and Y component maps to world Z. |
| | | 217 | | /// <param name="results">Caller-owned storage that receives covered voxels.</param> |
| | | 218 | | /// <param name="scratch">Reusable scratch storage for processed-grid and duplicate-voxel guards.</param> |
| | | 219 | | /// <param name="layerY">The world Y layer to cover. Defaults to zero.</param> |
| | | 220 | | /// <param name="padding">Value applied to the min/max bounds before normalization.</param> |
| | | 221 | | public static void GetCoveredVoxelsInto( |
| | | 222 | | GridWorld world, |
| | | 223 | | Vector2d boundsMin, |
| | | 224 | | Vector2d boundsMax, |
| | | 225 | | SwiftList<Voxel> results, |
| | | 226 | | GridTraceScratch scratch, |
| | | 227 | | Fixed64 layerY = default, |
| | | 228 | | Fixed64? padding = null) |
| | | 229 | | { |
| | 1 | 230 | | (Vector3d min, Vector3d max) = GridPlane2d.ToWorldBounds(boundsMin, boundsMax, layerY); |
| | 1 | 231 | | GetCoveredVoxelsInto(world, min, max, results, scratch, padding); |
| | 1 | 232 | | } |
| | | 233 | | |
| | | 234 | | /// <summary> |
| | | 235 | | /// Retrieves all scan cells within the given bounding area across relevant grids in the supplied world. |
| | | 236 | | /// </summary> |
| | | 237 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 238 | | /// <param name="boundsMin">The minimum corner of the bounding area.</param> |
| | | 239 | | /// <param name="boundsMax">The maximum corner of the bounding area.</param> |
| | | 240 | | /// <param name="padding">Value applied to the min/max bounds before snapping.</param> |
| | | 241 | | /// <returns>An enumerable of covered scan cells grouped by grid.</returns> |
| | | 242 | | public static IEnumerable<ScanCell> GetCoveredScanCells( |
| | | 243 | | GridWorld world, |
| | | 244 | | Vector3d boundsMin, |
| | | 245 | | Vector3d boundsMax, |
| | | 246 | | Fixed64? padding = null) |
| | | 247 | | { |
| | 13 | 248 | | if (world == null || !world.IsActive) |
| | 2 | 249 | | return System.Array.Empty<ScanCell>(); |
| | | 250 | | |
| | 11 | 251 | | return GetCoveredScanCellsIterator(world, boundsMin, boundsMax, padding); |
| | | 252 | | } |
| | | 253 | | |
| | | 254 | | /// <summary> |
| | | 255 | | /// Retrieves all scan cells within the given XZ-plane bounding area on the supplied world Y layer. |
| | | 256 | | /// </summary> |
| | | 257 | | /// <param name="world">The world whose grids should be queried.</param> |
| | | 258 | | /// <param name="boundsMin">The 2D minimum corner whose X component maps to world X and Y component maps to world Z. |
| | | 259 | | /// <param name="boundsMax">The 2D maximum corner whose X component maps to world X and Y component maps to world Z. |
| | | 260 | | /// <param name="layerY">The world Y layer to cover. Defaults to zero.</param> |
| | | 261 | | /// <param name="padding">Value applied to the min/max bounds before snapping.</param> |
| | | 262 | | /// <returns>An enumerable of covered scan cells grouped by grid.</returns> |
| | | 263 | | public static IEnumerable<ScanCell> GetCoveredScanCells( |
| | | 264 | | GridWorld world, |
| | | 265 | | Vector2d boundsMin, |
| | | 266 | | Vector2d boundsMax, |
| | | 267 | | Fixed64 layerY = default, |
| | | 268 | | Fixed64? padding = null) |
| | | 269 | | { |
| | 1 | 270 | | (Vector3d min, Vector3d max) = GridPlane2d.ToWorldBounds(boundsMin, boundsMax, layerY); |
| | 1 | 271 | | return GetCoveredScanCells(world, min, max, padding); |
| | | 272 | | } |
| | | 273 | | |
| | | 274 | | /// <summary> |
| | | 275 | | /// Clears and fills caller-owned storage with scan cells covered by the supplied bounding area. |
| | | 276 | | /// </summary> |
| | | 277 | | public static void GetCoveredScanCellsInto( |
| | | 278 | | GridWorld world, |
| | | 279 | | Vector3d boundsMin, |
| | | 280 | | Vector3d boundsMax, |
| | | 281 | | SwiftList<ScanCell> results, |
| | | 282 | | Fixed64? padding = null) |
| | | 283 | | { |
| | 5 | 284 | | SwiftThrowHelper.ThrowIfNull(results, nameof(results)); |
| | | 285 | | |
| | 4 | 286 | | results.Clear(); |
| | 4 | 287 | | if (world == null || !world.IsActive) |
| | 2 | 288 | | return; |
| | | 289 | | |
| | 2 | 290 | | AddCoveredScanCellsTo(world, boundsMin, boundsMax, results, padding); |
| | 2 | 291 | | } |
| | | 292 | | |
| | | 293 | | /// <summary> |
| | | 294 | | /// Clears and fills caller-owned storage with scan cells covered by the supplied XZ-plane bounding area. |
| | | 295 | | /// </summary> |
| | | 296 | | public static void GetCoveredScanCellsInto( |
| | | 297 | | GridWorld world, |
| | | 298 | | Vector2d boundsMin, |
| | | 299 | | Vector2d boundsMax, |
| | | 300 | | SwiftList<ScanCell> results, |
| | | 301 | | Fixed64 layerY = default, |
| | | 302 | | Fixed64? padding = null) |
| | | 303 | | { |
| | 1 | 304 | | (Vector3d min, Vector3d max) = GridPlane2d.ToWorldBounds(boundsMin, boundsMax, layerY); |
| | 1 | 305 | | GetCoveredScanCellsInto(world, min, max, results, padding); |
| | 1 | 306 | | } |
| | | 307 | | |
| | | 308 | | /// <summary> |
| | | 309 | | /// Clears and fills caller-owned storage using caller-owned scratch collections. |
| | | 310 | | /// </summary> |
| | | 311 | | public static void GetCoveredScanCellsInto( |
| | | 312 | | GridWorld world, |
| | | 313 | | Vector3d boundsMin, |
| | | 314 | | Vector3d boundsMax, |
| | | 315 | | SwiftList<ScanCell> results, |
| | | 316 | | GridScanScratch scratch, |
| | | 317 | | Fixed64? padding = null) |
| | | 318 | | { |
| | 6 | 319 | | SwiftThrowHelper.ThrowIfNull(results, nameof(results)); |
| | 5 | 320 | | SwiftThrowHelper.ThrowIfNull(scratch, nameof(scratch)); |
| | | 321 | | |
| | 4 | 322 | | results.Clear(); |
| | 4 | 323 | | if (world == null || !world.IsActive) |
| | 2 | 324 | | return; |
| | | 325 | | |
| | 2 | 326 | | AddCoveredScanCellsTo(world, boundsMin, boundsMax, results, scratch, padding); |
| | 2 | 327 | | } |
| | | 328 | | |
| | | 329 | | /// <summary> |
| | | 330 | | /// Clears and fills caller-owned storage using caller-owned scratch collections for an XZ-plane bounding area. |
| | | 331 | | /// </summary> |
| | | 332 | | public static void GetCoveredScanCellsInto( |
| | | 333 | | GridWorld world, |
| | | 334 | | Vector2d boundsMin, |
| | | 335 | | Vector2d boundsMax, |
| | | 336 | | SwiftList<ScanCell> results, |
| | | 337 | | GridScanScratch scratch, |
| | | 338 | | Fixed64 layerY = default, |
| | | 339 | | Fixed64? padding = null) |
| | | 340 | | { |
| | 1 | 341 | | (Vector3d min, Vector3d max) = GridPlane2d.ToWorldBounds(boundsMin, boundsMax, layerY); |
| | 1 | 342 | | GetCoveredScanCellsInto(world, min, max, results, scratch, padding); |
| | 1 | 343 | | } |
| | | 344 | | |
| | | 345 | | /// <summary> |
| | | 346 | | /// Appends covered scan cells without allocating an iterator for hot-path callers. |
| | | 347 | | /// </summary> |
| | | 348 | | internal static void AddCoveredScanCellsTo( |
| | | 349 | | GridWorld world, |
| | | 350 | | Vector3d boundsMin, |
| | | 351 | | Vector3d boundsMax, |
| | | 352 | | SwiftList<ScanCell> scanCells, |
| | | 353 | | Fixed64? padding = null) |
| | | 354 | | { |
| | 20 | 355 | | SwiftHashSet<ushort> processedGrids = SwiftHashSetPool<ushort>.Shared.Rent(); |
| | 20 | 356 | | SwiftHashSet<ScanCell> voxelRedundancyCheck = SwiftHashSetPool<ScanCell>.Shared.Rent(); |
| | | 357 | | |
| | | 358 | | try |
| | | 359 | | { |
| | 20 | 360 | | AddCoveredScanCellsCore( |
| | 20 | 361 | | world, |
| | 20 | 362 | | boundsMin, |
| | 20 | 363 | | boundsMax, |
| | 20 | 364 | | scanCells, |
| | 20 | 365 | | processedGrids, |
| | 20 | 366 | | voxelRedundancyCheck, |
| | 20 | 367 | | padding); |
| | 20 | 368 | | } |
| | | 369 | | finally |
| | | 370 | | { |
| | 20 | 371 | | SwiftHashSetPool<ushort>.Shared.Release(processedGrids); |
| | 20 | 372 | | SwiftHashSetPool<ScanCell>.Shared.Release(voxelRedundancyCheck); |
| | 20 | 373 | | } |
| | 20 | 374 | | } |
| | | 375 | | |
| | | 376 | | /// <summary> |
| | | 377 | | /// Appends covered scan cells using caller-owned scratch state for allocation-sensitive scans. |
| | | 378 | | /// </summary> |
| | | 379 | | internal static void AddCoveredScanCellsTo( |
| | | 380 | | GridWorld world, |
| | | 381 | | Vector3d boundsMin, |
| | | 382 | | Vector3d boundsMax, |
| | | 383 | | SwiftList<ScanCell> scanCells, |
| | | 384 | | GridScanScratch scratch, |
| | | 385 | | Fixed64? padding = null) |
| | | 386 | | { |
| | 520 | 387 | | scratch.Clear(); |
| | 520 | 388 | | AddCoveredScanCellsCore( |
| | 520 | 389 | | world, |
| | 520 | 390 | | boundsMin, |
| | 520 | 391 | | boundsMax, |
| | 520 | 392 | | scanCells, |
| | 520 | 393 | | scratch.ProcessedGrids, |
| | 520 | 394 | | scratch.ScanCellRedundancy, |
| | 520 | 395 | | padding); |
| | 520 | 396 | | } |
| | | 397 | | |
| | | 398 | | /// <summary> |
| | | 399 | | /// Appends covered voxels without allocating an iterator for hot-path callers. |
| | | 400 | | /// </summary> |
| | | 401 | | internal static void AddCoveredVoxelsTo( |
| | | 402 | | GridWorld world, |
| | | 403 | | Vector3d boundsMin, |
| | | 404 | | Vector3d boundsMax, |
| | | 405 | | SwiftList<Voxel> voxels, |
| | | 406 | | Fixed64? padding = null) |
| | | 407 | | { |
| | 2 | 408 | | SwiftHashSet<ushort> processedGrids = SwiftHashSetPool<ushort>.Shared.Rent(); |
| | 2 | 409 | | SwiftHashSet<Voxel> voxelRedundancyCheck = SwiftHashSetPool<Voxel>.Shared.Rent(); |
| | | 410 | | |
| | | 411 | | try |
| | | 412 | | { |
| | 2 | 413 | | AddCoveredVoxelsCore( |
| | 2 | 414 | | world, |
| | 2 | 415 | | boundsMin, |
| | 2 | 416 | | boundsMax, |
| | 2 | 417 | | voxels, |
| | 2 | 418 | | processedGrids, |
| | 2 | 419 | | voxelRedundancyCheck, |
| | 2 | 420 | | padding); |
| | 2 | 421 | | } |
| | | 422 | | finally |
| | | 423 | | { |
| | 2 | 424 | | SwiftHashSetPool<ushort>.Shared.Release(processedGrids); |
| | 2 | 425 | | SwiftHashSetPool<Voxel>.Shared.Release(voxelRedundancyCheck); |
| | 2 | 426 | | } |
| | 2 | 427 | | } |
| | | 428 | | |
| | | 429 | | /// <summary> |
| | | 430 | | /// Appends covered voxels using caller-owned scratch state for allocation-sensitive coverage scans. |
| | | 431 | | /// </summary> |
| | | 432 | | internal static void AddCoveredVoxelsTo( |
| | | 433 | | GridWorld world, |
| | | 434 | | Vector3d boundsMin, |
| | | 435 | | Vector3d boundsMax, |
| | | 436 | | SwiftList<Voxel> voxels, |
| | | 437 | | GridTraceScratch scratch, |
| | | 438 | | Fixed64? padding = null) |
| | | 439 | | { |
| | 2 | 440 | | scratch.Clear(); |
| | 2 | 441 | | AddCoveredVoxelsCore( |
| | 2 | 442 | | world, |
| | 2 | 443 | | boundsMin, |
| | 2 | 444 | | boundsMax, |
| | 2 | 445 | | voxels, |
| | 2 | 446 | | scratch.ProcessedGrids, |
| | 2 | 447 | | scratch.VoxelRedundancy, |
| | 2 | 448 | | padding); |
| | 2 | 449 | | } |
| | | 450 | | |
| | | 451 | | private static void AddCoveredVoxelsCore( |
| | | 452 | | GridWorld world, |
| | | 453 | | Vector3d boundsMin, |
| | | 454 | | Vector3d boundsMax, |
| | | 455 | | SwiftList<Voxel> voxels, |
| | | 456 | | SwiftHashSet<ushort> processedGrids, |
| | | 457 | | SwiftHashSet<Voxel> voxelRedundancyCheck, |
| | | 458 | | Fixed64? padding = null) |
| | | 459 | | { |
| | 4 | 460 | | (Vector3d queryMin, Vector3d queryMax) = |
| | 4 | 461 | | CreatePaddedOrderedBounds(boundsMin, boundsMax, padding); |
| | 4 | 462 | | (Vector3d candidateMin, Vector3d candidateMax) = |
| | 4 | 463 | | ExpandOrderedBounds(queryMin, queryMax, world.MaxTopologyCellEdge); |
| | | 464 | | |
| | 4 | 465 | | (int cellXMin, int cellYMin, int cellZMin, int cellXMax, int cellYMax, int cellZMax) = |
| | 4 | 466 | | world.GetSpatialGridCellBounds(candidateMin, candidateMax); |
| | | 467 | | |
| | 16 | 468 | | for (int cellZ = cellZMin; cellZ <= cellZMax; cellZ++) |
| | | 469 | | { |
| | 16 | 470 | | for (int cellY = cellYMin; cellY <= cellYMax; cellY++) |
| | | 471 | | { |
| | 16 | 472 | | for (int cellX = cellXMin; cellX <= cellXMax; cellX++) |
| | 4 | 473 | | AddCoveredVoxelsForSpatialCell( |
| | 4 | 474 | | world, |
| | 4 | 475 | | cellX, |
| | 4 | 476 | | cellY, |
| | 4 | 477 | | cellZ, |
| | 4 | 478 | | queryMin, |
| | 4 | 479 | | queryMax, |
| | 4 | 480 | | voxels, |
| | 4 | 481 | | processedGrids, |
| | 4 | 482 | | voxelRedundancyCheck); |
| | | 483 | | } |
| | | 484 | | } |
| | 4 | 485 | | } |
| | | 486 | | |
| | | 487 | | private static void AddCoveredVoxelsForSpatialCell( |
| | | 488 | | GridWorld world, |
| | | 489 | | int cellX, |
| | | 490 | | int cellY, |
| | | 491 | | int cellZ, |
| | | 492 | | Vector3d queryMin, |
| | | 493 | | Vector3d queryMax, |
| | | 494 | | SwiftList<Voxel> voxels, |
| | | 495 | | SwiftHashSet<ushort> processedGrids, |
| | | 496 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 497 | | { |
| | 4 | 498 | | int cellIndex = SwiftHashTools.CombineHashCodes(cellX, cellY, cellZ); |
| | 4 | 499 | | if (!world.SpatialGridHash.TryGetValue(cellIndex, out SwiftHashSet<ushort> gridList)) |
| | 0 | 500 | | return; |
| | | 501 | | |
| | 16 | 502 | | foreach (ushort gridIndex in gridList) |
| | | 503 | | { |
| | 4 | 504 | | if (!world.ActiveGrids.IsAllocated(gridIndex) || !processedGrids.Add(gridIndex)) |
| | | 505 | | continue; |
| | | 506 | | |
| | 4 | 507 | | AddCoveredGridVoxels( |
| | 4 | 508 | | world.ActiveGrids[gridIndex], |
| | 4 | 509 | | queryMin, |
| | 4 | 510 | | queryMax, |
| | 4 | 511 | | voxels, |
| | 4 | 512 | | voxelRedundancyCheck); |
| | | 513 | | } |
| | 4 | 514 | | } |
| | | 515 | | |
| | | 516 | | private static void AddCoveredScanCellsCore( |
| | | 517 | | GridWorld world, |
| | | 518 | | Vector3d boundsMin, |
| | | 519 | | Vector3d boundsMax, |
| | | 520 | | SwiftList<ScanCell> scanCells, |
| | | 521 | | SwiftHashSet<ushort> processedGrids, |
| | | 522 | | SwiftHashSet<ScanCell> voxelRedundancyCheck, |
| | | 523 | | Fixed64? padding = null) |
| | | 524 | | { |
| | 551 | 525 | | (Vector3d queryMin, Vector3d queryMax) = |
| | 551 | 526 | | CreatePaddedOrderedBounds(boundsMin, boundsMax, padding); |
| | 551 | 527 | | (Vector3d candidateMin, Vector3d candidateMax) = |
| | 551 | 528 | | ExpandOrderedBounds(queryMin, queryMax, world.MaxTopologyCellEdge); |
| | 551 | 529 | | (int cellXMin, int cellYMin, int cellZMin, int cellXMax, int cellYMax, int cellZMax) = |
| | 551 | 530 | | world.GetSpatialGridCellBounds(candidateMin, candidateMax); |
| | | 531 | | |
| | 2206 | 532 | | for (int cellZ = cellZMin; cellZ <= cellZMax; cellZ++) |
| | | 533 | | { |
| | 2208 | 534 | | for (int cellY = cellYMin; cellY <= cellYMax; cellY++) |
| | | 535 | | { |
| | 2218 | 536 | | for (int cellX = cellXMin; cellX <= cellXMax; cellX++) |
| | 557 | 537 | | AddCoveredScanCellsForSpatialCell( |
| | 557 | 538 | | world, |
| | 557 | 539 | | cellX, |
| | 557 | 540 | | cellY, |
| | 557 | 541 | | cellZ, |
| | 557 | 542 | | queryMin, |
| | 557 | 543 | | queryMax, |
| | 557 | 544 | | scanCells, |
| | 557 | 545 | | processedGrids, |
| | 557 | 546 | | voxelRedundancyCheck); |
| | | 547 | | } |
| | | 548 | | } |
| | 551 | 549 | | } |
| | | 550 | | |
| | | 551 | | private static void AddCoveredScanCellsForSpatialCell( |
| | | 552 | | GridWorld world, |
| | | 553 | | int cellX, |
| | | 554 | | int cellY, |
| | | 555 | | int cellZ, |
| | | 556 | | Vector3d queryMin, |
| | | 557 | | Vector3d queryMax, |
| | | 558 | | SwiftList<ScanCell> scanCells, |
| | | 559 | | SwiftHashSet<ushort> processedGrids, |
| | | 560 | | SwiftHashSet<ScanCell> voxelRedundancyCheck) |
| | | 561 | | { |
| | 557 | 562 | | int cellIndex = SwiftHashTools.CombineHashCodes(cellX, cellY, cellZ); |
| | 557 | 563 | | if (!world.SpatialGridHash.TryGetValue(cellIndex, out SwiftHashSet<ushort> gridList)) |
| | 6 | 564 | | return; |
| | | 565 | | |
| | 2206 | 566 | | foreach (ushort gridIndex in gridList) |
| | | 567 | | { |
| | 552 | 568 | | if (!world.ActiveGrids.IsAllocated(gridIndex) || !processedGrids.Add(gridIndex)) |
| | | 569 | | continue; |
| | | 570 | | |
| | 549 | 571 | | AddCoveredScanCellsForGrid( |
| | 549 | 572 | | world.ActiveGrids[gridIndex], |
| | 549 | 573 | | queryMin, |
| | 549 | 574 | | queryMax, |
| | 549 | 575 | | scanCells, |
| | 549 | 576 | | voxelRedundancyCheck); |
| | | 577 | | } |
| | 551 | 578 | | } |
| | | 579 | | |
| | | 580 | | private static void AddCoveredScanCellsForGrid( |
| | | 581 | | VoxelGrid currentGrid, |
| | | 582 | | Vector3d queryMin, |
| | | 583 | | Vector3d queryMax, |
| | | 584 | | SwiftList<ScanCell> scanCells, |
| | | 585 | | SwiftHashSet<ScanCell> voxelRedundancyCheck) |
| | | 586 | | { |
| | 549 | 587 | | if (currentGrid.Topology.Kind == GridTopologyKind.HexPrism) |
| | | 588 | | { |
| | 5 | 589 | | AddCoveredHexScanCellsForGrid( |
| | 5 | 590 | | currentGrid, |
| | 5 | 591 | | queryMin, |
| | 5 | 592 | | queryMax, |
| | 5 | 593 | | scanCells, |
| | 5 | 594 | | voxelRedundancyCheck); |
| | 5 | 595 | | return; |
| | | 596 | | } |
| | | 597 | | |
| | 544 | 598 | | if (!TryGetCoveredScanCellRange( |
| | 544 | 599 | | currentGrid, |
| | 544 | 600 | | queryMin, |
| | 544 | 601 | | queryMax, |
| | 544 | 602 | | out int xMin, |
| | 544 | 603 | | out int yMin, |
| | 544 | 604 | | out int zMin, |
| | 544 | 605 | | out int xMax, |
| | 544 | 606 | | out int yMax, |
| | 544 | 607 | | out int zMax)) |
| | | 608 | | { |
| | 3 | 609 | | return; |
| | | 610 | | } |
| | | 611 | | |
| | 541 | 612 | | currentGrid.AddScanCellsInRange( |
| | 541 | 613 | | xMin, |
| | 541 | 614 | | yMin, |
| | 541 | 615 | | zMin, |
| | 541 | 616 | | xMax, |
| | 541 | 617 | | yMax, |
| | 541 | 618 | | zMax, |
| | 541 | 619 | | scanCells, |
| | 541 | 620 | | voxelRedundancyCheck); |
| | 541 | 621 | | } |
| | | 622 | | |
| | | 623 | | private static IEnumerable<GridVoxelSet> TraceLineIterator( |
| | | 624 | | GridWorld world, |
| | | 625 | | Vector3d start, |
| | | 626 | | Vector3d end, |
| | | 627 | | Fixed64? padding, |
| | | 628 | | bool includeEnd) |
| | | 629 | | { |
| | 26 | 630 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping = new(); |
| | 26 | 631 | | SwiftHashSet<Voxel> voxelRedundancyCheck = SwiftHashSetPool<Voxel>.Shared.Rent(); |
| | 26 | 632 | | SwiftHashSet<ushort> processedGrids = SwiftHashSetPool<ushort>.Shared.Rent(); |
| | | 633 | | |
| | | 634 | | try |
| | | 635 | | { |
| | 26 | 636 | | AddTraceLineVoxelsToMapping( |
| | 26 | 637 | | world, |
| | 26 | 638 | | start, |
| | 26 | 639 | | end, |
| | 26 | 640 | | padding, |
| | 26 | 641 | | includeEnd, |
| | 26 | 642 | | gridVoxelMapping, |
| | 26 | 643 | | voxelRedundancyCheck, |
| | 26 | 644 | | processedGrids); |
| | | 645 | | |
| | 26 | 646 | | AddTraceLineEndVoxel( |
| | 26 | 647 | | world, |
| | 26 | 648 | | end, |
| | 26 | 649 | | includeEnd, |
| | 26 | 650 | | gridVoxelMapping, |
| | 26 | 651 | | voxelRedundancyCheck); |
| | | 652 | | |
| | 104 | 653 | | foreach (KeyValuePair<VoxelGrid, SwiftList<Voxel>> kvp in gridVoxelMapping) |
| | 26 | 654 | | yield return new GridVoxelSet(kvp.Key, kvp.Value); |
| | 26 | 655 | | } |
| | | 656 | | finally |
| | | 657 | | { |
| | 26 | 658 | | ReleaseGridVoxelMapping(gridVoxelMapping); |
| | 26 | 659 | | SwiftHashSetPool<Voxel>.Shared.Release(voxelRedundancyCheck); |
| | 26 | 660 | | SwiftHashSetPool<ushort>.Shared.Release(processedGrids); |
| | 26 | 661 | | } |
| | 26 | 662 | | } |
| | | 663 | | |
| | | 664 | | private static void AddTraceLineVoxelsToMapping( |
| | | 665 | | GridWorld world, |
| | | 666 | | Vector3d start, |
| | | 667 | | Vector3d end, |
| | | 668 | | Fixed64? padding, |
| | | 669 | | bool includeEnd, |
| | | 670 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping, |
| | | 671 | | SwiftHashSet<Voxel> voxelRedundancyCheck, |
| | | 672 | | SwiftHashSet<ushort> processedGrids) |
| | | 673 | | { |
| | 26 | 674 | | (Vector3d queryMin, Vector3d queryMax) = CreatePaddedOrderedBounds(start, end, padding); |
| | 26 | 675 | | (Vector3d candidateMin, Vector3d candidateMax) = |
| | 26 | 676 | | ExpandOrderedBounds(queryMin, queryMax, world.MaxTopologyCellEdge); |
| | | 677 | | |
| | 26 | 678 | | (int cellXMin, int cellYMin, int cellZMin, int cellXMax, int cellYMax, int cellZMax) = |
| | 26 | 679 | | world.GetSpatialGridCellBounds(candidateMin, candidateMax); |
| | | 680 | | |
| | 104 | 681 | | for (int cellZ = cellZMin; cellZ <= cellZMax; cellZ++) |
| | | 682 | | { |
| | 104 | 683 | | for (int cellY = cellYMin; cellY <= cellYMax; cellY++) |
| | | 684 | | { |
| | 110 | 685 | | for (int cellX = cellXMin; cellX <= cellXMax; cellX++) |
| | | 686 | | { |
| | 29 | 687 | | int cellIndex = SwiftHashTools.CombineHashCodes(cellX, cellY, cellZ); |
| | 29 | 688 | | if (!world.SpatialGridHash.TryGetValue(cellIndex, out SwiftHashSet<ushort> gridList)) |
| | | 689 | | continue; |
| | | 690 | | |
| | 28 | 691 | | AddTraceLineVoxelsForCell( |
| | 28 | 692 | | world, |
| | 28 | 693 | | gridList, |
| | 28 | 694 | | start, |
| | 28 | 695 | | end, |
| | 28 | 696 | | padding, |
| | 28 | 697 | | includeEnd, |
| | 28 | 698 | | gridVoxelMapping, |
| | 28 | 699 | | voxelRedundancyCheck, |
| | 28 | 700 | | processedGrids); |
| | | 701 | | } |
| | | 702 | | } |
| | | 703 | | } |
| | 26 | 704 | | } |
| | | 705 | | |
| | | 706 | | private static TraceLinePlan CreateTraceLinePlan( |
| | | 707 | | VoxelGrid grid, |
| | | 708 | | Vector3d start, |
| | | 709 | | Vector3d end, |
| | | 710 | | Fixed64? padding) |
| | | 711 | | { |
| | 19 | 712 | | (Vector3d snappedMin, Vector3d snappedMax) = |
| | 19 | 713 | | grid.NormalizeBounds(start, end, padding); |
| | | 714 | | |
| | 19 | 715 | | Vector3d traceStart = CreateTraceEndpoint(start, end, snappedMin, snappedMax, useMinWhenIncreasing: true); |
| | 19 | 716 | | Vector3d traceEnd = CreateTraceEndpoint(start, end, snappedMin, snappedMax, useMinWhenIncreasing: false); |
| | | 717 | | |
| | 19 | 718 | | Vector3d diff = traceEnd - traceStart; |
| | 19 | 719 | | Fixed64 steps = CalculateTraceSteps(grid, diff); |
| | | 720 | | |
| | 19 | 721 | | return new TraceLinePlan( |
| | 19 | 722 | | traceStart, |
| | 19 | 723 | | steps, |
| | 19 | 724 | | diff.X / (steps + Fixed64.One), |
| | 19 | 725 | | diff.Y / (steps + Fixed64.One), |
| | 19 | 726 | | diff.Z / (steps + Fixed64.One)); |
| | | 727 | | } |
| | | 728 | | |
| | | 729 | | private static Fixed64 CalculateTraceSteps(VoxelGrid grid, Vector3d diff) |
| | | 730 | | { |
| | 19 | 731 | | Vector3d delta = Vector3d.Abs(diff); |
| | 19 | 732 | | Fixed64 stepX = delta.X / grid.Topology.Metrics.CellWidth; |
| | 19 | 733 | | Fixed64 stepY = delta.Y / grid.Topology.Metrics.LayerHeight; |
| | 19 | 734 | | Fixed64 stepZ = delta.Z / grid.Topology.Metrics.CellLength; |
| | 19 | 735 | | return FixedMath.Ceil(FixedMath.Max(FixedMath.Max(stepX, stepY), stepZ)); |
| | | 736 | | } |
| | | 737 | | |
| | | 738 | | private static Vector3d CreateTraceEndpoint( |
| | | 739 | | Vector3d start, |
| | | 740 | | Vector3d end, |
| | | 741 | | Vector3d snappedMin, |
| | | 742 | | Vector3d snappedMax, |
| | | 743 | | bool useMinWhenIncreasing) |
| | | 744 | | { |
| | | 745 | | // Preserve the caller's trace direction while still using snapped bounds for coverage lookup. |
| | 54 | 746 | | return new Vector3d( |
| | 54 | 747 | | SelectTraceCoordinate(start.X, end.X, snappedMin.X, snappedMax.X, useMinWhenIncreasing), |
| | 54 | 748 | | SelectTraceCoordinate(start.Y, end.Y, snappedMin.Y, snappedMax.Y, useMinWhenIncreasing), |
| | 54 | 749 | | SelectTraceCoordinate(start.Z, end.Z, snappedMin.Z, snappedMax.Z, useMinWhenIncreasing)); |
| | | 750 | | } |
| | | 751 | | |
| | | 752 | | private static Fixed64 SelectTraceCoordinate( |
| | | 753 | | Fixed64 start, |
| | | 754 | | Fixed64 end, |
| | | 755 | | Fixed64 snappedMin, |
| | | 756 | | Fixed64 snappedMax, |
| | | 757 | | bool useMinWhenIncreasing) |
| | | 758 | | { |
| | 162 | 759 | | return (start <= end) == useMinWhenIncreasing ? snappedMin : snappedMax; |
| | | 760 | | } |
| | | 761 | | |
| | | 762 | | private static (Vector3d min, Vector3d max) CreatePaddedOrderedBounds( |
| | | 763 | | Vector3d min, |
| | | 764 | | Vector3d max, |
| | | 765 | | Fixed64? padding) |
| | | 766 | | { |
| | 766 | 767 | | Fixed64 fixedPadding = padding.HasValue && padding.Value > Fixed64.Zero |
| | 766 | 768 | | ? padding.Value |
| | 766 | 769 | | : Fixed64.Zero; |
| | | 770 | | |
| | 766 | 771 | | min -= fixedPadding; |
| | 766 | 772 | | max += fixedPadding; |
| | | 773 | | |
| | 766 | 774 | | (min.X, max.X) = min.X > max.X ? (max.X, min.X) : (min.X, max.X); |
| | 766 | 775 | | (min.Y, max.Y) = min.Y > max.Y ? (max.Y, min.Y) : (min.Y, max.Y); |
| | 766 | 776 | | (min.Z, max.Z) = min.Z > max.Z ? (max.Z, min.Z) : (min.Z, max.Z); |
| | | 777 | | |
| | 766 | 778 | | return (min, max); |
| | | 779 | | } |
| | | 780 | | |
| | | 781 | | private static (Vector3d min, Vector3d max) ExpandOrderedBounds( |
| | | 782 | | Vector3d min, |
| | | 783 | | Vector3d max, |
| | | 784 | | Fixed64 expansion) |
| | | 785 | | { |
| | 766 | 786 | | if (expansion <= Fixed64.Zero) |
| | 5 | 787 | | return (min, max); |
| | | 788 | | |
| | 761 | 789 | | return ( |
| | 761 | 790 | | new Vector3d(min.X - expansion, min.Y - expansion, min.Z - expansion), |
| | 761 | 791 | | new Vector3d(max.X + expansion, max.Y + expansion, max.Z + expansion)); |
| | | 792 | | } |
| | | 793 | | |
| | | 794 | | private static bool TryGetCoveredScanCellRange( |
| | | 795 | | VoxelGrid grid, |
| | | 796 | | Vector3d queryMin, |
| | | 797 | | Vector3d queryMax, |
| | | 798 | | out int xMin, |
| | | 799 | | out int yMin, |
| | | 800 | | out int zMin, |
| | | 801 | | out int xMax, |
| | | 802 | | out int yMax, |
| | | 803 | | out int zMax) |
| | | 804 | | { |
| | 544 | 805 | | xMin = 0; |
| | 544 | 806 | | yMin = 0; |
| | 544 | 807 | | zMin = 0; |
| | 544 | 808 | | xMax = 0; |
| | 544 | 809 | | yMax = 0; |
| | 544 | 810 | | zMax = 0; |
| | | 811 | | |
| | 544 | 812 | | (Vector3d snappedMin, Vector3d snappedMax) = grid.NormalizeBounds(queryMin, queryMax); |
| | 544 | 813 | | if (!TopologyVoxelRangeUtility.TryClipBoundsToGrid(grid, snappedMin, snappedMax, out Vector3d clippedMin, out Ve |
| | 3 | 814 | | return false; |
| | | 815 | | |
| | 541 | 816 | | (xMin, yMin, zMin) = grid.SnapToScanCell(clippedMin); |
| | 541 | 817 | | (xMax, yMax, zMax) = grid.SnapToScanCell(clippedMax); |
| | 541 | 818 | | return true; |
| | | 819 | | } |
| | | 820 | | |
| | | 821 | | private static void AddTraceLineVoxelsForCell( |
| | | 822 | | GridWorld world, |
| | | 823 | | SwiftHashSet<ushort> gridList, |
| | | 824 | | Vector3d start, |
| | | 825 | | Vector3d end, |
| | | 826 | | Fixed64? padding, |
| | | 827 | | bool includeEnd, |
| | | 828 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping, |
| | | 829 | | SwiftHashSet<Voxel> voxelRedundancyCheck, |
| | | 830 | | SwiftHashSet<ushort> processedGrids) |
| | | 831 | | { |
| | 118 | 832 | | foreach (ushort gridIndex in gridList) |
| | | 833 | | { |
| | 31 | 834 | | if (!world.ActiveGrids.IsAllocated(gridIndex) || !processedGrids.Add(gridIndex)) |
| | | 835 | | continue; |
| | | 836 | | |
| | 28 | 837 | | VoxelGrid currentGrid = world.ActiveGrids[gridIndex]; |
| | 28 | 838 | | if (!TryClipTraceSegmentToGrid( |
| | 28 | 839 | | currentGrid, |
| | 28 | 840 | | start, |
| | 28 | 841 | | end, |
| | 28 | 842 | | padding, |
| | 28 | 843 | | out Vector3d traceStart, |
| | 28 | 844 | | out Vector3d traceEnd, |
| | 28 | 845 | | out bool segmentEndsBeforeGlobalEnd)) |
| | | 846 | | { |
| | | 847 | | continue; |
| | | 848 | | } |
| | | 849 | | |
| | 27 | 850 | | SwiftList<Voxel> voxelList = SwiftListPool<Voxel>.Shared.Rent(); |
| | | 851 | | |
| | 27 | 852 | | AddTraceLineGridVoxels( |
| | 27 | 853 | | currentGrid, |
| | 27 | 854 | | traceStart, |
| | 27 | 855 | | traceEnd, |
| | 27 | 856 | | padding, |
| | 27 | 857 | | includeEnd || segmentEndsBeforeGlobalEnd, |
| | 27 | 858 | | voxelList, |
| | 27 | 859 | | voxelRedundancyCheck); |
| | | 860 | | |
| | 27 | 861 | | if (voxelList.Count > 0) |
| | 26 | 862 | | gridVoxelMapping.Add(currentGrid, voxelList); |
| | | 863 | | else |
| | 1 | 864 | | SwiftListPool<Voxel>.Shared.Release(voxelList); |
| | | 865 | | } |
| | 28 | 866 | | } |
| | | 867 | | |
| | | 868 | | private static void AddTraceLineGridVoxels( |
| | | 869 | | VoxelGrid currentGrid, |
| | | 870 | | Vector3d start, |
| | | 871 | | Vector3d end, |
| | | 872 | | Fixed64? padding, |
| | | 873 | | bool includeEnd, |
| | | 874 | | SwiftList<Voxel> voxelList, |
| | | 875 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 876 | | { |
| | 27 | 877 | | if (currentGrid.Topology.Kind == GridTopologyKind.HexPrism) |
| | | 878 | | { |
| | 8 | 879 | | AddHexTraceLineGridVoxels( |
| | 8 | 880 | | currentGrid, |
| | 8 | 881 | | start, |
| | 8 | 882 | | end, |
| | 8 | 883 | | padding, |
| | 8 | 884 | | includeEnd, |
| | 8 | 885 | | voxelList, |
| | 8 | 886 | | voxelRedundancyCheck); |
| | 8 | 887 | | return; |
| | | 888 | | } |
| | | 889 | | |
| | 19 | 890 | | TraceLinePlan plan = CreateTraceLinePlan(currentGrid, start, end, padding); |
| | | 891 | | |
| | 398 | 892 | | for (Fixed64 i = Fixed64.Zero; i <= plan.Steps; i += Fixed64.One) |
| | | 893 | | { |
| | 180 | 894 | | Vector3d tracePos = currentGrid.FloorToGrid( |
| | 180 | 895 | | new Vector3d( |
| | 180 | 896 | | plan.TraceStart.X + plan.StepX * i, |
| | 180 | 897 | | plan.TraceStart.Y + plan.StepY * i, |
| | 180 | 898 | | plan.TraceStart.Z + plan.StepZ * i)); |
| | | 899 | | |
| | 180 | 900 | | if (!currentGrid.TryGetVoxel(tracePos, out Voxel? voxel) || voxelRedundancyCheck.Add(voxel!) != true) |
| | | 901 | | continue; |
| | | 902 | | |
| | 158 | 903 | | voxelList.Add(voxel!); |
| | | 904 | | } |
| | | 905 | | |
| | 19 | 906 | | if (includeEnd) |
| | 15 | 907 | | AddTraceVoxelByPosition(currentGrid, end, voxelList, voxelRedundancyCheck); |
| | 19 | 908 | | } |
| | | 909 | | |
| | | 910 | | private static void AddHexTraceLineGridVoxels( |
| | | 911 | | VoxelGrid currentGrid, |
| | | 912 | | Vector3d start, |
| | | 913 | | Vector3d end, |
| | | 914 | | Fixed64? padding, |
| | | 915 | | bool includeEnd, |
| | | 916 | | SwiftList<Voxel> voxelList, |
| | | 917 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 918 | | { |
| | 8 | 919 | | CreateHexTraceEndpoints( |
| | 8 | 920 | | currentGrid, |
| | 8 | 921 | | start, |
| | 8 | 922 | | end, |
| | 8 | 923 | | padding, |
| | 8 | 924 | | out VoxelIndex startIndex, |
| | 8 | 925 | | out VoxelIndex endIndex); |
| | | 926 | | |
| | 8 | 927 | | int steps = CalculateHexTraceSteps(startIndex, endIndex); |
| | 8 | 928 | | if (steps == 0) |
| | | 929 | | { |
| | 1 | 930 | | AddTraceVoxelByIndex(currentGrid, startIndex, voxelList, voxelRedundancyCheck); |
| | 1 | 931 | | return; |
| | | 932 | | } |
| | | 933 | | |
| | 7 | 934 | | bool includeEndIndex = ShouldIncludeHexTraceEndIndex(currentGrid, end, endIndex, includeEnd); |
| | 7 | 935 | | int finalStep = includeEndIndex ? steps : steps - 1; |
| | 7 | 936 | | Fixed64 stepCount = new Fixed64(steps); |
| | 72 | 937 | | for (int i = 0; i <= finalStep; i++) |
| | | 938 | | { |
| | 29 | 939 | | Fixed64 t = new Fixed64(i) / stepCount; |
| | 29 | 940 | | VoxelIndex traceIndex = InterpolateHexTraceIndex(startIndex, endIndex, t); |
| | 29 | 941 | | AddTraceVoxelByIndex(currentGrid, traceIndex, voxelList, voxelRedundancyCheck); |
| | | 942 | | } |
| | 7 | 943 | | } |
| | | 944 | | |
| | | 945 | | private static void CreateHexTraceEndpoints( |
| | | 946 | | VoxelGrid grid, |
| | | 947 | | Vector3d start, |
| | | 948 | | Vector3d end, |
| | | 949 | | Fixed64? padding, |
| | | 950 | | out VoxelIndex startIndex, |
| | | 951 | | out VoxelIndex endIndex) |
| | | 952 | | { |
| | 8 | 953 | | (Vector3d snappedMin, Vector3d snappedMax) = grid.NormalizeBounds(start, end, padding); |
| | 8 | 954 | | Vector3d traceStart = grid.FloorToGrid(CreateTraceEndpoint( |
| | 8 | 955 | | start, |
| | 8 | 956 | | end, |
| | 8 | 957 | | snappedMin, |
| | 8 | 958 | | snappedMax, |
| | 8 | 959 | | useMinWhenIncreasing: true)); |
| | 8 | 960 | | Vector3d traceEnd = grid.FloorToGrid(CreateTraceEndpoint( |
| | 8 | 961 | | start, |
| | 8 | 962 | | end, |
| | 8 | 963 | | snappedMin, |
| | 8 | 964 | | snappedMax, |
| | 8 | 965 | | useMinWhenIncreasing: false)); |
| | | 966 | | |
| | 8 | 967 | | grid.TryGetVoxelIndex(traceStart, out startIndex); |
| | 8 | 968 | | grid.TryGetVoxelIndex(traceEnd, out endIndex); |
| | 8 | 969 | | } |
| | | 970 | | |
| | | 971 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 972 | | private static int CalculateHexTraceSteps(VoxelIndex start, VoxelIndex end) |
| | | 973 | | { |
| | 8 | 974 | | int qDelta = System.Math.Abs(end.x - start.x); |
| | 8 | 975 | | int rDelta = System.Math.Abs(end.z - start.z); |
| | 8 | 976 | | int sDelta = System.Math.Abs((-end.x - end.z) - (-start.x - start.z)); |
| | 8 | 977 | | int planarSteps = System.Math.Max(qDelta, System.Math.Max(rDelta, sDelta)); |
| | 8 | 978 | | int verticalSteps = System.Math.Abs(end.y - start.y); |
| | 8 | 979 | | return System.Math.Max(planarSteps, verticalSteps); |
| | | 980 | | } |
| | | 981 | | |
| | | 982 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 983 | | private static VoxelIndex InterpolateHexTraceIndex(VoxelIndex start, VoxelIndex end, Fixed64 t) |
| | | 984 | | { |
| | 29 | 985 | | Fixed64 q = Interpolate(new Fixed64(start.x), new Fixed64(end.x), t); |
| | 29 | 986 | | Fixed64 y = Interpolate(new Fixed64(start.y), new Fixed64(end.y), t); |
| | 29 | 987 | | Fixed64 r = Interpolate(new Fixed64(start.z), new Fixed64(end.z), t); |
| | 29 | 988 | | return HexCoordinateUtility.RoundAxial(q, y, r); |
| | | 989 | | } |
| | | 990 | | |
| | | 991 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 992 | | private static Fixed64 Interpolate(Fixed64 start, Fixed64 end, Fixed64 t) => |
| | 87 | 993 | | start + (end - start) * t; |
| | | 994 | | |
| | | 995 | | private static bool TryClipTraceSegmentToGrid( |
| | | 996 | | VoxelGrid grid, |
| | | 997 | | Vector3d start, |
| | | 998 | | Vector3d end, |
| | | 999 | | Fixed64? padding, |
| | | 1000 | | out Vector3d clippedStart, |
| | | 1001 | | out Vector3d clippedEnd, |
| | | 1002 | | out bool segmentEndsBeforeGlobalEnd) |
| | | 1003 | | { |
| | 28 | 1004 | | clippedStart = default; |
| | 28 | 1005 | | clippedEnd = default; |
| | 28 | 1006 | | segmentEndsBeforeGlobalEnd = false; |
| | | 1007 | | |
| | 28 | 1008 | | Fixed64 fixedPadding = padding.HasValue && padding.Value > Fixed64.Zero |
| | 28 | 1009 | | ? padding.Value |
| | 28 | 1010 | | : Fixed64.Zero; |
| | 28 | 1011 | | Vector3d boundsMin = grid.BoundsMin - fixedPadding; |
| | 28 | 1012 | | Vector3d boundsMax = grid.BoundsMax + fixedPadding; |
| | 28 | 1013 | | Fixed64 tMin = Fixed64.Zero; |
| | 28 | 1014 | | Fixed64 tMax = Fixed64.One; |
| | | 1015 | | |
| | 28 | 1016 | | if (!(ClipTraceSegmentAxis(start.X, end.X, boundsMin.X, boundsMax.X, ref tMin, ref tMax) |
| | 28 | 1017 | | && ClipTraceSegmentAxis(start.Y, end.Y, boundsMin.Y, boundsMax.Y, ref tMin, ref tMax) |
| | 28 | 1018 | | && ClipTraceSegmentAxis(start.Z, end.Z, boundsMin.Z, boundsMax.Z, ref tMin, ref tMax))) |
| | | 1019 | | { |
| | 1 | 1020 | | return false; |
| | | 1021 | | } |
| | | 1022 | | |
| | 27 | 1023 | | clippedStart = InterpolateTraceSegment(start, end, boundsMin, boundsMax, tMin); |
| | 27 | 1024 | | clippedEnd = InterpolateTraceSegment(start, end, boundsMin, boundsMax, tMax); |
| | 27 | 1025 | | segmentEndsBeforeGlobalEnd = tMax < Fixed64.One; |
| | 27 | 1026 | | return true; |
| | | 1027 | | } |
| | | 1028 | | |
| | | 1029 | | private static bool ClipTraceSegmentAxis( |
| | | 1030 | | Fixed64 start, |
| | | 1031 | | Fixed64 end, |
| | | 1032 | | Fixed64 boundsMin, |
| | | 1033 | | Fixed64 boundsMax, |
| | | 1034 | | ref Fixed64 tMin, |
| | | 1035 | | ref Fixed64 tMax) |
| | | 1036 | | { |
| | 84 | 1037 | | Fixed64 delta = end - start; |
| | 84 | 1038 | | if (delta == Fixed64.Zero) |
| | 38 | 1039 | | return start >= boundsMin && start <= boundsMax; |
| | | 1040 | | |
| | 46 | 1041 | | Fixed64 axisMin = (boundsMin - start) / delta; |
| | 46 | 1042 | | Fixed64 axisMax = (boundsMax - start) / delta; |
| | 46 | 1043 | | if (axisMin > axisMax) |
| | 5 | 1044 | | (axisMin, axisMax) = (axisMax, axisMin); |
| | | 1045 | | |
| | 46 | 1046 | | if (axisMin > tMin) |
| | 1 | 1047 | | tMin = axisMin; |
| | 46 | 1048 | | if (axisMax < tMax) |
| | 6 | 1049 | | tMax = axisMax; |
| | | 1050 | | |
| | 46 | 1051 | | return tMin <= tMax; |
| | | 1052 | | } |
| | | 1053 | | |
| | | 1054 | | private static bool ShouldIncludeHexTraceEndIndex( |
| | | 1055 | | VoxelGrid grid, |
| | | 1056 | | Vector3d end, |
| | | 1057 | | VoxelIndex endIndex, |
| | | 1058 | | bool includeEnd) |
| | | 1059 | | { |
| | 7 | 1060 | | if (includeEnd) |
| | 6 | 1061 | | return true; |
| | | 1062 | | |
| | 1 | 1063 | | return !grid.TryGetVoxelIndex(end, out VoxelIndex actualEndIndex) |
| | 1 | 1064 | | || actualEndIndex != endIndex; |
| | | 1065 | | } |
| | | 1066 | | |
| | | 1067 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1068 | | private static Vector3d InterpolateTraceSegment( |
| | | 1069 | | Vector3d start, |
| | | 1070 | | Vector3d end, |
| | | 1071 | | Vector3d boundsMin, |
| | | 1072 | | Vector3d boundsMax, |
| | | 1073 | | Fixed64 t) => |
| | 54 | 1074 | | new( |
| | 54 | 1075 | | InterpolateTraceAxis(start.X, end.X, boundsMin.X, boundsMax.X, t), |
| | 54 | 1076 | | InterpolateTraceAxis(start.Y, end.Y, boundsMin.Y, boundsMax.Y, t), |
| | 54 | 1077 | | InterpolateTraceAxis(start.Z, end.Z, boundsMin.Z, boundsMax.Z, t)); |
| | | 1078 | | |
| | | 1079 | | private static Fixed64 InterpolateTraceAxis( |
| | | 1080 | | Fixed64 start, |
| | | 1081 | | Fixed64 end, |
| | | 1082 | | Fixed64 boundsMin, |
| | | 1083 | | Fixed64 boundsMax, |
| | | 1084 | | Fixed64 t) |
| | | 1085 | | { |
| | 162 | 1086 | | Fixed64 delta = end - start; |
| | 162 | 1087 | | if (delta == Fixed64.Zero) |
| | 72 | 1088 | | return start; |
| | | 1089 | | |
| | 90 | 1090 | | if ((boundsMin - start) / delta == t) |
| | 18 | 1091 | | return boundsMin; |
| | 72 | 1092 | | if ((boundsMax - start) / delta == t) |
| | 17 | 1093 | | return boundsMax; |
| | | 1094 | | |
| | 55 | 1095 | | return start + delta * t; |
| | | 1096 | | } |
| | | 1097 | | |
| | | 1098 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1099 | | private static void AddTraceVoxelByPosition( |
| | | 1100 | | VoxelGrid grid, |
| | | 1101 | | Vector3d position, |
| | | 1102 | | SwiftList<Voxel> voxelList, |
| | | 1103 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 1104 | | { |
| | 15 | 1105 | | if (grid.TryGetVoxel(grid.FloorToGrid(position), out Voxel? voxel) |
| | 15 | 1106 | | && voxelRedundancyCheck.Add(voxel!)) |
| | | 1107 | | { |
| | 9 | 1108 | | voxelList.Add(voxel!); |
| | | 1109 | | } |
| | 15 | 1110 | | } |
| | | 1111 | | |
| | | 1112 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1113 | | private static void AddTraceVoxelByIndex( |
| | | 1114 | | VoxelGrid grid, |
| | | 1115 | | VoxelIndex index, |
| | | 1116 | | SwiftList<Voxel> voxelList, |
| | | 1117 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 1118 | | { |
| | 33 | 1119 | | if (grid.TryGetVoxel(index, out Voxel? voxel) |
| | 33 | 1120 | | && voxelRedundancyCheck.Add(voxel!)) |
| | | 1121 | | { |
| | 29 | 1122 | | voxelList.Add(voxel!); |
| | | 1123 | | } |
| | 33 | 1124 | | } |
| | | 1125 | | |
| | | 1126 | | private static void AddTraceLineEndVoxel( |
| | | 1127 | | GridWorld world, |
| | | 1128 | | Vector3d end, |
| | | 1129 | | bool includeEnd, |
| | | 1130 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping, |
| | | 1131 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 1132 | | { |
| | 26 | 1133 | | if (!includeEnd |
| | 26 | 1134 | | || !world.TryGetGridAndVoxel(end, out VoxelGrid? endGrid, out Voxel? endVoxel) |
| | 26 | 1135 | | || !voxelRedundancyCheck.Add(endVoxel!)) |
| | | 1136 | | { |
| | 26 | 1137 | | return; |
| | | 1138 | | } |
| | | 1139 | | |
| | 0 | 1140 | | if (!gridVoxelMapping.TryGetValue(endGrid!, out SwiftList<Voxel> endVoxelList)) |
| | | 1141 | | { |
| | 0 | 1142 | | endVoxelList = SwiftListPool<Voxel>.Shared.Rent(); |
| | 0 | 1143 | | gridVoxelMapping.Add(endGrid!, endVoxelList); |
| | | 1144 | | } |
| | | 1145 | | |
| | 0 | 1146 | | endVoxelList.Add(endVoxel!); |
| | 0 | 1147 | | } |
| | | 1148 | | |
| | | 1149 | | private static void ReleaseGridVoxelMapping(SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping) |
| | | 1150 | | { |
| | 844 | 1151 | | foreach (KeyValuePair<VoxelGrid, SwiftList<Voxel>> kvp in gridVoxelMapping) |
| | 211 | 1152 | | SwiftListPool<Voxel>.Shared.Release(kvp.Value); |
| | 211 | 1153 | | } |
| | | 1154 | | |
| | | 1155 | | private static IEnumerable<GridVoxelSet> GetCoveredVoxelsIterator( |
| | | 1156 | | GridWorld world, |
| | | 1157 | | Vector3d boundsMin, |
| | | 1158 | | Vector3d boundsMax, |
| | | 1159 | | Fixed64? padding) |
| | | 1160 | | { |
| | 185 | 1161 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping = new(); |
| | 185 | 1162 | | SwiftHashSet<Voxel> voxelRedundancyCheck = SwiftHashSetPool<Voxel>.Shared.Rent(); |
| | 185 | 1163 | | SwiftHashSet<ushort> processedGrids = SwiftHashSetPool<ushort>.Shared.Rent(); |
| | | 1164 | | |
| | | 1165 | | try |
| | | 1166 | | { |
| | 185 | 1167 | | AddCoveredVoxelsToMapping( |
| | 185 | 1168 | | world, |
| | 185 | 1169 | | boundsMin, |
| | 185 | 1170 | | boundsMax, |
| | 185 | 1171 | | padding, |
| | 185 | 1172 | | gridVoxelMapping, |
| | 185 | 1173 | | voxelRedundancyCheck, |
| | 185 | 1174 | | processedGrids); |
| | | 1175 | | |
| | 740 | 1176 | | foreach (KeyValuePair<VoxelGrid, SwiftList<Voxel>> kvp in gridVoxelMapping) |
| | 185 | 1177 | | yield return new GridVoxelSet(kvp.Key, kvp.Value); |
| | 185 | 1178 | | } |
| | | 1179 | | finally |
| | | 1180 | | { |
| | 185 | 1181 | | ReleaseGridVoxelMapping(gridVoxelMapping); |
| | 185 | 1182 | | SwiftHashSetPool<Voxel>.Shared.Release(voxelRedundancyCheck); |
| | 185 | 1183 | | SwiftHashSetPool<ushort>.Shared.Release(processedGrids); |
| | 185 | 1184 | | } |
| | 185 | 1185 | | } |
| | | 1186 | | |
| | | 1187 | | private static void AddCoveredVoxelsToMapping( |
| | | 1188 | | GridWorld world, |
| | | 1189 | | Vector3d boundsMin, |
| | | 1190 | | Vector3d boundsMax, |
| | | 1191 | | Fixed64? padding, |
| | | 1192 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping, |
| | | 1193 | | SwiftHashSet<Voxel> voxelRedundancyCheck, |
| | | 1194 | | SwiftHashSet<ushort> processedGrids) |
| | | 1195 | | { |
| | 185 | 1196 | | (Vector3d queryMin, Vector3d queryMax) = |
| | 185 | 1197 | | CreatePaddedOrderedBounds(boundsMin, boundsMax, padding); |
| | 185 | 1198 | | (Vector3d candidateMin, Vector3d candidateMax) = |
| | 185 | 1199 | | ExpandOrderedBounds(queryMin, queryMax, world.MaxTopologyCellEdge); |
| | | 1200 | | |
| | 185 | 1201 | | (int cellXMin, int cellYMin, int cellZMin, int cellXMax, int cellYMax, int cellZMax) = |
| | 185 | 1202 | | world.GetSpatialGridCellBounds(candidateMin, candidateMax); |
| | | 1203 | | |
| | 754 | 1204 | | for (int cellZ = cellZMin; cellZ <= cellZMax; cellZ++) |
| | | 1205 | | { |
| | 768 | 1206 | | for (int cellY = cellYMin; cellY <= cellYMax; cellY++) |
| | | 1207 | | { |
| | 804 | 1208 | | for (int cellX = cellXMin; cellX <= cellXMax; cellX++) |
| | | 1209 | | { |
| | 210 | 1210 | | int cellIndex = SwiftHashTools.CombineHashCodes(cellX, cellY, cellZ); |
| | 210 | 1211 | | if (world.SpatialGridHash.TryGetValue(cellIndex, out SwiftHashSet<ushort> gridList)) |
| | | 1212 | | { |
| | 200 | 1213 | | AddCoveredVoxelsForCell( |
| | 200 | 1214 | | world, |
| | 200 | 1215 | | gridList, |
| | 200 | 1216 | | queryMin, |
| | 200 | 1217 | | queryMax, |
| | 200 | 1218 | | gridVoxelMapping, |
| | 200 | 1219 | | voxelRedundancyCheck, |
| | 200 | 1220 | | processedGrids); |
| | | 1221 | | } |
| | | 1222 | | } |
| | | 1223 | | } |
| | | 1224 | | } |
| | 185 | 1225 | | } |
| | | 1226 | | |
| | | 1227 | | private static void AddCoveredVoxelsForCell( |
| | | 1228 | | GridWorld world, |
| | | 1229 | | SwiftHashSet<ushort> gridList, |
| | | 1230 | | Vector3d queryMin, |
| | | 1231 | | Vector3d queryMax, |
| | | 1232 | | SwiftDictionary<VoxelGrid, SwiftList<Voxel>> gridVoxelMapping, |
| | | 1233 | | SwiftHashSet<Voxel> voxelRedundancyCheck, |
| | | 1234 | | SwiftHashSet<ushort> processedGrids) |
| | | 1235 | | { |
| | 814 | 1236 | | foreach (ushort gridIndex in gridList) |
| | | 1237 | | { |
| | 207 | 1238 | | if (!world.ActiveGrids.IsAllocated(gridIndex) || !processedGrids.Add(gridIndex)) |
| | | 1239 | | continue; |
| | | 1240 | | |
| | 189 | 1241 | | VoxelGrid currentGrid = world.ActiveGrids[gridIndex]; |
| | 189 | 1242 | | SwiftList<Voxel> voxelList = SwiftListPool<Voxel>.Shared.Rent(); |
| | 189 | 1243 | | AddCoveredGridVoxels(currentGrid, queryMin, queryMax, voxelList, voxelRedundancyCheck); |
| | | 1244 | | |
| | 189 | 1245 | | if (voxelList.Count > 0) |
| | 185 | 1246 | | gridVoxelMapping.Add(currentGrid, voxelList); |
| | | 1247 | | else |
| | 4 | 1248 | | SwiftListPool<Voxel>.Shared.Release(voxelList); |
| | | 1249 | | } |
| | 200 | 1250 | | } |
| | | 1251 | | |
| | | 1252 | | private static void AddCoveredGridVoxels( |
| | | 1253 | | VoxelGrid currentGrid, |
| | | 1254 | | Vector3d queryMin, |
| | | 1255 | | Vector3d queryMax, |
| | | 1256 | | SwiftList<Voxel> voxelList, |
| | | 1257 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 1258 | | { |
| | 193 | 1259 | | if (currentGrid.Topology.Kind == GridTopologyKind.HexPrism) |
| | | 1260 | | { |
| | 7 | 1261 | | AddCoveredHexGridVoxels( |
| | 7 | 1262 | | currentGrid, |
| | 7 | 1263 | | queryMin, |
| | 7 | 1264 | | queryMax, |
| | 7 | 1265 | | voxelList, |
| | 7 | 1266 | | voxelRedundancyCheck); |
| | 7 | 1267 | | return; |
| | | 1268 | | } |
| | | 1269 | | |
| | 186 | 1270 | | if (!TopologyVoxelRangeUtility.TryGetCandidateRange( |
| | 186 | 1271 | | currentGrid, |
| | 186 | 1272 | | queryMin, |
| | 186 | 1273 | | queryMax, |
| | 186 | 1274 | | out VoxelIndex minIndex, |
| | 186 | 1275 | | out VoxelIndex maxIndex)) |
| | | 1276 | | { |
| | 1 | 1277 | | return; |
| | | 1278 | | } |
| | | 1279 | | |
| | 185 | 1280 | | currentGrid.AddVoxelsInIndexRange(minIndex, maxIndex, voxelList, voxelRedundancyCheck); |
| | 185 | 1281 | | } |
| | | 1282 | | |
| | | 1283 | | private static void AddCoveredHexGridVoxels( |
| | | 1284 | | VoxelGrid currentGrid, |
| | | 1285 | | Vector3d queryMin, |
| | | 1286 | | Vector3d queryMax, |
| | | 1287 | | SwiftList<Voxel> voxelList, |
| | | 1288 | | SwiftHashSet<Voxel> voxelRedundancyCheck) |
| | | 1289 | | { |
| | 7 | 1290 | | if (!TopologyVoxelRangeUtility.TryGetCandidateRange( |
| | 7 | 1291 | | currentGrid, |
| | 7 | 1292 | | queryMin, |
| | 7 | 1293 | | queryMax, |
| | 7 | 1294 | | out VoxelIndex minIndex, |
| | 7 | 1295 | | out VoxelIndex maxIndex)) |
| | | 1296 | | { |
| | 1 | 1297 | | return; |
| | | 1298 | | } |
| | | 1299 | | |
| | 6 | 1300 | | Fixed64 horizontalExpansion = currentGrid.Topology.Metrics.CellRadius; |
| | 6 | 1301 | | Fixed64 coverageMinX = queryMin.X - horizontalExpansion; |
| | 6 | 1302 | | Fixed64 coverageMaxX = queryMax.X + horizontalExpansion; |
| | 6 | 1303 | | Fixed64 coverageMinZ = queryMin.Z - horizontalExpansion; |
| | 6 | 1304 | | Fixed64 coverageMaxZ = queryMax.Z + horizontalExpansion; |
| | | 1305 | | |
| | 36 | 1306 | | for (int x = minIndex.x; x <= maxIndex.x; x++) |
| | | 1307 | | { |
| | 48 | 1308 | | for (int y = minIndex.y; y <= maxIndex.y; y++) |
| | | 1309 | | { |
| | 68 | 1310 | | for (int z = minIndex.z; z <= maxIndex.z; z++) |
| | | 1311 | | { |
| | 22 | 1312 | | if (currentGrid.TryGetVoxel(x, y, z, out Voxel? voxel) |
| | 22 | 1313 | | && IsHexVoxelCenterInHorizontalCoverage( |
| | 22 | 1314 | | voxel!, |
| | 22 | 1315 | | coverageMinX, |
| | 22 | 1316 | | coverageMaxX, |
| | 22 | 1317 | | coverageMinZ, |
| | 22 | 1318 | | coverageMaxZ) |
| | 22 | 1319 | | && voxelRedundancyCheck.Add(voxel!)) |
| | | 1320 | | { |
| | 11 | 1321 | | voxelList.Add(voxel!); |
| | | 1322 | | } |
| | | 1323 | | } |
| | | 1324 | | } |
| | | 1325 | | } |
| | 6 | 1326 | | } |
| | | 1327 | | |
| | | 1328 | | private static void AddCoveredHexScanCellsForGrid( |
| | | 1329 | | VoxelGrid currentGrid, |
| | | 1330 | | Vector3d queryMin, |
| | | 1331 | | Vector3d queryMax, |
| | | 1332 | | SwiftList<ScanCell> scanCells, |
| | | 1333 | | SwiftHashSet<ScanCell> scanCellRedundancyCheck) |
| | | 1334 | | { |
| | 5 | 1335 | | if (!TopologyVoxelRangeUtility.TryGetCandidateRange( |
| | 5 | 1336 | | currentGrid, |
| | 5 | 1337 | | queryMin, |
| | 5 | 1338 | | queryMax, |
| | 5 | 1339 | | out VoxelIndex minIndex, |
| | 5 | 1340 | | out VoxelIndex maxIndex)) |
| | | 1341 | | { |
| | 1 | 1342 | | return; |
| | | 1343 | | } |
| | | 1344 | | |
| | 4 | 1345 | | currentGrid.AddScanCellsInRange( |
| | 4 | 1346 | | minIndex.x / currentGrid.ScanCellSize, |
| | 4 | 1347 | | minIndex.y / currentGrid.ScanCellSize, |
| | 4 | 1348 | | minIndex.z / currentGrid.ScanCellSize, |
| | 4 | 1349 | | maxIndex.x / currentGrid.ScanCellSize, |
| | 4 | 1350 | | maxIndex.y / currentGrid.ScanCellSize, |
| | 4 | 1351 | | maxIndex.z / currentGrid.ScanCellSize, |
| | 4 | 1352 | | scanCells, |
| | 4 | 1353 | | scanCellRedundancyCheck); |
| | 4 | 1354 | | } |
| | | 1355 | | |
| | | 1356 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 1357 | | private static bool IsHexVoxelCenterInHorizontalCoverage( |
| | | 1358 | | Voxel voxel, |
| | | 1359 | | Fixed64 coverageMinX, |
| | | 1360 | | Fixed64 coverageMaxX, |
| | | 1361 | | Fixed64 coverageMinZ, |
| | | 1362 | | Fixed64 coverageMaxZ) |
| | | 1363 | | { |
| | 20 | 1364 | | Vector3d position = voxel.WorldPosition; |
| | 20 | 1365 | | return position.X >= coverageMinX |
| | 20 | 1366 | | && position.X <= coverageMaxX |
| | 20 | 1367 | | && position.Z >= coverageMinZ |
| | 20 | 1368 | | && position.Z <= coverageMaxZ; |
| | | 1369 | | } |
| | | 1370 | | |
| | | 1371 | | private static IEnumerable<ScanCell> GetCoveredScanCellsIterator( |
| | | 1372 | | GridWorld world, |
| | | 1373 | | Vector3d boundsMin, |
| | | 1374 | | Vector3d boundsMax, |
| | | 1375 | | Fixed64? padding) |
| | | 1376 | | { |
| | 11 | 1377 | | SwiftList<ScanCell> scanCells = SwiftListPool<ScanCell>.Shared.Rent(); |
| | 11 | 1378 | | SwiftHashSet<ushort> processedGrids = SwiftHashSetPool<ushort>.Shared.Rent(); |
| | 11 | 1379 | | SwiftHashSet<ScanCell> voxelRedundancyCheck = SwiftHashSetPool<ScanCell>.Shared.Rent(); |
| | | 1380 | | |
| | | 1381 | | try |
| | | 1382 | | { |
| | 11 | 1383 | | AddCoveredScanCellsCore( |
| | 11 | 1384 | | world, |
| | 11 | 1385 | | boundsMin, |
| | 11 | 1386 | | boundsMax, |
| | 11 | 1387 | | scanCells, |
| | 11 | 1388 | | processedGrids, |
| | 11 | 1389 | | voxelRedundancyCheck, |
| | 11 | 1390 | | padding); |
| | | 1391 | | |
| | 88 | 1392 | | foreach (ScanCell scanCell in scanCells) |
| | 33 | 1393 | | yield return scanCell; |
| | 11 | 1394 | | } |
| | | 1395 | | finally |
| | | 1396 | | { |
| | 11 | 1397 | | SwiftListPool<ScanCell>.Shared.Release(scanCells); |
| | 11 | 1398 | | SwiftHashSetPool<ushort>.Shared.Release(processedGrids); |
| | 11 | 1399 | | SwiftHashSetPool<ScanCell>.Shared.Release(voxelRedundancyCheck); |
| | 11 | 1400 | | } |
| | 11 | 1401 | | } |
| | | 1402 | | } |