1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165

use arrayvec::ArrayVec;

pub const FIXED_SIZE: usize = 32;

/// A rectangular entity. **Identifier must be unique.**
#[derive(Debug, Clone)]
#[repr(C)]
pub struct Entity {
    /// Identifier must be unique.
    pub id: u32,

    pub x: u32,
    pub y: u32,
    pub width: u32,
    pub height: u32,
}

/// A rectangular query region.
#[derive(Debug, Clone)]
pub struct Query {
    pub x: u32,
    pub y: u32,
    pub width: u32,
    pub height: u32,
}

impl From<Entity> for Query {
    fn from(value: Entity) -> Self {
        Self {
            x: value.x,
            y: value.y,
            width: value.width,
            height: value.height,
        }
    }
}

#[derive(Debug, Clone, Default)]
struct Entry(ArrayVec<u32, FIXED_SIZE>);

/// An extremely optimized fixed-size hash table implementation.
#[derive(Debug, Clone)]
pub struct Table<T: Default + Clone> {
    entries: Vec<T>,
}

impl<T: Default + Clone> Table<T> {
	/// Create a new table with `size` entries.
    pub fn new(size: usize) -> Self {
        let entries = vec![T::default(); size * 1000];
        Self {
            entries,
        }
    }

    #[inline(always)]
    fn index(&self, idx: u64) -> usize {
        (hash_u64(idx) % self.entries.len() as u64) as usize
    }

	/// Get a mutable reference to an entry from a 2D key.
    #[inline(always)]
    pub fn get_vector_mut(&mut self, x: u32, y: u32) -> &mut T {
        let idx = self.index(vector_hash(x, y));
        unsafe { self.entries.get_unchecked_mut(idx) }
    }

	/// Get a reference to an entry from a 2D key.
    #[inline(always)]
    pub fn get_vector(&self, x: u32, y: u32) -> &T {
        let idx = self.index(vector_hash(x, y));
        unsafe { self.entries.get_unchecked(idx) }
    }

	/// Get a reference to an entry from a scalar key.
    #[inline(always)]
    pub fn get_scalar(&self, s: u32) -> &T {
        let idx = self.index(hash_u64(s as u64));
        unsafe { self.entries.get_unchecked(idx) }
    }

	/// Get a mutable reference to an entry from a scalar key.
    #[inline(always)]
    pub fn get_scalar_mut(&mut self, s: u32) -> &mut T {
        let idx = self.index(hash_u64(s as u64));
        unsafe { self.entries.get_unchecked_mut(idx) }
    }

	/// Clear the table.
    pub fn clear(&mut self) {
        self.entries.clear();
    }
}

/// Spatial hash grid implementation.
#[derive(Debug, Clone)]
pub struct Grid {
    grid: Table<Entry>,
    shift: u32,
}

impl Grid {
	/// Create a new grid with a fixed bucket size and cell size.
    pub fn new(size: usize, shift: u32) -> Self {
        Self {
            grid: Table::new(size),
            shift,
        }
    }

	/// Insert an entity.
    pub fn insert(&mut self, entity: &Entity) {
        let sx = entity.x >> self.shift;
        let sy = entity.y >> self.shift;

        let ex = (entity.x + entity.width) >> self.shift;
        let ey = (entity.y + entity.height) >> self.shift;

        for y in sy..=ey {
            for x in sx..=ex {
				let cell = self.grid.get_vector_mut(x, y);
                unsafe { cell.0.push_unchecked(entity.id) };
            }
        }
    }

	/// Retrieve entities in a region.
    pub fn query(&mut self, query: &Query) -> Vec<u32> {
        let mut result = Vec::new();

        let sx = query.x >> self.shift;
        let sy = query.y >> self.shift;

        let ex = (query.x + query.width) >> self.shift;
        let ey = (query.y + query.height) >> self.shift;

        for y in sy..=ey {
            for x in sx..=ex {
                let region = self.grid.get_vector(x, y);
                for id in region.0.iter() {
                    if !result.contains(id) {
                        result.push(*id);
                    }
                }
            }
        }
        result
    }

	/// Clear the grid.
    pub fn clear(&mut self) {
        self.grid.clear();
    }
}

#[inline]
fn vector_hash(x: u32, y: u32) -> u64 {
	((x as u64) << 32) | y as u64
}

/// Identity hash for now
#[inline]
fn hash_u64(seed: u64) -> u64 {
    seed
}