From 503b53923d4d36acb385a40aa900a7e710f19756 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 5 Oct 2024 19:00:07 +0300 Subject: [PATCH 01/10] Add ray casting for primitive shapes --- crates/bevy_math/src/lib.rs | 2 + crates/bevy_math/src/ray.rs | 46 ++- crates/bevy_math/src/ray_cast/dim2/annulus.rs | 78 ++++ crates/bevy_math/src/ray_cast/dim2/arc.rs | 88 +++++ crates/bevy_math/src/ray_cast/dim2/capsule.rs | 193 ++++++++++ crates/bevy_math/src/ray_cast/dim2/circle.rs | 155 ++++++++ .../src/ray_cast/dim2/circular_sector.rs | 122 ++++++ .../src/ray_cast/dim2/circular_segment.rs | 88 +++++ crates/bevy_math/src/ray_cast/dim2/ellipse.rs | 97 +++++ crates/bevy_math/src/ray_cast/dim2/line.rs | 70 ++++ crates/bevy_math/src/ray_cast/dim2/mod.rs | 355 ++++++++++++++++++ crates/bevy_math/src/ray_cast/dim2/polygon.rs | 157 ++++++++ .../bevy_math/src/ray_cast/dim2/polyline.rs | 101 +++++ .../bevy_math/src/ray_cast/dim2/rectangle.rs | 118 ++++++ crates/bevy_math/src/ray_cast/dim2/rhombus.rs | 90 +++++ crates/bevy_math/src/ray_cast/dim2/segment.rs | 85 +++++ .../bevy_math/src/ray_cast/dim2/triangle.rs | 92 +++++ crates/bevy_math/src/ray_cast/dim3/capsule.rs | 204 ++++++++++ crates/bevy_math/src/ray_cast/dim3/cone.rs | 225 +++++++++++ .../src/ray_cast/dim3/conical_frustum.rs | 256 +++++++++++++ crates/bevy_math/src/ray_cast/dim3/cuboid.rs | 121 ++++++ .../bevy_math/src/ray_cast/dim3/cylinder.rs | 221 +++++++++++ crates/bevy_math/src/ray_cast/dim3/mod.rs | 349 +++++++++++++++++ crates/bevy_math/src/ray_cast/dim3/sphere.rs | 98 +++++ .../src/ray_cast/dim3/tetrahedron.rs | 198 ++++++++++ crates/bevy_math/src/ray_cast/dim3/torus.rs | 228 +++++++++++ .../bevy_math/src/ray_cast/dim3/triangle.rs | 100 +++++ crates/bevy_math/src/ray_cast/mod.rs | 7 + 28 files changed, 3943 insertions(+), 1 deletion(-) create mode 100644 crates/bevy_math/src/ray_cast/dim2/annulus.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/arc.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/capsule.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/circle.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/circular_sector.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/circular_segment.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/ellipse.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/line.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/mod.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/polygon.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/polyline.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/rectangle.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/rhombus.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/segment.rs create mode 100644 crates/bevy_math/src/ray_cast/dim2/triangle.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/capsule.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/cone.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/cuboid.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/cylinder.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/mod.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/sphere.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/torus.rs create mode 100644 crates/bevy_math/src/ray_cast/dim3/triangle.rs create mode 100644 crates/bevy_math/src/ray_cast/mod.rs diff --git a/crates/bevy_math/src/lib.rs b/crates/bevy_math/src/lib.rs index 984a068ff82a4..c496ab26e4be0 100644 --- a/crates/bevy_math/src/lib.rs +++ b/crates/bevy_math/src/lib.rs @@ -24,6 +24,7 @@ mod isometry; pub mod ops; pub mod primitives; mod ray; +pub mod ray_cast; mod rects; mod rotation2d; #[cfg(feature = "rand")] @@ -60,6 +61,7 @@ pub mod prelude { direction::{Dir2, Dir3, Dir3A}, ops, primitives::*, + ray_cast::{RayCast2d, RayCast3d, RayHit2d, RayHit3d}, BVec2, BVec3, BVec4, EulerRot, FloatExt, IRect, IVec2, IVec3, IVec4, Isometry2d, Isometry3d, Mat2, Mat3, Mat4, Quat, Ray2d, Ray3d, Rect, Rot2, StableInterpolate, URect, UVec2, UVec3, UVec4, Vec2, Vec2Swizzles, Vec3, Vec3Swizzles, Vec4, Vec4Swizzles, diff --git a/crates/bevy_math/src/ray.rs b/crates/bevy_math/src/ray.rs index df490a506cf49..25884938225b2 100644 --- a/crates/bevy_math/src/ray.rs +++ b/crates/bevy_math/src/ray.rs @@ -1,6 +1,6 @@ use crate::{ primitives::{InfinitePlane3d, Plane2d}, - Dir2, Dir3, Vec2, Vec3, + Dir2, Dir3, Isometry2d, Isometry3d, Vec2, Vec3, }; #[cfg(feature = "bevy_reflect")] @@ -55,6 +55,28 @@ impl Ray2d { } None } + + /// 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. @@ -104,6 +126,28 @@ impl Ray3d { } None } + + /// 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..10a1afaca413e --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/annulus.rs @@ -0,0 +1,78 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl RayCast2d 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 + d.sqrt(); + + if t < max_distance { + let intersection = ray.get_point(t); + let direction = Dir2::new_unchecked(-intersection / self.inner_circle.radius); + 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), Vec2::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, Vec2::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), Vec2::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), Vec2::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), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..c9c852bdde40f --- /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 RayCast2d 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 = d.sqrt(); + 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 = ops::sin(self.radius * (FRAC_PI_2 + self.half_angle)); + if p2.y >= arc_bottom_y { + let normal = Dir2::new_unchecked(p2 / self.radius); + 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 = ops::sin(self.radius * (FRAC_PI_2 + self.half_angle)); + if p1.y >= arc_bottom_y { + let normal = Dir2::new_unchecked(-p1 / self.radius); + 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), Vec2::NEG_X))); + assert!(!arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.9), Vec2::NEG_Y))); + assert!(!arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Vec2::Y))); + + // Ray hits the arc. + assert!(arc.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.75), Vec2::NEG_X))); + assert!(arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.9), Vec2::Y))); + assert!(arc.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Vec2::NEG_Y))); + + // Check correct hit distance and normal. + let ray = Ray2d::new(Vec2::ZERO, Vec2::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), Vec2::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, Vec2::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..c55424db81366 --- /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 RayCast2d 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 - baoa * baoa - radius_squared * baba; + let d = b * b - a * c; + + if d >= 0.0 { + let is_inside_rect_horizontal = c < 0.0; + let is_inside_rect_vertical = ray.origin.y.abs() < self.half_length; + let intersects_hemisphere = is_inside_rect_horizontal && { + // The ray origin intersects one of the hemicircles if the distance + // between the ray origin and hemicircle center is negative. + Vec2::new(ray.origin.x, self.half_length - ray.origin.y.abs()).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 + d.sqrt()) / a + } else { + (-b - d.sqrt()) / 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_unchecked(Vec2::new(-ray.direction.x.signum(), 0.0)); + return Some(RayHit2d::new(t, normal)); + } + + // Next, we check the hemicircles for intersections. + // It's enough to only check one hemicircle and just take the side into account. + + // Offset between the ray origin and the center of the hit hemicircle. + 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 = d.sqrt(); + + 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 = if is_origin_inside { + Dir2::new_unchecked(-offset_ray.get_point(t2) / self.radius) + } else { + Dir2::new_unchecked(offset_ray.get_point(t2) / self.radius) + }; + 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_unchecked(-offset_ray.get_point(t1) / self.radius); + 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), Vec2::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), Vec2::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, Vec2::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 hemicircle, and inside the bottom hemicircle. + + // Inside the rectangle. + let ray = Ray2d::new(Vec2::ZERO, Vec2::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, Vec2::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 hemicircle. + 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), Vec2::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 hemicircle. + 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), Vec2::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), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.6), Vec2::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..8643ab6e66150 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/circle.rs @@ -0,0 +1,155 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl RayCast2d 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 = if is_inside { + Dir2::new_unchecked(-point / self.radius) + } else { + Dir2::new_unchecked(point / self.radius) + }; + 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 - d.sqrt(); + + 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 + d.sqrt(); + 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), Vec2::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, Vec2::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, Vec2::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), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..6fc4f1326b5f3 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs @@ -0,0 +1,122 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl RayCast2d 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() + && ray.origin.angle_to(Vec2::Y).abs() < self.arc.half_angle + { + return Some(RayHit2d::new(0.0, -ray.direction)); + } + + // Check for intersections 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 for intersection with the line segment between the origin and the arc's first endpoint. + let left_endpoint = self.arc.left_endpoint(); + + let segment_direction = Dir2::new_unchecked(-left_endpoint / self.radius()); + let mut segment = Segment2d::new(segment_direction, self.radius()); + let mut segment_iso = Isometry2d::from_translation(left_endpoint / 2.0); + + if let Some(intersection) = segment.ray_cast(segment_iso, 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. + segment.direction = + Dir2::new_unchecked(Vec2::new(-segment.direction.x, segment.direction.y)); + segment_iso.translation.x = -segment_iso.translation.x; + + if let Some(intersection) = segment.ray_cast(segment_iso, ray, max_distance, true) { + if 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), Vec2::X))); + assert!(!sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -0.1), Vec2::NEG_Y))); + assert!(!sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Vec2::Y))); + + // Ray hits the circular sector. + assert!(sector.intersects_local_ray(Ray2d::new(Vec2::new(0.5, 0.2), Vec2::NEG_X))); + assert!(sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -0.1), Vec2::Y))); + assert!(sector.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Vec2::NEG_Y))); + + // Check correct hit distance and normal for outside hits. + let ray = Ray2d::new(Vec2::new(0.0, 0.0), Vec2::Y); + let hit = sector.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(0.0, Dir2::SOUTH_WEST))); + + let ray = Ray2d::new(Vec2::new(0.0, 1.5), Vec2::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); + assert_eq!( + hit, + Some(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 + )) + ); + + // Interior hit for solid sector. + let ray = Ray2d::new(Vec2::new(0.0, sector.apothem()), Vec2::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), Vec2::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); + assert_eq!( + hit, + Some(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 + )) + ); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..00a462c0c1fdd --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs @@ -0,0 +1,88 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl RayCast2d 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. + if solid + && ray.origin.length_squared() < self.radius().squared() + && ray.origin.y >= start.y.min(end.y) + { + 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(Dir2::new(end - start).unwrap(), 2.0 * self.radius()); + if let Some(intersection) = segment.ray_cast( + Isometry2d::from_translation(start.midpoint(end)), + ray, + max_distance, + true, + ) { + if 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), Vec2::NEG_X))); + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.5), Vec2::NEG_Y))); + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Vec2::Y))); + + // Ray hits the circular segment. + assert!(segment.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.75), Vec2::NEG_X))); + assert!(segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.9), Vec2::Y))); + assert!(segment.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.1), Vec2::NEG_Y))); + + // Check correct hit distance and normal for outside hits. + let ray = Ray2d::new(Vec2::ZERO, Vec2::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), Vec2::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()), Vec2::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), Vec2::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, Vec2::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..d4241f1c8bdea --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/ellipse.rs @@ -0,0 +1,97 @@ +use crate::prelude::*; + +impl RayCast2d 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 implemention 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 = d.sqrt(); + + // 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); + let normal = Dir2::new_unchecked(hit_point * inv_half_size); + Some(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_unchecked(-hit_point * inv_half_size); + 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), Vec2::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, Vec2::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, Vec2::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), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..fc564a1db19dc --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/line.rs @@ -0,0 +1,70 @@ +use crate::prelude::*; + +impl RayCast2d 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_unchecked(-self.direction.perp()); + + 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 normal_dot_dir.abs() < f32::EPSILON { + // Check if the ray is collinear with the line, within `f32::EPSILON`. + if normal_dot_origin.abs() < 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_unchecked(-normal_dot_dir.signum() * normal), + )) + } +} + +#[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), Vec2::NEG_Y); + let hit = line.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(3.0, Dir2::NORTH_WEST))); + + // Hit from below at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(2.0, -1.0), Vec2::Y); + let hit = line.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(3.0, Dir2::SOUTH_EAST))); + + // 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), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Vec2::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..992ac781f58f5 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/mod.rs @@ -0,0 +1,355 @@ +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 shapes in two-dimensional space. +pub trait RayCast2d { + /// 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), Vec2::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, Vec2::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), Vec2::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, Vec2::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), Vec2::X))); + /// assert!(!circle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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), Vec2::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), Vec2::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), Vec2::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), Vec2::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), Vec2::X))); + /// assert!(!circle.intersects_ray(iso, Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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), Vec2::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..ac3114af22197 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/polygon.rs @@ -0,0 +1,157 @@ +use crate::prelude::*; + +// TODO: Polygons should probably have their own type for this along with a BVH acceleration structure. + +impl RayCast2d 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 RayCast2d for BoxedPolygon { + #[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 RayCast2d 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, translation) = Segment2d::from_points(vertex1, vertex2); + if let Some(hit) = segment.ray_cast( + Isometry2d::from_translation(translation), + 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 mut closest_intersection: Option = None; + let mut intersection_count = 0; + + // Iterate through vertices to create edges + for i in 0..vertices.len() { + let start = vertices[i]; + let end = if i == vertices.len() - 1 { + // Connect the last vertex to the first vertex to close the polygon + vertices[0] + } else { + vertices[i + 1] + }; + + // Create the edge + let segment = Segment2d::new(Dir2::new(end - start).unwrap(), start.distance(end)); + + // Cast the ray against the edge + if let Some(intersection) = segment.ray_cast( + Isometry2d::from_translation(start.midpoint(end)), + ray, + max_distance, + true, + ) { + intersection_count += 1; + if let Some(ref closest) = closest_intersection { + if intersection.distance < closest.distance { + closest_intersection = Some(intersection); + } + } else { + closest_intersection = Some(intersection); + } + } + } + + // 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 = BoxedPolygon::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), Vec2::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, Vec2::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, Vec2::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, Vec2::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), Vec2::Y))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.0), Vec2::X))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -1.0), Vec2::X))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.0), Vec2::Y))); + assert!(!polygon.intersects_local_ray(Ray2d::new(Vec2::new(-2.0, 0.0), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..84dc344f0ccea --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/polyline.rs @@ -0,0 +1,101 @@ +use crate::prelude::*; + +// TODO: Polylines should probably have their own type for this along with a BVH acceleration structure. + +impl RayCast2d 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) + } +} + +impl RayCast2d for BoxedPolyline2d { + #[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 { + let mut closest_intersection: Option = None; + + // Iterate through vertices to create edges + for i in 0..(vertices.len() - 1) { + let start = vertices[i]; + let end = vertices[i + 1]; + + // Create the edge + let segment = Segment2d::new(Dir2::new(end - start).unwrap(), start.distance(end)); + + // Cast the ray against the edge + if let Some(intersection) = segment.ray_cast( + Isometry2d::from_translation(start.midpoint(end)), + 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::*; + + #[test] + fn local_ray_cast_polyline_2d() { + let polyline = BoxedPolyline2d::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), Vec2::NEG_Y); + let hit = polyline.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(4.0, Dir2::NORTH_WEST))); + + let ray = Ray2d::new(Vec2::new(0.0, 4.0), Vec2::NEG_Y); + let hit = polyline.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(4.0, Dir2::NORTH_EAST))); + + // Hit from below. + let ray = Ray2d::new(Vec2::new(-4.0, -4.0), Vec2::Y); + let hit = polyline.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(4.0, Dir2::SOUTH_EAST))); + + let ray = Ray2d::new(Vec2::new(0.0, -4.0), Vec2::Y); + let hit = polyline.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(4.0, Dir2::SOUTH_WEST))); + + // Hit from the side. + let ray = Ray2d::new(Vec2::new(-2.0, 0.0), Vec2::X); + let hit = polyline.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(2.0, Dir2::SOUTH_WEST))); + + // Ray goes past the left endpoint. + assert!(!polyline.intersects_local_ray(Ray2d::new(Vec2::new(-7.0, 2.0), Vec2::NEG_Y))); + + // Ray goes past the right endpoint. + assert!(!polyline.intersects_local_ray(Ray2d::new(Vec2::new(7.0, -2.0), Vec2::Y))); + + // Ray points away from the polyline. + assert!(!polyline.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 0.2), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Vec2::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..a5ba09a5fd915 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/rectangle.rs @@ -0,0 +1,118 @@ +use crate::prelude::*; + +impl RayCast2d 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), Vec2::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, Vec2::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, Vec2::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, Vec2::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), Vec2::Y))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, 1.0), Vec2::X))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(0.0, -1.0), Vec2::X))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(2.0, 0.0), Vec2::Y))); + assert!(!rectangle.intersects_local_ray(Ray2d::new(Vec2::new(-2.0, 0.0), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..e10da7f573cad --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/rhombus.rs @@ -0,0 +1,90 @@ +use crate::prelude::*; + +impl RayCast2d 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 + && ray.origin.x.abs() / self.half_diagonals.x + + ray.origin.y.abs() / 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 difference = end - start; + let length = difference.length(); + let segment = Segment2d::new(Dir2::new_unchecked(difference / length), length); + + if let Some(intersection) = segment.ray_cast( + Isometry2d::from_translation(start.midpoint(end)), + ray, + max_distance, + true, + ) { + if 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), Vec2::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_eq!(hit.distance, SQRT_2 / 2.0); + 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), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(0.0, 2.0), Vec2::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..28cb2f7b9da57 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/segment.rs @@ -0,0 +1,85 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl RayCast2d for Segment2d { + #[inline] + fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { + // Direction perpendicular to the line segment. + let normal = Dir2::new_unchecked(-self.direction.perp()); + + 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 normal_dot_dir.abs() < f32::EPSILON { + // Check if the ray is collinear with the line, within `f32::EPSILON`. + if normal_dot_origin.abs() < 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; + } + + // Check if we are within `self.half_length`. + let intersection = ray.origin + *ray.direction * distance; + if intersection.length_squared() > self.half_length.squared() { + return None; + } + + Some(RayHit2d::new( + distance, + Dir2::new_unchecked(-normal_dot_dir.signum() * normal), + )) + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn local_ray_cast_segment_2d() { + let segment = Segment2d { + direction: Dir2::NORTH_EAST, + half_length: 5.0, + }; + + // Hit from above at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Vec2::NEG_Y); + let hit = segment.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(3.0, Dir2::NORTH_WEST))); + + // Hit from below at a 45 degree angle. + let ray = Ray2d::new(Vec2::new(2.0, -1.0), Vec2::Y); + let hit = segment.local_ray_cast(ray, f32::MAX, true); + assert_eq!(hit, Some(RayHit2d::new(3.0, Dir2::SOUTH_EAST))); + + // 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), Vec2::NEG_Y))); + + // Ray goes past the right endpoint. + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(6.0, -2.0), Vec2::Y))); + + // Ray points away from the line segment. + assert!(!segment.intersects_local_ray(Ray2d::new(Vec2::new(1.0, 2.0), Vec2::Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-2.0, 1.0), Vec2::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..ac8d9c539de6f --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim2/triangle.rs @@ -0,0 +1,92 @@ +use crate::prelude::*; + +impl RayCast2d 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(Dir2::new(end - start).unwrap(), start.distance(end)); + + if let Some(intersection) = segment.ray_cast( + Isometry2d::from_translation(start.midpoint(end)), + 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), Vec2::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); + assert_eq!(hit, Some(RayHit2d::new(SQRT_2, Dir2::NORTH_EAST))); + + // Ray origin is inside of the solid triangle. + let ray = Ray2d::new(Vec2::splat(0.5), Vec2::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), Vec2::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), Vec2::NEG_Y))); + + // Hit distance exceeds max distance. + let ray = Ray2d::new(Vec2::new(-1.0, 1.0), Vec2::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..18ecf48276ec0 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/capsule.rs @@ -0,0 +1,204 @@ +use crate::prelude::*; + +// This is mostly the same as `Capsule2d`, but with 3D types. +impl RayCast3d 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 = ray.origin.y.abs() < 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 - ray.origin.y.abs()).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 + d.sqrt()) / a + } else { + (-b - d.sqrt()) / 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_unchecked(Vec3::new( + point.x.copysign(-ray.direction.x) * radius_recip, + 0.0, + point.z.copysign(-ray.direction.z) * radius_recip, + )); + 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 = d.sqrt(); + + 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 = if is_origin_inside { + Dir3::new_unchecked(-offset_ray.get_point(t2) / self.radius) + } else { + Dir3::new_unchecked(offset_ray.get_point(t2) / self.radius) + }; + 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_unchecked(-offset_ray.get_point(t1) / self.radius); + 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), Vec3::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), Vec3::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, Vec3::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, Vec3::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, Vec3::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), Vec3::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), Vec3::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), Vec3::Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.6, 0.0), Vec3::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..520dac865f855 --- /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 RayCast3d 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_unchecked(ba / self.height); + 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 + discriminant.sqrt()) / 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_unchecked(ba / self.height); + 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(top_distance_squared.sqrt(), 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 - discriminant.sqrt()) / 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), Vec3::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, Vec3::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, Vec3::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, Vec3::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), Vec3::Y))); + assert!(cone.intersects_local_ray(Ray3d::new(Vec3::new(0.4, 0.0, 0.0), Vec3::X))); + + // Ray points away from the cone. + assert!(!cone.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Vec3::Y))); + assert!(!cone.intersects_local_ray(Ray3d::new(Vec3::new(0.6, 0.0, 0.0), Vec3::X))); + + // Edge case: The ray is pointing straight down at the tip. + let ray = Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Vec3::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), Vec3::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..7b3a843a56f50 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs @@ -0,0 +1,256 @@ +use crate::prelude::*; + +impl RayCast3d 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_unchecked(ba / self.height); + 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_unchecked(-ba / self.height); + 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 + discriminant.sqrt()) / 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_unchecked(-ba / self.height); + 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_unchecked(ba / self.height); + 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 - discriminant.sqrt()) / 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), Vec3::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, Vec3::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, Vec3::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, Vec3::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), Vec3::Y))); + assert!(frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.5, 0.5, 0.0), Vec3::X))); + + // Ray points away from the frustum. + assert!(!frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Vec3::Y))); + assert!(!frustum.intersects_local_ray(Ray3d::new(Vec3::new(0.75, 0.5, 0.0), Vec3::X))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Vec3::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..2683d6b4ff329 --- /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 RayCast3d 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_unchecked(-ray.direction.signum() * normal_abs); + 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_unchecked(-ray.direction.signum() * normal_abs); + 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), Vec3::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, Vec3::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, Vec3::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, Vec3::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), Vec3::Y))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Vec3::X))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, -1.0, 0.0), Vec3::X))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Vec3::Z))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(0.0, -1.0, 0.0), Vec3::Z))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Vec3::Y))); + assert!(!cuboid.intersects_local_ray(Ray3d::new(Vec3::new(-2.0, 0.0, 0.0), Vec3::Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Vec3::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..a63db9c94af84 --- /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 RayCast3d 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_unchecked(ba / (2.0 * self.half_height)); + 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_unchecked(-ba / (2.0 * self.half_height)); + 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 + discriminant.sqrt()) / 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_unchecked(-ba / height_squared.sqrt()); + 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_unchecked(ba / height_squared.sqrt()); + 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 - discriminant.sqrt()) / 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), Vec3::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, Vec3::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, Vec3::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, Vec3::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), Vec3::Y))); + assert!(cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.4, 0.9, 0.0), Vec3::X))); + + // Ray points away from the cylinder. + assert!(!cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 1.1, 0.0), Vec3::Y))); + assert!(!cylinder.intersects_local_ray(Ray3d::new(Vec3::new(0.6, 1.1, 0.0), Vec3::X))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Vec3::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..cb544467e90bf --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/mod.rs @@ -0,0 +1,349 @@ +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 shapes in three-dimensional space. +pub trait RayCast3d { + /// 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), Vec3::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, Vec3::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), Vec3::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, Vec3::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), Vec3::X))); + /// assert!(!sphere.intersects_local_ray(Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Vec3::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), Vec3::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), Vec3::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), Vec3::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), Vec3::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), Vec3::X))); + /// assert!(!sphere.intersects_ray(iso, Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Vec3::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), Vec3::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..90c569402da97 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/sphere.rs @@ -0,0 +1,98 @@ +use ops::FloatPow; + +use crate::prelude::*; + +// This is the same as `Sphere`, but with 3D types. +impl RayCast3d 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 = if is_inside { + Dir3::new_unchecked(-point / self.radius) + } else { + Dir3::new_unchecked(point / self.radius) + }; + 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 - d.sqrt(); + + 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 + d.sqrt(); + 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), Vec3::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, Vec3::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, Vec3::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), Vec3::Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 2.0, 0.0), Vec3::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..94c0152af8f26 --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs @@ -0,0 +1,198 @@ +use crate::prelude::*; + +impl RayCast3d 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), Vec3::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), Vec3::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), Vec3::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), + Vec3::new(1.0, -1.0, 1.0) + ))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(0.0, 1.0, 0.0), Vec3::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..3d865a0860ebf --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/torus.rs @@ -0,0 +1,228 @@ +use ops::FloatPow; + +use crate::prelude::*; + +impl RayCast3d 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 (k3 * (k3 * k3 - k2) + k1).abs() < 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 = q.sqrt(); + z = 2.0 * q_sqrt * ops::cos(ops::acos(r / (q * q_sqrt)) / 3.0); + } else { + // 2 intersections + let q_sqrt = (h.sqrt() + r.abs()).cubed(); + z = r.signum() * (q_sqrt + q / q_sqrt).abs(); + } + + z = c2 - z; + + let mut d1 = z - 3.0 * c2; + let mut d2 = z * z - 3.0 * c0; + + if d1.abs() < 1.0e-4 { + if d2 < 0.0 { + return None; + } + d2 = d2.sqrt(); + } else { + if d1 < 0.0 { + return None; + } + d1 = (d1 / 2.0).sqrt(); + d2 = c1 / d1; + } + + let mut distance = f32::MAX; + + let discriminant1 = d1 * d1 - z + d2; + if discriminant1 > 0.0 { + let d_sqrt = discriminant1.sqrt(); + 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 = discriminant2.sqrt(); + 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), Vec3::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, Vec3::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), Vec3::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), Vec3::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), Vec3::Y); + assert!(!torus.intersects_local_ray(ray)); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(2.0, 0.0, 0.0), Vec3::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..7d3cf61bcd78e --- /dev/null +++ b/crates/bevy_math/src/ray_cast/dim3/triangle.rs @@ -0,0 +1,100 @@ +use crate::prelude::*; + +impl RayCast3d 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), Vec3::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), Vec3::new(-1.0, -1.0, 0.0)); + 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), Vec3::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), Vec3::new(1.0, 1.0, 0.0)); + 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), Vec3::NEG_Y))); + + // Hit distance exceeds max distance. + let ray = Ray3d::new(Vec3::new(-0.5, 1.0, 0.0), Vec3::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..f51855b28154a --- /dev/null +++ b/crates/bevy_math/src/ray_cast/mod.rs @@ -0,0 +1,7 @@ +//! Ray casting types and functionality. + +mod dim2; +mod dim3; + +pub use dim2::{RayCast2d, RayHit2d}; +pub use dim3::{RayCast3d, RayHit3d}; From c8683354750f0f95a718bbb7b169aa267505a728 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 5 Oct 2024 19:16:03 +0300 Subject: [PATCH 02/10] Rename traits to `PrimitiveRayCastNd` --- crates/bevy_math/src/lib.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/annulus.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/arc.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/capsule.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/circle.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/circular_sector.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/circular_segment.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/ellipse.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/line.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/mod.rs | 6 ++++-- crates/bevy_math/src/ray_cast/dim2/polygon.rs | 6 +++--- crates/bevy_math/src/ray_cast/dim2/polyline.rs | 4 ++-- crates/bevy_math/src/ray_cast/dim2/rectangle.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/rhombus.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/segment.rs | 2 +- crates/bevy_math/src/ray_cast/dim2/triangle.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/capsule.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/cone.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/cuboid.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/cylinder.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/mod.rs | 6 ++++-- crates/bevy_math/src/ray_cast/dim3/sphere.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/torus.rs | 2 +- crates/bevy_math/src/ray_cast/dim3/triangle.rs | 2 +- crates/bevy_math/src/ray_cast/mod.rs | 4 ++-- 27 files changed, 37 insertions(+), 33 deletions(-) diff --git a/crates/bevy_math/src/lib.rs b/crates/bevy_math/src/lib.rs index c496ab26e4be0..e6560f607a5e1 100644 --- a/crates/bevy_math/src/lib.rs +++ b/crates/bevy_math/src/lib.rs @@ -61,7 +61,7 @@ pub mod prelude { direction::{Dir2, Dir3, Dir3A}, ops, primitives::*, - ray_cast::{RayCast2d, RayCast3d, RayHit2d, RayHit3d}, + ray_cast::{PrimitiveRayCast2d, PrimitiveRayCast3d, RayHit2d, RayHit3d}, BVec2, BVec3, BVec4, EulerRot, FloatExt, IRect, IVec2, IVec3, IVec4, Isometry2d, Isometry3d, Mat2, Mat3, Mat4, Quat, Ray2d, Ray3d, Rect, Rot2, StableInterpolate, URect, UVec2, UVec3, UVec4, Vec2, Vec2Swizzles, Vec3, Vec3Swizzles, Vec4, Vec4Swizzles, diff --git a/crates/bevy_math/src/ray_cast/dim2/annulus.rs b/crates/bevy_math/src/ray_cast/dim2/annulus.rs index 10a1afaca413e..cb463391fa74e 100644 --- a/crates/bevy_math/src/ray_cast/dim2/annulus.rs +++ b/crates/bevy_math/src/ray_cast/dim2/annulus.rs @@ -2,7 +2,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast2d for Annulus { +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(); diff --git a/crates/bevy_math/src/ray_cast/dim2/arc.rs b/crates/bevy_math/src/ray_cast/dim2/arc.rs index c9c852bdde40f..88f7c601bae9a 100644 --- a/crates/bevy_math/src/ray_cast/dim2/arc.rs +++ b/crates/bevy_math/src/ray_cast/dim2/arc.rs @@ -4,7 +4,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast2d for Arc2d { +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. diff --git a/crates/bevy_math/src/ray_cast/dim2/capsule.rs b/crates/bevy_math/src/ray_cast/dim2/capsule.rs index c55424db81366..1ebdca7512b57 100644 --- a/crates/bevy_math/src/ray_cast/dim2/capsule.rs +++ b/crates/bevy_math/src/ray_cast/dim2/capsule.rs @@ -1,7 +1,7 @@ use crate::prelude::*; // This is mostly the same as `Capsule3d`, but with 2D types. -impl RayCast2d for Capsule2d { +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/ diff --git a/crates/bevy_math/src/ray_cast/dim2/circle.rs b/crates/bevy_math/src/ray_cast/dim2/circle.rs index 8643ab6e66150..86a48edd59678 100644 --- a/crates/bevy_math/src/ray_cast/dim2/circle.rs +++ b/crates/bevy_math/src/ray_cast/dim2/circle.rs @@ -2,7 +2,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast2d for Circle { +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) diff --git a/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs b/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs index 6fc4f1326b5f3..301fb7ff66113 100644 --- a/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs +++ b/crates/bevy_math/src/ray_cast/dim2/circular_sector.rs @@ -2,7 +2,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast2d for CircularSector { +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 diff --git a/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs index 00a462c0c1fdd..312341eb1d658 100644 --- a/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs +++ b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs @@ -2,7 +2,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast2d for CircularSegment { +impl PrimitiveRayCast2d for CircularSegment { #[inline] fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { let start = self.arc.left_endpoint(); diff --git a/crates/bevy_math/src/ray_cast/dim2/ellipse.rs b/crates/bevy_math/src/ray_cast/dim2/ellipse.rs index d4241f1c8bdea..053128580578d 100644 --- a/crates/bevy_math/src/ray_cast/dim2/ellipse.rs +++ b/crates/bevy_math/src/ray_cast/dim2/ellipse.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast2d for Ellipse { +impl PrimitiveRayCast2d for Ellipse { #[inline] fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { // Adapted from: diff --git a/crates/bevy_math/src/ray_cast/dim2/line.rs b/crates/bevy_math/src/ray_cast/dim2/line.rs index fc564a1db19dc..2d3a2b341e6ff 100644 --- a/crates/bevy_math/src/ray_cast/dim2/line.rs +++ b/crates/bevy_math/src/ray_cast/dim2/line.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast2d for Line2d { +impl PrimitiveRayCast2d for Line2d { #[inline] fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { // Direction perpendicular to the line. diff --git a/crates/bevy_math/src/ray_cast/dim2/mod.rs b/crates/bevy_math/src/ray_cast/dim2/mod.rs index 992ac781f58f5..8edef1a1ba6b1 100644 --- a/crates/bevy_math/src/ray_cast/dim2/mod.rs +++ b/crates/bevy_math/src/ray_cast/dim2/mod.rs @@ -41,8 +41,10 @@ impl RayHit2d { } } -/// A trait for intersecting rays with shapes in two-dimensional space. -pub trait RayCast2d { +/// 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`. /// diff --git a/crates/bevy_math/src/ray_cast/dim2/polygon.rs b/crates/bevy_math/src/ray_cast/dim2/polygon.rs index ac3114af22197..230319c467eb8 100644 --- a/crates/bevy_math/src/ray_cast/dim2/polygon.rs +++ b/crates/bevy_math/src/ray_cast/dim2/polygon.rs @@ -2,21 +2,21 @@ use crate::prelude::*; // TODO: Polygons should probably have their own type for this along with a BVH acceleration structure. -impl RayCast2d for Polygon { +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 RayCast2d for BoxedPolygon { +impl PrimitiveRayCast2d for BoxedPolygon { #[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 RayCast2d for RegularPolygon { +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()); diff --git a/crates/bevy_math/src/ray_cast/dim2/polyline.rs b/crates/bevy_math/src/ray_cast/dim2/polyline.rs index 84dc344f0ccea..90d99514f2814 100644 --- a/crates/bevy_math/src/ray_cast/dim2/polyline.rs +++ b/crates/bevy_math/src/ray_cast/dim2/polyline.rs @@ -2,14 +2,14 @@ use crate::prelude::*; // TODO: Polylines should probably have their own type for this along with a BVH acceleration structure. -impl RayCast2d for Polyline2d { +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) } } -impl RayCast2d for BoxedPolyline2d { +impl PrimitiveRayCast2d for BoxedPolyline2d { #[inline] fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { local_ray_cast_polyline(&self.vertices, ray, max_distance) diff --git a/crates/bevy_math/src/ray_cast/dim2/rectangle.rs b/crates/bevy_math/src/ray_cast/dim2/rectangle.rs index a5ba09a5fd915..b7d044f821e38 100644 --- a/crates/bevy_math/src/ray_cast/dim2/rectangle.rs +++ b/crates/bevy_math/src/ray_cast/dim2/rectangle.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast2d for Rectangle { +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/ diff --git a/crates/bevy_math/src/ray_cast/dim2/rhombus.rs b/crates/bevy_math/src/ray_cast/dim2/rhombus.rs index e10da7f573cad..6223909374326 100644 --- a/crates/bevy_math/src/ray_cast/dim2/rhombus.rs +++ b/crates/bevy_math/src/ray_cast/dim2/rhombus.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast2d for Rhombus { +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 diff --git a/crates/bevy_math/src/ray_cast/dim2/segment.rs b/crates/bevy_math/src/ray_cast/dim2/segment.rs index 28cb2f7b9da57..598eb65ea149d 100644 --- a/crates/bevy_math/src/ray_cast/dim2/segment.rs +++ b/crates/bevy_math/src/ray_cast/dim2/segment.rs @@ -2,7 +2,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast2d for Segment2d { +impl PrimitiveRayCast2d for Segment2d { #[inline] fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, _solid: bool) -> Option { // Direction perpendicular to the line segment. diff --git a/crates/bevy_math/src/ray_cast/dim2/triangle.rs b/crates/bevy_math/src/ray_cast/dim2/triangle.rs index ac8d9c539de6f..fad45db6a7ba8 100644 --- a/crates/bevy_math/src/ray_cast/dim2/triangle.rs +++ b/crates/bevy_math/src/ray_cast/dim2/triangle.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast2d for Triangle2d { +impl PrimitiveRayCast2d for Triangle2d { #[inline] fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { let [a, b, c] = self.vertices; diff --git a/crates/bevy_math/src/ray_cast/dim3/capsule.rs b/crates/bevy_math/src/ray_cast/dim3/capsule.rs index 18ecf48276ec0..ee5d7bb84b92c 100644 --- a/crates/bevy_math/src/ray_cast/dim3/capsule.rs +++ b/crates/bevy_math/src/ray_cast/dim3/capsule.rs @@ -1,7 +1,7 @@ use crate::prelude::*; // This is mostly the same as `Capsule2d`, but with 3D types. -impl RayCast3d for Capsule3d { +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/ diff --git a/crates/bevy_math/src/ray_cast/dim3/cone.rs b/crates/bevy_math/src/ray_cast/dim3/cone.rs index 520dac865f855..3dc611112371d 100644 --- a/crates/bevy_math/src/ray_cast/dim3/cone.rs +++ b/crates/bevy_math/src/ray_cast/dim3/cone.rs @@ -1,7 +1,7 @@ use crate::prelude::*; // NOTE: This is largely a copy of the `ConicalFrustum` implementation, but simplified for only one base. -impl RayCast3d for Cone { +impl PrimitiveRayCast3d for Cone { #[inline] fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { // Adapted from: diff --git a/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs b/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs index 7b3a843a56f50..ef6319908b401 100644 --- a/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs +++ b/crates/bevy_math/src/ray_cast/dim3/conical_frustum.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast3d for ConicalFrustum { +impl PrimitiveRayCast3d for ConicalFrustum { #[inline] fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { // Adapted from: diff --git a/crates/bevy_math/src/ray_cast/dim3/cuboid.rs b/crates/bevy_math/src/ray_cast/dim3/cuboid.rs index 2683d6b4ff329..eb81a24304f87 100644 --- a/crates/bevy_math/src/ray_cast/dim3/cuboid.rs +++ b/crates/bevy_math/src/ray_cast/dim3/cuboid.rs @@ -1,7 +1,7 @@ use crate::prelude::*; // This is the same as `Rectangle`, but with 3D types. -impl RayCast3d for Cuboid { +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/ diff --git a/crates/bevy_math/src/ray_cast/dim3/cylinder.rs b/crates/bevy_math/src/ray_cast/dim3/cylinder.rs index a63db9c94af84..7c9f53b201f6c 100644 --- a/crates/bevy_math/src/ray_cast/dim3/cylinder.rs +++ b/crates/bevy_math/src/ray_cast/dim3/cylinder.rs @@ -1,7 +1,7 @@ use crate::prelude::*; // NOTE: This is largely a copy of the `ConicalFrustum` implementation, but simplified for only one radius. -impl RayCast3d for Cylinder { +impl PrimitiveRayCast3d for Cylinder { #[inline] fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, solid: bool) -> Option { // Adapted from: diff --git a/crates/bevy_math/src/ray_cast/dim3/mod.rs b/crates/bevy_math/src/ray_cast/dim3/mod.rs index cb544467e90bf..52d1a038beb89 100644 --- a/crates/bevy_math/src/ray_cast/dim3/mod.rs +++ b/crates/bevy_math/src/ray_cast/dim3/mod.rs @@ -36,8 +36,10 @@ impl RayHit3d { } } -/// A trait for intersecting rays with shapes in three-dimensional space. -pub trait RayCast3d { +/// 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`. /// diff --git a/crates/bevy_math/src/ray_cast/dim3/sphere.rs b/crates/bevy_math/src/ray_cast/dim3/sphere.rs index 90c569402da97..0fe2e5e14e413 100644 --- a/crates/bevy_math/src/ray_cast/dim3/sphere.rs +++ b/crates/bevy_math/src/ray_cast/dim3/sphere.rs @@ -3,7 +3,7 @@ use ops::FloatPow; use crate::prelude::*; // This is the same as `Sphere`, but with 3D types. -impl RayCast3d for Sphere { +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) diff --git a/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs b/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs index 94c0152af8f26..d685a72d4959b 100644 --- a/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs +++ b/crates/bevy_math/src/ray_cast/dim3/tetrahedron.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast3d for Tetrahedron { +impl PrimitiveRayCast3d for Tetrahedron { #[inline] fn intersects_local_ray(&self, ray: Ray3d) -> bool { // Tetrahedron-ray intersection test using scalar triple products. diff --git a/crates/bevy_math/src/ray_cast/dim3/torus.rs b/crates/bevy_math/src/ray_cast/dim3/torus.rs index 3d865a0860ebf..052e51d6940ca 100644 --- a/crates/bevy_math/src/ray_cast/dim3/torus.rs +++ b/crates/bevy_math/src/ray_cast/dim3/torus.rs @@ -2,7 +2,7 @@ use ops::FloatPow; use crate::prelude::*; -impl RayCast3d for Torus { +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; diff --git a/crates/bevy_math/src/ray_cast/dim3/triangle.rs b/crates/bevy_math/src/ray_cast/dim3/triangle.rs index 7d3cf61bcd78e..e3e3fed2690d4 100644 --- a/crates/bevy_math/src/ray_cast/dim3/triangle.rs +++ b/crates/bevy_math/src/ray_cast/dim3/triangle.rs @@ -1,6 +1,6 @@ use crate::prelude::*; -impl RayCast3d for Triangle3d { +impl PrimitiveRayCast3d for Triangle3d { #[inline] fn local_ray_cast(&self, ray: Ray3d, max_distance: f32, _solid: bool) -> Option { // Adapted from: diff --git a/crates/bevy_math/src/ray_cast/mod.rs b/crates/bevy_math/src/ray_cast/mod.rs index f51855b28154a..e7b978b3674a7 100644 --- a/crates/bevy_math/src/ray_cast/mod.rs +++ b/crates/bevy_math/src/ray_cast/mod.rs @@ -3,5 +3,5 @@ mod dim2; mod dim3; -pub use dim2::{RayCast2d, RayHit2d}; -pub use dim3::{RayCast3d, RayHit3d}; +pub use dim2::{PrimitiveRayCast2d, RayHit2d}; +pub use dim3::{PrimitiveRayCast3d, RayHit3d}; From 8c110fbd344f0b2d01592e3cfdf0f24393c0711f Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 5 Oct 2024 19:46:06 +0300 Subject: [PATCH 03/10] Add benchmarks --- benches/Cargo.toml | 10 ++ benches/benches/bevy_math/ray_cast_2d.rs | 101 +++++++++++++++++++ benches/benches/bevy_math/ray_cast_3d.rs | 120 +++++++++++++++++++++++ 3 files changed, 231 insertions(+) create mode 100644 benches/benches/bevy_math/ray_cast_2d.rs create mode 100644 benches/benches/bevy_math/ray_cast_3d.rs diff --git a/benches/Cargo.toml b/benches/Cargo.toml index cc586a3e66241..c0ff45e8fdf37 100644 --- a/benches/Cargo.toml +++ b/benches/Cargo.toml @@ -72,6 +72,16 @@ name = "bezier" path = "benches/bevy_math/bezier.rs" harness = false +[[bench]] +name = "ray_cast_2d" +path = "benches/bevy_math/ray_cast_2d.rs" +harness = false + +[[bench]] +name = "ray_cast_3d" +path = "benches/bevy_math/ray_cast_3d.rs" +harness = false + [[bench]] name = "torus" path = "benches/bevy_render/torus.rs" 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..69ffd5dc34c1d --- /dev/null +++ b/benches/benches/bevy_math/ray_cast_2d.rs @@ -0,0 +1,101 @@ +use std::time::Duration; + +use bevy_math::{prelude::*, FromRng, ShapeSample}; +use criterion::{ + black_box, criterion_group, criterion_main, measurement::WallTime, BenchmarkGroup, Criterion, +}; +use rand::{rngs::StdRng, Rng, SeedableRng}; + +const SAMPLES: usize = 100_000; + +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)| { + 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.gen_range(0.1..2.5)) + }); + bench_shape(&mut group, &mut rng, "arc", |rng| { + Arc2d::new( + rng.gen_range(0.1..2.5), + rng.gen_range(0.1..std::f32::consts::PI), + ) + }); + bench_shape(&mut group, &mut rng, "circular_sector", |rng| { + CircularSector::new( + rng.gen_range(0.1..2.5), + rng.gen_range(0.1..std::f32::consts::PI), + ) + }); + bench_shape(&mut group, &mut rng, "circular_segment", |rng| { + CircularSegment::new( + rng.gen_range(0.1..2.5), + rng.gen_range(0.1..std::f32::consts::PI), + ) + }); + bench_shape(&mut group, &mut rng, "ellipse", |rng| { + Ellipse::new(rng.gen_range(0.1..2.5), rng.gen_range(0.1..2.5)) + }); + bench_shape(&mut group, &mut rng, "annulus", |rng| { + Annulus::new(rng.gen_range(0.1..1.25), rng.gen_range(1.26..2.5)) + }); + bench_shape(&mut group, &mut rng, "capsule2d", |rng| { + Capsule2d::new(rng.gen_range(0.1..1.25), rng.gen_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "rectangle", |rng| { + Rectangle::new(rng.gen_range(0.1..5.0), rng.gen_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "rhombus", |rng| { + Rhombus::new(rng.gen_range(0.1..5.0), rng.gen_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 { + direction: Dir2::from_rng(rng), + half_length: rng.gen_range(0.1..5.0), + }); + bench_shape(&mut group, &mut rng, "regular_polygon", |rng| { + RegularPolygon::new(rng.gen_range(0.1..2.5), rng.gen_range(3..6)) + }); + bench_shape(&mut group, &mut rng, "triangle2d", |rng| { + Triangle2d::new( + Vec2::new(rng.gen_range(-7.5..7.5), rng.gen_range(-7.5..7.5)), + Vec2::new(rng.gen_range(-7.5..7.5), rng.gen_range(-7.5..7.5)), + Vec2::new(rng.gen_range(-7.5..7.5), rng.gen_range(-7.5..7.5)), + ) + }); +} + +criterion_group!(benches, ray_cast_2d); +criterion_main!(benches); 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..3787cb642568d --- /dev/null +++ b/benches/benches/bevy_math/ray_cast_3d.rs @@ -0,0 +1,120 @@ +use std::time::Duration; + +use bevy_math::{prelude::*, FromRng, ShapeSample}; +use criterion::{ + black_box, criterion_group, criterion_main, measurement::WallTime, BenchmarkGroup, Criterion, +}; +use rand::{rngs::StdRng, Rng, SeedableRng}; + +const SAMPLES: usize = 100_000; + +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)| { + 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.gen_range(0.1..2.5)) + }); + bench_shape(&mut group, &mut rng, "cuboid", |rng| { + Cuboid::new( + rng.gen_range(0.1..5.0), + rng.gen_range(0.1..5.0), + rng.gen_range(0.1..5.0), + ) + }); + bench_shape(&mut group, &mut rng, "cylinder", |rng| { + Cylinder::new(rng.gen_range(0.1..2.5), rng.gen_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "cone", |rng| { + Cone::new(rng.gen_range(0.1..2.5), rng.gen_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "conical_frustum", |rng| { + ConicalFrustum { + radius_top: rng.gen_range(0.1..2.5), + radius_bottom: rng.gen_range(0.1..2.5), + height: rng.gen_range(0.1..5.0), + } + }); + bench_shape(&mut group, &mut rng, "capsule3d", |rng| { + Capsule3d::new(rng.gen_range(0.1..2.5), rng.gen_range(0.1..5.0)) + }); + bench_shape(&mut group, &mut rng, "triangle3d", |rng| { + Triangle3d::new( + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + ) + }); + bench_shape(&mut group, &mut rng, "tetrahedron", |rng| { + Tetrahedron::new( + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + Vec3::new( + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + rng.gen_range(-7.5..7.5), + ), + ) + }); + bench_shape(&mut group, &mut rng, "torus", |rng| { + Torus::new(rng.gen_range(0.1..1.25), rng.gen_range(1.26..2.5)) + }); +} + +criterion_group!(benches, ray_cast_3d); +criterion_main!(benches); From a9562e7ec6208a58963372147ae9d1d56ae29b15 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sun, 6 Oct 2024 01:53:03 +0300 Subject: [PATCH 04/10] Add 2D ray casting example --- Cargo.toml | 11 ++ examples/math/ray_cast_2d.rs | 338 +++++++++++++++++++++++++++++++++++ 2 files changed, 349 insertions(+) create mode 100644 examples/math/ray_cast_2d.rs diff --git a/Cargo.toml b/Cargo.toml index bd4efd833718d..b7d34ea53da28 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3269,6 +3269,17 @@ 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 = "Rendering Primitives" +description = "Shows off ray casting for primitive shapes in 2D" +category = "Math" +wasm = true + [[example]] name = "render_primitives" path = "examples/math/render_primitives.rs" diff --git a/examples/math/ray_cast_2d.rs b/examples/math/ray_cast_2d.rs new file mode 100644 index 0000000000000..a2327b7270565 --- /dev/null +++ b/examples/math/ray_cast_2d.rs @@ -0,0 +1,338 @@ +//! 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}, + }, + prelude::*, + window::PrimaryWindow, +}; + +fn main() { + App::new() + .add_plugins(DefaultPlugins) + .insert_gizmo_config( + DefaultGizmoConfigGroup, + GizmoConfig { + line_width: 3.0, + ..default() + }, + ) + .init_resource::() + .add_systems(Startup, setup) + .add_systems( + Update, + (draw_shapes, ray_follow_cursor, rotate_ray, ray_cast).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, Vec2::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)] +#[allow(missing_docs)] +pub enum Shape2d { + Circle(Circle), + Arc(Arc2d), + CircularSector(CircularSector), + CircularSegment(CircularSegment), + Ellipse(Ellipse), + Annulus(Annulus), + Rectangle(Rectangle), + Rhombus(Rhombus), + Line(Line2d), + Segment(Segment2d), + Polyline(BoxedPolyline2d), + Polygon(BoxedPolygon), + RegularPolygon(RegularPolygon), + Triangle(Triangle2d), + Capsule(Capsule2d), +} + +fn setup(mut commands: Commands) { + // Spawn camera + commands.spawn(Camera2d); + + 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(BoxedPolygon::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(Dir2::from_xy(1.0, 0.5).unwrap(), 200.0)), + Shape2d::Polyline(BoxedPolyline2d::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))); +} + +/// 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: Query<&Window, With>, + camera: Query<(&Camera, &GlobalTransform)>, + mut ray: ResMut, +) { + let window = windows.single(); + let (camera, camera_transform) = camera.single(); + + if let Some(cursor_world_pos) = window + .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) { + if button.pressed(MouseButton::Left) { + ray.direction = Rot2::radians(0.015) * ray.direction; + } + if button.pressed(MouseButton::Right) { + ray.direction = Rot2::radians(-0.015) * ray.direction; + } +} + +/// Performs ray casts against all shapes in the scene. +fn ray_cast(query: Query<(&Shape2d, &Transform)>, mut gizmos: Gizmos, ray: Res) { + let max_distance = 10_000.0; + + 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 (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 = 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); + gizmos.circle_2d(iso, 2.5, ORANGE); + gizmos.circle_2d(iso, 2.0, ORANGE); + gizmos.circle_2d(iso, 1.0, ORANGE); + gizmos.circle_2d(iso, 1.0, ORANGE); + gizmos.circle_2d(iso, 0.5, ORANGE); + } else { + gizmos.line_2d(ray.origin, ray.get_point(max_distance), CYAN_600); + } +} + +/// Draws all shapes in the scene. +fn draw_shapes(query: Query<(&Shape2d, &GlobalTransform)>, mut gizmos: Gizmos) { + for (shape, global_transform) in &query { + let transform = global_transform.compute_transform(); + let pos = transform.translation.truncate(); + let rot = Rot2::radians(transform.rotation.to_euler(EulerRot::XYZ).2); + gizmos.primitive_2d(shape, Isometry2d::new(pos, rot), Color::WHITE); + } +} + +// Trait implementations for `Shape2d` to make ray casts and drawing shapes easier. + +impl Primitive2d for Shape2d {} + +impl PrimitiveRayCast2d for Shape2d { + fn local_ray_distance(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { + use Shape2d::*; + + match self { + Circle(circle) => circle.local_ray_distance(ray, max_distance, solid), + Arc(arc) => arc.local_ray_distance(ray, max_distance, solid), + CircularSector(sector) => sector.local_ray_distance(ray, max_distance, solid), + CircularSegment(segment) => segment.local_ray_distance(ray, max_distance, solid), + Ellipse(ellipse) => ellipse.local_ray_distance(ray, max_distance, solid), + Annulus(annulus) => annulus.local_ray_distance(ray, max_distance, solid), + Rectangle(rectangle) => rectangle.local_ray_distance(ray, max_distance, solid), + Rhombus(rhombus) => rhombus.local_ray_distance(ray, max_distance, solid), + Line(line) => line.local_ray_distance(ray, max_distance, solid), + Segment(segment) => segment.local_ray_distance(ray, max_distance, solid), + Polyline(polyline) => polyline.local_ray_distance(ray, max_distance, solid), + Polygon(polygon) => polygon.local_ray_distance(ray, max_distance, solid), + RegularPolygon(polygon) => polygon.local_ray_distance(ray, max_distance, solid), + Triangle(triangle) => triangle.local_ray_distance(ray, max_distance, solid), + Capsule(capsule) => capsule.local_ray_distance(ray, max_distance, solid), + } + } + + 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<'w, 's, Config> GizmoPrimitive2d for Gizmos<'w, 's, Config> +where + Config: GizmoConfigGroup, +{ + type Output<'a> = () where Self: 'a; + + fn primitive_2d( + &mut self, + primitive: &Shape2d, + isometry: Isometry2d, + color: impl Into, + ) -> Self::Output<'_> { + use Shape2d::*; + + match &primitive { + Circle(shape) => { + self.primitive_2d(shape, isometry, color); + } + Arc(shape) => { + self.primitive_2d(shape, isometry, color); + } + CircularSector(shape) => self.primitive_2d(shape, isometry, color), + CircularSegment(shape) => self.primitive_2d(shape, isometry, color), + Ellipse(shape) => { + self.primitive_2d(shape, isometry, color); + } + Annulus(shape) => { + self.primitive_2d(shape, isometry, color); + } + Rectangle(shape) => { + self.primitive_2d(shape, isometry, color); + } + Rhombus(shape) => { + self.primitive_2d(shape, isometry, color); + } + Line(shape) => { + self.primitive_2d(shape, isometry, color); + } + Segment(shape) => { + self.primitive_2d(shape, isometry, color); + } + Polyline(shape) => { + self.primitive_2d(shape, isometry, color); + } + Polygon(shape) => { + self.primitive_2d(shape, isometry, color); + } + RegularPolygon(shape) => self.primitive_2d(shape, isometry, color), + Triangle(shape) => { + self.primitive_2d(shape, isometry, color); + } + Capsule(shape) => { + self.primitive_2d(shape, isometry, color); + } + } + } +} From 435c6442cd240ba1f798183a0f95736df2044e11 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sun, 6 Oct 2024 01:55:18 +0300 Subject: [PATCH 05/10] Fix example name --- Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Cargo.toml b/Cargo.toml index b7d34ea53da28..a56d1064acc25 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3275,7 +3275,7 @@ path = "examples/math/ray_cast_2d.rs" doc-scrape-examples = true [package.metadata.example.ray_cast_2d] -name = "Rendering Primitives" +name = "2D Ray Casting for Primitives" description = "Shows off ray casting for primitive shapes in 2D" category = "Math" wasm = true From 523ee42ef0f608dbd3c5f669a5f791df47f67098 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Tue, 8 Oct 2024 02:28:56 +0300 Subject: [PATCH 06/10] Fix some bugs --- crates/bevy_math/src/ray_cast/dim2/arc.rs | 4 ++-- .../bevy_math/src/ray_cast/dim2/circular_segment.rs | 13 +++++++++---- crates/bevy_math/src/ray_cast/dim3/torus.rs | 2 +- 3 files changed, 12 insertions(+), 7 deletions(-) diff --git a/crates/bevy_math/src/ray_cast/dim2/arc.rs b/crates/bevy_math/src/ray_cast/dim2/arc.rs index 88f7c601bae9a..b9d603acd7a3e 100644 --- a/crates/bevy_math/src/ray_cast/dim2/arc.rs +++ b/crates/bevy_math/src/ray_cast/dim2/arc.rs @@ -30,7 +30,7 @@ impl PrimitiveRayCast2d for Arc2d { 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 = ops::sin(self.radius * (FRAC_PI_2 + self.half_angle)); + let arc_bottom_y = self.radius * ops::sin(FRAC_PI_2 + self.half_angle); if p2.y >= arc_bottom_y { let normal = Dir2::new_unchecked(p2 / self.radius); return Some(RayHit2d::new(t2, normal)); @@ -41,7 +41,7 @@ impl PrimitiveRayCast2d for Arc2d { if t1 <= max_distance { // The ray hit the inside of the arc. let p1 = ray.get_point(t1); - let arc_bottom_y = ops::sin(self.radius * (FRAC_PI_2 + self.half_angle)); + let arc_bottom_y = self.radius * ops::sin(FRAC_PI_2 + self.half_angle); if p1.y >= arc_bottom_y { let normal = Dir2::new_unchecked(-p1 / self.radius); return Some(RayHit2d::new(t1, normal)); diff --git a/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs index 312341eb1d658..91e8dfce3c49e 100644 --- a/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs +++ b/crates/bevy_math/src/ray_cast/dim2/circular_segment.rs @@ -9,10 +9,9 @@ impl PrimitiveRayCast2d for CircularSegment { let end = self.arc.right_endpoint(); // First, if the segment is solid, check if the ray origin is inside of it. - if solid - && ray.origin.length_squared() < self.radius().squared() - && ray.origin.y >= start.y.min(end.y) - { + 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)); } @@ -24,6 +23,12 @@ impl PrimitiveRayCast2d for CircularSegment { // Check if the segment connecting the arc's endpoints is intersecting the ray. let segment = Segment2d::new(Dir2::new(end - start).unwrap(), 2.0 * self.radius()); + + 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.ray_cast( Isometry2d::from_translation(start.midpoint(end)), ray, diff --git a/crates/bevy_math/src/ray_cast/dim3/torus.rs b/crates/bevy_math/src/ray_cast/dim3/torus.rs index 052e51d6940ca..8e15ccb085266 100644 --- a/crates/bevy_math/src/ray_cast/dim3/torus.rs +++ b/crates/bevy_math/src/ray_cast/dim3/torus.rs @@ -119,7 +119,7 @@ fn torus_ray_distance( z = 2.0 * q_sqrt * ops::cos(ops::acos(r / (q * q_sqrt)) / 3.0); } else { // 2 intersections - let q_sqrt = (h.sqrt() + r.abs()).cubed(); + let q_sqrt = ops::cbrt(h.sqrt() + r.abs()); z = r.signum() * (q_sqrt + q / q_sqrt).abs(); } From 36d72f7580f2676187353dd421d3c48e71e4c2c8 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Tue, 8 Oct 2024 02:29:23 +0300 Subject: [PATCH 07/10] Add 3D example --- Cargo.toml | 11 ++ examples/math/ray_cast_2d.rs | 42 +++--- examples/math/ray_cast_3d.rs | 251 +++++++++++++++++++++++++++++++++++ 3 files changed, 279 insertions(+), 25 deletions(-) create mode 100644 examples/math/ray_cast_3d.rs diff --git a/Cargo.toml b/Cargo.toml index a56d1064acc25..f4e93e987b70e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -3280,6 +3280,17 @@ 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/examples/math/ray_cast_2d.rs b/examples/math/ray_cast_2d.rs index a2327b7270565..a13ca93be34d3 100644 --- a/examples/math/ray_cast_2d.rs +++ b/examples/math/ray_cast_2d.rs @@ -74,9 +74,6 @@ pub enum Shape2d { } fn setup(mut commands: Commands) { - // Spawn camera - commands.spawn(Camera2d); - let shapes = [ Shape2d::Circle(Circle::new(50.0)), Shape2d::Arc(Arc2d::new(50.0, 1.25)), @@ -128,6 +125,23 @@ fn setup(mut commands: Commands) { 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); + + // Spawn instructions + commands.spawn( + TextBundle::from_section( + "Move the cursor to move the ray.\nLeft mouse button to rotate the ray counterclockwise.\nRight mouse button to rotate the ray clockwise.", + TextStyle::default(), + ) + .with_style(Style { + position_type: PositionType::Absolute, + top: Val::Px(12.0), + left: Val::Px(12.0), + ..default() + }), + ); } /// Spawns a shape at a given column and row. @@ -234,28 +248,6 @@ fn draw_shapes(query: Query<(&Shape2d, &GlobalTransform)>, mut gizmos: Gizmos) { impl Primitive2d for Shape2d {} impl PrimitiveRayCast2d for Shape2d { - fn local_ray_distance(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { - use Shape2d::*; - - match self { - Circle(circle) => circle.local_ray_distance(ray, max_distance, solid), - Arc(arc) => arc.local_ray_distance(ray, max_distance, solid), - CircularSector(sector) => sector.local_ray_distance(ray, max_distance, solid), - CircularSegment(segment) => segment.local_ray_distance(ray, max_distance, solid), - Ellipse(ellipse) => ellipse.local_ray_distance(ray, max_distance, solid), - Annulus(annulus) => annulus.local_ray_distance(ray, max_distance, solid), - Rectangle(rectangle) => rectangle.local_ray_distance(ray, max_distance, solid), - Rhombus(rhombus) => rhombus.local_ray_distance(ray, max_distance, solid), - Line(line) => line.local_ray_distance(ray, max_distance, solid), - Segment(segment) => segment.local_ray_distance(ray, max_distance, solid), - Polyline(polyline) => polyline.local_ray_distance(ray, max_distance, solid), - Polygon(polygon) => polygon.local_ray_distance(ray, max_distance, solid), - RegularPolygon(polygon) => polygon.local_ray_distance(ray, max_distance, solid), - Triangle(triangle) => triangle.local_ray_distance(ray, max_distance, solid), - Capsule(capsule) => capsule.local_ray_distance(ray, max_distance, solid), - } - } - fn local_ray_cast(&self, ray: Ray2d, max_distance: f32, solid: bool) -> Option { use Shape2d::*; diff --git a/examples/math/ray_cast_3d.rs b/examples/math/ray_cast_3d.rs new file mode 100644 index 0000000000000..4ff70ce97c66a --- /dev/null +++ b/examples/math/ray_cast_3d.rs @@ -0,0 +1,251 @@ +//! 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_width: 3.0, + ..default() + }, + ) + .init_resource::() + .add_systems(Startup, setup) + .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, Vec3::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)] +#[allow(missing_docs)] +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 { + shadows_enabled: true, + intensity: 20_000_000., + range: 100.0, + shadow_depth_bias: 0.2, + ..default() + }, + Transform::from_xyz(8.0, 16.0, 8.0), + )); + + // Spawn instructions + commands.spawn( + TextBundle::from_section( + "Point the cursor at the shapes to cast rays.", + TextStyle::default(), + ) + .with_style(Style { + position_type: PositionType::Absolute, + top: Val::Px(12.0), + left: Val::Px(12.0), + ..default() + }), + ); +} + +/// 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