From 4a12def90024f4156f83638fa047533eb10b836a Mon Sep 17 00:00:00 2001 From: Ryan Date: Thu, 14 Jul 2022 03:40:26 -0700 Subject: [PATCH] Redesign the spatial index API and add raycast example --- build/entity.rs | 2 +- examples/cow_sphere.rs | 14 +- examples/raycast.rs | 172 +++++++++++++++++++++++++ src/block_pos.rs | 5 +- src/bvh.rs | 141 ++++++++++---------- src/spatial_index.rs | 284 +++++++++++++++++++++-------------------- src/text.rs | 6 + src/util.rs | 125 ++++++++++++++++-- 8 files changed, 520 insertions(+), 229 deletions(-) create mode 100644 examples/raycast.rs diff --git a/build/entity.rs b/build/entity.rs index 1c0d2e6..b8e7696 100644 --- a/build/entity.rs +++ b/build/entity.rs @@ -527,7 +527,7 @@ const AGEABLE_MOB: Class = Class { const ANIMAL: Class = Class { name: "animal", - inherit: Some(&PATHFINDER_MOB), + inherit: Some(&AGEABLE_MOB), fields: &[], events: &[], }; diff --git a/examples/cow_sphere.rs b/examples/cow_sphere.rs index b65b24f..89d7417 100644 --- a/examples/cow_sphere.rs +++ b/examples/cow_sphere.rs @@ -34,7 +34,7 @@ struct Game { const MAX_PLAYERS: usize = 10; -const SPAWN_POS: BlockPos = BlockPos::new(0, 99, -35); +const SPAWN_POS: BlockPos = BlockPos::new(0, 100, -35); #[async_trait] impl Config for Game { @@ -139,7 +139,6 @@ impl Config for Game { let radius = 6.0 + ((time * TAU / 2.5).sin() + 1.0) / 2.0 * 10.0; - // TODO: use eye position. let player_pos = server .clients .iter() @@ -147,19 +146,22 @@ impl Config for Game { .map(|c| c.1.position()) .unwrap_or_default(); + // TODO: hardcoded eye pos. + let eye_pos = Vec3::new(player_pos.x, player_pos.y + 1.6, player_pos.z); + for (cow_id, p) in cows.iter().cloned().zip(fibonacci_spiral(cow_count)) { let cow = server.entities.get_mut(cow_id).expect("missing cow"); let rotated = p * rot; - let transformed = rotated * radius + [0.5, 100.0, 0.5]; + let transformed = rotated * radius + [0.5, SPAWN_POS.y as f64 + 1.0, 0.5]; let yaw = f32::atan2(rotated.z as f32, rotated.x as f32).to_degrees() - 90.0; let (looking_yaw, looking_pitch) = - to_yaw_and_pitch((player_pos - transformed).normalized()); + to_yaw_and_pitch((eye_pos - transformed).normalized()); cow.set_position(transformed); cow.set_yaw(yaw); - cow.set_pitch(looking_pitch); - cow.set_head_yaw(looking_yaw); + cow.set_pitch(looking_pitch as f32); + cow.set_head_yaw(looking_yaw as f32); } } } diff --git a/examples/raycast.rs b/examples/raycast.rs new file mode 100644 index 0000000..853d2ec --- /dev/null +++ b/examples/raycast.rs @@ -0,0 +1,172 @@ +use std::collections::HashSet; +use std::net::SocketAddr; +use std::sync::atomic::{AtomicUsize, Ordering}; + +use log::LevelFilter; +use valence::async_trait; +use valence::block::{BlockPos, BlockState}; +use valence::client::GameMode; +use valence::config::{Config, ServerListPing}; +use valence::dimension::DimensionId; +use valence::entity::{EntityData, EntityKind}; +use valence::server::{Server, SharedServer, ShutdownResult}; +use valence::text::{Color, TextFormat}; +use valence::util::from_yaw_and_pitch; +use vek::Vec3; + +pub fn main() -> ShutdownResult { + env_logger::Builder::new() + .filter_module("valence", LevelFilter::Trace) + .parse_default_env() + .init(); + + valence::start_server(Game { + player_count: AtomicUsize::new(0), + }) +} + +struct Game { + player_count: AtomicUsize, +} + +const MAX_PLAYERS: usize = 10; + +const SPAWN_POS: BlockPos = BlockPos::new(0, 100, -8); + +const PLAYER_EYE_HEIGHT: f64 = 1.6; + +#[async_trait] +impl Config for Game { + fn max_connections(&self) -> usize { + // We want status pings to be successful even if the server is full. + MAX_PLAYERS + 64 + } + + fn online_mode(&self) -> bool { + // You'll want this to be true on real servers. + false + } + + async fn server_list_ping( + &self, + _server: &SharedServer, + _remote_addr: SocketAddr, + ) -> ServerListPing { + ServerListPing::Respond { + online_players: self.player_count.load(Ordering::SeqCst) as i32, + max_players: MAX_PLAYERS as i32, + description: "Hello Valence!".color(Color::AQUA), + favicon_png: Some(include_bytes!("../assets/favicon.png")), + } + } + + fn init(&self, server: &mut Server) { + let (world_id, world) = server.worlds.create(DimensionId::default()); + world.meta.set_flat(true); + + let size = 5; + for z in -size..size { + for x in -size..size { + world.chunks.create([x, z]); + } + } + + world.chunks.set_block_state(SPAWN_POS, BlockState::BEDROCK); + + const SHEEP_COUNT: usize = 10; + for i in 0..SHEEP_COUNT { + let offset = (i as f64 - (SHEEP_COUNT - 1) as f64 / 2.0) * 1.25; + + let (_, sheep) = server.entities.create(EntityKind::Sheep); + sheep.set_world(world_id); + sheep.set_position([offset + 0.5, SPAWN_POS.y as f64 + 1.0, 0.0]); + sheep.set_yaw(180.0); + sheep.set_head_yaw(180.0); + + if let EntityData::Sheep(sheep) = sheep.data_mut() { + // Shear the sheep. + sheep.set_sheep_state(0b1_0000); + } + } + } + + fn update(&self, server: &mut Server) { + let (world_id, world) = server.worlds.iter_mut().next().unwrap(); + + let mut hit_entities = HashSet::new(); + + server.clients.retain(|_, client| { + if client.created_tick() == server.shared.current_tick() { + if self + .player_count + .fetch_update(Ordering::SeqCst, Ordering::SeqCst, |count| { + (count < MAX_PLAYERS).then_some(count + 1) + }) + .is_err() + { + client.disconnect("The server is full!".color(Color::RED)); + return false; + } + + client.spawn(world_id); + client.set_game_mode(GameMode::Creative); + client.teleport( + [ + SPAWN_POS.x as f64 + 0.5, + SPAWN_POS.y as f64 + 1.0, + SPAWN_POS.z as f64 + 0.5, + ], + 0.0, + 0.0, + ); + + world.meta.player_list_mut().insert( + client.uuid(), + client.username().to_owned(), + client.textures().cloned(), + client.game_mode(), + 0, + None, + ); + + client.send_message( + "Look at a sheep to change its ".italic() + + "color".italic().color(Color::GREEN) + + ".", + ) + } + + if client.is_disconnected() { + self.player_count.fetch_sub(1, Ordering::SeqCst); + world.meta.player_list_mut().remove(client.uuid()); + return false; + } + + let client_pos = client.position(); + + let origin = Vec3::new(client_pos.x, client_pos.y + PLAYER_EYE_HEIGHT, client_pos.z); + let direction = from_yaw_and_pitch(client.yaw() as f64, client.pitch() as f64); + + if let Some(hit) = world.spatial_index.raycast(origin, direction, |hit| { + server + .entities + .get(hit.entity) + .map_or(false, |e| e.kind() == EntityKind::Sheep) + }) { + hit_entities.insert(hit.entity); + } + + true + }); + + for (id, e) in server.entities.iter_mut() { + if let EntityData::Sheep(sheep) = e.data_mut() { + if hit_entities.contains(&id) { + sheep.set_sheep_state(5); + } else { + sheep.set_sheep_state(0); + } + } + } + } +} diff --git a/src/block_pos.rs b/src/block_pos.rs index eddb6b8..f890262 100644 --- a/src/block_pos.rs +++ b/src/block_pos.rs @@ -32,10 +32,7 @@ impl Encode for BlockPos { let (x, y, z) = (self.x as u64, self.y as u64, self.z as u64); (x << 38 | z << 38 >> 26 | y & 0xfff).encode(w) } - _ => bail!( - "block position {:?} is out of range", - (self.x, self.y, self.z) - ), + _ => bail!("out of range: {self:?}"), } } } diff --git a/src/bvh.rs b/src/bvh.rs index 0bfd128..a866afa 100644 --- a/src/bvh.rs +++ b/src/bvh.rs @@ -3,10 +3,13 @@ //! [bvh]: https://en.wikipedia.org/wiki/Bounding_volume_hierarchy //! [`SpatialIndex`]: crate::spatial_index::SpatialIndex +use std::iter::FusedIterator; use std::mem; use approx::relative_eq; -use rayon::iter::{IntoParallelRefIterator, ParallelIterator}; +use rayon::iter::{ + IndexedParallelIterator, IntoParallelRefIterator, IntoParallelRefMutIterator, ParallelIterator, +}; use vek::Aabb; #[derive(Clone)] @@ -16,13 +19,6 @@ pub struct Bvh { root: NodeIdx, } -#[derive(Clone, Copy, PartialEq, Eq)] -pub enum TraverseStep { - Miss, - Hit, - Return(T), -} - #[derive(Clone)] struct InternalNode { bb: Aabb, @@ -33,7 +29,7 @@ struct InternalNode { #[derive(Clone)] struct LeafNode { bb: Aabb, - id: T, + data: T, } // TODO: we could use usize here to store more elements. @@ -53,7 +49,7 @@ impl Bvh { self.internal_nodes.clear(); self.leaf_nodes - .extend(leaves.into_iter().map(|(id, bb)| LeafNode { bb, id })); + .extend(leaves.into_iter().map(|(id, bb)| LeafNode { bb, data: id })); let leaf_count = self.leaf_nodes.len(); @@ -98,40 +94,88 @@ impl Bvh { debug_assert_eq!(self.internal_nodes.len(), self.leaf_nodes.len() - 1); } - pub fn traverse(&self, mut f: F) -> Option - where - F: FnMut(Option<&T>, Aabb) -> TraverseStep, - { + pub fn traverse(&self) -> Option> { if !self.leaf_nodes.is_empty() { - self.traverse_rec(self.root, &mut f) + Some(Node::from_idx(self, self.root)) } else { None } } - fn traverse_rec(&self, idx: NodeIdx, f: &mut F) -> Option - where - F: FnMut(Option<&T>, Aabb) -> TraverseStep, - { - if idx < self.internal_nodes.len() as NodeIdx { - let internal = &self.internal_nodes[idx as usize]; + pub fn iter( + &self, + ) -> impl ExactSizeIterator)> + FusedIterator + Clone + '_ { + self.leaf_nodes.iter().map(|leaf| (&leaf.data, leaf.bb)) + } - match f(None, internal.bb) { - TraverseStep::Miss => None, - TraverseStep::Hit => self - .traverse_rec(internal.left, f) - .or_else(|| self.traverse_rec(internal.right, f)), - TraverseStep::Return(u) => Some(u), - } + #[allow(dead_code)] + pub fn iter_mut( + &mut self, + ) -> impl ExactSizeIterator)> + FusedIterator + '_ { + self.leaf_nodes + .iter_mut() + .map(|leaf| (&mut leaf.data, leaf.bb)) + } + + pub fn par_iter(&self) -> impl IndexedParallelIterator)> + Clone + '_ { + self.leaf_nodes.par_iter().map(|leaf| (&leaf.data, leaf.bb)) + } + + #[allow(dead_code)] + pub fn par_iter_mut( + &mut self, + ) -> impl IndexedParallelIterator)> + '_ { + self.leaf_nodes + .par_iter_mut() + .map(|leaf| (&mut leaf.data, leaf.bb)) + } +} + +pub enum Node<'a, T> { + Internal(Internal<'a, T>), + Leaf { data: &'a T, bb: Aabb }, +} + +impl<'a, T> Node<'a, T> { + fn from_idx(bvh: &'a Bvh, idx: NodeIdx) -> Self { + if idx < bvh.internal_nodes.len() as NodeIdx { + Self::Internal(Internal { bvh, idx }) } else { - let leaf = &self.leaf_nodes[(idx - self.internal_nodes.len() as NodeIdx) as usize]; - - match f(Some(&leaf.id), leaf.bb) { - TraverseStep::Miss | TraverseStep::Hit => None, - TraverseStep::Return(u) => Some(u), + let leaf = &bvh.leaf_nodes[(idx - bvh.internal_nodes.len() as NodeIdx) as usize]; + Self::Leaf { + data: &leaf.data, + bb: leaf.bb, } } } + + pub fn bb(&self) -> Aabb { + match self { + Node::Internal(int) => int.bb(), + Node::Leaf { bb, .. } => *bb, + } + } +} + +pub struct Internal<'a, T> { + bvh: &'a Bvh, + idx: NodeIdx, +} + +impl<'a, T> Internal<'a, T> { + pub fn split(self) -> (Aabb, Node<'a, T>, Node<'a, T>) { + let internal = &self.bvh.internal_nodes[self.idx as usize]; + + let bb = internal.bb; + let left = Node::from_idx(self.bvh, internal.left); + let right = Node::from_idx(self.bvh, internal.right); + + (bb, left, right) + } + + pub fn bb(&self) -> Aabb { + self.bvh.internal_nodes[self.idx as usize].bb + } } fn build_rec( @@ -266,34 +310,3 @@ impl Default for Bvh { Self::new() } } - -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn empty() { - let mut bvh = Bvh::new(); - - bvh.traverse(|_, _| TraverseStep::Return(())); - bvh.build([]); - - bvh.build([(5, Aabb::default())]); - bvh.traverse(|_, _| TraverseStep::Return(())); - } - - #[test] - fn overlapping() { - let mut bvh = Bvh::new(); - - bvh.build([ - ((), Aabb::default()), - ((), Aabb::default()), - ((), Aabb::default()), - ((), Aabb::default()), - ((), Aabb::new_empty(5.0.into())), - ]); - - bvh.traverse(|_, _| TraverseStep::Return(())); - } -} diff --git a/src/spatial_index.rs b/src/spatial_index.rs index 96967b9..8889574 100644 --- a/src/spatial_index.rs +++ b/src/spatial_index.rs @@ -1,10 +1,13 @@ //! Efficient spatial entity queries. +use std::iter::FusedIterator; + +use rayon::iter::{IndexedParallelIterator, ParallelIterator}; use vek::{Aabb, Vec3}; -use crate::bvh::Bvh; -pub use crate::bvh::TraverseStep; +use crate::bvh::{Bvh, Node}; use crate::entity::{Entities, EntityId}; +use crate::util::ray_box_intersect; use crate::world::WorldId; /// A data structure for fast spatial queries on entity [hitboxes]. This is used @@ -25,9 +28,9 @@ impl SpatialIndex { } #[doc(hidden)] - #[deprecated = "This is for documentation tests only"] + #[deprecated = "This is for documentation tests only!"] pub fn example_new() -> Self { - println!("Don't call me!"); + dbg!("Don't call me from outside tests!"); Self::new() } @@ -69,86 +72,157 @@ impl SpatialIndex { C: FnMut(Aabb) -> bool, F: FnMut(EntityId, Aabb) -> Option, { - self.traverse(|e, bb| { - if collides(bb) { - e.and_then(|id| f(id, bb)) - .map_or(TraverseStep::Hit, TraverseStep::Return) - } else { - TraverseStep::Miss - } - }) - } + fn query_rec(node: Node, collides: &mut C, f: &mut F) -> Option + where + C: FnMut(Aabb) -> bool, + F: FnMut(EntityId, Aabb) -> Option, + { + match node { + Node::Internal(int) => { + let (bb, left, right) = int.split(); - // TODO: accept predicate here. Might want to skip invisible entities, for - // instance. - pub fn raycast(&self, origin: Vec3, direction: Vec3) -> Option { - debug_assert!( - direction.is_normalized(), - "the ray direction must be normalized" - ); - - let mut hit: Option = 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, - }); + if collides(bb) { + query_rec(left, collides, f).or_else(|| query_rec(right, collides, f)) + } else { + None + } + } + Node::Leaf { data, bb } => { + if collides(bb) { + f(*data, bb) + } else { + None } - 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 + query_rec(self.bvh.traverse()?, &mut collides, &mut f) } - pub fn raycast_all(&self, origin: Vec3, direction: Vec3, mut f: F) -> Option + /// Casts a ray defined by `origin` and `direction` through entity hitboxes + /// and returns the closest intersection for which `f` returns `true`. + /// + /// `f` is a predicate which can be used to filter intersections. For + /// instance, if a ray is shot from a player's eye position, you probably + /// don't want the ray to intersect with the player's own hitbox. + /// + /// If no intersections are found or if `f` never returns `true` then `None` + /// is returned. Additionally, the given ray direction must be + /// normalized. + /// + /// # Examples + /// + /// ``` + /// # #[allow(deprecated)] + /// # let si = valence::spatial_index::SpatialIndex::example_new(); + /// use valence::vek::*; + /// + /// let origin = Vec3::new(0.0, 0.0, 0.0); + /// let direction = Vec3::new(1.0, 1.0, 1.0).normalized(); + /// + /// // Assume `si` is the spatial index. + /// if let Some(hit) = si.raycast(origin, direction, |_| true) { + /// println!("Raycast hit! {hit:?}"); + /// } + /// ``` + pub fn raycast( + &self, + origin: Vec3, + direction: Vec3, + mut f: F, + ) -> Option where - F: FnMut(RaycastHit) -> Option, + F: FnMut(&RaycastHit) -> bool, { - debug_assert!( - direction.is_normalized(), - "the ray direction must be normalized" - ); + fn raycast_rec( + node: Node, + hit: &mut Option, + near: f64, + far: f64, + origin: Vec3, + direction: Vec3, + f: &mut impl FnMut(&RaycastHit) -> bool, + ) { + if let Some(hit) = hit { + if hit.near <= near { + return; + } + } - self.traverse( - |entity, bb| match (ray_box_intersection(origin, direction, bb), entity) { - (Some((near, far)), Some(entity)) => { - let hit = RaycastHit { - entity, + match node { + Node::Internal(int) => { + let (_, left, right) = int.split(); + + let int_left = ray_box_intersect(origin, direction, left.bb()); + let int_right = ray_box_intersect(origin, direction, right.bb()); + + match (int_left, int_right) { + (Some((near_left, far_left)), Some((near_right, far_right))) => { + // Explore closest subtree first. + if near_left < near_right { + raycast_rec(left, hit, near_left, far_left, origin, direction, f); + raycast_rec( + right, hit, near_right, far_right, origin, direction, f, + ); + } else { + raycast_rec( + right, hit, near_right, far_right, origin, direction, f, + ); + raycast_rec(left, hit, near_left, far_left, origin, direction, f); + } + } + (Some((near, far)), None) => { + raycast_rec(left, hit, near, far, origin, direction, f) + } + (None, Some((near, far))) => { + raycast_rec(right, hit, near, far, origin, direction, f) + } + (None, None) => {} + } + } + Node::Leaf { data, bb } => { + let this_hit = RaycastHit { + entity: *data, bb, near, far, }; - f(hit).map_or(TraverseStep::Hit, TraverseStep::Return) + + if f(&this_hit) { + *hit = Some(this_hit); + } } - (Some(_), None) => TraverseStep::Hit, - (None, _) => TraverseStep::Miss, - }, - ) + } + } + + debug_assert!( + direction.is_normalized(), + "the ray direction must be normalized" + ); + + let root = self.bvh.traverse()?; + let (near, far) = ray_box_intersect(origin, direction, root.bb())?; + + let mut hit = None; + raycast_rec(root, &mut hit, near, far, origin, direction, &mut f); + hit } - /// Explores the spatial index according to `f`. - /// - /// This is a low-level function that should only be used if the other - /// methods on this type are too restrictive. - pub fn traverse(&self, mut f: F) -> Option - where - F: FnMut(Option, Aabb) -> TraverseStep, - { - self.bvh.traverse(|e, bb| f(e.cloned(), bb)) + /// Returns an iterator over all entities and their hitboxes in + /// an unspecified order. + pub fn iter( + &self, + ) -> impl ExactSizeIterator)> + FusedIterator + Clone + '_ { + self.bvh.iter().map(|(&id, bb)| (id, bb)) + } + + /// Returns a parallel iterator over all entities and their + /// hitboxes in an unspecified order. + pub fn par_iter( + &self, + ) -> impl IndexedParallelIterator)> + Clone + '_ { + self.bvh.par_iter().map(|(&id, bb)| (id, bb)) } pub(crate) fn update(&mut self, entities: &Entities, id: WorldId) { @@ -162,8 +236,8 @@ impl SpatialIndex { } /// Represents an intersection between a ray and an entity's axis-aligned -/// bounding box. -#[derive(Clone, Copy, PartialEq)] +/// bounding box (hitbox). +#[derive(Clone, Copy, PartialEq, Debug)] pub struct RaycastHit { /// The [`EntityId`] of the entity that was hit by the ray. pub entity: EntityId, @@ -177,75 +251,3 @@ pub struct RaycastHit { /// represents the point at which the ray exits the bounding box. pub far: f64, } - -fn ray_box_intersection(ro: Vec3, rd: Vec3, bb: Aabb) -> 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); - } - } - } - } -} diff --git a/src/text.rs b/src/text.rs index c4fc6b4..aaf3dc4 100644 --- a/src/text.rs +++ b/src/text.rs @@ -397,6 +397,12 @@ impl> std::ops::AddAssign for Text { } } +impl From for Text { + fn from(c: char) -> Self { + Text::text(String::from(c)) + } +} + impl From for Text { fn from(s: String) -> Self { Text::text(s) diff --git a/src/util.rs b/src/util.rs index 0a0d79b..bd52549 100644 --- a/src/util.rs +++ b/src/util.rs @@ -14,7 +14,7 @@ use crate::chunk_pos::ChunkPos; /// Usernames are valid if they match the regex `^[a-zA-Z0-9_]{3,16}$`. /// /// # Examples -/// +/// /// ``` /// use valence::util::valid_username; /// @@ -62,7 +62,7 @@ where ), max: Vec3::new( bottom.x + size.x / 2.0.as_(), - bottom.y, + bottom.y + size.y, bottom.z + size.z / 2.0.as_(), ), }; @@ -75,19 +75,118 @@ where /// Takes a normalized direction vector and returns a `(yaw, pitch)` tuple in /// degrees. /// -// /// This function is the inverse of [`from_yaw_and_pitch`]. -pub fn to_yaw_and_pitch(d: Vec3) -> (f32, f32) { +/// This function is the inverse of [`from_yaw_and_pitch`] except for the case +/// where the direction is straight up or down. +pub fn to_yaw_and_pitch(d: Vec3) -> (f64, f64) { debug_assert!(d.is_normalized(), "the given vector should be normalized"); - let yaw = f32::atan2(d.z as f32, d.x as f32).to_degrees() - 90.0; - let pitch = -(d.y as f32).asin().to_degrees(); + let yaw = f64::atan2(d.z, d.x).to_degrees() - 90.0; + let pitch = -(d.y).asin().to_degrees(); (yaw, pitch) } -// /// Takes yaw and pitch angles (in degrees) and returns a normalized -// direction /// vector. -// /// -// /// This function is the inverse of [`to_yaw_and_pitch`]. -// pub fn from_yaw_and_pitch(yaw: f32, pitch: f32) -> Vec3 { -// todo!() -// } +/// Takes yaw and pitch angles (in degrees) and returns a normalized +/// direction vector. +/// +/// This function is the inverse of [`to_yaw_and_pitch`]. +pub fn from_yaw_and_pitch(yaw: f64, pitch: f64) -> Vec3 { + let yaw = (yaw + 90.0).to_radians(); + let pitch = (-pitch).to_radians(); + + let xz_len = pitch.cos(); + Vec3::new(yaw.cos() * xz_len, pitch.sin(), yaw.sin() * xz_len) +} + +/// Calculates the intersection between an axis-aligned bounding box and a ray +/// defined by its origin `ro` and direction `rd`. +/// +/// If an intersection occurs, `Some((near, far))` is returned. `near` and `far` +/// are the distance from the origin to the closest and furthest intersection +/// points respectively. If the intersection occurs inside the bounding box, +/// then `near` is zero. +pub fn ray_box_intersect(ro: Vec3, rd: Vec3, bb: Aabb) -> 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 approx::assert_relative_eq; + use rand::random; + + use super::*; + + #[test] + fn yaw_pitch_round_trip() { + for _ in 0..=100 { + let d = (Vec3::new(random(), random(), random()) * 2.0 - 1.0).normalized(); + + let (yaw, pitch) = to_yaw_and_pitch(d); + let d_new = from_yaw_and_pitch(yaw, pitch); + + assert_relative_eq!(d, d_new, epsilon = f64::EPSILON * 100.0); + } + } + + #[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_intersect(ro, rd, bb) { + assert!(near.is_finite()); + assert!(far.is_finite()); + assert!(near <= far); + assert!(near >= 0.0); + assert!(far >= 0.0); + } + } + } + } +}