Add spatial index

examples/chunk.rs

@@ -7,7 +7,7 @@ use valence::client::GameMode;
 use valence::config::{Config, ServerListPing};
 use valence::text::Color;
 use valence::{
-    async_trait, ChunkPos, ClientMut, DimensionId, EntityType, Server, ShutdownResult, Text,
+    async_trait, ChunkPos, ClientMut, DimensionId, Server, ShutdownResult, Text,
     TextFormat, WorldId, WorldsMut,
 };
 

src/bvh.rs

@@ -2,7 +2,7 @@ use std::mem;
 
 use approx::relative_eq;
 use rayon::iter::{IntoParallelRefIterator, ParallelIterator};
-use vek::Aabr;
+use vek::Aabb;
 
 #[derive(Clone)]
 pub struct Bvh<T> {
@@ -11,19 +11,27 @@ pub struct Bvh<T> {
     root: NodeIdx,
 }
 
+#[derive(Clone, Copy, PartialEq, Eq)]
+pub enum TraverseStep<T> {
+    Miss,
+    Hit,
+    Return(T),
+}
+
 #[derive(Clone)]
 struct InternalNode {
-    bb: Aabr<f32>,
+    bb: Aabb<f64>,
     left: NodeIdx,
     right: NodeIdx,
 }
 
 #[derive(Clone)]
 struct LeafNode<T> {
-    bb: Aabr<f32>,
+    bb: Aabb<f64>,
     id: T,
 }
 
+// TODO: we could use usize here to store more elements.
 type NodeIdx = u32;
 
 impl<T: Send + Sync> Bvh<T> {
@@ -35,7 +43,7 @@ impl<T: Send + Sync> Bvh<T> {
         }
     }
 
-    pub fn build(&mut self, leaves: impl IntoIterator<Item = (T, Aabr<f32>)>) {
+    pub fn build(&mut self, leaves: impl IntoIterator<Item = (T, Aabb<f64>)>) {
         self.leaf_nodes.clear();
         self.internal_nodes.clear();
 
@@ -52,7 +60,7 @@ impl<T: Send + Sync> Bvh<T> {
         self.internal_nodes.resize(
             leaf_count - 1,
             InternalNode {
-                bb: Aabr::default(),
+                bb: Aabb::default(),
                 left: NodeIdx::MAX,
                 right: NodeIdx::MAX,
             },
@@ -71,7 +79,7 @@ impl<T: Send + Sync> Bvh<T> {
             .leaf_nodes
             .par_iter()
             .map(|l| l.bb)
-            .reduce(|| id, Aabr::union);
+            .reduce(|| id, Aabb::union);
 
         self.root = build_rec(
             0,
@@ -85,74 +93,49 @@ impl<T: Send + Sync> Bvh<T> {
         debug_assert_eq!(self.internal_nodes.len(), self.leaf_nodes.len() - 1);
     }
 
-    pub fn find<C, F, U>(&self, mut collides: C, mut find: F) -> Option<U>
+    pub fn traverse<F, U>(&self, mut f: F) -> Option<U>
     where
-        C: FnMut(Aabr<f32>) -> bool,
+        F: FnMut(Option<&T>, Aabb<f64>) -> TraverseStep<U>,
-        F: FnMut(&T, Aabr<f32>) -> Option<U>,
     {
         if !self.leaf_nodes.is_empty() {
-            self.find_rec(self.root, &mut collides, &mut find)
+            self.traverse_rec(self.root, &mut f)
         } else {
             None
         }
     }
 
-    fn find_rec<C, F, U>(&self, idx: NodeIdx, collides: &mut C, find: &mut F) -> Option<U>
+    fn traverse_rec<F, U>(&self, idx: NodeIdx, f: &mut F) -> Option<U>
     where
-        C: FnMut(Aabr<f32>) -> bool,
+        F: FnMut(Option<&T>, Aabb<f64>) -> TraverseStep<U>,
-        F: FnMut(&T, Aabr<f32>) -> Option<U>,
     {
         if idx < self.internal_nodes.len() as NodeIdx {
             let internal = &self.internal_nodes[idx as usize];
 
-            if collides(internal.bb) {
+            match f(None, internal.bb) {
-                if let Some(found) = self.find_rec(internal.left, collides, find) {
+                TraverseStep::Miss => None,
-                    return Some(found);
+                TraverseStep::Hit => self
-                }
+                    .traverse_rec(internal.left, f)
-
+                    .or_else(|| self.traverse_rec(internal.right, f)),
-                if let Some(found) = self.find_rec(internal.right, collides, find) {
+                TraverseStep::Return(u) => Some(u),
-                    return Some(found);
-                }
             }
         } else {
             let leaf = &self.leaf_nodes[(idx - self.internal_nodes.len() as NodeIdx) as usize];
 
-            if collides(leaf.bb) {
+            match f(Some(&leaf.id), leaf.bb) {
-                return find(&leaf.id, leaf.bb);
+                TraverseStep::Miss | TraverseStep::Hit => None,
+                TraverseStep::Return(u) => Some(u),
             }
         }
-
-        None
-    }
-
-    pub fn visit(&self, mut f: impl FnMut(Aabr<f32>, usize)) {
-        if !self.leaf_nodes.is_empty() {
-            self.visit_rec(self.root, 0, &mut f);
-        }
-    }
-
-    pub fn visit_rec(&self, idx: NodeIdx, depth: usize, f: &mut impl FnMut(Aabr<f32>, usize)) {
-        if idx >= self.internal_nodes.len() as NodeIdx {
-            let leaf = &self.leaf_nodes[(idx - self.internal_nodes.len() as NodeIdx) as usize];
-            f(leaf.bb, depth);
-        } else {
-            let internal = &self.internal_nodes[idx as usize];
-
-            self.visit_rec(internal.left, depth + 1, f);
-            self.visit_rec(internal.right, depth + 1, f);
-
-            f(internal.bb, depth);
-        }
     }
 }
 
 fn build_rec<T: Send>(
     idx: NodeIdx,
-    bounds: Aabr<f32>,
+    bounds: Aabb<f64>,
     internal_nodes: &mut [InternalNode],
     leaf_nodes: &mut [LeafNode<T>],
     total_leaf_count: NodeIdx,
-) -> (NodeIdx, Aabr<f32>) {
+) -> (NodeIdx, Aabb<f64>) {
     debug_assert_eq!(leaf_nodes.len() - 1, internal_nodes.len());
 
     if leaf_nodes.len() == 1 {
@@ -163,19 +146,26 @@ fn build_rec<T: Send>(
     debug_assert!(bounds.is_valid());
     let dims = bounds.max - bounds.min;
 
-    let (mut split, bounds_left, bounds_right) = if dims.x >= dims.y {
+    let (mut split, bounds_left, bounds_right) = if dims.x >= dims.y && dims.x >= dims.z {
         let mid = middle(bounds.min.x, bounds.max.x);
         let [bounds_left, bounds_right] = bounds.split_at_x(mid);
 
         let p = partition(leaf_nodes, |l| middle(l.bb.min.x, l.bb.max.x) <= mid);
 
         (p, bounds_left, bounds_right)
-    } else {
+    } else if dims.y >= dims.x && dims.y >= dims.z {
         let mid = middle(bounds.min.y, bounds.max.y);
         let [bounds_left, bounds_right] = bounds.split_at_y(mid);
 
         let p = partition(leaf_nodes, |l| middle(l.bb.min.y, l.bb.max.y) <= mid);
 
+        (p, bounds_left, bounds_right)
+    } else {
+        let mid = middle(bounds.min.z, bounds.max.z);
+        let [bounds_left, bounds_right] = bounds.split_at_z(mid);
+
+        let p = partition(leaf_nodes, |l| middle(l.bb.min.z, l.bb.max.z) <= mid);
+
         (p, bounds_left, bounds_right)
     };
 
@@ -262,7 +252,7 @@ fn partition<T>(s: &mut [T], mut pred: impl FnMut(&T) -> bool) -> usize {
     true_count
 }
 
-fn middle(a: f32, b: f32) -> f32 {
+fn middle(a: f64, b: f64) -> f64 {
     (a + b) / 2.0
 }
 
@@ -280,11 +270,11 @@ mod tests {
     fn empty() {
         let mut bvh = Bvh::new();
 
-        bvh.find(|_| false, |_, _| Some(()));
+        bvh.traverse(|_, _| TraverseStep::Return(()));
         bvh.build([]);
 
-        bvh.build([(5, Aabr::default())]);
+        bvh.build([(5, Aabb::default())]);
-        bvh.find(|_| false, |_, _| Some(()));
+        bvh.traverse(|_, _| TraverseStep::Return(()));
     }
 
     #[test]
@@ -292,13 +282,13 @@ mod tests {
         let mut bvh = Bvh::new();
 
         bvh.build([
-            ((), Aabr::default()),
+            ((), Aabb::default()),
-            ((), Aabr::default()),
+            ((), Aabb::default()),
-            ((), Aabr::default()),
+            ((), Aabb::default()),
-            ((), Aabr::default()),
+            ((), Aabb::default()),
-            ((), Aabr::new_empty(5.0.into())),
+            ((), Aabb::new_empty(5.0.into())),
         ]);
 
-        bvh.find(|_| false, |_, _| Some(()));
+        bvh.traverse(|_, _| TraverseStep::Return(()));
     }
 }

src/client.rs

@@ -52,7 +52,7 @@ impl Clients {
     }
 
     pub fn count(&self) -> usize {
-        self.sm.count()
+        self.sm.len()
     }
 
     pub fn get(&self, client: ClientId) -> Option<&Client> {

src/entity.rs

@@ -4,6 +4,7 @@ pub mod types;
 use std::collections::hash_map::Entry;
 use std::collections::HashMap;
 use std::iter::FusedIterator;
+use std::num::NonZeroU32;
 use std::ops::Deref;
 
 use bitfield_struct::bitfield;
@@ -24,6 +25,7 @@ use crate::var_int::VarInt;
 pub struct Entities {
     sm: SlotMap<Entity>,
     uuid_to_entity: HashMap<Uuid, EntityId>,
+    network_id_to_entity: HashMap<NonZeroU32, u32>,
 }
 
 pub struct EntitiesMut<'a>(&'a mut Entities);
@@ -41,12 +43,13 @@ impl Entities {
         Self {
             sm: SlotMap::new(),
             uuid_to_entity: HashMap::new(),
+            network_id_to_entity: HashMap::new(),
         }
     }
 
     /// Returns the number of live entities.
-    pub fn count(&self) -> usize {
+    pub fn len(&self) -> usize {
-        self.sm.count()
+        self.sm.len()
     }
 
     /// Gets the [`EntityId`] of the entity with the given UUID in an efficient
@@ -76,6 +79,10 @@ impl<'a> EntitiesMut<'a> {
         Self(entities)
     }
 
+    pub fn reborrow(&mut self) -> EntitiesMut {
+        EntitiesMut(self.0)
+    }
+
     /// Spawns a new entity with the default data. The new entity's [`EntityId`]
     /// is returned.
     ///
@@ -87,7 +94,7 @@ impl<'a> EntitiesMut<'a> {
     }
 
     /// Like [`create`](Entities::create), but requires specifying the new
-    /// entity's UUID. This is useful for deserialization.
+    /// entity's UUID.
     ///
     /// The provided UUID must not conflict with an existing entity UUID in this
     /// world. If it does, `None` is returned and the entity is not spawned.
@@ -95,7 +102,7 @@ impl<'a> EntitiesMut<'a> {
         match self.0.uuid_to_entity.entry(uuid) {
             Entry::Occupied(_) => None,
             Entry::Vacant(ve) => {
-                let (id, entity) = self.0.sm.insert(Entity {
+                let (k, e) = self.0.sm.insert(Entity {
                     flags: EntityFlags(0),
                     meta: EntityMeta::new(EntityType::Marker),
                     new_position: Vec3::default(),
@@ -107,11 +114,9 @@ impl<'a> EntitiesMut<'a> {
                     uuid,
                 });
 
-                ve.insert(EntityId(id));
+                ve.insert(EntityId(k));
 
-                // TODO: insert into partition.
+                Some((EntityId(k), EntityMut(e)))
-
-                Some((EntityId(id), EntityMut(entity)))
             }
         }
     }
@@ -123,7 +128,11 @@ impl<'a> EntitiesMut<'a> {
                 .remove(&e.uuid)
                 .expect("UUID should have been in UUID map");
 
-            // TODO: remove entity from partition.
+            self.0
+                .network_id_to_entity
+                .remove(&entity.0.version())
+                .expect("network ID should have been in the network ID map");
+
             true
         } else {
             false
@@ -131,8 +140,23 @@ impl<'a> EntitiesMut<'a> {
     }
 
     pub fn retain(&mut self, mut f: impl FnMut(EntityId, EntityMut) -> bool) {
-        // TODO
+        self.0.sm.retain(|k, v| {
-        self.0.sm.retain(|k, v| f(EntityId(k), EntityMut(v)))
+            if f(EntityId(k), EntityMut(v)) {
+                true
+            } else {
+                self.0
+                    .uuid_to_entity
+                    .remove(&v.uuid)
+                    .expect("UUID should have been in UUID map");
+
+                self.0
+                    .network_id_to_entity
+                    .remove(&k.version())
+                    .expect("network ID should have been in the network ID map");
+
+                false
+            }
+        });
     }
 
     pub fn get_mut(&mut self, entity: EntityId) -> Option<EntityMut> {
@@ -170,8 +194,7 @@ pub struct EntityId(Key);
 
 impl EntityId {
     pub(crate) fn to_network_id(self) -> i32 {
-        // ID 0 is reserved for clients.
+        self.0.version().get() as i32
-        self.0.index() as i32 + 1
     }
 }
 

src/lib.rs

@@ -23,6 +23,7 @@ mod packets;
 mod protocol;
 pub mod server;
 mod slotmap;
+pub mod spatial_index;
 pub mod text;
 pub mod util;
 mod var_int;
@@ -39,6 +40,7 @@ pub use dimension::{Dimension, DimensionId};
 pub use entity::{Entities, EntitiesMut, Entity, EntityId, EntityType};
 pub use ident::Ident;
 pub use server::{start_server, NewClientData, Server, ShutdownResult};
+pub use spatial_index::{SpatialIndex, SpatialIndexMut};
 pub use text::{Text, TextFormat};
 pub use uuid::Uuid;
 pub use world::{WorldId, WorldMeta, WorldMetaMut, WorldMut, WorldRef, Worlds, WorldsMut};

src/packets.rs

@@ -1133,7 +1133,7 @@ pub mod play {
 
                 #[cfg(test)]
                 #[test]
-                fn test_s2c_play_packet_order() {
+                fn s2c_play_packet_order() {
                     let ids = [
                         $(
                             (stringify!($packet), $packet::PACKET_ID),
@@ -1141,7 +1141,13 @@ pub mod play {
                     ];
 
                     if let Some(w) = ids.windows(2).find(|w| w[0].1 >= w[1].1) {
-                        panic!("the {} and {} variants of the s2c play packet enum are not properly sorted by their packet ID", w[0].0, w[1].0);
+                        panic!(
+                            "the {} (ID {:#x}) and {} (ID {:#x}) variants of the s2c play packet enum are not properly sorted by their packet ID",
+                            w[0].0,
+                            w[0].1,
+                            w[1].0,
+                            w[1].1
+                        );
                     }
                 }
             }
@@ -1181,8 +1187,8 @@ pub mod play {
             SpawnPosition,
             EntityMetadata,
             EntityVelocity,
-            EntityTeleport,
             TimeUpdate,
+            EntityTeleport,
         }
     }
 
@@ -1771,7 +1777,7 @@ pub mod play {
 
             #[cfg(test)]
             #[test]
-            fn test_c2s_play_packet_order() {
+            fn c2s_play_packet_order() {
                 let ids = [
                     $(
                         (stringify!($packet), $packet::PACKET_ID),
@@ -1779,7 +1785,13 @@ pub mod play {
                 ];
 
                 if let Some(w) = ids.windows(2).find(|w| w[0].1 >= w[1].1) {
-                    panic!("the {} and {} variants of the c2s play packet enum are not properly sorted by their packet ID", w[0].0, w[1].0);
+                    panic!(
+                        "the {} (ID {:#x}) and {} (ID {:#x}) variants of the c2s play packet enum are not properly sorted by their packet ID",
+                        w[0].0,
+                        w[0].1,
+                        w[1].0,
+                        w[1].1
+                    );
                 }
             }
         }

src/server.rs

@@ -372,6 +372,8 @@ fn do_update_loop(server: Server, mut worlds: WorldsMut) -> ShutdownResult {
                 }
             });
 
+            world.spatial_index.update(world.entities.reborrow());
+
             world.clients.par_iter_mut().for_each(|(_, mut client)| {
                 client.update(&server, &world.entities, &world.chunks, &world.meta);
             });

src/slotmap.rs

@@ -1,9 +1,6 @@
-//! Like the `slotmap` crate, but uses no unsafe code and has rayon support.
-
 use std::iter::FusedIterator;
 use std::mem;
-use std::num::{NonZeroU32, NonZeroU64};
+use std::num::NonZeroU32;
-use std::sync::atomic::{AtomicU64, Ordering};
 
 use rayon::iter::{
     IndexedParallelIterator, IntoParallelRefIterator, IntoParallelRefMutIterator, ParallelIterator,
@@ -16,34 +13,30 @@ pub struct SlotMap<T> {
     next_free_head: u32,
     /// The number of occupied slots.
     count: u32,
+    /// Version counter.
+    version: NonZeroU32,
 }
 
 #[derive(Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash, Debug)]
 pub struct Key {
     index: u32,
-    // Split the u64 version into two u32 fields so that the key is 12 bytes on 64 bit systems.
+    version: NonZeroU32,
-    version_high: NonZeroU32,
-    version_low: u32,
 }
 
 impl Key {
-    fn new(index: u32, version: NonZeroU64) -> Self {
+    pub fn new(index: u32, version: NonZeroU32) -> Self {
-        Self {
+        Self { index, version }
-            index,
-            version_high: NonZeroU32::new((version.get() >> 32) as u32)
-                .expect("versions <= 0x00000000ffffffff are illegal"),
-            version_low: version.get() as u32,
-        }
     }
 
-    fn new_unique(index: u32) -> Self {
+    fn new_unique(index: u32, version: &mut NonZeroU32) -> Self {
-        static NEXT: AtomicU64 = AtomicU64::new(u64::MAX);
+        *version = NonZeroU32::new(version.get().wrapping_add(1)).unwrap_or_else(|| {
+            log::warn!("slotmap version overflow");
+            ONE
+        });
 
-        let version = NEXT.fetch_sub(1, Ordering::SeqCst);
         Self {
             index,
-            version_high: ((version >> 32) as u32).try_into().unwrap(),
+            version: *version,
-            version_low: version as u32,
         }
     }
 
@@ -51,15 +44,19 @@ impl Key {
         self.index
     }
 
-    pub fn version(self) -> NonZeroU64 {
+    pub fn version(self) -> NonZeroU32 {
-        let n = (self.version_high.get() as u64) << 32 | self.version_low as u64;
+        self.version
-        NonZeroU64::new(n).expect("version should be nonzero")
     }
 }
 
+const ONE: NonZeroU32 = match NonZeroU32::new(1) {
+    Some(n) => n,
+    None => unreachable!(),
+};
+
 #[derive(Clone, Debug)]
 enum Slot<T> {
-    Occupied { value: T, version: NonZeroU64 },
+    Occupied { value: T, version: NonZeroU32 },
     Free { next_free: u32 },
 }
 
@@ -69,10 +66,11 @@ impl<T> SlotMap<T> {
             slots: Vec::new(),
             next_free_head: 0,
             count: 0,
+            version: ONE,
         }
     }
 
-    pub fn count(&self) -> usize {
+    pub fn len(&self) -> usize {
         self.count as usize
     }
 
@@ -87,7 +85,7 @@ impl<T> SlotMap<T> {
             self.count += 1;
             self.next_free_head += 1;
 
-            let key = Key::new_unique(self.next_free_head - 1);
+            let key = Key::new_unique(self.next_free_head - 1, &mut self.version);
 
             self.slots.push(Slot::Occupied {
                 value: f(key),
@@ -107,7 +105,7 @@ impl<T> SlotMap<T> {
                 Slot::Free { next_free } => *next_free,
             };
 
-            let key = Key::new_unique(self.next_free_head);
+            let key = Key::new_unique(self.next_free_head, &mut self.version);
 
             *slot = Slot::Occupied {
                 value: f(key),
@@ -258,7 +256,7 @@ mod tests {
         assert_eq!(sm.remove(k0), Some(10));
 
         sm.clear();
-        assert_eq!(sm.count(), 0);
+        assert_eq!(sm.len(), 0);
     }
 
     #[test]
@@ -272,15 +270,9 @@ mod tests {
         sm.retain(|k, _| k == k1);
 
         assert_eq!(sm.get(k1), Some(&20));
-        assert_eq!(sm.count(), 1);
+        assert_eq!(sm.len(), 1);
 
         assert_eq!(sm.get(k0), None);
         assert_eq!(sm.get(k2), None);
     }
-
-    #[test]
-    #[should_panic]
-    fn bad_key() {
-        let _ = Key::new(0, NonZeroU64::new(0x00000000ffffffff).unwrap());
-    }
 }

src/spatial_index.rs

@@ -0,0 +1,209 @@
+use std::ops::Deref;
+
+use vek::{Aabb, Vec3};
+
+use crate::bvh::Bvh;
+pub use crate::bvh::TraverseStep;
+use crate::{EntitiesMut, EntityId};
+
+pub struct SpatialIndex {
+    bvh: Bvh<EntityId>,
+}
+
+pub struct SpatialIndexMut<'a>(&'a mut SpatialIndex);
+
+impl<'a> Deref for SpatialIndexMut<'a> {
+    type Target = SpatialIndex;
+
+    fn deref(&self) -> &Self::Target {
+        &self.0
+    }
+}
+
+impl SpatialIndex {
+    pub(crate) fn new() -> Self {
+        Self { bvh: Bvh::new() }
+    }
+
+    pub fn traverse<F, T>(&self, mut f: F) -> Option<T>
+    where
+        F: FnMut(Option<EntityId>, Aabb<f64>) -> TraverseStep<T>,
+    {
+        self.bvh.traverse(|e, bb| f(e.cloned(), bb))
+    }
+
+    pub fn query<C, F, T>(&self, mut collides: C, mut f: F) -> Option<T>
+    where
+        C: FnMut(Aabb<f64>) -> bool,
+        F: FnMut(EntityId, Aabb<f64>) -> Option<T>,
+    {
+        self.traverse(|e, bb| {
+            if collides(bb) {
+                e.and_then(|id| f(id, bb))
+                    .map_or(TraverseStep::Hit, TraverseStep::Return)
+            } else {
+                TraverseStep::Miss
+            }
+        })
+    }
+
+    pub fn raycast(&self, origin: Vec3<f64>, direction: Vec3<f64>) -> Option<RaycastHit> {
+        debug_assert!(
+            direction.is_normalized(),
+            "the ray direction must be normalized"
+        );
+
+        let mut hit: Option<RaycastHit> = None;
+
+        self.traverse::<_, ()>(|entity, bb| {
+            if let Some((near, far)) = ray_box_intersection(origin, direction, bb) {
+                if hit.as_ref().map_or(true, |hit| near < hit.near) {
+                    if let Some(entity) = entity {
+                        hit = Some(RaycastHit {
+                            entity,
+                            bb,
+                            near,
+                            far,
+                        });
+                    }
+                    TraverseStep::Hit
+                } else {
+                    // Do not explore subtrees that cannot produce an intersection closer than the
+                    // closest we've seen so far.
+                    TraverseStep::Miss
+                }
+            } else {
+                TraverseStep::Miss
+            }
+        });
+
+        hit
+    }
+
+    pub fn raycast_all<F, T>(&self, origin: Vec3<f64>, direction: Vec3<f64>, mut f: F) -> Option<T>
+    where
+        F: FnMut(RaycastHit) -> Option<T>,
+    {
+        debug_assert!(
+            direction.is_normalized(),
+            "the ray direction must be normalized"
+        );
+
+        self.traverse(
+            |entity, bb| match (ray_box_intersection(origin, direction, bb), entity) {
+                (Some((near, far)), Some(entity)) => {
+                    let hit = RaycastHit {
+                        entity,
+                        bb,
+                        near,
+                        far,
+                    };
+                    f(hit).map_or(TraverseStep::Hit, TraverseStep::Return)
+                }
+                (Some(_), None) => TraverseStep::Hit,
+                (None, _) => TraverseStep::Miss,
+            },
+        )
+    }
+}
+
+impl<'a> SpatialIndexMut<'a> {
+    pub(crate) fn new(si: &'a mut SpatialIndex) -> Self {
+        Self(si)
+    }
+
+    pub(crate) fn update(&mut self, entities: EntitiesMut) {
+        self.0
+            .bvh
+            .build(entities.iter().map(|(id, e)| (id, e.hitbox())))
+    }
+}
+
+/// Represents an intersection between a ray and an entity's axis-aligned
+/// bounding box.
+#[derive(Clone, Copy, PartialEq)]
+pub struct RaycastHit {
+    /// The [`EntityId`] of the entity that was hit by the ray.
+    pub entity: EntityId,
+    /// The bounding box of the entity that was hit.
+    pub bb: Aabb<f64>,
+    /// The distance from the ray origin to the closest intersection point.
+    /// If the origin of the ray is inside the bounding box, then this will be
+    /// zero.
+    pub near: f64,
+    /// The distance from the ray origin to the second intersection point. This
+    /// represents the point at which the ray exits the bounding box.
+    pub far: f64,
+}
+
+fn ray_box_intersection(ro: Vec3<f64>, rd: Vec3<f64>, bb: Aabb<f64>) -> Option<(f64, f64)> {
+    let mut near = -f64::INFINITY;
+    let mut far = f64::INFINITY;
+
+    for i in 0..3 {
+        // Rust's definition of min and max properly handle the NaNs that these
+        // computations might produce.
+        let t0 = (bb.min[i] - ro[i]) / rd[i];
+        let t1 = (bb.max[i] - ro[i]) / rd[i];
+
+        near = near.max(t0.min(t1));
+        far = far.min(t0.max(t1));
+    }
+
+    if near <= far && far >= 0.0 {
+        Some((near.max(0.0), far))
+    } else {
+        None
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn ray_box_edge_cases() {
+        let bb = Aabb {
+            min: Vec3::new(0.0, 0.0, 0.0),
+            max: Vec3::new(1.0, 1.0, 1.0),
+        };
+
+        let ros = [
+            // On a corner
+            Vec3::new(0.0, 0.0, 0.0),
+            // Outside
+            Vec3::new(-0.5, 0.5, -0.5),
+            // In the center
+            Vec3::new(0.5, 0.5, 0.5),
+            // On an edge
+            Vec3::new(0.0, 0.5, 0.0),
+            // On a face
+            Vec3::new(0.0, 0.5, 0.5),
+            // Outside slabs
+            Vec3::new(-2.0, -2.0, -2.0),
+        ];
+
+        let rds = [
+            Vec3::new(1.0, 0.0, 0.0),
+            Vec3::new(-1.0, 0.0, 0.0),
+            Vec3::new(0.0, 1.0, 0.0),
+            Vec3::new(0.0, -1.0, 0.0),
+            Vec3::new(0.0, 0.0, 1.0),
+            Vec3::new(0.0, 0.0, -1.0),
+        ];
+
+        assert!(rds.iter().all(|d| d.is_normalized()));
+
+        for ro in ros {
+            for rd in rds {
+                if let Some((near, far)) = ray_box_intersection(ro, rd, bb) {
+                    assert!(near.is_finite());
+                    assert!(far.is_finite());
+                    assert!(near <= far);
+                    assert!(near >= 0.0);
+                    assert!(far >= 0.0);
+                }
+            }
+        }
+    }
+}

src/util.rs

@@ -2,7 +2,7 @@ use std::iter::FusedIterator;
 
 use num::cast::AsPrimitive;
 use num::Float;
-use vek::{Aabb, Extent3, Vec3};
+use vek::{Aabb, Vec3};
 
 use crate::ChunkPos;
 

src/world.rs

@@ -4,7 +4,10 @@ use std::ops::Deref;
 use rayon::iter::ParallelIterator;
 
 use crate::slotmap::{Key, SlotMap};
-use crate::{Chunks, ChunksMut, Clients, ClientsMut, DimensionId, Entities, EntitiesMut, Server};
+use crate::{
+    Chunks, ChunksMut, Clients, ClientsMut, DimensionId, Entities, EntitiesMut, Server,
+    SpatialIndex, SpatialIndexMut,
+};
 
 pub struct Worlds {
     sm: SlotMap<World>,
@@ -33,7 +36,7 @@ impl Worlds {
     }
 
     pub fn count(&self) -> usize {
-        self.sm.count()
+        self.sm.len()
     }
 
     pub fn get(&self, world: WorldId) -> Option<WorldRef> {
@@ -58,6 +61,7 @@ impl<'a> WorldsMut<'a> {
         let (id, world) = self.0.sm.insert(World {
             clients: Clients::new(),
             entities: Entities::new(),
+            spatial_index: SpatialIndex::new(),
             chunks: Chunks::new(
                 self.server.clone(),
                 (self.server.dimension(dim).height / 16) as u32,
@@ -122,6 +126,7 @@ impl<'a> WorldsMut<'a> {
 pub(crate) struct World {
     clients: Clients,
     entities: Entities,
+    spatial_index: SpatialIndex,
     chunks: Chunks,
     meta: WorldMeta,
 }
@@ -130,6 +135,7 @@ pub(crate) struct World {
 pub struct WorldRef<'a> {
     pub clients: &'a Clients,
     pub entities: &'a Entities,
+    pub spatial_index: &'a SpatialIndex,
     pub chunks: &'a Chunks,
     pub meta: &'a WorldMeta,
 }
@@ -139,6 +145,7 @@ impl<'a> WorldRef<'a> {
         Self {
             clients: &w.clients,
             entities: &w.entities,
+            spatial_index: &w.spatial_index,
             chunks: &w.chunks,
             meta: &w.meta,
         }
@@ -149,6 +156,7 @@ impl<'a> WorldRef<'a> {
 pub struct WorldMut<'a> {
     pub clients: ClientsMut<'a>,
     pub entities: EntitiesMut<'a>,
+    pub spatial_index: SpatialIndexMut<'a>,
     pub chunks: ChunksMut<'a>,
     pub meta: WorldMetaMut<'a>,
 }
@@ -158,6 +166,7 @@ impl<'a> WorldMut<'a> {
         WorldMut {
             clients: ClientsMut::new(&mut w.clients),
             entities: EntitiesMut::new(&mut w.entities),
+            spatial_index: SpatialIndexMut::new(&mut w.spatial_index),
             chunks: ChunksMut::new(&mut w.chunks),
             meta: WorldMetaMut(&mut w.meta),
         }
@@ -167,6 +176,7 @@ impl<'a> WorldMut<'a> {
         WorldRef {
             clients: &self.clients,
             entities: &self.entities,
+            spatial_index: &self.spatial_index,
             chunks: &self.chunks,
             meta: &self.meta,
         }