diff --git a/Cargo.toml b/Cargo.toml index b993cbb7d1dce..6004a3804f0c0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -4456,6 +4456,28 @@ description = "Exhibits different modes of constructing cubic curves using splin category = "Math" wasm = true +[[example]] +name = "ray_cast_2d" +path = "examples/math/ray_cast_2d.rs" +doc-scrape-examples = true + +[package.metadata.example.ray_cast_2d] +name = "2D Ray Casting for Primitives" +description = "Shows off ray casting for primitive shapes in 2D" +category = "Math" +wasm = true + +[[example]] +name = "ray_cast_3d" +path = "examples/math/ray_cast_3d.rs" +doc-scrape-examples = true + +[package.metadata.example.ray_cast_3d] +name = "3D Ray Casting for Primitives" +description = "Shows off ray casting for primitive shapes in 3D" +category = "Math" +wasm = true + [[example]] name = "render_primitives" path = "examples/math/render_primitives.rs" diff --git a/benches/benches/bevy_math/main.rs b/benches/benches/bevy_math/main.rs index 33a2d113ac94a..c48f7ba79f956 100644 --- a/benches/benches/bevy_math/main.rs +++ b/benches/benches/bevy_math/main.rs @@ -2,5 +2,12 @@ use criterion::criterion_main; mod bezier; mod bounding; +mod ray_cast_2d; +mod ray_cast_3d; -criterion_main!(bezier::benches, bounding::benches); +criterion_main!( + bezier::benches, + bounding::benches, + ray_cast_2d::benches, + ray_cast_3d::benches +); diff --git a/benches/benches/bevy_math/ray_cast_2d.rs b/benches/benches/bevy_math/ray_cast_2d.rs new file mode 100644 index 0000000000000..99e823c5809a8 --- /dev/null +++ b/benches/benches/bevy_math/ray_cast_2d.rs @@ -0,0 +1,100 @@ +use std::time::Duration; + +use bevy_math::{prelude::*, FromRng, ShapeSample}; +use criterion::{criterion_group, measurement::WallTime, BenchmarkGroup, Criterion}; +use rand::{rngs::StdRng, RngExt as _, SeedableRng}; + +const SAMPLES: usize = 100_000; + +criterion_group!(benches, ray_cast_2d); + +fn bench_shape( + group: &mut BenchmarkGroup<'_, WallTime>, + rng: &mut StdRng, + name: &str, + shape_constructor: impl Fn(&mut StdRng) -> S, +) { + group.bench_function(format!("{name}_ray_cast"), |b| { + // Generate random shapes and rays. + let shapes = (0..SAMPLES) + .map(|_| shape_constructor(rng)) + .collect::>(); + let rays = (0..SAMPLES) + .map(|_| Ray2d { + origin: Circle::new(10.0).sample_interior(rng), + direction: Dir2::from_rng(rng), + }) + .collect::>(); + let items = shapes.into_iter().zip(rays).collect::>(); + + // Cast rays against the shapes. + b.iter(|| { + items.iter().for_each(|(shape, ray)| { + core::hint::black_box(shape.local_ray_cast(*ray, f32::MAX, false)); + }); + }); + }); +} + +fn ray_cast_2d(c: &mut Criterion) { + let mut group = c.benchmark_group("ray_cast_2d_100k"); + group.warm_up_time(Duration::from_millis(500)); + + let mut rng = StdRng::seed_from_u64(46); + + bench_shape(&mut group, &mut rng, "circle", |rng| { + Circle::new(rng.random_range(0.1..2.5)) + }); + bench_shape(&mut group, &mut rng, "arc", |rng| { + Arc2d::new( + rng.random_range(0.1..2.5), + rng.random_range(0.1..std::f32::consts::PI), + ) + }); + bench_shape(&mut group, &mut rng, "circular_sector", |rng| { + CircularSector::new( + rng.random_range(0.1..2.5), + rng.random_range(0.1..std::f32::consts::PI), + ) + }); + bench_shape(&mut group, &mut rng, "circular_segment", |rng| { + CircularSegment::new( + rng.random_range(0.1..2.5), + rng.random_range(0.1..std::f32::consts::PI), + ) + }); + bench_shape(&mut group, &mut rng, "ellipse", |rng| { + Ellipse::new(rng.random_range(0.1..2.5), rng.random_range(0.1..2.5)) + }); + bench_shape(&mut group, &mut rng, "annulus", |rng| { + Annulus::new(rng.random_range(0.1..1.25), rng.random_range(1.26..2.5)) + }); + bench_shape(&mut group, &mut rng, "capsule2d", |rng| { + Capsule2d::new(rng.random_range(0.1..1.25), rng.random_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "rectangle", |rng| { + Rectangle::new(rng.random_range(0.1..5.0), rng.random_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "rhombus", |rng| { + Rhombus::new(rng.random_range(0.1..5.0), rng.random_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "line2d", |rng| Line2d { + direction: Dir2::from_rng(rng), + }); + bench_shape(&mut group, &mut rng, "segment2d", |rng| { + Segment2d::new( + rng.random::() * rng.random_range(0.1..2.5), + rng.random::() * rng.random_range(0.1..2.5), + ) + }); + bench_shape(&mut group, &mut rng, "regular_polygon", |rng| { + RegularPolygon::new(rng.random_range(0.1..2.5), rng.random_range(3..6)) + }); + bench_shape(&mut group, &mut rng, "triangle2d", |rng| { + Triangle2d::new( + Vec2::new(rng.random_range(-7.5..7.5), rng.random_range(-7.5..7.5)), + Vec2::new(rng.random_range(-7.5..7.5), rng.random_range(-7.5..7.5)), + Vec2::new(rng.random_range(-7.5..7.5), rng.random_range(-7.5..7.5)), + ) + }); +} diff --git a/benches/benches/bevy_math/ray_cast_3d.rs b/benches/benches/bevy_math/ray_cast_3d.rs new file mode 100644 index 0000000000000..666b897651445 --- /dev/null +++ b/benches/benches/bevy_math/ray_cast_3d.rs @@ -0,0 +1,117 @@ +use std::time::Duration; + +use bevy_math::{prelude::*, FromRng, ShapeSample}; +use criterion::{criterion_group, measurement::WallTime, BenchmarkGroup, Criterion}; +use rand::{rngs::StdRng, RngExt as _, SeedableRng}; + +const SAMPLES: usize = 100_000; + +criterion_group!(benches, ray_cast_3d); + +fn bench_shape( + group: &mut BenchmarkGroup<'_, WallTime>, + rng: &mut StdRng, + name: &str, + shape_constructor: impl Fn(&mut StdRng) -> S, +) { + group.bench_function(format!("{name}_ray_cast"), |b| { + // Generate random shapes and rays. + let shapes = (0..SAMPLES) + .map(|_| shape_constructor(rng)) + .collect::>(); + let rays = (0..SAMPLES) + .map(|_| Ray3d { + origin: Sphere::new(10.0).sample_interior(rng), + direction: Dir3::from_rng(rng), + }) + .collect::>(); + let items = shapes.into_iter().zip(rays).collect::>(); + + // Cast rays against the shapes. + b.iter(|| { + items.iter().for_each(|(shape, ray)| { + core::hint::black_box(shape.local_ray_cast(*ray, f32::MAX, false)); + }); + }); + }); +} + +fn ray_cast_3d(c: &mut Criterion) { + let mut group = c.benchmark_group("ray_cast_3d_100k"); + group.warm_up_time(Duration::from_millis(500)); + + let mut rng = StdRng::seed_from_u64(46); + + bench_shape(&mut group, &mut rng, "sphere", |rng| { + Sphere::new(rng.random_range(0.1..2.5)) + }); + bench_shape(&mut group, &mut rng, "cuboid", |rng| { + Cuboid::new( + rng.random_range(0.1..5.0), + rng.random_range(0.1..5.0), + rng.random_range(0.1..5.0), + ) + }); + bench_shape(&mut group, &mut rng, "cylinder", |rng| { + Cylinder::new(rng.random_range(0.1..2.5), rng.random_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "cone", |rng| { + Cone::new(rng.random_range(0.1..2.5), rng.random_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "conical_frustum", |rng| { + ConicalFrustum { + radius_top: rng.random_range(0.1..2.5), + radius_bottom: rng.random_range(0.1..2.5), + height: rng.random_range(0.1..5.0), + } + }); + bench_shape(&mut group, &mut rng, "capsule3d", |rng| { + Capsule3d::new(rng.random_range(0.1..2.5), rng.random_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "triangle3d", |rng| { + Triangle3d::new( + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + ) + }); + bench_shape(&mut group, &mut rng, "tetrahedron", |rng| { + Tetrahedron::new( + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + Vec3::new( + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + rng.random_range(-7.5..7.5), + ), + ) + }); + bench_shape(&mut group, &mut rng, "torus", |rng| { + Torus::new(rng.random_range(0.1..1.25), rng.random_range(1.26..2.5)) + }); +} diff --git a/crates/bevy_math/src/lib.rs b/crates/bevy_math/src/lib.rs index f49c019bede78..7ec6e373aa966 100644 --- a/crates/bevy_math/src/lib.rs +++ b/crates/bevy_math/src/lib.rs @@ -39,6 +39,7 @@ mod mat3; pub mod ops; pub mod primitives; mod ray; +pub mod ray_cast; mod rects; mod rotation2d; @@ -78,7 +79,9 @@ pub mod prelude { direction::{Dir2, Dir3, Dir3A}, ivec2, ivec3, ivec4, mat2, mat3, mat3a, mat4, ops, primitives::*, - quat, uvec2, uvec3, uvec4, vec2, vec3, vec3a, vec4, BVec2, BVec3, BVec3A, BVec4, BVec4A, + quat, + ray_cast::{PrimitiveRayCast2d, PrimitiveRayCast3d, RayHit2d, RayHit3d}, + uvec2, uvec3, uvec4, vec2, vec3, vec3a, vec4, BVec2, BVec3, BVec3A, BVec4, BVec4A, EulerRot, FloatExt, IRect, IVec2, IVec3, IVec4, Isometry2d, Isometry3d, Mat2, Mat3, Mat3A, Mat4, Quat, Ray2d, Ray3d, Rect, Rot2, StableInterpolate, URect, UVec2, UVec3, UVec4, Vec2, Vec2Swizzles, Vec3, Vec3A, Vec3Swizzles, Vec4, Vec4Swizzles, diff --git a/crates/bevy_math/src/ray.rs b/crates/bevy_math/src/ray.rs index 0240ba2a71353..8e15ec0bb7ca1 100644 --- a/crates/bevy_math/src/ray.rs +++ b/crates/bevy_math/src/ray.rs @@ -1,7 +1,7 @@ use crate::{ ops, primitives::{InfinitePlane3d, Plane2d}, - Dir2, Dir3, Vec2, Vec3, + Dir2, Dir3, Isometry2d, Isometry3d, Vec2, Vec3, }; #[cfg(feature = "bevy_reflect")] @@ -64,6 +64,28 @@ impl Ray2d { self.intersect_plane(plane_origin, plane) .map(|distance| self.get_point(distance)) } + + /// Returns `self` transformed by the given [isometry]. + /// + /// [isometry]: crate::Isometry2d + #[inline] + pub fn transformed_by(&self, isometry: Isometry2d) -> Self { + Self { + origin: isometry.transform_point(self.origin), + direction: isometry.rotation * self.direction, + } + } + + /// Returns `self` transformed by the inverse of the given [isometry]. + /// + /// [isometry]: crate::Isometry2d + #[inline] + pub fn inverse_transformed_by(&self, isometry: Isometry2d) -> Self { + Self { + origin: isometry.inverse_transform_point(self.origin), + direction: isometry.rotation.inverse() * self.direction, + } + } } /// An infinite half-line starting at `origin` and going in `direction` in 3D space. @@ -125,6 +147,28 @@ impl Ray3d { self.intersect_plane(plane_origin, plane) .map(|distance| self.get_point(distance)) } + + /// Returns `self` transformed by the given [isometry]. + /// + /// [isometry]: crate::Isometry3d + #[inline] + pub fn transformed_by(&self, isometry: Isometry3d) -> Self { + Self { + origin: isometry.transform_point(self.origin).into(), + direction: isometry.rotation * self.direction, + } + } + + /// Returns `self` transformed by the inverse of the given [isometry]. + /// + /// [isometry]: crate::Isometry3d + #[inline] + pub fn inverse_transformed_by(&self, isometry: Isometry3d) -> Self { + Self { + origin: isometry.inverse_transform_point(self.origin).into(), + direction: isometry.rotation.inverse() * self.direction, + } + } } #[cfg(test)] diff --git a/crates/bevy_math/src/ray_cast/dim2/annulus.rs b/crates/bevy_math/src/ray_cast/dim2/annulus.rs new file mode 100644 index 0000000000000..7509aa80e0142 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/annulus.rs @@ -0,0 +1,78 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl PrimitiveRayCast2d for Annulus { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + let length_squared = ray.origin.length_squared(); + let inner_radius_squared = self.inner_circle.radius.squared(); + + // Squared distance between ray origin and inner circle boundary + let inner_circle_distance_squared = length_squared - inner_radius_squared; + + if inner_circle_distance_squared < 0.0 { + // The ray origin is inside of the inner circle, the "hole". + // + // This is equivalent to a ray-circle intersection test where the ray origin + // is inside of the hollow circle. See the `Circle` ray casting implementation. + + let b = ray.origin.dot(*ray.direction); + let d = b.squared() - inner_circle_distance_squared; + let t = -b + ops::sqrt(d); + + if t < max_distance { + let intersection = ray.get_point(t); + let direction = Dir2::new(-intersection / self.inner_circle.radius).ok()?; + return Some(RayHit2d::new(t, direction)); + } + } else if length_squared < self.outer_circle.radius.squared() { + // The ray origin is inside of the annulus, in the area between the inner and outer circle. + if solid { + return Some(RayHit2d::new(0.0, -ray.direction)); + } else if let Some(hit) = self.inner_circle.local_ray_cast(ray, max_distance, solid) { + return Some(hit); + } + } + + self.outer_circle.local_ray_cast(ray, max_distance, solid) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_annulus() { + let annulus = Annulus::new(0.5, 1.0); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.0), Dir2::NEG_X); + let hit = annulus.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::X))); + + // Ray origin is inside of the hole (smaller circle). + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = annulus.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::NEG_X))); + + // Ray origin is inside of the solid annulus. + let ray = Ray2d::new(Vec2::new(0.75, 0.0), Dir2::X); + let hit = annulus.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_X))); + + // Ray origin is inside of the hollow annulus. + let ray = Ray2d::new(Vec2::new(0.75, 0.0), Dir2::X); + let hit = annulus.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(0.25, Dir2::NEG_X))); + + // Ray points away from the annulus. + assert!(!annulus.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = annulus.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/arc.rs b/crates/bevy_math/src/ray_cast/dim2/arc.rs new file mode 100644 index 0000000000000..fe473c07f33f0 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/arc.rs @@ -0,0 +1,88 @@ +use core::f32::consts::FRAC_PI_2; + +use ops::FloatPow; + +use crate::prelude::*; + +impl PrimitiveRayCast2d for Arc2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { + // Adapted from the `Circle` ray casting implementation. + + let b = ray.origin.dot(*ray.direction); + let c = ray.origin.length_squared() - self.radius.squared(); + + if c > 0.0 && b > 0.0 { + // No intersections: The ray direction points away from the circle, and the ray origin is outside of the circle. + return None; + } + + let d = b.squared() - c; + + if d < 0.0 { + // No solution, no intersections. + return None; + } + + let d_sqrt = ops::sqrt(d); + let t2 = -b - d_sqrt; + + if t2 > 0.0 && t2 <= max_distance { + // The ray hit the outside of the arc. + let p2 = ray.get_point(t2); + let arc_bottom_y = self.radius * ops::sin(FRAC_PI_2 + self.half_angle); + if p2.y >= arc_bottom_y { + let normal = Dir2::new(p2 / self.radius).ok()?; + return Some(RayHit2d::new(t2, normal)); + } + } + + let t1 = -b + d_sqrt; + if t1 <= max_distance { + // The ray hit the inside of the arc. + let p1 = ray.get_point(t1); + let arc_bottom_y = self.radius * ops::sin(FRAC_PI_2 + self.half_angle); + if p1.y >= arc_bottom_y { + let normal = Dir2::new(-p1 / self.radius).ok()?; + return Some(RayHit2d::new(t1, normal)); + } + } + + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f32::consts::PI; + + #[test] + fn local_ray_cast_arc() { + let arc = Arc2d::new(1.0, PI / 4.0); + + // Ray points away from the arc. + assert!(!arc.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.25), Dir2::NEG_X))); + assert!(!arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.9), Dir2::NEG_Y))); + assert!(!arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Dir2::Y))); + + // Ray hits the arc. + assert!(arc.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.75), Dir2::NEG_X))); + assert!(arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.9), Dir2::Y))); + assert!(arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Dir2::NEG_Y))); + + // Check correct hit distance and normal. + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = arc.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::NEG_Y))); + + let ray = Ray2d::new(Vec2::new(0.0, 1.5), Dir2::NEG_Y); + let hit = arc.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = arc.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/capsule.rs b/crates/bevy_math/src/ray_cast/dim2/capsule.rs new file mode 100644 index 0000000000000..2f94c10cae447 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/capsule.rs @@ -0,0 +1,193 @@ +use crate::prelude::*; + +// This is mostly the same as `Capsule3d`, but with 2D types. +impl PrimitiveRayCast2d for Capsule2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + // Adapted from Inigo Quilez's ray-capsule intersection algorithm: https://iquilezles.org/articles/intersectors/ + + let radius_squared = self.radius * self.radius; + + let ba = 2.0 * self.half_length; + let oa = Vec2::new(ray.origin.x, ray.origin.y + self.half_length); + + let baba = ba * ba; + let bard = ba * ray.direction.y; + let baoa = ba * oa.y; + let rdoa = ray.direction.dot(oa); + let oaoa = oa.dot(oa); + + // Note: We use `f32::EPSILON` to avoid division by zero later for rays parallel to the capsule's axis. + let a = (baba - bard * bard).max(f32::EPSILON); + let b = baba * rdoa - baoa * bard; + let c = baba * (oaoa - radius_squared) - baoa * baoa; + let d = b * b - a * c; + + if d >= 0.0 { + let is_inside_rect_horizontal = c < 0.0; + let is_inside_rect_vertical = ops::abs(ray.origin.y) < self.half_length; + let intersects_hemisphere = is_inside_rect_horizontal && { + // The ray origin intersects one of the semicircles if the distance + // between the ray origin and semicircle center is negative. + Vec2::new(ray.origin.x, self.half_length - ops::abs(ray.origin.y)).length_squared() + < radius_squared + }; + let is_origin_inside = + intersects_hemisphere || (is_inside_rect_horizontal && is_inside_rect_vertical); + + if solid && is_origin_inside { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + let t = if is_origin_inside { + (-b + ops::sqrt(d)) / a + } else { + (-b - ops::sqrt(d)) / a + }; + + let y = baoa + t * bard; + + // Check if the ray hit the rectangular part. + let hit_rectangle = y > 0.0 && y < baba; + if hit_rectangle && t > 0.0 { + if t > max_distance { + return None; + } + + // The ray hit the side of the rectangle. + let normal = Dir2::new(Vec2::new(-ray.direction.x.signum(), 0.0)).ok()?; + return Some(RayHit2d::new(t, normal)); + } + + // Next, we check the semicircles for intersections. + // It's enough to only check one semicircle and just take the side into account. + + // Offset between the ray origin and the center of the hit semicircle. + let offset_ray = Ray2d { + origin: if y <= 0.0 { + oa + } else { + Vec2::new(ray.origin.x, ray.origin.y - self.half_length) + }, + direction: ray.direction, + }; + + // See `Circle` ray casting implementation. + + let b = offset_ray.origin.dot(*ray.direction); + let c = offset_ray.origin.length_squared() - radius_squared; + + // No intersections if the ray direction points away from the ball and the ray origin is outside of the ball. + if c > 0.0 && b > 0.0 { + return None; + } + + let d = b * b - c; + + if d < 0.0 { + // No solution, no intersection. + return None; + } + + let d_sqrt = ops::sqrt(d); + + let t2 = if is_origin_inside { + -b + d_sqrt + } else { + -b - d_sqrt + }; + + if t2 > 0.0 && t2 <= max_distance { + // The ray origin is outside of the hemisphere that was hit. + let dir = Dir2::new( + if is_origin_inside { -1.0 } else { 1.0 } * offset_ray.get_point(t2) + / self.radius, + ) + .ok()?; + return Some(RayHit2d::new(t2, dir)); + } + + // The ray hit the hemisphere that the ray origin is in. + // The distance corresponding to the boundary hit is the first root. + let t1 = -b + d_sqrt; + + if t1 > max_distance { + return None; + } + + let dir = Dir2::new(-offset_ray.get_point(t1) / self.radius).ok()?; + return Some(RayHit2d::new(t1, dir)); + } + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + use core::f32::consts::SQRT_2; + + #[test] + fn local_ray_cast_capsule_2d() { + let capsule = Capsule2d::new(1.0, 2.0); + + // The Y coordinate corresponding to the angle PI/4 on a circle with the capsule's radius. + let circle_frac_pi_4_y = capsule.radius * SQRT_2 / 2.0; + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.0), Dir2::NEG_X); + let hit = capsule.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::X))); + + let ray = Ray2d::new(Vec2::new(-2.0, 1.0 + circle_frac_pi_4_y), Dir2::X); + let hit = capsule.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 1.0 + capsule.radius - circle_frac_pi_4_y); + assert_relative_eq!(hit.normal, Dir2::NORTH_WEST); + + // Ray origin is inside of the solid capsule. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = capsule.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_X))); + + // Ray origin is inside of the hollow capsule. + // Test three cases: inside the rectangle, inside the top semicircle, and inside the bottom semicircle. + + // Inside the rectangle. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::NEG_X))); + + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(2.0, Dir2::NEG_Y))); + + // Inside the top semicircle. + let ray = Ray2d::new(Vec2::new(0.0, 1.0), Dir2::NORTH_EAST); + let hit = capsule.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 1.0); + assert_relative_eq!(hit.normal, Dir2::SOUTH_WEST); + + let ray = Ray2d::new(Vec2::new(0.0, 1.0), Dir2::NEG_Y); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(3.0, Dir2::Y))); + + // Inside the bottom semicircle. + let ray = Ray2d::new(Vec2::new(0.0, -1.0), Dir2::SOUTH_WEST); + let hit = capsule.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 1.0); + assert_relative_eq!(hit.normal, Dir2::NORTH_EAST); + + let ray = Ray2d::new(Vec2::new(0.0, -1.0), Dir2::Y); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(3.0, Dir2::NEG_Y))); + + // Ray points away from the capsule. + assert!(!capsule.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.1), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.6), Dir2::NEG_Y); + let hit = capsule.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/circle.rs b/crates/bevy_math/src/ray_cast/dim2/circle.rs new file mode 100644 index 0000000000000..533584c3f005e --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/circle.rs @@ -0,0 +1,152 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl PrimitiveRayCast2d for Circle { + #[inline] + fn local_ray_distance(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + local_ray_distance_with_circle(self.radius, ray, solid) + .and_then(|(distance, _)| (distance <= max_distance).then_some(distance)) + } + + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + local_ray_distance_with_circle(self.radius, ray, solid).and_then(|(distance, is_inside)| { + if solid && is_inside { + Some(RayHit2d::new(0.0, -ray.direction)) + } else if distance <= max_distance { + let point = ray.get_point(distance); + let normal = + Dir2::new(if is_inside { -1.0 } else { 1.0 } * point / self.radius).ok()?; + Some(RayHit2d::new(distance, normal)) + } else { + None + } + }) + } +} + +#[inline] +fn local_ray_distance_with_circle(radius: f32, ray: Ray2d, solid: bool) -> Option<(f32, bool)> { + // The function representing any point on a ray is: + // + // P(t) = O + tD + // + // where O is the ray origin and D is the ray direction. We need to find the value t + // that represents the distance at which the ray intersects the sphere. + // + // Spherical shapes can be represented with the following implicit equations: + // + // Circle: x^2 + y^2 = R^2 + // Sphere: x^2 + y^2 + z^2 = R^2 + // + // Representing the coordinates with a point P, we get an implicit function: + // + // length_squared(P) - R^2 = 0 + // + // Substituting P for the equation of a ray: + // + // length_squared(O + tD) - R^2 = 0 + // + // Expanding this equation, we get: + // + // length_squared(D) * t^2 + 2 * dot(O, D) * t + length_squared(O) - R^2 = 0 + // + // This is a quadratic equation with: + // + // a = length_squared(D) = 1 (the ray direction is normalized) + // b = 2 * dot(O, D) + // c = length_squared(O) - R^2 + // + // The discriminant is d = b^2 - 4ac = b^2 - 4c. + // + // 1. If d < 0, there is no valid solution, and the ray does not intersect the sphere. + // 2. If d = 0, there is one root given by t = -b / 2a. With limited precision, we can ignore this case. + // 3. If d > 0, we get two roots. + // + // The two roots for case (3) are: + // + // t1 = (-b + sqrt(d)) / 2a = (-b + sqrt(d)) / 2 + // t2 = (-b - sqrt(d)) / 2a = (-b - sqrt(d)) / 2 + // + // If a root is negative, the intersection is behind the ray's origin and therefore ignored. + // + // We can actually simplify the computations further with: + // + // b = dot(O, D) + // d = b^2 - c + // t1 = -b + sqrt(d) + // t2 = -b - sqrt(d) + // + // Proof, denoting the original variables with _o and the simplified versions with _s: + // + // t1_o = t1_s + // (-b_o + sqrt(d_o)) / 2 = -b_s + sqrt(d_s) + // (-2 * dot(O, D) + sqrt((2 * dot(O, D))^2 - 4c)) / 2 = -dot(O, D) + sqrt(dot(O, D)^2 - c) + // -2 * dot(O, D) + sqrt(4 * dot(O, D)^2 - 4c) = -2 * dot(O, D) + 2 * sqrt(dot(O, D)^2 - c) + // sqrt(4 * dot(O, D)^2 - 4c) = 2 * sqrt(dot(O, D)^2 - c) + // sqrt(4 * dot(O, D)^2 - 4c) = sqrt(4 * (dot(O, D)^2 - c)) + // sqrt(4 * dot(O, D)^2 - 4c) = sqrt(4 * dot(O, D)^2 - 4c) + + // The squared distance between the ray origin and the boundary of the circle. + let c = ray.origin.length_squared() - radius.squared(); + + if c > 0.0 { + // The ray origin is outside of the ball. + let b = ray.origin.dot(*ray.direction); + + if b > 0.0 { + // The ray points away from the circle, so there can be no hits. + return None; + } + + // The distance corresponding to the boundary hit is the second root. + let d = b.squared() - c; + let t2 = -b - ops::sqrt(d); + + Some((t2, false)) + } else if solid { + // The ray origin is inside of the solid circle. + Some((0.0, true)) + } else { + // The ray origin is inside of the hollow circle. + // The distance corresponding to the boundary hit is the first root. + let b = ray.origin.dot(*ray.direction); + let d = b.squared() - c; + let t1 = -b + ops::sqrt(d); + Some((t1, true)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_circle() { + let circle = Circle::new(1.0); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.0), Dir2::NEG_X); + let hit = circle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::X))); + + // Ray origin is inside of the solid circle. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = circle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_X))); + + // Ray origin is inside of the hollow circle. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = circle.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::NEG_X))); + + // Ray points away from the circle. + assert!(!circle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = circle.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs b/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs new file mode 100644 index 0000000000000..c03decc1f90fc --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs @@ -0,0 +1,121 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl PrimitiveRayCast2d for CircularSector { + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + // First, if the sector is solid, check if the ray origin is inside of it. + if solid + && ray.origin.length_squared() < self.radius().squared() + && ops::abs(ray.origin.angle_to(Vec2::Y)) < self.arc.half_angle + { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + // Check for intersections with the circular arc. + let mut closest = self.arc.local_ray_cast(ray, max_distance, true); + + // Check for intersection with the line segment between the origin and the arc's first endpoint. + let left_endpoint = self.arc.left_endpoint(); + let left_segment = Segment2d::new(Vec2::ZERO, left_endpoint); + + if let Some(intersection) = left_segment.local_ray_cast(ray, max_distance, true) { + if let Some(closest) = closest.filter(|_| self.arc.is_minor()) { + // If the arc is at most half of the circle and the ray is intersecting both the arc and the line segment, + // we can return early with the closer hit, as the ray cannot also be intersecting the second line segment. + return if closest.distance <= intersection.distance { + Some(closest) + } else { + Some(intersection) + }; + } + closest = Some(intersection); + } + + // Check for intersection with the line segment between the origin and the arc's second endpoint. + // We can just flip the segment about the Y axis since the sides are symmetrical. + let right_endpoint = self.arc.right_endpoint(); + let right_segment = Segment2d::new(Vec2::ZERO, right_endpoint); + + if let Some(intersection) = right_segment.local_ray_cast(ray, max_distance, true) + && (closest.is_none() || intersection.distance < closest.unwrap().distance) + { + closest = Some(intersection); + } + + closest + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f32::consts::PI; + + #[test] + fn local_ray_cast_sector() { + let sector = CircularSector::new(1.0, PI / 4.0); + + // Ray points away from the circular sector. + assert!(!sector.intersects_local_ray(Ray2d::new(Vec2::new(0.5, 0.2), Dir2::X))); + assert!(!sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -0.1), Dir2::NEG_Y))); + assert!(!sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Dir2::Y))); + + // Ray hits the circular sector. + assert!(sector.intersects_local_ray(Ray2d::new(Vec2::new(0.5, 0.2), Dir2::NEG_X))); + assert!(sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -0.1), Dir2::Y))); + assert!(sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Dir2::NEG_Y))); + + // Check correct hit distance and normal for outside hits. + let ray = Ray2d::new(Vec2::new(0.0, 0.0), Dir2::Y); + let hit = sector + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(0.0, Dir2::SOUTH_WEST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + let ray = Ray2d::new(Vec2::new(0.0, 1.5), Dir2::NEG_Y); + let hit = sector.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::Y))); + + let ray = Ray2d::new(Vec2::new(-1.0, 0.0), Dir2::NORTH_EAST); + let hit = sector + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new( + // Half the distance between the leftmost and topmost points on a circle. + ops::hypot(sector.radius(), sector.radius()) / 2.0, + Dir2::SOUTH_WEST, + ); + assert!(ops::abs(hit.distance - expected_hit.distance) < 0.000_001); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Interior hit for solid sector. + let ray = Ray2d::new(Vec2::new(0.0, sector.apothem()), Dir2::Y); + let hit = sector.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_Y))); + + // Interior hits for hollow sector. + let ray = Ray2d::new(Vec2::new(0.0, 0.5), Dir2::Y); + let hit = sector.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::NEG_Y))); + + let ray = Ray2d::new(Vec2::new(0.0, 1.0), Dir2::SOUTH_EAST); + let hit = sector + .local_ray_cast(ray, f32::MAX, false) + .expect("hit exists"); + let expected_hit = RayHit2d::new( + // Half the distance between the topmost and rightmost points on a circle. + ops::hypot(sector.radius(), sector.radius()) / 2.0, + Dir2::NORTH_WEST, + ); + assert!(ops::abs(hit.distance - expected_hit.distance) < 0.000_001); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = sector.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs new file mode 100644 index 0000000000000..8edcb33896419 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs @@ -0,0 +1,88 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl PrimitiveRayCast2d for CircularSegment { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + let start = self.arc.left_endpoint(); + let end = self.arc.right_endpoint(); + + // First, if the segment is solid, check if the ray origin is inside of it. + let is_inside = ray.origin.length_squared() < self.radius().squared() + && ray.origin.y >= start.y.min(end.y); + if solid && is_inside { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + // Check for intersection with the circular arc. + let mut closest = None; + if let Some(intersection) = self.arc.local_ray_cast(ray, max_distance, true) { + closest = Some(intersection); + } + + // Check if the segment connecting the arc's endpoints is intersecting the ray. + let segment = Segment2d::new(start, end); + + if !is_inside && ray.origin.y >= start.y.min(end.y) { + // The ray is above the segment and cannot intersect with the segment. + return closest; + } + + if let Some(intersection) = segment.local_ray_cast(ray, max_distance, true) + && (closest.is_none() || intersection.distance < closest.unwrap().distance) + { + closest = Some(intersection); + } + + closest + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f32::consts::PI; + + #[test] + fn local_ray_cast_segment() { + let segment = CircularSegment::new(1.0, PI / 4.0); + + // Ray points away from the circular segment. + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.25), Dir2::NEG_X))); + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.5), Dir2::NEG_Y))); + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Dir2::Y))); + + // Ray hits the circular segment. + assert!(segment.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.75), Dir2::NEG_X))); + assert!(segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.9), Dir2::Y))); + assert!(segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Dir2::NEG_Y))); + + // Check correct hit distance and normal for outside hits. + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = segment.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(segment.apothem(), Dir2::NEG_Y))); + + let ray = Ray2d::new(Vec2::new(0.0, 1.5), Dir2::NEG_Y); + let hit = segment.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::Y))); + + // Interior hit for solid segment. + let ray = Ray2d::new(Vec2::new(0.0, segment.apothem()), Dir2::Y); + let hit = segment.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_Y))); + + // Interior hit for hollow segment. + let ray = Ray2d::new(Vec2::new(0.0, segment.apothem() + 0.01), Dir2::Y); + let hit = segment.local_ray_cast(ray, f32::MAX, false); + assert_eq!( + hit, + Some(RayHit2d::new(segment.sagitta() - 0.01, Dir2::NEG_Y)) + ); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = segment.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/ellipse.rs b/crates/bevy_math/src/ray_cast/dim2/ellipse.rs new file mode 100644 index 0000000000000..f50fc7358c09c --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/ellipse.rs @@ -0,0 +1,99 @@ +use crate::prelude::*; + +impl PrimitiveRayCast2d for Ellipse { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + // Adapted from: + // - The `Circle` ray casting implementation + // - Inigo Quilez's ray-ellipse intersection algorithm: https://www.shadertoy.com/view/NdccWH + + // If the ellipse is just a circle, use the ray casting implementation from `Circle`. + if self.half_size.x == self.half_size.y { + return Circle::new(self.half_size.x).local_ray_cast(ray, max_distance, solid); + } + + // Normalize the ray origin to the ellipse's half-size. + let inv_half_size = self.half_size.recip(); + let origin_n = ray.origin * inv_half_size; + + // First, if the ellipse is solid, check if the ray origin is inside of it. + if solid && origin_n.length_squared() < 1.0 { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + // Normalize the ray direction to the ellipse's half-size. + let direction_n = *ray.direction * inv_half_size; + + // Compute the terms of the quadratic equation (see circle ray casting), + // but modified to simplify the computations. + let a = direction_n.length_squared(); + let b = origin_n.dot(direction_n); + let c = origin_n.length_squared(); + + // Discriminant (modified) + let d = b * b - a * (c - 1.0); + + if d < 0.0 { + // No solution, no intersection. + return None; + } + + let d_sqrt = ops::sqrt(d); + + // Compute the second root of the quadratic equation, a potential intersection. + let t2 = (-b - d_sqrt) / a; + if t2 > 0.0 && t2 < max_distance { + // The ray origin is outside of the ellipse and a hit was found. + // The distance corresponding to the boundary hit is the second root. + let hit_point = ray.get_point(t2); + // [`None`] if dir length zero which shouldn't happen + Dir2::new(hit_point * inv_half_size) + .ok() + .map(|normal| RayHit2d::new(t2, normal)) + } else { + // The ray origin is inside of the hollow ellipse. + // The distance corresponding to the boundary hit is the first root. + let t1 = (-b + d_sqrt) / a; + if t1 > 0.0 && t1 < max_distance { + let hit_point = ray.get_point(t1); + let normal = Dir2::new(-hit_point * inv_half_size).ok()?; + Some(RayHit2d::new(t1, normal)) + } else { + None + } + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_ellipse() { + let ellipse = Ellipse::new(1.0, 0.5); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.0), Dir2::NEG_X); + let hit = ellipse.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::X))); + + // Ray origin is inside of the solid ellipse. + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = ellipse.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_Y))); + + // Ray origin is inside of the hollow ellipse. + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = ellipse.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::NEG_Y))); + + // Ray points away from the ellipse. + assert!(!ellipse.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = ellipse.local_ray_cast(ray, 1.0, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/line.rs b/crates/bevy_math/src/ray_cast/dim2/line.rs new file mode 100644 index 0000000000000..3410be712bd7d --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/line.rs @@ -0,0 +1,78 @@ +use crate::prelude::*; + +impl PrimitiveRayCast2d for Line2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { + // Direction perpendicular to the line. + let normal = Dir2::new(-self.direction.perp()).ok()?; + + let normal_dot_origin = normal.dot(-ray.origin); + let normal_dot_dir = normal.dot(*ray.direction); + + // Check if the ray is parallel to the line, within `f32::EPSILON`. + if ops::abs(normal_dot_dir) < f32::EPSILON { + // Check if the ray is collinear with the line, within `f32::EPSILON`. + if ops::abs(normal_dot_origin) < f32::EPSILON { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + return None; + } + + let distance = normal_dot_origin / normal_dot_dir; + + if distance < 0.0 || distance > max_distance { + return None; + } + + Some(RayHit2d::new( + distance, + Dir2::new(-normal_dot_dir.signum() * normal).ok()?, + )) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_line_2d() { + let line = Line2d { + direction: Dir2::NORTH_EAST, + }; + + // Hit from above at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NEG_Y); + let hit = line + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(3.0, Dir2::NORTH_WEST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Hit from below at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(2.0, -1.0), Dir2::Y); + let hit = line + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(3.0, Dir2::SOUTH_EAST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // If the ray is parallel to the line (within epsilon) but not collinear, they should not intersect. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NORTH_EAST); + assert!(!line.intersects_local_ray(ray)); + + // If the ray is collinear with the line (within epsilon), they should intersect. + let ray = Ray2d::new(Vec2::new(-2.0, -2.0), Dir2::NORTH_EAST); + assert!(line.intersects_local_ray(ray)); + + // Ray points away from the line. + assert!(!line.intersects_local_ray(Ray2d::new(Vec2::new(1.0, 2.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NEG_Y); + let hit = line.local_ray_cast(ray, 2.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/mod.rs b/crates/bevy_math/src/ray_cast/dim2/mod.rs new file mode 100644 index 0000000000000..fafbdffeadda8 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/mod.rs @@ -0,0 +1,357 @@ +mod annulus; +mod arc; +mod capsule; +mod circle; +mod circular_sector; +mod circular_segment; +mod ellipse; +mod line; +mod polygon; +mod polyline; +mod rectangle; +mod rhombus; +mod segment; +mod triangle; + +use crate::{Dir2, Isometry2d, Ray2d}; +#[cfg(all(feature = "bevy_reflect", feature = "serialize"))] +use bevy_reflect::{ReflectDeserialize, ReflectSerialize}; + +/// An intersection between a ray and a shape in two-dimensional space. +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "bevy_reflect", derive(bevy_reflect::Reflect))] +#[cfg_attr(feature = "bevy_reflect", reflect(Debug, PartialEq))] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr( + all(feature = "bevy_reflect", feature = "serialize"), + reflect(Serialize, Deserialize) +)] +pub struct RayHit2d { + /// The distance between the point of intersection and the ray origin. + pub distance: f32, + /// The surface normal on the shape at the point of intersection. + pub normal: Dir2, +} + +impl RayHit2d { + /// Creates a new [`RayHit2d`] from the given distance and surface normal at the point of intersection. + #[inline] + pub const fn new(distance: f32, normal: Dir2) -> Self { + Self { distance, normal } + } +} + +/// A trait for intersecting rays with [primitive shapes] in two-dimensional space. +/// +/// [primitive shapes]: crate::primitives +pub trait PrimitiveRayCast2d { + /// Computes the distance to the closest intersection along the given `ray`, expressed in the local space of `self`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding distance. + /// + /// # Example + /// + /// Casting a ray against a solid circle might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::X); + /// let circle = Circle::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(distance) = circle.local_ray_distance(ray, max_distance, solid) { + /// // The ray intersects the circle at a distance of 1.0. + /// assert_eq!(distance, 1.0); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(distance); + /// assert_eq!(point, Vec2::new(-1.0, 0.0)); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + /// let circle = Circle::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(distance) = circle.local_ray_distance(ray, max_distance, solid) { + /// // The ray origin is inside of the hollow circle, and hit its boundary. + /// assert_eq!(distance, circle.radius); + /// assert_eq!(ray.get_point(distance), Vec2::new(1.0, 0.0)); + /// } + /// ``` + #[inline] + fn local_ray_distance(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + self.local_ray_cast(ray, max_distance, solid) + .map(|hit| hit.distance) + } + + /// Computes the closest intersection along the given `ray`, expressed in the local space of `self`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding intersection. + /// + /// # Example + /// + /// Casting a ray against a solid circle might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::X); + /// let circle = Circle::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(hit) = circle.local_ray_cast(ray, max_distance, solid) { + /// // The ray intersects the circle at a distance of 1.0. + /// // The hit normal at the point of intersection is -X. + /// assert_eq!(hit.distance, 1.0); + /// assert_eq!(hit.normal, Dir2::NEG_X); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(hit.distance); + /// assert_eq!(point, Vec2::new(-1.0, 0.0)); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + /// let circle = Circle::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(hit) = circle.local_ray_cast(ray, max_distance, solid) { + /// // The ray origin is inside of the hollow circle, and hit its boundary. + /// assert_eq!(hit.distance, circle.radius); + /// assert_eq!(hit.normal, Dir2::NEG_X); + /// assert_eq!(ray.get_point(hit.distance), Vec2::new(1.0, 0.0)); + /// } + /// ``` + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option; + + /// Returns `true` if `self` intersects the given `ray` in the local space of `self`. + /// + /// # Example + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// // Define a circle with a radius of `1.0` centered at the origin. + /// let circle = Circle::new(1.0); + /// + /// // Test for ray intersections. + /// assert!(circle.intersects_local_ray(Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::X))); + /// assert!(!circle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::X))); + /// ``` + #[inline] + fn intersects_local_ray(&self, ray: Ray2d) -> bool { + self.local_ray_distance(ray, f32::MAX, true).is_some() + } + + /// Computes the distance to the closest intersection along the given `ray` for `self` transformed by `iso`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding distance. + /// + /// # Example + /// + /// Casting a ray against a solid circle might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::new(-1.0, 0.0), Dir2::X); + /// let circle = Circle::new(1.0); + /// let iso = Isometry2d::from_translation(Vec2::new(1.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(distance) = circle.ray_distance(iso, ray, max_distance, solid) { + /// // The ray intersects the circle at a distance of 1.0. + /// assert_eq!(distance, 1.0); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(distance); + /// assert_eq!(point, Vec2::ZERO); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::new(1.0, 0.0), Dir2::X); + /// let circle = Circle::new(1.0); + /// let iso = Isometry2d::from_translation(Vec2::new(1.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(distance) = circle.ray_distance(iso, ray, max_distance, solid) { + /// // The ray origin is inside of the hollow circle, and hit its boundary. + /// assert_eq!(distance, circle.radius); + /// assert_eq!(ray.get_point(distance), Vec2::new(2.0, 0.0)); + /// } + /// ``` + #[inline] + fn ray_distance( + &self, + iso: Isometry2d, + ray: Ray2d, + max_distance: f32, + solid: bool, + ) -> Option { + let local_ray = ray.inverse_transformed_by(iso); + self.local_ray_distance(local_ray, max_distance, solid) + } + + /// Computes the closest intersection along the given `ray` for `self` transformed by `iso`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding intersection. + /// + /// # Example + /// + /// Casting a ray against a solid circle might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::new(-1.0, 0.0), Dir2::X); + /// let circle = Circle::new(1.0); + /// let iso = Isometry2d::from_translation(Vec2::new(1.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(hit) = circle.ray_cast(iso, ray, max_distance, solid) { + /// // The ray intersects the circle at a distance of 1.0. + /// // The hit normal at the point of intersection is -X. + /// assert_eq!(hit.distance, 1.0); + /// assert_eq!(hit.normal, Dir2::NEG_X); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(hit.distance); + /// assert_eq!(point, Vec2::ZERO); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray2d::new(Vec2::new(1.0, 0.0), Dir2::X); + /// let circle = Circle::new(1.0); + /// let iso = Isometry2d::from_translation(Vec2::new(1.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(hit) = circle.ray_cast(iso, ray, max_distance, solid) { + /// // The ray origin is inside of the hollow circle, and hit its boundary. + /// assert_eq!(hit.distance, circle.radius); + /// assert_eq!(hit.normal, Dir2::NEG_X); + /// assert_eq!(ray.get_point(hit.distance), Vec2::new(2.0, 0.0)); + /// } + /// ``` + #[inline] + fn ray_cast( + &self, + iso: Isometry2d, + ray: Ray2d, + max_distance: f32, + solid: bool, + ) -> Option { + let local_ray = ray.inverse_transformed_by(iso); + self.local_ray_cast(local_ray, max_distance, solid) + .map(|mut hit| { + hit.normal = iso.rotation * hit.normal; + hit + }) + } + + /// Returns `true` if `self` transformed by `iso` intersects the given `ray`. + /// + /// # Example + /// + /// ``` + /// use bevy_math::prelude::*; + /// + /// // Define a circle with a radius of `1.0` shifted by `1.0` along the X axis. + /// let circle = Circle::new(1.0); + /// let iso = Isometry2d::from_translation(Vec2::new(1.0, 0.0)); + /// + /// // Test for ray intersections. + /// assert!(circle.intersects_ray(iso, Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::X))); + /// assert!(!circle.intersects_ray(iso, Ray2d::new(Vec2::new(0.0, 2.0), Dir2::X))); + /// ``` + #[inline] + fn intersects_ray(&self, iso: Isometry2d, ray: Ray2d) -> bool { + self.ray_distance(iso, ray, f32::MAX, true).is_some() + } +} + +#[cfg(test)] +mod tests { + use core::f32::consts::SQRT_2; + + use crate::prelude::*; + use approx::assert_relative_eq; + + #[test] + fn ray_cast_2d() { + let rectangle = Rectangle::new(2.0, 1.0); + let iso = Isometry2d::new(Vec2::new(2.0, 0.0), Rot2::degrees(45.0)); + + // Cast a ray on the transformed rectangle. + let ray = Ray2d::new(Vec2::new(-1.0, SQRT_2 / 2.0), Dir2::X); + let hit = rectangle.ray_cast(iso, ray, f32::MAX, true).unwrap(); + + assert_relative_eq!(hit.distance, 3.0); + assert_eq!(hit.normal, Dir2::NORTH_WEST); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/polygon.rs b/crates/bevy_math/src/ray_cast/dim2/polygon.rs new file mode 100644 index 0000000000000..fddba9d6428bb --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/polygon.rs @@ -0,0 +1,126 @@ +use crate::prelude::*; + +// TODO: Polygons should probably have their own type for this along with a BVH acceleration structure. + +impl PrimitiveRayCast2d for Polygon { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + local_ray_cast_polygon(&self.vertices, ray, max_distance, solid) + } +} + +impl PrimitiveRayCast2d for RegularPolygon { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + let rot = Rot2::radians(self.external_angle_radians()); + + let mut vertex1 = Vec2::new(0.0, self.circumradius()); + let mut vertex2; + + let mut closest_hit: Option = None; + let mut hit_any = false; + + for _ in 0..self.sides { + vertex2 = rot * vertex1; + let segment = Segment2d::new(vertex1, vertex2); + if let Some(hit) = segment.local_ray_cast(ray, max_distance, solid) { + if closest_hit.is_none() || hit.distance < closest_hit.unwrap().distance { + closest_hit = Some(hit); + } + + if hit_any { + // This is the second intersection. + // There can be no more intersections. + return closest_hit; + } + + hit_any = true; + } + vertex1 = vertex2; + } + + // There are either zero or one intersections. + if solid && hit_any { + Some(RayHit2d::new(0.0, -ray.direction)) + } else { + closest_hit + } + } +} + +#[inline] +fn local_ray_cast_polygon( + vertices: &[Vec2], + ray: Ray2d, + max_distance: f32, + solid: bool, +) -> Option { + let (closest_intersection, intersection_count) = vertices + .array_windows::<2>() + .copied() + .chain(vertices.last().zip(vertices.first()).map(|(a, b)| [*a, *b])) + .map(|[start, end]| Segment2d::new(start, end)) + .filter_map(|segment| segment.local_ray_cast(ray, max_distance, true)) + .fold((None::, 0), |(closest, hit_count), hit| { + let closest_hit = if closest.is_some_and(|h| h.distance < hit.distance) { + closest + } else { + Some(hit) + }; + (closest_hit, hit_count + 1) + }); + + // check if the ray is inside the polygon + if solid && intersection_count % 2 == 1 { + Some(RayHit2d::new(0.0, -ray.direction)) + } else { + closest_intersection + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_polygon() { + // Same as the rectangle test, but with a polygon shape. + let polygon = Polygon::new([ + Vec2::new(1.0, 0.5), + Vec2::new(-1.0, 0.5), + Vec2::new(-1.0, -0.5), + Vec2::new(1.0, -0.5), + ]); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.0), Dir2::NEG_X); + let hit = polygon.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::X))); + + // Ray origin is inside of the solid polygon. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = polygon.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_X))); + + // Ray origin is inside of the hollow polygon. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = polygon.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::NEG_X))); + + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = polygon.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::NEG_Y))); + + // Ray points away from the polygon. + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::Y))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.0), Dir2::X))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -1.0), Dir2::X))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.0), Dir2::Y))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = polygon.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/polyline.rs b/crates/bevy_math/src/ray_cast/dim2/polyline.rs new file mode 100644 index 0000000000000..551f9d900d2b1 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/polyline.rs @@ -0,0 +1,91 @@ +use crate::prelude::*; + +// TODO: Polylines should probably have their own type for this along with a BVH acceleration structure. + +impl PrimitiveRayCast2d for Polyline2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { + local_ray_cast_polyline(&self.vertices, ray, max_distance) + } +} + +#[inline] +fn local_ray_cast_polyline(vertices: &[Vec2], ray: Ray2d, max_distance: f32) -> Option { + vertices + .array_windows::<2>() + .map(|[start, end]| Segment2d::new(*start, *end)) + .filter_map(|segment| segment.local_ray_cast(ray, max_distance, true)) + .min_by_key(|hit| crate::FloatOrd(hit.distance)) +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_polyline_2d() { + let polyline = Polyline2d::new([ + Vec2::new(-6.0, -2.0), + Vec2::new(-2.0, 2.0), + Vec2::new(2.0, -2.0), + Vec2::new(6.0, 2.0), + ]); + + // Hit from above. + let ray = Ray2d::new(Vec2::new(-4.0, 4.0), Dir2::NEG_Y); + let hit = polyline + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(4.0, Dir2::NORTH_WEST); + assert!(ops::abs(hit.distance - expected_hit.distance) < 0.000_001); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + let ray = Ray2d::new(Vec2::new(0.0, 4.0), Dir2::NEG_Y); + let hit = polyline + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(4.0, Dir2::NORTH_EAST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Hit from below. + let ray = Ray2d::new(Vec2::new(-4.0, -4.0), Dir2::Y); + let hit = polyline + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(4.0, Dir2::SOUTH_EAST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + let ray = Ray2d::new(Vec2::new(0.0, -4.0), Dir2::Y); + let hit = polyline + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(4.0, Dir2::SOUTH_WEST); + assert!(ops::abs(hit.distance - expected_hit.distance) < 0.000_001); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Hit from the side. + let ray = Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::X); + let hit = polyline + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(2.0, Dir2::SOUTH_WEST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Ray goes past the left endpoint. + assert!(!polyline.intersects_local_ray(Ray2d::new(Vec2::new(-7.0, 2.0), Dir2::NEG_Y))); + + // Ray goes past the right endpoint. + assert!(!polyline.intersects_local_ray(Ray2d::new(Vec2::new(7.0, -2.0), Dir2::Y))); + + // Ray points away from the polyline. + assert!(!polyline.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.2), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NEG_Y); + let hit = polyline.local_ray_cast(ray, 2.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/rectangle.rs b/crates/bevy_math/src/ray_cast/dim2/rectangle.rs new file mode 100644 index 0000000000000..d2f16c7e9184b --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/rectangle.rs @@ -0,0 +1,118 @@ +use crate::prelude::*; + +impl PrimitiveRayCast2d for Rectangle { + #[inline] + fn local_ray_distance(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + // Adapted from Inigo Quilez's algorithm: https://iquilezles.org/articles/intersectors/ + + let direction_recip_abs = ray.direction.recip().abs(); + + // Note: The operations here were modified and rearranged to avoid the Inf - Inf = NaN result + // for the edge case where a component of the ray direction is zero and the reciprocal + // is infinity. The NaN would break the early return below. + let n = -ray.direction.signum() * ray.origin; + let t1 = direction_recip_abs * (n - self.half_size); + let t2 = direction_recip_abs * (n + self.half_size); + + let distance_near = t1.max_element(); + let distance_far = t2.min_element(); + + if distance_near > distance_far || distance_far < 0.0 { + return None; + } + + if distance_near > 0.0 { + // The ray hit the outside of the rectangle. + Some(distance_near) + } else if solid { + // The ray origin is inside of the solid rectangle. + Some(0.0) + } else if distance_far <= max_distance { + // The ray hit the inside of the hollow rectangle. + Some(distance_far) + } else { + None + } + } + + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + // Adapted from Inigo Quilez's algorithm: https://iquilezles.org/articles/intersectors/ + + let direction_recip_abs = ray.direction.recip().abs(); + + // Note: The operations here were modified and rearranged to avoid the Inf - Inf = NaN result + // for the edge case where a component of the ray direction is zero and the reciprocal + // is infinity. The NaN would break the early return below. + let n = -ray.direction.signum() * ray.origin; + let t1 = direction_recip_abs * (n - self.half_size); + let t2 = direction_recip_abs * (n + self.half_size); + + let distance_near = t1.max_element(); + let distance_far = t2.min_element(); + + if distance_near > distance_far || distance_far < 0.0 || distance_near > max_distance { + return None; + } + + if distance_near > 0.0 { + // The ray hit the outside of the rectangle. + // Note: We could also just have an if-else here, but it was measured to be ~15% slower. + let normal_abs = Vec2::from(Vec2::splat(distance_near).cmple(t1)); + let normal = Dir2::new(-ray.direction.signum() * normal_abs).ok()?; + Some(RayHit2d::new(distance_near, normal)) + } else if solid { + // The ray origin is inside of the solid rectangle. + Some(RayHit2d::new(0.0, -ray.direction)) + } else if distance_far <= max_distance { + // The ray hit the inside of the hollow rectangle. + // Note: We could also just have an if-else here, but it was measured to be ~15% slower. + let normal_abs = Vec2::from(t2.cmple(Vec2::splat(distance_far))); + let normal = Dir2::new(-ray.direction.signum() * normal_abs).ok()?; + Some(RayHit2d::new(distance_far, normal)) + } else { + None + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_rectangle() { + let rectangle = Rectangle::new(2.0, 1.0); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.0), Dir2::NEG_X); + let hit = rectangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::X))); + + // Ray origin is inside of the solid rectangle. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = rectangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_X))); + + // Ray origin is inside of the hollow rectangle. + let ray = Ray2d::new(Vec2::ZERO, Dir2::X); + let hit = rectangle.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(1.0, Dir2::NEG_X))); + + let ray = Ray2d::new(Vec2::ZERO, Dir2::Y); + let hit = rectangle.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::NEG_Y))); + + // Ray points away from the rectangle. + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::Y))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.0), Dir2::X))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -1.0), Dir2::X))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.0), Dir2::Y))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(-2.0, 0.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = rectangle.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/rhombus.rs b/crates/bevy_math/src/ray_cast/dim2/rhombus.rs new file mode 100644 index 0000000000000..b2f8d3cec8037 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/rhombus.rs @@ -0,0 +1,83 @@ +use crate::prelude::*; + +impl PrimitiveRayCast2d for Rhombus { + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + // First, if the segment is solid, check if the ray origin is inside of it. + if solid + && ops::abs(ray.origin.x) / self.half_diagonals.x + + ops::abs(ray.origin.y) / self.half_diagonals.y + <= 1.0 + { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + let mut closest: Option = None; + + let top = Vec2::new(0.0, self.half_diagonals.y); + let bottom = Vec2::new(0.0, -self.half_diagonals.y); + let left = Vec2::new(-self.half_diagonals.x, 0.0); + let right = Vec2::new(self.half_diagonals.x, 0.0); + + let edges = [(top, left), (bottom, right), (top, right), (bottom, left)]; + let mut hit_any = false; + + // Check edges for intersections. There can be either zero or two intersections. + for (start, end) in edges.into_iter() { + let segment = Segment2d::new(start, end); + + if let Some(intersection) = segment.local_ray_cast(ray, max_distance, true) + && (closest.is_none() || intersection.distance < closest.unwrap().distance) + { + closest = Some(intersection); + + if hit_any { + // This is the second intersection, the exit point. + // There can be no more intersections. + break; + } + + hit_any = true; + } + } + + closest + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + use core::f32::consts::SQRT_2; + + #[test] + fn local_ray_cast_rhombus() { + let rhombus = Rhombus::new(2.0, 2.0); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(2.0, 0.5), Dir2::NEG_X); + let hit = rhombus.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 1.5); + assert_relative_eq!(hit.normal, Dir2::NORTH_EAST); + + // Ray origin is inside of the solid rhombus. + let ray = Ray2d::new(Vec2::ZERO, Dir2::NORTH_EAST); + let hit = rhombus.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 0.0); + assert_relative_eq!(hit.normal, Dir2::SOUTH_WEST); + + // Ray origin is inside of the hollow rhombus. + let ray = Ray2d::new(Vec2::ZERO, Dir2::NORTH_EAST); + let hit = rhombus.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert!(ops::abs(hit.distance - SQRT_2 / 2.0) < 0.000_001); + assert_relative_eq!(hit.normal, Dir2::SOUTH_WEST); + + // Ray points away from the rhombus. + assert!(!rhombus.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Dir2::NEG_Y); + let hit = rhombus.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/segment.rs b/crates/bevy_math/src/ray_cast/dim2/segment.rs new file mode 100644 index 0000000000000..3b515876aa11c --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/segment.rs @@ -0,0 +1,96 @@ +use crate::prelude::*; + +impl PrimitiveRayCast2d for Segment2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { + // Unit normal to the supporting line of the segment. + let normal = -self.direction().perp(); + + let denominator = normal.dot(*ray.direction); + + // Parallel? + if ops::abs(denominator) < f32::EPSILON { + // Collinear? + let numerator = normal.dot(self.point1() - ray.origin); + if ops::abs(numerator) < f32::EPSILON { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + return None; + } + + // Distance along the ray to the supporting line. + let numerator = normal.dot(self.point1() - ray.origin); + let distance = numerator / denominator; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Compute the intersection point. + let intersection = ray.origin + *ray.direction * distance; + + // Check whether the intersection lies on the finite segment. + let segment = self.point2() - self.point1(); + let t = (intersection - self.point1()).dot(segment) / segment.length_squared(); + + if !(0.0..=1.0).contains(&t) { + return None; + } + + Some(RayHit2d::new( + distance, + Dir2::new(-denominator.signum() * normal).ok()?, + )) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_segment_2d() { + let segment = Segment2d::new(Vec2::NEG_ONE * 5.0, Vec2::ONE * 5.0); + + // Hit from above at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NEG_Y); + let hit = segment + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(3.0, Dir2::NORTH_WEST); + assert_eq!(hit.distance, expected_hit.distance); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Hit from below at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(2.0, -1.0), Dir2::Y); + let hit = segment + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(3.0, Dir2::SOUTH_EAST); + assert!(ops::abs(hit.distance - expected_hit.distance) < 0.000_001); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // If the ray is parallel to the line segment (within epsilon) but not collinear, they should not intersect. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NORTH_EAST); + assert!(!segment.intersects_local_ray(ray)); + + // If the ray is collinear with the line segment (within epsilon), they should intersect. + let ray = Ray2d::new(Vec2::new(-2.0, -2.0), Dir2::NORTH_EAST); + assert!(segment.intersects_local_ray(ray)); + + // Ray goes past the left endpoint. + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(-6.0, 2.0), Dir2::NEG_Y))); + + // Ray goes past the right endpoint. + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(6.0, -2.0), Dir2::Y))); + + // Ray points away from the line segment. + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(1.0, 2.0), Dir2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::NEG_Y); + let hit = segment.local_ray_cast(ray, 2.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim2/triangle.rs b/crates/bevy_math/src/ray_cast/dim2/triangle.rs new file mode 100644 index 0000000000000..e2bc3313e722e --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/triangle.rs @@ -0,0 +1,91 @@ +use crate::prelude::*; + +impl PrimitiveRayCast2d for Triangle2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + let [a, b, c] = self.vertices; + + if solid { + // First, check if the ray starts inside the triangle. + let ab = b - a; + let bc = c - b; + let ca = a - c; + + // Compute the dot products between the edge normals and the offset from the ray origin to each corner. + // If the dot product for an edge is positive, the ray origin is on the interior triangle side relative to that edge. + let dot1 = ab.perp_dot(ray.origin - a); + let dot2 = bc.perp_dot(ray.origin - b); + let dot3 = ca.perp_dot(ray.origin - c); + + // If all three dot products are positive, the ray origin is guaranteed to be inside of the triangle. + if dot1 > 0.0 && dot2 > 0.0 && dot3 > 0.0 { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + } + + let mut closest_intersection: Option = None; + + // Ray cast against each edge to find the closest intersection, if one exists. + for (start, end) in [(a, b), (b, c), (c, a)] { + let segment = Segment2d::new(start, end); + + if let Some(intersection) = segment.local_ray_cast(ray, max_distance, true) { + if let Some(ref closest) = closest_intersection { + if intersection.distance < closest.distance { + closest_intersection = Some(intersection); + } + } else { + closest_intersection = Some(intersection); + } + } + } + + closest_intersection + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f32::consts::SQRT_2; + + #[test] + fn local_ray_cast_triangle_2d() { + let triangle = Triangle2d::new( + Vec2::new(0.0, 2.0), + Vec2::new(0.0, 0.0), + Vec2::new(2.0, 0.0), + ); + + // Ray origin is outside of the shape. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Dir2::X); + let hit = triangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(2.0, Dir2::NEG_X))); + + let ray = Ray2d::new(Vec2::new(2.0, 2.0), Dir2::SOUTH_WEST); + let hit = triangle + .local_ray_cast(ray, f32::MAX, true) + .expect("hit exists"); + let expected_hit = RayHit2d::new(SQRT_2, Dir2::NORTH_EAST); + assert!(ops::abs(hit.distance - expected_hit.distance) < 0.000_001); + assert!(ops::abs(hit.normal.distance(*expected_hit.normal)) < 0.000_001); + + // Ray origin is inside of the solid triangle. + let ray = Ray2d::new(Vec2::splat(0.5), Dir2::X); + let hit = triangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::NEG_X))); + + // Ray origin is inside of the hollow triangle. + let ray = Ray2d::new(Vec2::new(0.5, 0.5), Dir2::NEG_Y); + let hit = triangle.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit2d::new(0.5, Dir2::Y))); + + // Ray points away from the triangle. + assert!(!triangle.intersects_local_ray(Ray2d::new(Vec2::new(1.0, -1.0), Dir2::NEG_Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-1.0, 1.0), Dir2::X); + let hit = triangle.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/capsule.rs b/crates/bevy_math/src/ray_cast/dim3/capsule.rs new file mode 100644 index 0000000000000..46a8fa2aa371a --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/capsule.rs @@ -0,0 +1,205 @@ +use crate::prelude::*; + +// This is mostly the same as `Capsule2d`, but with 3D types. +impl PrimitiveRayCast3d for Capsule3d { + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Adapted from Inigo Quilez's ray-capsule intersection algorithm: https://iquilezles.org/articles/intersectors/ + + let radius_squared = self.radius * self.radius; + + let ba = 2.0 * self.half_length; + let oa = Vec3::new(ray.origin.x, ray.origin.y + self.half_length, ray.origin.z); + + let baba = ba * ba; + let bard = ba * ray.direction.y; + let baoa = ba * oa.y; + let rdoa = ray.direction.dot(oa); + let oaoa = oa.dot(oa); + + // Note: We use `f32::EPSILON` to avoid division by zero later for rays parallel to the capsule's axis. + let a = (baba - bard * bard).max(f32::EPSILON); + let b = baba * rdoa - baoa * bard; + let c = baba * oaoa - baoa * baoa - radius_squared * baba; + let d = b * b - a * c; + + if d >= 0.0 { + let is_inside_cylinder_horizontal = c < 0.0; + let is_inside_cylinder_vertical = ops::abs(ray.origin.y) < self.half_length; + let intersects_hemisphere = is_inside_cylinder_horizontal && { + // The ray origin intersects one of the hemispheres if the distance + // between the ray origin and hemisphere center is negative. + Vec2::new(ray.origin.x, self.half_length - ops::abs(ray.origin.y)).length_squared() + < radius_squared + }; + let is_origin_inside = intersects_hemisphere + || (is_inside_cylinder_horizontal && is_inside_cylinder_vertical); + + if solid && is_origin_inside { + return Some(RayHit3d::new(0.0, -ray.direction)); + } + + let cylinder_distance = if is_origin_inside { + (-b + ops::sqrt(d)) / a + } else { + (-b - ops::sqrt(d)) / a + }; + + let y = baoa + cylinder_distance * bard; + + // Check if the ray hit the cylindrical part. + let hit_rectangle = y > 0.0 && y < baba; + if hit_rectangle && cylinder_distance > 0.0 { + if cylinder_distance > max_distance { + return None; + } + // The ray hit the side of the rectangle. + let point = ray.get_point(cylinder_distance); + let radius_recip = self.radius.recip(); + let normal = Dir3::new(Vec3::new( + ops::copysign(point.x, -ray.direction.x) * radius_recip, + 0.0, + ops::copysign(point.z, -ray.direction.z) * radius_recip, + )) + .ok()?; + return Some(RayHit3d::new(cylinder_distance, normal)); + } + + // Next, we check the hemispheres for intersections. + // It's enough to only check one hemisphere and just take the side into account. + + // Offset between the ray origin and the center of the hit hemisphere. + let offset_ray = Ray3d { + origin: if y <= 0.0 { + oa + } else { + Vec3::new(ray.origin.x, ray.origin.y - self.half_length, ray.origin.z) + }, + direction: ray.direction, + }; + + // See `Sphere` ray casting implementation. + + let b = offset_ray.origin.dot(*ray.direction); + let c = offset_ray.origin.length_squared() - radius_squared; + + // No intersections if the ray direction points away from the ball and the ray origin is outside of the ball. + if c > 0.0 && b > 0.0 { + return None; + } + + let d = b * b - c; + + if d < 0.0 { + // No solution, no intersection. + return None; + } + + let d_sqrt = ops::sqrt(d); + + let t2 = if is_origin_inside { + -b + d_sqrt + } else { + -b - d_sqrt + }; + + if t2 > 0.0 && t2 <= max_distance { + // The ray origin is outside of the hemisphere that was hit. + let dir = Dir3::new( + if is_origin_inside { -1.0 } else { 1.0 } * offset_ray.get_point(t2) + / self.radius, + ) + .ok()?; + return Some(RayHit3d::new(t2, dir)); + } + + // The ray hit the hemisphere that the ray origin is in. + // The distance corresponding to the boundary hit is the first root. + let t1 = -b + d_sqrt; + + if t1 > max_distance { + return None; + } + + let dir = Dir3::new(-offset_ray.get_point(t1) / self.radius).ok()?; + return Some(RayHit3d::new(t1, dir)); + } + None + } +} + +#[cfg(test)] +mod tests { + use super::*; + use approx::assert_relative_eq; + use core::f32::consts::SQRT_2; + + #[test] + fn local_ray_cast_capsule_3d() { + let capsule = Capsule3d::new(1.0, 2.0); + + // The Y coordinate corresponding to the angle PI/4 on a circle with the capsule's radius. + let circle_frac_pi_4_y = capsule.radius * SQRT_2 / 2.0; + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = capsule.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::X))); + + let ray = Ray3d::new(Vec3::new(-2.0, 1.0 + circle_frac_pi_4_y, 0.0), Dir3::X); + let hit = capsule.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 1.0 + capsule.radius - circle_frac_pi_4_y); + assert_relative_eq!(hit.normal, Dir3::from_xyz(-1.0, 1.0, 0.0).unwrap()); + + // Ray origin is inside of the solid capsule. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = capsule.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::NEG_X))); + + // Ray origin is inside of the hollow capsule. + // Test three cases: inside the rectangle, inside the top hemisphere, and inside the bottom hemisphere. + + // Inside the rectangle. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::NEG_X))); + + let ray = Ray3d::new(Vec3::ZERO, Dir3::Y); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(2.0, Dir3::NEG_Y))); + + // Inside the top hemisphere. + let ray = Ray3d::new( + Vec3::new(0.0, 1.0, 0.0), + Dir3::from_xyz(1.0, 1.0, 0.0).unwrap(), + ); + let hit = capsule.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 1.0); + assert_relative_eq!(hit.normal, Dir3::from_xyz(-1.0, -1.0, 0.0).unwrap()); + + let ray = Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Dir3::NEG_Y); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(3.0, Dir3::Y))); + + // Inside the bottom hemisphere. + let ray = Ray3d::new( + Vec3::new(0.0, -1.0, 0.0), + Dir3::from_xyz(-1.0, -1.0, 0.0).unwrap(), + ); + let hit = capsule.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 1.0); + assert_relative_eq!(hit.normal, Dir3::from_xyz(1.0, 1.0, 0.0).unwrap()); + + let ray = Ray3d::new(Vec3::new(0.0, -1.0, 0.0), Dir3::Y); + let hit = capsule.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(3.0, Dir3::NEG_Y))); + + // Ray points away from the capsule. + assert!(!capsule.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 2.1, 0.0), Dir3::Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.6, 0.0), Dir3::NEG_Y); + let hit = capsule.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/cone.rs b/crates/bevy_math/src/ray_cast/dim3/cone.rs new file mode 100644 index 0000000000000..a71f723fce9f8 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/cone.rs @@ -0,0 +1,225 @@ +use crate::prelude::*; + +// NOTE: This is largely a copy of the `ConicalFrustum` implementation, but simplified for only one base. +impl PrimitiveRayCast3d for Cone { + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Adapted from: + // - Inigo Quilez's capped cone ray intersection algorithm: https://iquilezles.org/articles/intersectors/ + // - http://lousodrome.net/blog/light/2017/01/03/intersection-of-a-ray-and-a-cone/ + + let half_height = self.height * 0.5; + let height_squared = self.height * self.height; + let radius_squared = self.radius * self.radius; + + let a = Vec3::new(0.0, half_height, 0.0); + let b = -a; + let ba = b - a; + let oa = ray.origin - a; + let ob = ray.origin - b; + + let oa_dot_ba = oa.dot(ba); + let ob_dot_ba = ob.dot(ba); + + // The ray origin is inside of the cone if both of the following are true: + // 1. The origin is between the top and bottom. + // 2. The origin is within the circular slice determined by the distance from the base. + let is_inside = oa_dot_ba >= 0.0 && oa_dot_ba <= height_squared && { + // Compute the radius of the circular slice. + // Derived geometrically from the triangular cross-section. + // + // let y = ob_dot_ba / self.height; + // let slope = self.height / self.radius; + // + // let delta_radius = y / slope + // = y / (self.height / self.radius) + // = y * self.radius / self.height + // = ob_dot_ba / self.height * self.radius / self.height + // = ob_dot_ba * self.radius / (self.height * self.height); + // let radius = self.radius + delta_radius; + let delta_radius = ob_dot_ba * self.radius / height_squared; + let radius = self.radius + delta_radius; + + // The squared orthogonal distance from the cone axis + let ortho_distance_squared = ray.origin.xz().length_squared(); + + ortho_distance_squared < radius * radius + }; + + if is_inside { + if solid { + return Some(RayHit3d::new(0.0, -ray.direction)); + } + + let dir_dot_ba = ray.direction.dot(ba); + let dir_dot_oa = ray.direction.dot(oa); + let top_distance_squared = oa.length_squared(); + + // Base + if oa_dot_ba >= 0.0 && dir_dot_ba > 0.0 { + let distance = -ob_dot_ba / dir_dot_ba; + + if distance <= max_distance { + // Check if the point of intersection is within the bottom circle. + let distance_at_bottom_squared = + (ob + ray.direction * distance).length_squared(); + if distance_at_bottom_squared < radius_squared { + // The ray hit the bottom of the cone. + let normal = -Dir3::new(ba / self.height).ok()?; + return Some(RayHit3d::new(distance, normal)); + } + } + } + + // The ray hit the lateral surface of the cone. + // Because the ray is known to be inside of the shape, no further checks are needed. + + let height_pow_4 = height_squared * height_squared; + let hypot_squared = height_squared + radius_squared; + + // Quadratic equation coefficients a, b, c + let a = height_pow_4 - dir_dot_ba * dir_dot_ba * hypot_squared; + let b = height_pow_4 * dir_dot_oa - oa_dot_ba * dir_dot_ba * hypot_squared; + let c = height_pow_4 * top_distance_squared - oa_dot_ba * oa_dot_ba * hypot_squared; + let discriminant = b * b - a * c; + + // Two roots: + // t1 = (-b - discriminant.sqrt()) / a + // t2 = (-b + discriminant.sqrt()) / a + // For the inside case, we want t2. + let distance = (-b + ops::sqrt(discriminant)) / a; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Squared distance from top along cone axis at the point of intersection + let hit_y_squared = oa_dot_ba + distance * dir_dot_ba; + + let normal = -Dir3::new( + height_pow_4 * (oa + distance * ray.direction) - ba * hypot_squared * hit_y_squared, + ) + .ok()?; + + Some(RayHit3d::new(distance, normal)) + } else { + // The ray origin is outside of the cone. + + let dir_dot_ba = ray.direction.dot(ba); + let dir_dot_oa = ray.direction.dot(oa); + let top_distance_squared = oa.length_squared(); + + // Base + if ob_dot_ba > 0.0 && dir_dot_ba < 0.0 { + let distance = -ob_dot_ba / dir_dot_ba; + + if distance <= max_distance { + // Check if the point of intersection is within the bottom circle. + let distance_at_bottom_squared = + (ob + ray.direction * distance).length_squared(); + if distance_at_bottom_squared < radius_squared { + // The ray hit the bottom of the cone. + let normal = Dir3::new(ba / self.height).ok()?; + return Some(RayHit3d::new(distance, normal)); + } + } + } + + // Check for intersections with the lateral surface of the cone. + + let height_pow_4 = height_squared * height_squared; + let hypot_squared = height_squared + radius_squared; + + // Quadratic equation coefficients a, b, c + let a = height_pow_4 - dir_dot_ba * dir_dot_ba * hypot_squared; + let b = height_pow_4 * dir_dot_oa - oa_dot_ba * dir_dot_ba * hypot_squared; + let c = height_pow_4 * top_distance_squared - oa_dot_ba * oa_dot_ba * hypot_squared; + let discriminant = b * b - a * c; + + if discriminant < 0.0 { + return if ray.direction.y == -1.0 { + // Edge case: The ray is pointing straight down at the tip. + Some(RayHit3d::new(ops::sqrt(top_distance_squared), Dir3::Y)) + } else { + None + }; + } + + // Two roots: + // t1 = (-b - discriminant.sqrt()) / a + // t2 = (-b + discriminant.sqrt()) / a + // For the outside case, we want t1. + let distance = (-b - ops::sqrt(discriminant)) / a; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Squared distance from top along cone axis at the point of intersection + let hit_y_squared = oa_dot_ba + distance * dir_dot_ba; + + if hit_y_squared < 0.0 || hit_y_squared > height_squared { + // The point of intersection is outside of the height of the cone. + return None; + } + + let normal = Dir3::new( + height_pow_4 * (oa + distance * ray.direction) - ba * hypot_squared * hit_y_squared, + ) + .ok()?; + + Some(RayHit3d::new(distance, normal)) + } + } +} + +#[cfg(test)] +mod tests { + use approx::assert_relative_eq; + + use super::*; + + #[test] + fn local_ray_cast_cone() { + let cone = Cone::new(1.0, 2.0); + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = cone.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 1.5); + assert_relative_eq!(hit.normal, Dir3::from_xyz(2.0, 1.0, 0.0).unwrap()); + + // Ray origin is inside of the solid cone. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = cone.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::NEG_X))); + + // Ray origin is inside of the hollow cone. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = cone.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 0.5); + assert_relative_eq!(hit.normal, Dir3::from_xyz(-2.0, -1.0, 0.0).unwrap()); + let ray = Ray3d::new(Vec3::ZERO, Dir3::NEG_Y); + let hit = cone.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::Y))); + + // Ray hits the cone. + assert!(cone.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 0.9, 0.0), Dir3::Y))); + assert!(cone.intersects_local_ray(Ray3d::new(Vec3::new(0.4, 0.0, 0.0), Dir3::X))); + + // Ray points away from the cone. + assert!(!cone.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Dir3::Y))); + assert!(!cone.intersects_local_ray(Ray3d::new(Vec3::new(0.6, 0.0, 0.0), Dir3::X))); + + // Edge case: The ray is pointing straight down at the tip. + let ray = Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Dir3::NEG_Y); + let hit = cone.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_relative_eq!(hit.distance, 0.1); + assert_eq!(hit.normal, Dir3::Y); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::NEG_Y); + let hit = cone.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs b/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs new file mode 100644 index 0000000000000..1837c36fc599b --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs @@ -0,0 +1,256 @@ +use crate::prelude::*; + +impl PrimitiveRayCast3d for ConicalFrustum { + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Adapted from: + // - Inigo Quilez's capped cone ray intersection algorithm: https://iquilezles.org/articles/intersectors/ + // - http://lousodrome.net/blog/light/2017/01/03/intersection-of-a-ray-and-a-cone/ + + let half_height = self.height * 0.5; + let height_squared = self.height * self.height; + let radius_bottom_squared = self.radius_bottom * self.radius_bottom; + let radius_top_squared = self.radius_top * self.radius_top; + + let a = Vec3::new(0.0, half_height, 0.0); + let b = -a; + let ba = b - a; + let oa = ray.origin - a; + let ob = ray.origin - b; + + let oa_dot_ba = oa.dot(ba); + let ob_dot_ba = ob.dot(ba); + + // The ray origin is inside of the frustum if both of the following are true: + // 1. The origin is between the top and bottom bases. + // 2. The origin is within the circular slice determined by the distance from the base. + let is_inside = oa_dot_ba >= 0.0 && oa_dot_ba <= height_squared && { + // Compute the radius of the circular slice. + // Derived geometrically from the trapezoidal cross-section. + // + // let y = ob_dot_ba / self.height; + // let slope = self.height / (self.radius_bottom - self.radius_top); + // + // let delta_radius = y / slope + // = y / (self.height / (self.radius_bottom - self.radius_top)) + // = y * (self.radius_bottom - self.radius_top) / self.height + // = ob_dot_ba / self.height * (self.radius_bottom - self.radius_top) / self.height + // = ob_dot_ba * (self.radius_bottom - self.radius_top) / (self.height * self.height); + // let radius = self.radius_bottom + delta_radius; + let delta_radius = ob_dot_ba * (self.radius_bottom - self.radius_top) / height_squared; + let radius = self.radius_bottom + delta_radius; + + // The squared orthogonal distance from the frustum axis + let ortho_distance_squared = ray.origin.xz().length_squared(); + + ortho_distance_squared < radius * radius + }; + + if is_inside { + if solid { + return Some(RayHit3d::new(0.0, -ray.direction)); + } + + let dir_dot_ba = ray.direction.dot(ba); + let dir_dot_oa = ray.direction.dot(oa); + let top_distance_squared = oa.length_squared(); + + // Caps + if oa_dot_ba >= 0.0 && dir_dot_ba > 0.0 { + let distance = -ob_dot_ba / dir_dot_ba; + + if distance <= max_distance { + // Check if the point of intersection is within the bottom circle. + let distance_at_bottom_squared = + (ob + ray.direction * distance).length_squared(); + if distance_at_bottom_squared < radius_bottom_squared { + // The ray hit the bottom of the frustum. + let normal = -Dir3::new(ba / self.height).ok()?; + return Some(RayHit3d::new(distance, normal)); + } + } + } else if ob_dot_ba <= 0.0 { + // Check if the point of intersection is within the top circle. + // Here we delay the division in the distance computation. + if (oa * dir_dot_ba - ray.direction * oa_dot_ba).length_squared() + < radius_top_squared * dir_dot_ba * dir_dot_ba + { + // The ray hit the top of the frustum. + let distance = -oa_dot_ba / dir_dot_ba; + let normal = -Dir3::new(-ba / self.height).ok()?; + return (distance <= max_distance).then_some(RayHit3d::new(distance, normal)); + } + } + + // The ray hit the lateral surface of the conical frustum. + // Because the ray is known to be inside of the shape, no further checks are needed. + + let radius_difference = self.radius_top - self.radius_bottom; + let height_pow_4 = height_squared * height_squared; + let hypot_squared = height_squared + radius_difference * radius_difference; + + // Quadratic equation coefficients a, b, c + let a = height_pow_4 - dir_dot_ba * dir_dot_ba * hypot_squared; + let b = height_pow_4 * dir_dot_oa - oa_dot_ba * dir_dot_ba * hypot_squared + + height_squared * self.radius_top * radius_difference * dir_dot_ba; + let c = height_pow_4 * top_distance_squared - oa_dot_ba * oa_dot_ba * hypot_squared + + height_squared + * self.radius_top + * (radius_difference * oa_dot_ba * 2.0 - height_squared * self.radius_top); + let discriminant = b * b - a * c; + + // Two roots: + // t1 = (-b - discriminant.sqrt()) / a + // t2 = (-b + discriminant.sqrt()) / a + // For the inside case, we want t2. + let distance = (-b + ops::sqrt(discriminant)) / a; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Squared distance from top along frustum axis at the point of intersection + let hit_y_squared = oa_dot_ba + distance * dir_dot_ba; + + let normal = -Dir3::new( + height_squared + * (height_squared * (oa + distance * ray.direction) + + radius_difference * ba * self.radius_top) + - ba * hypot_squared * hit_y_squared, + ) + .ok()?; + + Some(RayHit3d::new(distance, normal)) + } else { + // The ray origin is outside of the cone. + + let dir_dot_ba = ray.direction.dot(ba); + let dir_dot_oa = ray.direction.dot(oa); + let top_distance_squared = oa.length_squared(); + + // Caps + if oa_dot_ba < 0.0 && dir_dot_ba > 0.0 { + // Check if the point of intersection is within the top circle. + // Here we delay the division in the distance computation. + if (oa * dir_dot_ba - ray.direction * oa_dot_ba).length_squared() + < radius_top_squared * dir_dot_ba * dir_dot_ba + { + // The ray hit the top of the frustum. + let distance = -oa_dot_ba / dir_dot_ba; + let normal = Dir3::new(-ba / self.height).ok()?; + return (distance <= max_distance).then_some(RayHit3d::new(distance, normal)); + } + } else if ob_dot_ba > 0.0 && dir_dot_ba < 0.0 { + let distance = -ob_dot_ba / dir_dot_ba; + + if distance <= max_distance { + // Check if the point of intersection is within the bottom circle. + let distance_at_bottom_squared = + (ob + ray.direction * distance).length_squared(); + if distance_at_bottom_squared < radius_bottom_squared { + // The ray hit the bottom of the frustum. + let normal = Dir3::new(ba / self.height).ok()?; + return Some(RayHit3d::new(distance, normal)); + } + } + } + + // Check for intersections with the lateral surface of the conical frustum. + + let radius_difference = self.radius_top - self.radius_bottom; + let height_pow_4 = height_squared * height_squared; + let hypot_squared = height_squared + radius_difference * radius_difference; + + // Quadratic equation coefficients a, b, c + let a = height_pow_4 - dir_dot_ba * dir_dot_ba * hypot_squared; + let b = height_pow_4 * dir_dot_oa - oa_dot_ba * dir_dot_ba * hypot_squared + + height_squared * self.radius_top * radius_difference * dir_dot_ba; + let c = height_pow_4 * top_distance_squared - oa_dot_ba * oa_dot_ba * hypot_squared + + height_squared + * self.radius_top + * (radius_difference * oa_dot_ba * 2.0 - height_squared * self.radius_top); + let discriminant = b * b - a * c; + + if discriminant < 0.0 { + return None; + } + + // Two roots: + // t1 = (-b - discriminant.sqrt()) / a + // t2 = (-b + discriminant.sqrt()) / a + // For the outside case, we want t1. + let distance = (-b - ops::sqrt(discriminant)) / a; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Squared distance from top along frustum axis at the point of intersection + let hit_y_squared = oa_dot_ba + distance * dir_dot_ba; + + if hit_y_squared < 0.0 || hit_y_squared > height_squared { + // The point of intersection is outside of the height of the frustum. + return None; + } + + let normal = Dir3::new( + height_squared + * (height_squared * (oa + distance * ray.direction) + + radius_difference * ba * self.radius_top) + - ba * hypot_squared * hit_y_squared, + ) + .ok()?; + + Some(RayHit3d::new(distance, normal)) + } + } +} + +#[cfg(test)] +mod tests { + use approx::assert_relative_eq; + + use super::*; + + #[test] + fn local_ray_cast_conical_frustum() { + let frustum = ConicalFrustum { + radius_top: 0.5, + radius_bottom: 1.0, + height: 2.0, + }; + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = frustum.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 1.25); + assert_relative_eq!(hit.normal, Dir3::from_xyz(4.0, 1.0, 0.0).unwrap()); + + // Ray origin is inside of the solid frustum. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = frustum.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::NEG_X))); + + // Ray origin is inside of the hollow frustum. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = frustum.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 0.75); + assert_relative_eq!(hit.normal, Dir3::from_xyz(-4.0, -1.0, 0.0).unwrap()); + let ray = Ray3d::new(Vec3::ZERO, Dir3::Y); + let hit = frustum.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::NEG_Y))); + + // Ray hits the frustum. + assert!(frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 0.9, 0.0), Dir3::Y))); + assert!(frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.5, 0.5, 0.0), Dir3::X))); + + // Ray points away from the frustum. + assert!(!frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Dir3::Y))); + assert!(!frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.75, 0.5, 0.0), Dir3::X))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::NEG_Y); + let hit = frustum.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/cuboid.rs b/crates/bevy_math/src/ray_cast/dim3/cuboid.rs new file mode 100644 index 0000000000000..a41bd20c8ada5 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/cuboid.rs @@ -0,0 +1,121 @@ +use crate::prelude::*; + +// This is the same as `Rectangle`, but with 3D types. +impl PrimitiveRayCast3d for Cuboid { + #[inline] + fn local_ray_distance(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Adapted from Inigo Quilez's algorithm: https://iquilezles.org/articles/intersectors/ + + let direction_recip_abs = ray.direction.recip().abs(); + + // Note: The operations here were modified and rearranged to avoid the Inf - Inf = NaN result + // for the edge case where a component of the ray direction is zero and the reciprocal + // is infinity. The NaN would break the early return below. + let n = -ray.direction.signum() * ray.origin; + let t1 = direction_recip_abs * (n - self.half_size); + let t2 = direction_recip_abs * (n + self.half_size); + + let distance_near = t1.max_element(); + let distance_far = t2.min_element(); + + if distance_near > distance_far || distance_far < 0.0 { + return None; + } + + if distance_near > 0.0 { + // The ray hit the outside of the rectangle. + Some(distance_near) + } else if solid { + // The ray origin is inside of the solid rectangle. + Some(0.0) + } else if distance_far <= max_distance { + // The ray hit the inside of the hollow rectangle. + Some(distance_far) + } else { + None + } + } + + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Adapted from Inigo Quilez's algorithm: https://iquilezles.org/articles/intersectors/ + + let direction_recip_abs = ray.direction.recip().abs(); + + // Note: The operations here were modified and rearranged to avoid the Inf - Inf = NaN result + // for the edge case where a component of the ray direction is zero and the reciprocal + // is infinity. The NaN would break the early return below. + let n = -ray.direction.signum() * ray.origin; + let t1 = direction_recip_abs * (n - self.half_size); + let t2 = direction_recip_abs * (n + self.half_size); + + let distance_near = t1.max_element(); + let distance_far = t2.min_element(); + + if distance_near > distance_far || distance_far < 0.0 || distance_near > max_distance { + return None; + } + + if distance_near > 0.0 { + // The ray hit the outside of the rectangle. + // Note: We could also just have an if-else here, but it was measured to be ~15% slower. + let normal_abs = Vec3::from(Vec3::splat(distance_near).cmple(t1)); + let normal = Dir3::new(-ray.direction.signum() * normal_abs).ok()?; + Some(RayHit3d::new(distance_near, normal)) + } else if solid { + // The ray origin is inside of the solid rectangle. + Some(RayHit3d::new(0.0, -ray.direction)) + } else if distance_far <= max_distance { + // The ray hit the inside of the hollow rectangle. + // Note: We could also just have an if-else here, but it was measured to be ~15% slower. + let normal_abs = Vec3::from(t2.cmple(Vec3::splat(distance_far))); + let normal = Dir3::new(-ray.direction.signum() * normal_abs).ok()?; + Some(RayHit3d::new(distance_far, normal)) + } else { + None + } + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_cuboid() { + let cuboid = Cuboid::new(2.0, 1.0, 0.5); + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = cuboid.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::X))); + + // Ray origin is inside of the solid cuboid. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = cuboid.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::NEG_X))); + + // Ray origin is inside of the hollow cuboid. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = cuboid.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::NEG_X))); + + let ray = Ray3d::new(Vec3::ZERO, Dir3::Y); + let hit = cuboid.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(0.5, Dir3::NEG_Y))); + + // Ray points away from the cuboid. + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Dir3::Y))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Dir3::X))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, -1.0, 0.0), Dir3::X))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Dir3::Z))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, -1.0, 0.0), Dir3::Z))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::Y))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(-2.0, 0.0, 0.0), Dir3::Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::NEG_Y); + let hit = cuboid.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/cylinder.rs b/crates/bevy_math/src/ray_cast/dim3/cylinder.rs new file mode 100644 index 0000000000000..0720f5f2c0a1a --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/cylinder.rs @@ -0,0 +1,221 @@ +use crate::prelude::*; + +// NOTE: This is largely a copy of the `ConicalFrustum` implementation, but simplified for only one radius. +impl PrimitiveRayCast3d for Cylinder { + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Adapted from: + // - Inigo Quilez's capped cone ray intersection algorithm: https://iquilezles.org/articles/intersectors/ + // - http://lousodrome.net/blog/light/2017/01/03/intersection-of-a-ray-and-a-cone/ + + let radius_squared = self.radius * self.radius; + let height_squared = 4.0 * self.half_height * self.half_height; + + let a = Vec3::new(0.0, self.half_height, 0.0); + let b = -a; + let ba = b - a; + let oa = ray.origin - a; + let ob = ray.origin - b; + + let oa_dot_ba = oa.dot(ba); + let ob_dot_ba = ob.dot(ba); + + // The ray origin is inside of the cylinder if both of the following are true: + // 1. The origin is between the top and bottom bases. + // 2. The origin is within the circular slice determined by the distance from the base. + let is_inside = oa_dot_ba >= 0.0 + && oa_dot_ba <= height_squared + && ray.origin.xz().length_squared() < self.radius * self.radius; + + if is_inside { + if solid { + return Some(RayHit3d::new(0.0, -ray.direction)); + } + + let dir_dot_ba = ray.direction.dot(ba); + let dir_dot_oa = ray.direction.dot(oa); + let top_distance_squared = oa.length_squared(); + + // Caps + if oa_dot_ba >= 0.0 && dir_dot_ba > 0.0 { + let distance = -ob_dot_ba / dir_dot_ba; + + if distance <= max_distance { + // Check if the point of intersection is within the bottom circle. + let distance_at_bottom_squared = + (ob + ray.direction * distance).length_squared(); + if distance_at_bottom_squared < radius_squared { + // The ray hit the bottom of the cylinder. + let normal = -Dir3::new(ba / (2.0 * self.half_height)).ok()?; + return Some(RayHit3d::new(distance, normal)); + } + } + } else if ob_dot_ba <= 0.0 { + // Check if the point of intersection is within the top circle. + // Here we delay the division in the distance computation. + if (oa * dir_dot_ba - ray.direction * oa_dot_ba).length_squared() + < radius_squared * dir_dot_ba * dir_dot_ba + { + // The ray hit the top of the cylinder. + let distance = -oa_dot_ba / dir_dot_ba; + let normal = -Dir3::new(-ba / (2.0 * self.half_height)).ok()?; + return (distance <= max_distance).then_some(RayHit3d::new(distance, normal)); + } + } + + // The ray hit the cylindrical surface. + // Because the ray is known to be inside of the shape, no further checks are needed. + + let height_pow_4 = height_squared * height_squared; + + // Quadratic equation coefficients a, b, c + let a = height_pow_4 - dir_dot_ba * dir_dot_ba * height_squared; + let b = height_pow_4 * dir_dot_oa - oa_dot_ba * dir_dot_ba * height_squared; + let c = height_pow_4 * top_distance_squared + - oa_dot_ba * oa_dot_ba * height_squared + - height_pow_4 * radius_squared; + let discriminant = b * b - a * c; + + // Two roots: + // t1 = (-b - discriminant.sqrt()) / a + // t2 = (-b + discriminant.sqrt()) / a + // For the inside case, we want t2. + let distance = (-b + ops::sqrt(discriminant)) / a; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Squared distance from top along cylinder axis at the point of intersection + let hit_y_squared = oa_dot_ba + distance * dir_dot_ba; + + let normal = -Dir3::new( + height_pow_4 * (oa + distance * ray.direction) + - ba * height_squared * hit_y_squared, + ) + .ok()?; + + Some(RayHit3d::new(distance, normal)) + } else { + // The ray origin is outside of the cone. + + let dir_dot_ba = ray.direction.dot(ba); + let dir_dot_oa = ray.direction.dot(oa); + let top_distance_squared = oa.length_squared(); + + // Caps + if oa_dot_ba < 0.0 && dir_dot_ba > 0.0 { + // The distance between the point of intersection and the top circle must be within the top radius. + // Here we delay the division in the distance computation. + if (oa * dir_dot_ba - ray.direction * oa_dot_ba).length_squared() + < radius_squared * dir_dot_ba * dir_dot_ba + { + // The ray hit the top of the cylinder. + let distance = -oa_dot_ba / dir_dot_ba; + let normal = Dir3::new(-ba / ops::sqrt(height_squared)).ok()?; + return (distance <= max_distance).then_some(RayHit3d::new(distance, normal)); + } + } else if ob_dot_ba > 0.0 && dir_dot_ba < 0.0 { + let distance = -ob_dot_ba / dir_dot_ba; + + if distance <= max_distance { + // The distance between the point of intersection and the bottom circle must be within the bottom radius. + let distance_at_bottom_squared = + (ob + ray.direction * distance).length_squared(); + if distance_at_bottom_squared < radius_squared { + // The ray hit the bottom of the cylinder. + let normal = Dir3::new(ba / ops::sqrt(height_squared)).ok()?; + return Some(RayHit3d::new(distance, normal)); + } + } + } + + // Check for intersections with the lateral surface of the cylinder. + + let height_pow_4 = height_squared * height_squared; + + // Quadratic equation coefficients a, b, c + let a = height_pow_4 - dir_dot_ba * dir_dot_ba * height_squared; + let b = height_pow_4 * dir_dot_oa - oa_dot_ba * dir_dot_ba * height_squared; + let c = height_pow_4 * top_distance_squared + - oa_dot_ba * oa_dot_ba * height_squared + - height_pow_4 * radius_squared; + let discriminant = b * b - a * c; + + if discriminant < 0.0 { + return None; + } + + // Two roots: + // t1 = (-b - discriminant.sqrt()) / a + // t2 = (-b + discriminant.sqrt()) / a + // For the outside case, we want t1. + let distance = (-b - ops::sqrt(discriminant)) / a; + + if distance < 0.0 || distance > max_distance { + return None; + } + + // Squared distance from top along cylinder axis at the point of intersection + let hit_y_squared = oa_dot_ba + distance * dir_dot_ba; + + if hit_y_squared < 0.0 || hit_y_squared > height_squared { + // The point of intersection is outside of the height of the cylinder. + return None; + } + + let normal = Dir3::new( + height_pow_4 * (oa + distance * ray.direction) + - ba * height_squared * hit_y_squared, + ) + .ok()?; + + Some(RayHit3d::new(distance, normal)) + } + } +} + +#[cfg(test)] +mod tests { + use approx::assert_relative_eq; + + use super::*; + + #[test] + fn local_ray_cast_cylinder() { + let cylinder = Cylinder::new(1.0, 2.0); + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = cylinder.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_eq!(hit.distance, 1.0); + assert_relative_eq!(hit.normal, Dir3::X); + + // Ray origin is inside of the solid cylinder. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = cylinder.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::NEG_X))); + + // Ray origin is inside of the hollow cylinder. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = cylinder.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_eq!(hit.distance, 1.0); + assert_relative_eq!(hit.normal, Dir3::NEG_X); + let ray = Ray3d::new(Vec3::ZERO, Dir3::Y); + let hit = cylinder.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::NEG_Y))); + + // Ray hits the cylinder. + assert!(cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 0.9, 0.0), Dir3::Y))); + assert!(cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.4, 0.9, 0.0), Dir3::X))); + + // Ray points away from the cylinder. + assert!(!cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Dir3::Y))); + assert!(!cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.6, 1.1, 0.0), Dir3::X))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::NEG_Y); + let hit = cylinder.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/mod.rs b/crates/bevy_math/src/ray_cast/dim3/mod.rs new file mode 100644 index 0000000000000..0c21da2ea2c0e --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/mod.rs @@ -0,0 +1,351 @@ +mod capsule; +mod cone; +mod conical_frustum; +mod cuboid; +mod cylinder; +mod sphere; +mod tetrahedron; +mod torus; +mod triangle; + +use crate::prelude::*; +#[cfg(all(feature = "bevy_reflect", feature = "serialize"))] +use bevy_reflect::{ReflectDeserialize, ReflectSerialize}; + +/// An intersection between a ray and a shape in three-dimensional space. +#[derive(Clone, Copy, Debug, PartialEq)] +#[cfg_attr(feature = "bevy_reflect", derive(bevy_reflect::Reflect))] +#[cfg_attr(feature = "bevy_reflect", reflect(Debug, PartialEq))] +#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))] +#[cfg_attr( + all(feature = "bevy_reflect", feature = "serialize"), + reflect(Serialize, Deserialize) +)] +pub struct RayHit3d { + /// The distance between the point of intersection and the ray origin. + pub distance: f32, + /// The surface normal on the shape at the point of intersection. + pub normal: Dir3, +} + +impl RayHit3d { + /// Creates a new [`RayHit3d`] from the given distance and surface normal at the point of intersection. + #[inline] + pub const fn new(distance: f32, normal: Dir3) -> Self { + Self { distance, normal } + } +} + +/// A trait for intersecting rays with [primitive shapes] in three-dimensional space. +/// +/// [primitive shapes]: crate::primitives +pub trait PrimitiveRayCast3d { + /// Computes the distance to the closest intersection along the given `ray`, expressed in the local space of `self`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding distance. + /// + /// # Example + /// + /// Casting a ray against a solid sphere might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::new(-2.0, 0.0, 0.0), Dir3::X); + /// let sphere = Sphere::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(distance) = sphere.local_ray_distance(ray, max_distance, solid) { + /// // The ray intersects the sphere at a distance of 1.0. + /// assert_eq!(distance, 1.0); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(distance); + /// assert_eq!(point, Vec3::new(-1.0, 0.0, 0.0)); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + /// let sphere = Sphere::new(1.0); + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(distance) = sphere.local_ray_distance(ray, max_distance, solid) { + /// // The ray origin is inside of the hollow sphere, and hit its boundary. + /// assert_eq!(distance, sphere.radius); + /// assert_eq!(ray.get_point(distance), Vec3::new(1.0, 0.0, 0.0)); + /// } + /// ``` + #[inline] + fn local_ray_distance(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + self.local_ray_cast(ray, max_distance, solid) + .map(|hit| hit.distance) + } + + /// Computes the closest intersection along the given `ray`, expressed in the local space of `self`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding intersection. + /// + /// # Example + /// + /// Casting a ray against a solid sphere might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::new(-2.0, 0.0, 0.0), Dir3::X); + /// let sphere = Sphere::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(hit) = sphere.local_ray_cast(ray, max_distance, solid) { + /// // The ray intersects the sphere at a distance of 1.0. + /// // The hit normal at the point of intersection is -X. + /// assert_eq!(hit.distance, 1.0); + /// assert_eq!(hit.normal, Dir3::NEG_X); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(hit.distance); + /// assert_eq!(point, Vec3::new(-1.0, 0.0, 0.0)); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + /// let sphere = Sphere::new(1.0); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(hit) = sphere.local_ray_cast(ray, max_distance, solid) { + /// // The ray origin is inside of the hollow sphere, and hit its boundary. + /// assert_eq!(hit.distance, sphere.radius); + /// assert_eq!(hit.normal, Dir3::NEG_X); + /// assert_eq!(ray.get_point(hit.distance), Vec3::new(1.0, 0.0, 0.0)); + /// } + /// ``` + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option; + + /// Returns `true` if `self` intersects the given `ray` in the local space of `self`. + /// + /// # Example + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// // Define a sphere with a radius of `1.0` centered at the origin. + /// let sphere = Sphere::new(1.0); + /// + /// // Test for ray intersections. + /// assert!(sphere.intersects_local_ray(Ray3d::new(Vec3::new(-2.0, 0.0, 0.0), Dir3::X))); + /// assert!(!sphere.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::X))); + /// ``` + #[inline] + fn intersects_local_ray(&self, ray: Ray3d) -> bool { + self.local_ray_distance(ray, f32::MAX, true).is_some() + } + + /// Computes the distance to the closest intersection along the given `ray` for `self` transformed by `iso`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding distance. + /// + /// # Example + /// + /// Casting a ray against a solid sphere might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::new(-1.0, 0.0, 0.0), Dir3::X); + /// let sphere = Sphere::new(1.0); + /// let iso = Isometry3d::from_translation(Vec3::new(1.0, 0.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(distance) = sphere.ray_distance(iso, ray, max_distance, solid) { + /// // The ray intersects the sphere at a distance of 1.0. + /// assert_eq!(distance, 1.0); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(distance); + /// assert_eq!(point, Vec3::ZERO); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::new(1.0, 0.0, 0.0), Dir3::X); + /// let sphere = Sphere::new(1.0); + /// let iso = Isometry3d::from_translation(Vec3::new(1.0, 0.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(distance) = sphere.ray_distance(iso, ray, max_distance, solid) { + /// // The ray origin is inside of the hollow sphere, and hit its boundary. + /// assert_eq!(distance, sphere.radius); + /// assert_eq!(ray.get_point(distance), Vec3::new(2.0, 0.0, 0.0)); + /// } + /// ``` + #[inline] + fn ray_distance( + &self, + iso: Isometry3d, + ray: Ray3d, + max_distance: f32, + solid: bool, + ) -> Option { + let local_ray = ray.inverse_transformed_by(iso); + self.local_ray_distance(local_ray, max_distance, solid) + } + + /// Computes the closest intersection along the given `ray` for `self` transformed by `iso`. + /// Returns `None` if no intersection is found or if the distance exceeds the given `max_distance`. + /// + /// `solid` determines whether the shape should be treated as solid or hollow when the ray origin is in the interior + /// of the shape. If `solid` is `true`, the distance of the hit will be `Some(0.0)`. Otherwise, the ray will travel + /// until it hits the boundary, and compute the corresponding intersection. + /// + /// # Example + /// + /// Casting a ray against a solid sphere might look like this: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::new(-1.0, 0.0, 0.0), Dir3::X); + /// let sphere = Sphere::new(1.0); + /// let iso = Isometry3d::from_translation(Vec3::new(1.0, 0.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = true; + /// + /// if let Some(hit) = sphere.ray_cast(iso, ray, max_distance, solid) { + /// // The ray intersects the sphere at a distance of 1.0. + /// // The hit normal at the point of intersection is -X. + /// assert_eq!(hit.distance, 1.0); + /// assert_eq!(hit.normal, Dir3::NEG_X); + /// + /// // The point of intersection can be computed using the distance along the ray: + /// let point = ray.get_point(hit.distance); + /// assert_eq!(point, Vec3::ZERO); + /// } + /// ``` + /// + /// If the ray origin is inside of a solid shape, the hit distance will be `0.0`. + /// This could be used to ignore intersections where the ray starts from inside of the shape. + /// + /// If the ray origin is instead inside of a hollow shape, the point of intersection + /// will be at the boundary of the shape: + /// + /// ``` + /// # use bevy_math::prelude::*; + /// # + /// let ray = Ray3d::new(Vec3::new(1.0, 0.0, 0.0), Dir3::X); + /// let sphere = Sphere::new(1.0); + /// let iso = Isometry3d::from_translation(Vec3::new(1.0, 0.0, 0.0)); + /// + /// let max_distance = f32::MAX; + /// let solid = false; + /// + /// if let Some(hit) = sphere.ray_cast(iso, ray, max_distance, solid) { + /// // The ray origin is inside of the hollow sphere, and hit its boundary. + /// assert_eq!(hit.distance, sphere.radius); + /// assert_eq!(hit.normal, Dir3::NEG_X); + /// assert_eq!(ray.get_point(hit.distance), Vec3::new(2.0, 0.0, 0.0)); + /// } + /// ``` + #[inline] + fn ray_cast( + &self, + iso: Isometry3d, + ray: Ray3d, + max_distance: f32, + solid: bool, + ) -> Option { + let local_ray = ray.inverse_transformed_by(iso); + self.local_ray_cast(local_ray, max_distance, solid) + .map(|mut hit| { + hit.normal = iso.rotation * hit.normal; + hit + }) + } + + /// Returns `true` if `self` transformed by `iso` intersects the given `ray`. + /// + /// # Example + /// + /// ``` + /// use bevy_math::prelude::*; + /// + /// // Define a sphere with a radius of `1.0` shifted by `1.0` along the X axis. + /// let sphere = Sphere::new(1.0); + /// let iso = Isometry3d::from_translation(Vec3::new(1.0, 0.0, 0.0)); + /// + /// // Test for ray intersections. + /// assert!(sphere.intersects_ray(iso, Ray3d::new(Vec3::new(-2.0, 0.0, 0.0), Dir3::X))); + /// assert!(!sphere.intersects_ray(iso, Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::X))); + /// ``` + #[inline] + fn intersects_ray(&self, iso: Isometry3d, ray: Ray3d) -> bool { + self.ray_distance(iso, ray, f32::MAX, true).is_some() + } +} + +#[cfg(test)] +mod tests { + use core::f32::consts::{PI, SQRT_2}; + + use crate::prelude::*; + use approx::assert_relative_eq; + + #[test] + fn ray_cast_3d() { + let cuboid = Cuboid::new(2.0, 1.0, 1.0); + let iso = Isometry3d::new(Vec3::new(2.0, 0.0, 0.0), Quat::from_rotation_z(PI / 4.0)); + + // Cast a ray on the transformed cuboid. + let ray = Ray3d::new(Vec3::new(-1.0, SQRT_2 / 2.0, 0.0), Dir3::X); + let hit = cuboid.ray_cast(iso, ray, f32::MAX, true).unwrap(); + + assert_relative_eq!(hit.distance, 3.0, epsilon = 1.0e-6); + assert_relative_eq!(hit.normal, Dir3::from_xyz(-1.0, 1.0, 0.0).unwrap()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/sphere.rs b/crates/bevy_math/src/ray_cast/dim3/sphere.rs new file mode 100644 index 0000000000000..14fac979e775e --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/sphere.rs @@ -0,0 +1,95 @@ +use ops::FloatPow; + +use crate::prelude::*; + +// This is the same as `Sphere`, but with 3D types. +impl PrimitiveRayCast3d for Sphere { + #[inline] + fn local_ray_distance(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + local_ray_distance_with_sphere(self.radius, ray, solid) + .and_then(|(distance, _)| (distance <= max_distance).then_some(distance)) + } + + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + local_ray_distance_with_sphere(self.radius, ray, solid).and_then(|(distance, is_inside)| { + if solid && is_inside { + Some(RayHit3d::new(0.0, -ray.direction)) + } else if distance <= max_distance { + let point = ray.get_point(distance); + let normal = + Dir3::new(if is_inside { -1.0 } else { 1.0 } * point / self.radius).ok()?; + Some(RayHit3d::new(distance, normal)) + } else { + None + } + }) + } +} + +#[inline] +fn local_ray_distance_with_sphere(radius: f32, ray: Ray3d, solid: bool) -> Option<(f32, bool)> { + // See `Circle` for the math and detailed explanation of how this works. + + // The squared distance between the ray origin and the boundary of the sphere. + let c = ray.origin.length_squared() - radius.squared(); + + if c > 0.0 { + // The ray origin is outside of the sphere. + let b = ray.origin.dot(*ray.direction); + + if b > 0.0 { + // The ray points away from the sphere, so there can be no hits. + return None; + } + + // The distance corresponding to the boundary hit is the second root. + let d = b.squared() - c; + let t2 = -b - ops::sqrt(d); + + Some((t2, false)) + } else if solid { + // The ray origin is inside of the solid sphere. + Some((0.0, true)) + } else { + // The ray origin is inside of the hollow sphere. + // The distance corresponding to the boundary hit is the first root. + let b = ray.origin.dot(*ray.direction); + let d = b.squared() - c; + let t1 = -b + ops::sqrt(d); + Some((t1, true)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_sphere() { + let sphere = Sphere::new(1.0); + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = sphere.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::X))); + + // Ray origin is inside of the solid sphere. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = sphere.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::NEG_X))); + + // Ray origin is inside of the hollow sphere. + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = sphere.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::NEG_X))); + + // Ray points away from the sphere. + assert!(!sphere.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Dir3::NEG_Y); + let hit = sphere.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs b/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs new file mode 100644 index 0000000000000..a89c99b9bec39 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs @@ -0,0 +1,198 @@ +use crate::prelude::*; + +impl PrimitiveRayCast3d for Tetrahedron { + #[inline] + fn intersects_local_ray(&self, ray: Ray3d) -> bool { + // Tetrahedron-ray intersection test using scalar triple products. + // An alternative could use Plücker coordinates, but that can be less efficient and more complex. + // + // Reference: https://realtimecollisiondetection.net/blog/?p=13 + + // Translate the ray and the tetrahedron such that the ray origin is at (0, 0, 0). + let q = *ray.direction; + let v = self.vertices; + let a = v[0] - ray.origin; + let b = v[1] - ray.origin; + let c = v[2] - ray.origin; + let d = v[3] - ray.origin; + + // Determine if the origin is inside the tetrahedron using triple scalar products. + let abc = triple_scalar_product(a, b, c); + let abd = triple_scalar_product(a, b, d); + let acd = triple_scalar_product(a, c, d); + let bcd = triple_scalar_product(b, c, d); + + let ab = b - a; + let ac = c - a; + let ad = d - a; + let sign = triple_scalar_product(ab, ac, ad).signum(); + + let is_inside = if sign == 1.0 { + abc <= 0.0 && abd >= 0.0 && acd <= 0.0 && bcd >= 0.0 + } else { + abc > 0.0 && abd < 0.0 && acd > 0.0 && bcd < 0.0 + }; + + if is_inside { + return true; + } + + let qab = sign * triple_scalar_product(q, a, b); + let qbc = sign * triple_scalar_product(q, b, c); + let qac = sign * triple_scalar_product(q, a, c); + + // ABC + if qab >= 0.0 && qbc >= 0.0 && qac < 0.0 { + return true; + } + + let qad = sign * triple_scalar_product(q, a, d); + let qbd = sign * triple_scalar_product(q, b, d); + + // BAD + if qab < 0.0 && qad >= 0.0 && qbd < 0.0 { + return true; + } + + let qcd = sign * triple_scalar_product(q, c, d); + + // CDA + if qcd >= 0.0 && qad < 0.0 && qac >= 0.0 { + return true; + } + + // DCB + if qcd < 0.0 && qbc < 0.0 && qbd >= 0.0 { + return true; + } + + false + } + + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + // Tetrahedron-ray intersection test using scalar triple products. + // An alternative could use Plücker coordinates, but that can be less efficient and more complex. + // + // Note: The ray cast could be much more efficient if we could assume a specific triangle orientation and ignore interior cases. + // There's likely room for optimization here. + // + // Reference: https://realtimecollisiondetection.net/blog/?p=13 + + // Translate the ray and the tetrahedron such that the ray origin is at (0, 0, 0). + let q = *ray.direction; + let v = self.vertices; + let a = v[0] - ray.origin; + let b = v[1] - ray.origin; + let c = v[2] - ray.origin; + let d = v[3] - ray.origin; + + // Determine if the origin is inside the tetrahedron using triple scalar products. + let abc = triple_scalar_product(a, b, c); + let abd = triple_scalar_product(a, b, d); + let acd = triple_scalar_product(a, c, d); + let bcd = triple_scalar_product(b, c, d); + + // Get the sign of the signed volume of the tetrahedron, which determines the orientation. + let ab = b - a; + let ac = c - a; + let ad = d - a; + let orientation = triple_scalar_product(ab, ac, ad).signum(); + + let is_inside = if orientation == 1.0 { + abc <= 0.0 && abd >= 0.0 && acd <= 0.0 && bcd >= 0.0 + } else { + abc > 0.0 && abd < 0.0 && acd > 0.0 && bcd < 0.0 + }; + + if solid && is_inside { + return Some(RayHit3d::new(0.0, -ray.direction)); + } + + let sign = if is_inside { -orientation } else { orientation }; + + // Now, we check each face for intersections using scalar triple products. + // The ray intersects a face if and only if the ray lies clockwise to each edge of the face. + + let qab = sign * triple_scalar_product(q, a, b); + let qbc = sign * triple_scalar_product(q, b, c); + let qac = sign * triple_scalar_product(q, a, c); + + // ABC + if qab >= 0.0 && qbc >= 0.0 && qac < 0.0 { + return Triangle3d::new(v[0], v[1], v[2]).local_ray_cast(ray, max_distance, solid); + } + + let qad = sign * triple_scalar_product(q, a, d); + let qbd = sign * triple_scalar_product(q, b, d); + + // BAD + if qab < 0.0 && qad >= 0.0 && qbd < 0.0 { + return Triangle3d::new(v[1], v[0], v[3]).local_ray_cast(ray, max_distance, solid); + } + + let qcd = sign * triple_scalar_product(q, c, d); + + // CDA + if qcd >= 0.0 && qad < 0.0 && qac >= 0.0 { + return Triangle3d::new(v[2], v[3], v[0]).local_ray_cast(ray, max_distance, solid); + } + + // DCB + if qcd < 0.0 && qbc < 0.0 && qbd >= 0.0 { + return Triangle3d::new(v[3], v[2], v[1]).local_ray_cast(ray, max_distance, solid); + } + + None + } +} + +#[inline] +fn triple_scalar_product(a: Vec3, b: Vec3, c: Vec3) -> f32 { + // Glam can optimize this better than a.dot(b.cross(c)) + Mat3::from_cols(a, b, c).determinant() +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_tetrahedron() { + let tetrahedron = Tetrahedron::new( + Vec3::new(-1.0, 0.0, 1.0), + Vec3::new(-1.0, 0.0, -1.0), + Vec3::new(1.0, 0.0, -1.0), + Vec3::new(-1.0, 2.0, -1.0), + ); + + // Hit from above. + let ray = Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Dir3::NEG_Y); + let hit = tetrahedron.local_ray_cast(ray, f32::MAX, true); + assert_eq!( + hit, + Some(RayHit3d::new(1.0, Dir3::from_xyz(1.0, 1.0, 1.0).unwrap())) + ); + + // Ray origin is inside of the solid tetrahedron. + let ray = Ray3d::new(Vec3::new(-0.5, 0.25, -0.5), Dir3::NEG_X); + let hit = tetrahedron.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(0.0, Dir3::X))); + + // Ray origin is inside of the hollow tetrahedron. + let ray = Ray3d::new(Vec3::new(-0.5, 0.25, -0.5), Dir3::NEG_X); + let hit = tetrahedron.local_ray_cast(ray, f32::MAX, false); + assert_eq!(hit, Some(RayHit3d::new(0.5, Dir3::X))); + + // Ray points away from the tetrahedron. + assert!(!tetrahedron.intersects_local_ray(Ray3d::new( + Vec3::new(0.0, 1.1, 0.0), + Dir3::new(Vec3::new(1.0, -1.0, 1.0)).unwrap() + ))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Dir3::NEG_Y); + let hit = tetrahedron.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/torus.rs b/crates/bevy_math/src/ray_cast/dim3/torus.rs new file mode 100644 index 0000000000000..e08f9cdfe8827 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/torus.rs @@ -0,0 +1,228 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl PrimitiveRayCast3d for Torus { + fn local_ray_distance(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + let minor_radius_squared = self.minor_radius * self.minor_radius; + + let is_inside = (self.major_radius - ray.origin.xz().length()).squared() + + ray.origin.y.squared() + < minor_radius_squared; + + if solid && is_inside { + return Some(0.0); + } + + let major_radius_squared = self.major_radius * self.major_radius; + + torus_ray_distance(*self, minor_radius_squared, major_radius_squared, ray) + .filter(|d| *d <= max_distance) + } + + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { + let minor_radius_squared = self.minor_radius * self.minor_radius; + let major_radius_squared = self.major_radius * self.major_radius; + + let is_inside = (self.major_radius - ray.origin.xz().length()).squared() + + ray.origin.y.squared() + < minor_radius_squared; + + if solid && is_inside { + return Some(RayHit3d::new(0.0, -ray.direction)); + } + + let distance = torus_ray_distance(*self, minor_radius_squared, major_radius_squared, ray) + .filter(|d| *d <= max_distance)?; + + let point = ray.get_point(distance); + + // df(x)/dx + let mut normal = Dir3::new( + point + * (point.length_squared() + - minor_radius_squared + - major_radius_squared * Vec3::new(1.0, -1.0, 1.0)), + ) + .ok()?; + + if is_inside { + normal = -normal; + } + + Some(RayHit3d::new(distance, normal)) + } +} + +#[inline] +fn torus_ray_distance( + torus: Torus, + minor_radius_squared: f32, + major_radius_squared: f32, + ray: Ray3d, +) -> Option { + // Degree 4 equation + // f(x) = (|x|^2 + R^2 - r^2)^2 - 4R^2 * (x^2 + y^2) = 0 + // + // Adapted from Inigo Quilez's algorithm: + // + // - https://iquilezles.org/articles/intersectors/ + // - https://www.shadertoy.com/view/4sBGDy + + let mut po = 1.0; + + let origin_distance_squared = ray.origin.length_squared(); + let origin_dot_dir = ray.origin.dot(*ray.direction); + + // Bounding sphere + let h = origin_dot_dir.squared() - origin_distance_squared + + (torus.major_radius + torus.minor_radius).squared(); + if h < 0.0 { + return None; + } + + // Quartic equation + let k = (origin_distance_squared - major_radius_squared - minor_radius_squared) * 0.5; + let mut k3 = origin_dot_dir; + let mut k2 = origin_dot_dir.squared() + major_radius_squared * ray.direction.y.squared() + k; + let mut k1 = k * origin_dot_dir + major_radius_squared * ray.origin.y * ray.direction.y; + let mut k0 = k * k + major_radius_squared * ray.origin.y.squared() + - major_radius_squared * minor_radius_squared; + + // Prevent c1 from being too close to zero. + if ops::abs(k3 * (k3 * k3 - k2) + k1) < 0.01 { + po = -1.0; + core::mem::swap(&mut k1, &mut k3); + k0 = 1.0 / k0; + k1 *= k0; + k2 *= k0; + k3 *= k0; + } + + let mut c2 = 2.0 * k2 - 3.0 * k3 * k3; + let mut c1 = k3 * (k3 * k3 - k2) + k1; + let mut c0 = k3 * (k3 * (-3.0 * k3 * k3 + 4.0 * k2) - 8.0 * k1) + 4.0 * k0; + + c2 /= 3.0; + c1 *= 2.0; + c0 /= 3.0; + + let q = c2 * c2 + c0; + let r = 3.0 * c0 * c2 - c2 * c2 * c2 - c1 * c1; + + let h = r * r - q * q * q; + let mut z: f32; + + if h < 0.0 { + // 4 intersections + let q_sqrt = ops::sqrt(q); + z = 2.0 * q_sqrt * ops::cos(ops::acos(r / (q * q_sqrt)) / 3.0); + } else { + // 2 intersections + let q_sqrt = ops::cbrt(ops::sqrt(h) + ops::abs(r)); + z = r.signum() * ops::abs(q_sqrt + q / q_sqrt); + } + + z = c2 - z; + + let mut d1 = z - 3.0 * c2; + let mut d2 = z * z - 3.0 * c0; + + if ops::abs(d1) < 1.0e-4 { + if d2 < 0.0 { + return None; + } + d2 = ops::sqrt(d2); + } else { + if d1 < 0.0 { + return None; + } + d1 = ops::sqrt(d1 / 2.0); + d2 = c1 / d1; + } + + let mut distance = f32::MAX; + + let discriminant1 = d1 * d1 - z + d2; + if discriminant1 > 0.0 { + let d_sqrt = ops::sqrt(discriminant1); + let (t1, t2) = if po < 0.0 { + (2.0 / (-d1 - d_sqrt - k3), 2.0 / (-d1 + d_sqrt - k3)) + } else { + (-d1 - d_sqrt - k3, -d1 + d_sqrt - k3) + }; + + if t1 > 0.0 { + distance = t1; + } + + if t2 > 0.0 && t2 < distance { + distance = t2; + } + } + + let discriminant2 = d1 * d1 - z - d2; + if discriminant2 > 0.0 { + let d_sqrt = ops::sqrt(discriminant2); + let (t1, t2) = if po < 0.0 { + (2.0 / (d1 - d_sqrt - k3), 2.0 / (d1 + d_sqrt - k3)) + } else { + (d1 - d_sqrt - k3, d1 + d_sqrt - k3) + }; + + if t1 > 0.0 && t1 < distance { + distance = t1; + } + + if t2 > 0.0 && t2 < distance { + distance = t2; + } + } + + Some(distance) +} + +#[cfg(test)] +mod tests { + use approx::assert_relative_eq; + + use super::*; + + #[test] + fn local_ray_cast_torus() { + let torus = Torus::new(0.5, 1.0); + + // Ray origin is outside of the shape. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_X); + let hit = torus.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_relative_eq!(hit.distance, 1.0); + assert_eq!(hit.normal, Dir3::X); + + // Ray origin is inside of the hole (smaller circle). + let ray = Ray3d::new(Vec3::ZERO, Dir3::X); + let hit = torus.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_relative_eq!(hit.distance, 0.5, epsilon = 1.0e-6); + assert_eq!(hit.normal, Dir3::NEG_X); + + // Ray origin is inside of the solid torus. + let ray = Ray3d::new(Vec3::new(0.75, 0.0, 0.0), Dir3::X); + let hit = torus.local_ray_cast(ray, f32::MAX, true).unwrap(); + assert_relative_eq!(hit.distance, 0.0); + assert_eq!(hit.normal, Dir3::NEG_X); + + // Ray origin is inside of the hollow torus. + let ray = Ray3d::new(Vec3::new(0.75, 0.0, 0.0), Dir3::X); + let hit = torus.local_ray_cast(ray, f32::MAX, false).unwrap(); + assert_relative_eq!(hit.distance, 0.25); + assert_eq!(hit.normal, Dir3::NEG_X); + + // Ray points away from the torus. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::Y); + assert!(!torus.intersects_local_ray(ray)); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Dir3::NEG_Y); + let hit = torus.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/dim3/triangle.rs b/crates/bevy_math/src/ray_cast/dim3/triangle.rs new file mode 100644 index 0000000000000..21f27d3938d8d --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/triangle.rs @@ -0,0 +1,106 @@ +use crate::prelude::*; + +impl PrimitiveRayCast3d for Triangle3d { + #[inline] + fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, _solid: bool) -> Option { + // Adapted from: + // - Inigo Quilez's algorithm: https://iquilezles.org/articles/intersectors/ + // - Möller-Trumbore ray-triangle intersection algorithm: https://en.wikipedia.org/wiki/Möller-Trumbore_intersection_algorithm + // + // NOTE: This implementation does not handle rays that are coplanar with the triangle. + + let [a, b, c] = self.vertices; + + // Edges from vertex A to B and C + let ab = b - a; + let ac = c - a; + + // Triangle normal using right-hand rule, assuming CCW winding. + let n = ab.cross(ac); + let det = n.dot(*ray.direction); + + // This check is important for robustness, and also seems to improve performance. + if det == 0.0 { + // The triangle normal and ray direction are perpendicular. + return None; + } + + // Note: Here we could check whether the ray intersects the half-space defined by the triangle, + // but the branching just seems to regress performance. + + let ao = ray.origin - a; + + // Note: For some reason, the compiler produces significantly more optimized instructions + // with these specific operations instead of ao.cross(*ray.direction). + let ray_normal = -ray.direction.cross(ao); + + // To check if there is an intersection, we compute the barycentric coordinates (u, v, w). + // w can be computed based on u and v because u + v + w = 1. + let inv_det = det.recip(); + let u = -inv_det * ac.dot(ray_normal); + let v = inv_det * ab.dot(ray_normal); + + // All barycentric coordinates of a point must be positive for it to be within the shape. + if u < 0.0 || v < 0.0 || u + v > 1.0 { + return None; + } + + // Minimum signed distance between the ray origin and the triangle plane. + let signed_origin_distance = ao.dot(n); + + // Take the ray direction into account. + let distance = -inv_det * signed_origin_distance; + + // Note: Computing this here seems to be faster than doing it inside of the branch below. + let normal = Dir3::new(signed_origin_distance.signum() * n).ok()?; + + (distance > 0.0 && distance <= max_distance).then_some(RayHit3d::new(distance, normal)) + } +} + +#[cfg(test)] +mod tests { + use super::*; + use core::f32::consts::SQRT_2; + + #[test] + fn local_ray_cast_triangle_3d() { + let triangle = Triangle3d::new( + Vec3::new(-1.0, 0.0, 1.0), + Vec3::new(-1.0, 0.0, -1.0), + Vec3::new(1.0, 0.0, -1.0), + ); + + // Hit from above. + let ray = Ray3d::new(Vec3::new(-0.5, 1.0, 0.0), Dir3::NEG_Y); + let hit = triangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::Y))); + + let ray = Ray3d::new( + Vec3::new(0.5, 1.0, 0.0), + Dir3::new(Vec3::new(-1.0, -1.0, 0.0)).unwrap(), + ); + let hit = triangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(SQRT_2, Dir3::Y))); + + // Hit from below. + let ray = Ray3d::new(Vec3::new(-0.5, -1.0, 0.0), Dir3::Y); + let hit = triangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(1.0, Dir3::NEG_Y))); + + let ray = Ray3d::new( + Vec3::new(-1.5, -1.0, 0.0), + Dir3::new(Vec3::new(1.0, 1.0, 0.0)).unwrap(), + ); + let hit = triangle.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit3d::new(SQRT_2, Dir3::NEG_Y))); + + // Ray points away from the triangle. + assert!(!triangle.intersects_local_ray(Ray3d::new(Vec3::new(0.6, 1.0, 0.0), Dir3::NEG_Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(-0.5, 1.0, 0.0), Dir3::NEG_Y); + let hit = triangle.local_ray_cast(ray, 0.5, true); + assert!(hit.is_none()); + } +} diff --git a/crates/bevy_math/src/ray_cast/mod.rs b/crates/bevy_math/src/ray_cast/mod.rs new file mode 100644 index 0000000000000..26544cba720b1 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/mod.rs @@ -0,0 +1,7 @@ +//! Ray casting types and functionality for [primitive shapes](crate::primitives). + +mod dim2; +mod dim3; + +pub use dim2::{PrimitiveRayCast2d, RayHit2d}; +pub use dim3::{PrimitiveRayCast3d, RayHit3d}; diff --git a/crates/bevy_mesh/src/primitives/dim2.rs b/crates/bevy_mesh/src/primitives/dim2.rs index 7ad0fc3826a5d..20ac7bdb8966c 100644 --- a/crates/bevy_mesh/src/primitives/dim2.rs +++ b/crates/bevy_mesh/src/primitives/dim2.rs @@ -1123,7 +1123,7 @@ impl From for Mesh { pub struct Capsule2dMeshBuilder { /// The [`Capsule2d`] shape. pub capsule: Capsule2d, - /// The number of vertices used for one hemicircle. + /// The number of vertices used for one semicircle. /// The total number of vertices for the capsule mesh will be two times the resolution. /// /// The default is `16`. @@ -1141,7 +1141,7 @@ impl Default for Capsule2dMeshBuilder { impl Capsule2dMeshBuilder { /// Creates a new [`Capsule2dMeshBuilder`] from a given radius, length, and the number of vertices - /// used for one hemicircle. The total number of vertices for the capsule mesh will be two times the resolution. + /// used for one semicircle. The total number of vertices for the capsule mesh will be two times the resolution. #[inline] pub fn new(radius: f32, length: f32, resolution: u32) -> Self { Self { @@ -1150,7 +1150,7 @@ impl Capsule2dMeshBuilder { } } - /// Sets the number of vertices used for one hemicircle. + /// Sets the number of vertices used for one semicircle. /// The total number of vertices for the capsule mesh will be two times the resolution. #[inline] pub const fn resolution(mut self, resolution: u32) -> Self { @@ -1182,7 +1182,7 @@ impl MeshBuilder for Capsule2dMeshBuilder { 0.0 }; - // How much the hemicircle radius is of the total half-height of the capsule. + // How much the semicircle radius is of the total half-height of the capsule. // This is used to prevent the UVs from stretching between the semicircles. let radius_frac = self.capsule.radius / (self.capsule.half_length + self.capsule.radius); diff --git a/crates/bevy_mesh/src/primitives/extrusion.rs b/crates/bevy_mesh/src/primitives/extrusion.rs index 9df4821eb392b..665ee87553921 100644 --- a/crates/bevy_mesh/src/primitives/extrusion.rs +++ b/crates/bevy_mesh/src/primitives/extrusion.rs @@ -165,7 +165,7 @@ impl ExtrusionBuilder { } impl ExtrusionBuilder { - /// Sets the number of vertices used for each hemicircle at the ends of the extrusion. + /// Sets the number of vertices used for each semicircle at the ends of the extrusion. pub fn resolution(mut self, resolution: u32) -> Self { self.base_builder.resolution = resolution; self diff --git a/examples/README.md b/examples/README.md index ef5a3f61c9e25..e0658ae9bf886 100644 --- a/examples/README.md +++ b/examples/README.md @@ -421,6 +421,8 @@ Example | Description Example | Description --- | --- +[2D Ray Casting for Primitives](../examples/math/ray_cast_2d.rs) | Shows off ray casting for primitive shapes in 2D +[3D Ray Casting for Primitives](../examples/math/ray_cast_3d.rs) | Shows off ray casting for primitive shapes in 3D [Bounding Volume Intersections (2D)](../examples/math/bounding_2d.rs) | Showcases bounding volumes and intersection tests [Cubic Splines](../examples/math/cubic_splines.rs) | Exhibits different modes of constructing cubic curves using splines [Custom Primitives](../examples/math/custom_primitives.rs) | Demonstrates how to add custom primitives and useful traits for them. diff --git a/examples/math/ray_cast_2d.rs b/examples/math/ray_cast_2d.rs new file mode 100644 index 0000000000000..348760f1da0cf --- /dev/null +++ b/examples/math/ray_cast_2d.rs @@ -0,0 +1,381 @@ +//! Demonstrates ray casting for primitive shapes in 2D. +//! +//! Note that this is only intended to showcase the core ray casting methods for primitive shapes, +//! not how to perform large-scale ray casting in a real application. +//! +//! There are many optimizations that could be done, such as checking for intersections with bounding boxes before checking +//! for intersections with the actual shapes, and using an acceleration structure such as a Bounding Volume Hierarchy (BVH) +//! to speed up ray queries in large worlds. + +use bevy::{ + color::palettes::{ + css::*, + tailwind::{CYAN_600, LIME_500}, + }, + gizmos::gizmos::GizmoBuffer, + prelude::*, + window::PrimaryWindow, +}; + +fn main() { + App::new() + .add_plugins(DefaultPlugins) + .insert_gizmo_config( + DefaultGizmoConfigGroup, + GizmoConfig { + line: GizmoLineConfig { + width: 3.0, + ..Default::default() + }, + ..default() + }, + ) + .init_resource::() + .add_systems(Startup, setup) + .add_systems(PostStartup, setup_text) + .add_systems( + Update, + ( + draw_shapes, + ray_follow_cursor, + rotate_ray, + ray_cast.pipe(mark_closest_hit), + ) + .chain(), + ) + .run(); +} + +/// The world-space ray that is being cast from the cursor position. +#[derive(Resource, Deref, DerefMut)] +struct CursorRay(Ray2d); + +impl Default for CursorRay { + fn default() -> Self { + Self(Ray2d::new(Vec2::ZERO, Dir2::Y)) + } +} + +const X_EXTENT: f32 = 800.; +const Y_EXTENT: f32 = 150.; +const ROWS: u32 = 2; +const COLUMNS: u32 = 6; + +/// An enum for supported 2D shapes. +/// +/// Various trait implementations can be found at the bottom of this file. +#[derive(Component, Clone, Debug)] +#[expect(missing_docs, reason = "testing code")] +pub enum Shape2d { + Circle(Circle), + Arc(Arc2d), + CircularSector(CircularSector), + CircularSegment(CircularSegment), + Ellipse(Ellipse), + Annulus(Annulus), + Rectangle(Rectangle), + Rhombus(Rhombus), + Line(Line2d), + Segment(Segment2d), + Polyline(Polyline2d), + Polygon(Polygon), + RegularPolygon(RegularPolygon), + Triangle(Triangle2d), + Capsule(Capsule2d), +} + +/// A marker to differentiate which shape is currently being hit +#[derive(Component, Clone, Copy, Debug)] +struct IsHit; + +fn setup(mut commands: Commands) { + let shapes = [ + Shape2d::Circle(Circle::new(50.0)), + Shape2d::Arc(Arc2d::new(50.0, 1.25)), + Shape2d::CircularSector(CircularSector::new(50.0, 1.25)), + Shape2d::CircularSegment(CircularSegment::new(50.0, 1.25)), + Shape2d::Ellipse(Ellipse::new(25.0, 50.0)), + Shape2d::Annulus(Annulus::new(25.0, 50.0)), + Shape2d::Capsule(Capsule2d::new(25.0, 50.0)), + Shape2d::Rectangle(Rectangle::new(50.0, 100.0)), + Shape2d::Rhombus(Rhombus::new(75.0, 100.0)), + Shape2d::RegularPolygon(RegularPolygon::new(50.0, 6)), + Shape2d::Triangle(Triangle2d::new( + Vec2::Y * 50.0, + Vec2::new(-50.0, -50.0), + Vec2::new(50.0, -50.0), + )), + Shape2d::Polygon(Polygon::new([ + Vec2::ZERO, + Vec2::new(70.0, 45.0), + Vec2::new(80.0, -50.0), + Vec2::new(-60.0, -30.0), + Vec2::new(-40.0, 60.0), + ])), + Shape2d::Segment(Segment2d::new(Vec2::ZERO, Vec2::new(1.0, 0.5) * 200.0)), + Shape2d::Polyline(Polyline2d::new([ + Vec2::new(-120.0, -50.0), + Vec2::new(-30.0, 30.0), + Vec2::new(50.0, -40.0), + Vec2::new(120.0, 50.0), + ])), + Shape2d::Line(Line2d { + direction: Dir2::from_xy(1.0, -0.5).unwrap(), + }), + ]; + + // Spawn two rows of shapes + for i in 0..COLUMNS { + for j in 0..ROWS { + spawn_shape( + &mut commands, + shapes[(i + j * COLUMNS) as usize].clone(), + i as usize, + j as usize, + ); + } + } + + // Spawn remaining shapes at specific positions + commands.spawn((shapes[12].clone(), Transform::from_xyz(-200.0, -250.0, 0.0))); + commands.spawn((shapes[13].clone(), Transform::from_xyz(200.0, -250.0, 0.0))); + commands.spawn((shapes[14].clone(), Transform::from_xyz(300.0, 250.0, 0.0))); + + // Spawn camera + commands.spawn(Camera2d); +} + +fn setup_text(mut commands: Commands, cameras: Query<(Entity, &Camera)>) { + let active_camera = cameras + .iter() + .find_map(|(entity, camera)| camera.is_active.then_some(entity)) + .expect("run condition ensures existence"); + + // Spawn instructions + commands.spawn(( + Node { + justify_self: JustifySelf::Center, + top: px(5), + ..Default::default() + }, + UiTargetCamera(active_camera), + children![ + Text::default(), + TextLayout::justify(Justify::Center), + children![TextSpan::new( + "Move the cursor to move the ray.\nLeft mouse button to rotate the ray counterclockwise.\nRight mouse button to rotate the ray clockwise.", + )], + ], + )); +} + +/// Spawns a shape at a given column and row. +fn spawn_shape(commands: &mut Commands, shape: Shape2d, column: usize, row: usize) { + commands.spawn(( + shape, + Transform::from_xyz( + -X_EXTENT / 2. + column as f32 / (COLUMNS - 1) as f32 * X_EXTENT, + Y_EXTENT / 2. - row as f32 / (ROWS - 1) as f32 * Y_EXTENT, + 0.0, + ), + )); +} + +/// Moves `CursorRay` to follow the cursor position. +fn ray_follow_cursor( + windows: Single<&Window, With>, + camera: Single<(&Camera, &GlobalTransform)>, + mut ray: ResMut, +) { + let (camera, camera_transform) = camera.into_inner(); + + if let Some(cursor_world_pos) = windows + .cursor_position() + .and_then(|cursor| camera.viewport_to_world_2d(camera_transform, cursor).ok()) + { + ray.origin = cursor_world_pos; + } +} + +/// Rotates the ray when the left or right mouse button is pressed. +fn rotate_ray(button: ResMut>, mut ray: ResMut) { + const ROTATION_SPEED: f32 = 0.05; + if button.pressed(MouseButton::Left) { + ray.direction = Rot2::radians(ROTATION_SPEED) * ray.direction; + } + if button.pressed(MouseButton::Right) { + ray.direction = Rot2::radians(-ROTATION_SPEED) * ray.direction; + } +} + +/// Performs ray casts against all shapes in the scene. +fn ray_cast( + query: Query<(Entity, &Shape2d, &Transform)>, + mut gizmos: Gizmos, + ray: Res, +) -> Option { + let max_distance = 10_000.0; + + let mut closest_hit_entity = None; + let mut closest_hit = None; + let mut closest_hit_distance = f32::MAX; + + // Iterate over all shapes. + // NOTE: A more efficient implementation would use an acceleration structure such as + // a Bounding Volume Hierarchy (BVH), and test the ray against bounding boxes first. + for (entity, shape, transform) in &query { + let rotation = Rot2::radians(transform.rotation.to_euler(EulerRot::XYZ).2); + let iso = Isometry2d::new(transform.translation.truncate(), rotation); + + // Cast the ray against the shape transformed by the isometry. + // The shape is treated as hollow, meaning that the ray can intersect the shape's boundary from the inside. + // NOTE: This method is provided by the `PrimitiveRayCast2d` trait. + let Some(hit) = shape.ray_cast(iso, ray.0, max_distance, false) else { + continue; + }; + + if hit.distance < closest_hit_distance { + closest_hit_entity.replace(entity); + closest_hit = Some((ray.get_point(hit.distance), hit.normal)); + closest_hit_distance = hit.distance; + } + } + + // Draw the ray and the closest hit point. + if let Some((point, normal)) = closest_hit { + // Ray + gizmos.line_2d(ray.origin, point, LIME_500); + + // Normal + gizmos + .arrow_2d(point, point + 50.0 * *normal, RED) + .with_tip_length(5.0); + + // Hit point + let iso = Isometry2d::from_translation(point); + gizmos.circle_2d(iso, 3.0, ORANGE); + } else { + gizmos.line_2d(ray.origin, ray.get_point(max_distance), CYAN_600); + } + + closest_hit_entity +} + +/// mark the hit entity with marker component to render with color +fn mark_closest_hit( + In(opt_entity): In>, + mut cmds: Commands, + previous_hit: Query>, +) { + // add marker if we hit anything + for e in &previous_hit { + cmds.entity(e).remove::(); + } + // remove the marker from old hits from last frame + if let Some(e) = opt_entity { + cmds.entity(e).insert(IsHit); + } +} + +/// Draws all shapes in the scene. +fn draw_shapes(query: Query<(&Shape2d, &GlobalTransform, Has)>, mut gizmos: Gizmos) { + for (shape, global_transform, is_hit) in &query { + let transform = global_transform.compute_transform(); + let pos = transform.translation.truncate(); + let rot = Rot2::radians(transform.rotation.to_euler(EulerRot::XYZ).2); + let color = if is_hit { + Color::Srgba(Srgba::RED) + } else { + Color::WHITE + }; + gizmos.primitive_2d(shape, Isometry2d::new(pos, rot), color); + } +} + +// Trait implementations for `Shape2d` to make ray casts and drawing shapes easier. + +impl Primitive2d for Shape2d {} + +impl PrimitiveRayCast2d for Shape2d { + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + use Shape2d::*; + + match self { + Circle(circle) => circle.local_ray_cast(ray, max_distance, solid), + Arc(arc) => arc.local_ray_cast(ray, max_distance, solid), + CircularSector(sector) => sector.local_ray_cast(ray, max_distance, solid), + CircularSegment(segment) => segment.local_ray_cast(ray, max_distance, solid), + Ellipse(ellipse) => ellipse.local_ray_cast(ray, max_distance, solid), + Annulus(annulus) => annulus.local_ray_cast(ray, max_distance, solid), + Rectangle(rectangle) => rectangle.local_ray_cast(ray, max_distance, solid), + Rhombus(rhombus) => rhombus.local_ray_cast(ray, max_distance, solid), + Line(line) => line.local_ray_cast(ray, max_distance, solid), + Segment(segment) => segment.local_ray_cast(ray, max_distance, solid), + Polyline(polyline) => polyline.local_ray_cast(ray, max_distance, solid), + Polygon(polygon) => polygon.local_ray_cast(ray, max_distance, solid), + RegularPolygon(polygon) => polygon.local_ray_cast(ray, max_distance, solid), + Triangle(triangle) => triangle.local_ray_cast(ray, max_distance, solid), + Capsule(capsule) => capsule.local_ray_cast(ray, max_distance, solid), + } + } +} + +impl GizmoPrimitive2d for GizmoBuffer +where + Config: GizmoConfigGroup, + Clear: 'static + Send + Sync, +{ + type Output<'a> + = () + where + Self: 'a; + + fn primitive_2d( + &mut self, + primitive: &Shape2d, + isometry: impl Into, + color: impl Into, + ) -> Self::Output<'_> { + match &primitive { + Shape2d::Circle(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Arc(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::CircularSector(shape) => self.primitive_2d(shape, isometry, color), + Shape2d::CircularSegment(shape) => self.primitive_2d(shape, isometry, color), + Shape2d::Ellipse(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Annulus(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Rectangle(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Rhombus(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Line(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Segment(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Polyline(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Polygon(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::RegularPolygon(shape) => self.primitive_2d(shape, isometry, color), + Shape2d::Triangle(shape) => { + self.primitive_2d(shape, isometry, color); + } + Shape2d::Capsule(shape) => { + self.primitive_2d(shape, isometry, color); + } + } + } +} diff --git a/examples/math/ray_cast_3d.rs b/examples/math/ray_cast_3d.rs new file mode 100644 index 0000000000000..33f6bf0164c07 --- /dev/null +++ b/examples/math/ray_cast_3d.rs @@ -0,0 +1,264 @@ +//! Demonstrates ray casting for primitive shapes in 3D. +//! +//! Note that this is only intended to showcase the core ray casting methods for primitive shapes, +//! not how to perform large-scale ray casting in a real application. +//! +//! There are many optimizations that could be done, such as checking for intersections with bounding boxes before checking +//! for intersections with the actual shapes, and using an acceleration structure such as a Bounding Volume Hierarchy (BVH) +//! to speed up ray queries in large worlds. + +use std::f32::consts::FRAC_PI_4; + +use bevy::{color::palettes::css::*, prelude::*, window::PrimaryWindow}; + +fn main() { + App::new() + .add_plugins(DefaultPlugins) + .insert_gizmo_config( + DefaultGizmoConfigGroup, + GizmoConfig { + line: GizmoLineConfig { + width: 3.0, + ..Default::default() + }, + ..default() + }, + ) + .init_resource::() + .add_systems(Startup, setup) + .add_systems(PostStartup, setup_text) + .add_systems(Update, (update_cursor_ray, rotate_shapes, ray_cast).chain()) + .run(); +} + +/// The world-space ray that is being cast from the cursor position. +#[derive(Resource, Deref, DerefMut)] +struct CursorRay(Ray3d); + +impl Default for CursorRay { + fn default() -> Self { + Self(Ray3d::new(Vec3::ZERO, Dir3::NEG_Z)) + } +} + +#[derive(Component)] +struct AngularVelocity(f32); + +const SHAPE_COUNT: u32 = 9; +const SHAPES_X_EXTENT: f32 = 14.0; + +/// An enum for supported 3D shapes. +/// +/// Various trait implementations can be found at the bottom of this file. +#[derive(Component, Clone, Debug)] +#[expect(missing_docs, reason = "testing code")] +pub enum Shape3d { + Sphere(Sphere), + Cuboid(Cuboid), + Cylinder(Cylinder), + Cone(Cone), + ConicalFrustum(ConicalFrustum), + Capsule(Capsule3d), + Triangle(Triangle3d), + Tetrahedron(Tetrahedron), + Torus(Torus), +} + +fn setup( + mut commands: Commands, + mut meshes: ResMut>, + mut materials: ResMut>, +) { + let shapes = [ + Shape3d::Sphere(Sphere::default()), + Shape3d::Cuboid(Cuboid::default()), + Shape3d::Cylinder(Cylinder::default()), + Shape3d::Cone(Cone::default()), + Shape3d::ConicalFrustum(ConicalFrustum::default()), + Shape3d::Capsule(Capsule3d::default()), + Shape3d::Torus(Torus::default()), + Shape3d::Triangle(Triangle3d::default()), + Shape3d::Tetrahedron(Tetrahedron::default()), + ]; + + let material = materials.add(Color::WHITE); + + // Spawn the shapes + for i in 0..SHAPE_COUNT { + spawn_shape( + &mut commands, + &mut meshes, + &material, + shapes[i as usize].clone(), + i as usize, + ); + } + + // Spawn camera + commands.spawn(( + Camera3d::default(), + Transform::from_xyz(0.0, 6., 10.0).looking_at(Vec3::new(0., 1., 0.), Vec3::Y), + )); + + // Spawn light + commands.spawn(( + PointLight { + shadow_maps_enabled: true, + intensity: 20_000_000., + range: 100.0, + shadow_depth_bias: 0.2, + ..default() + }, + Transform::from_xyz(8.0, 16.0, 8.0), + )); +} + +fn setup_text(mut commands: Commands, cameras: Query<(Entity, &Camera)>) { + let active_camera = cameras + .iter() + .find_map(|(entity, camera)| camera.is_active.then_some(entity)) + .expect("run condition ensures existence"); + + // Spawn instructions + commands.spawn(( + Node { + justify_self: JustifySelf::Center, + top: px(5), + ..Default::default() + }, + UiTargetCamera(active_camera), + children![ + Text::default(), + TextLayout::justify(Justify::Center), + children![TextSpan::new( + "Point the cursor at the shapes to cast rays." + )], + ], + )); +} + +/// Spawns a shape at a given column and row. +fn spawn_shape( + commands: &mut Commands, + meshes: &mut ResMut>, + material: &Handle, + shape: Shape3d, + column: usize, +) { + commands.spawn(( + shape.clone(), + Mesh3d(meshes.add(shape)), + MeshMaterial3d(material.clone()), + Transform::from_xyz( + -SHAPES_X_EXTENT / 2. + column as f32 / (SHAPE_COUNT - 1) as f32 * SHAPES_X_EXTENT, + 2.0, + 0.0, + ) + .with_rotation(Quat::from_rotation_x(-FRAC_PI_4)), + AngularVelocity(0.5), + )); +} + +/// Rotates the shapes. +fn rotate_shapes(mut query: Query<(&mut Transform, &AngularVelocity)>, time: Res