spatial index works probably

1 files changed, 17 insertions, 7 deletions

diff --git a/server/client-lib/src/spatial_index.rs b/server/client-lib/src/spatial_index.rs
index 3f0d5ad0..de8543f1 100644
--- a/server/client-lib/src/spatial_index.rs
+++ b/server/client-lib/src/spatial_index.rs
@@ -15,17 +15,19 @@
     along with this program.  If not, see <https://www.gnu.org/licenses/>.
 
 */
-use hurrycurry_protocol::glam::{IVec2, Vec2};
+use hurrycurry_protocol::glam::{ivec2, IVec2, Vec2};
 use std::{collections::HashMap, hash::Hash};
 
-// TODO stub implementation. please implement
 pub struct SpatialIndex<T> {
     entries: HashMap<T, Vec2>,
     bins: HashMap<IVec2, Vec<T>>,
 }
 
+// TODO maybe dont use 1x1 bins but something bigger like 10x10 for better perf
+
 impl<T: Eq + Hash + Copy> SpatialIndex<T> {
     pub fn update_entry(&mut self, id: T, position: Vec2) {
+        self.remove_entry(id);
         self.entries.insert(id, position);
         let e = self.bins.entry(position.as_ivec2()).or_default();
         if !e.contains(&id) {
@@ -46,12 +48,20 @@ impl<T: Eq + Hash + Copy> SpatialIndex<T> {
         }
     }
     pub fn query(&self, position: Vec2, radius: f32, mut cb: impl FnMut(T, Vec2)) {
-        // TODO query bins
-        self.all(|pl, p| {
-            if p.distance(position) < radius {
-                cb(pl, p)
+        let p = position.as_ivec2();
+        let r = radius.ceil() as i32;
+        for xo in -r..=r {
+            for yo in -r..=r {
+                if let Some(bin) = self.bins.get(&(p + ivec2(xo, yo))) {
+                    for &id in bin {
+                        let p = *self.entries.get(&id).unwrap();
+                        if p.distance(position) < radius {
+                            cb(id, p)
+                        }
+                    }
+                }
             }
-        })
+        }
     }
 }
 impl<T> Default for SpatialIndex<T> {