| | | 1 | | //======================================================================= |
| | | 2 | | // SwiftFixedSpatialHash.cs |
| | | 3 | | //======================================================================= |
| | | 4 | | // MIT License, Copyright (c) 2024–present David Oravsky (mrdav30) |
| | | 5 | | // See LICENSE file in the project root for full license information. |
| | | 6 | | //======================================================================= |
| | | 7 | | |
| | | 8 | | using System; |
| | | 9 | | using System.Runtime.CompilerServices; |
| | | 10 | | using FixedMathSharp; |
| | | 11 | | |
| | | 12 | | namespace SwiftCollections.Query; |
| | | 13 | | |
| | | 14 | | /// <summary> |
| | | 15 | | /// Represents a fixed-point spatial hash optimized for deterministic broad-phase spatial queries. |
| | | 16 | | /// </summary> |
| | | 17 | | public class SwiftFixedSpatialHash<T> : SwiftSpatialHash<T, FixedBoundVolume> |
| | | 18 | | { |
| | | 19 | | private readonly Fixed64 _cellSize; |
| | | 20 | | |
| | | 21 | | /// <summary> |
| | | 22 | | /// Initializes a new instance of the <see cref="SwiftFixedSpatialHash{T}"/> class with the specified capacity and c |
| | | 23 | | /// </summary> |
| | | 24 | | public SwiftFixedSpatialHash(int capacity, Fixed64 cellSize) |
| | 26 | 25 | | : this(capacity, cellSize, SwiftSpatialHashOptions.Default) { } |
| | | 26 | | |
| | | 27 | | /// <summary> |
| | | 28 | | /// Initializes a new instance of the <see cref="SwiftFixedSpatialHash{T}"/> class with the specified capacity, cell |
| | | 29 | | /// </summary> |
| | | 30 | | public SwiftFixedSpatialHash(int capacity, Fixed64 cellSize, SwiftSpatialHashOptions options) |
| | 14 | 31 | | : base(capacity, new FixedBoundVolumeCellMapper(cellSize), options) |
| | | 32 | | { |
| | 12 | 33 | | _cellSize = cellSize; |
| | 12 | 34 | | } |
| | | 35 | | |
| | | 36 | | /// <summary> |
| | | 37 | | /// Collects broad-phase candidates registered in the spatial-hash cell |
| | | 38 | | /// containing a point. Callers remain responsible for exact filtering. |
| | | 39 | | /// </summary> |
| | | 40 | | /// <param name="point">The fixed-point position whose cell should be queried.</param> |
| | | 41 | | /// <param name="results">The caller-owned result collection.</param> |
| | | 42 | | public void CollectPointCandidates(Vector3d point, System.Collections.Generic.ICollection<T> results) => |
| | 8 | 43 | | CollectCellCandidates(GetCellIndex(point), results); |
| | | 44 | | |
| | | 45 | | /// <summary> |
| | | 46 | | /// Maps a fixed-point position to its exact spatial-hash cell. |
| | | 47 | | /// </summary> |
| | | 48 | | /// <param name="point">The fixed-point position to map.</param> |
| | | 49 | | /// <returns>The containing spatial-hash cell.</returns> |
| | | 50 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 51 | | public SwiftSpatialHashCellIndex GetCellIndex(Vector3d point) => |
| | 9 | 52 | | new( |
| | 9 | 53 | | ToCell(point.X, _cellSize), |
| | 9 | 54 | | ToCell(point.Y, _cellSize), |
| | 9 | 55 | | ToCell(point.Z, _cellSize)); |
| | | 56 | | |
| | | 57 | | [MethodImpl(MethodImplOptions.AggressiveInlining)] |
| | | 58 | | private static int ToCell(Fixed64 value, Fixed64 cellSize) |
| | | 59 | | { |
| | 141 | 60 | | if ((uint)cellSize.m_rawValue == 0U) |
| | | 61 | | { |
| | 105 | 62 | | int wholeValue = (int)(value.m_rawValue >> 32); |
| | 105 | 63 | | int wholeCellSize = (int)(cellSize.m_rawValue >> 32); |
| | 105 | 64 | | int wholeQuotient = wholeValue / wholeCellSize; |
| | 105 | 65 | | return wholeValue % wholeCellSize < 0 |
| | 105 | 66 | | ? wholeQuotient - 1 |
| | 105 | 67 | | : wholeQuotient; |
| | | 68 | | } |
| | | 69 | | |
| | 36 | 70 | | long quotient = value.m_rawValue / cellSize.m_rawValue; |
| | 36 | 71 | | if (value.m_rawValue % cellSize.m_rawValue < 0L) |
| | 9 | 72 | | quotient--; |
| | | 73 | | |
| | 36 | 74 | | if (quotient < int.MinValue) |
| | 9 | 75 | | return int.MinValue; |
| | 27 | 76 | | if (quotient > int.MaxValue) |
| | 9 | 77 | | return int.MaxValue; |
| | 18 | 78 | | return (int)quotient; |
| | | 79 | | } |
| | | 80 | | |
| | | 81 | | private sealed class FixedBoundVolumeCellMapper : ISpatialHashCellMapper<FixedBoundVolume> |
| | | 82 | | { |
| | | 83 | | private readonly Fixed64 _cellSize; |
| | | 84 | | |
| | 14 | 85 | | public FixedBoundVolumeCellMapper(Fixed64 cellSize) |
| | | 86 | | { |
| | 14 | 87 | | if (cellSize <= Fixed64.Zero) |
| | 2 | 88 | | throw new ArgumentOutOfRangeException(nameof(cellSize), cellSize, "Cell size must be greater than zero." |
| | | 89 | | |
| | 12 | 90 | | _cellSize = cellSize; |
| | 12 | 91 | | } |
| | | 92 | | |
| | | 93 | | public void GetCellRange(FixedBoundVolume bounds, out SwiftSpatialHashCellIndex minCell, out SwiftSpatialHashCel |
| | | 94 | | { |
| | 19 | 95 | | minCell = new SwiftSpatialHashCellIndex( |
| | 19 | 96 | | ToCell(bounds.Min.X), |
| | 19 | 97 | | ToCell(bounds.Min.Y), |
| | 19 | 98 | | ToCell(bounds.Min.Z)); |
| | | 99 | | |
| | 19 | 100 | | maxCell = new SwiftSpatialHashCellIndex( |
| | 19 | 101 | | ToCell(bounds.Max.X), |
| | 19 | 102 | | ToCell(bounds.Max.Y), |
| | 19 | 103 | | ToCell(bounds.Max.Z)); |
| | 19 | 104 | | } |
| | | 105 | | |
| | | 106 | | private int ToCell(Fixed64 value) |
| | | 107 | | { |
| | 114 | 108 | | return SwiftFixedSpatialHash<T>.ToCell(value, _cellSize); |
| | | 109 | | } |
| | | 110 | | } |
| | | 111 | | } |