diff --git a/crates/bevy_math/src/bounding/bounded2d.rs b/crates/bevy_math/src/bounding/bounded2d/mod.rs similarity index 79% rename from crates/bevy_math/src/bounding/bounded2d.rs rename to crates/bevy_math/src/bounding/bounded2d/mod.rs index 78caf6abe4ad9..163a3776bbbaa 100644 --- a/crates/bevy_math/src/bounding/bounded2d.rs +++ b/crates/bevy_math/src/bounding/bounded2d/mod.rs @@ -1,6 +1,22 @@ +mod primitive_impls; + +use glam::Mat2; + use super::BoundingVolume; use crate::prelude::Vec2; +/// Computes the geometric center of the given set of points. +#[inline(always)] +fn point_cloud_2d_center(points: &[Vec2]) -> Vec2 { + assert!( + !points.is_empty(), + "cannot compute the center of an empty set of points" + ); + + let denom = 1.0 / points.len() as f32; + points.iter().fold(Vec2::ZERO, |acc, point| acc + *point) * denom +} + /// A trait with methods that return 2D bounded volumes for a shape pub trait Bounded2d { /// Get an axis-aligned bounding box for the shape with the given translation and rotation. @@ -21,6 +37,41 @@ pub struct Aabb2d { pub max: Vec2, } +impl Aabb2d { + /// Computes the smallest [`Aabb2d`] containing the given set of points, + /// transformed by `translation` and `rotation`. + /// + /// # Panics + /// + /// Panics if the given set of points is empty. + #[inline(always)] + pub fn from_point_cloud(translation: Vec2, rotation: f32, points: &[Vec2]) -> Aabb2d { + // Transform all points by rotation + let rotation_mat = Mat2::from_angle(rotation); + let mut iter = points.iter().map(|point| rotation_mat * *point); + + let first = iter + .next() + .expect("point cloud must contain at least one point for Aabb2d construction"); + + let (min, max) = iter.fold((first, first), |(prev_min, prev_max), point| { + (point.min(prev_min), point.max(prev_max)) + }); + + Aabb2d { + min: min + translation, + max: max + translation, + } + } + + /// Computes the smallest [`BoundingCircle`] containing this [`Aabb2d`]. + #[inline(always)] + pub fn bounding_circle(&self) -> BoundingCircle { + let radius = self.min.distance(self.max) / 2.0; + BoundingCircle::new(self.center(), radius) + } +} + impl BoundingVolume for Aabb2d { type Position = Vec2; type HalfSize = Vec2; @@ -207,11 +258,43 @@ impl BoundingCircle { } } + /// Computes a [`BoundingCircle`] containing the given set of points, + /// transformed by `translation` and `rotation`. + /// + /// The bounding circle is not guaranteed to be the smallest possible. + #[inline(always)] + pub fn from_point_cloud(translation: Vec2, rotation: f32, points: &[Vec2]) -> BoundingCircle { + let center = point_cloud_2d_center(points); + let mut radius_squared = 0.0; + + for point in points { + // Get squared version to avoid unnecessary sqrt calls + let distance_squared = point.distance_squared(center); + if distance_squared > radius_squared { + radius_squared = distance_squared; + } + } + + BoundingCircle::new( + Mat2::from_angle(rotation) * center + translation, + radius_squared.sqrt(), + ) + } + /// Get the radius of the bounding circle #[inline(always)] pub fn radius(&self) -> f32 { self.circle.radius } + + /// Computes the smallest [`Aabb2d`] containing this [`BoundingCircle`]. + #[inline(always)] + pub fn aabb_2d(&self) -> Aabb2d { + Aabb2d { + min: self.center - Vec2::splat(self.radius()), + max: self.center + Vec2::splat(self.radius()), + } + } } impl BoundingVolume for BoundingCircle { diff --git a/crates/bevy_math/src/bounding/bounded2d/primitive_impls.rs b/crates/bevy_math/src/bounding/bounded2d/primitive_impls.rs new file mode 100644 index 0000000000000..c60854995399c --- /dev/null +++ b/crates/bevy_math/src/bounding/bounded2d/primitive_impls.rs @@ -0,0 +1,466 @@ +//! Contains [`Bounded2d`] implementations for [geometric primitives](crate::primitives). + +use glam::{Mat2, Vec2}; + +use crate::primitives::{ + BoxedPolygon, BoxedPolyline2d, Circle, Ellipse, Line2d, Plane2d, Polygon, Polyline2d, + Rectangle, RegularPolygon, Segment2d, Triangle2d, +}; + +use super::{Aabb2d, Bounded2d, BoundingCircle}; + +impl Bounded2d for Circle { + fn aabb_2d(&self, translation: Vec2, _rotation: f32) -> Aabb2d { + Aabb2d { + min: translation - Vec2::splat(self.radius), + max: translation + Vec2::splat(self.radius), + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + BoundingCircle::new(translation, self.radius) + } +} + +impl Bounded2d for Ellipse { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + // V = (hh * cos(beta), hh * sin(beta)) + // #####*##### + // ### | ### + // # hh | # + // # *---------* U = (hw * cos(alpha), hw * sin(alpha)) + // # hw # + // ### ### + // ########### + + let (hw, hh) = (self.half_width, self.half_height); + + // Sine and cosine of rotation angle alpha. + let (alpha_sin, alpha_cos) = rotation.sin_cos(); + + // Sine and cosine of alpha + pi/2. We can avoid the trigonometric functions: + // sin(beta) = sin(alpha + pi/2) = cos(alpha) + // cos(beta) = cos(alpha + pi/2) = -sin(alpha) + let (beta_sin, beta_cos) = (alpha_cos, -alpha_sin); + + // Compute points U and V, the extremes of the ellipse + let (ux, uy) = (hw * alpha_cos, hw * alpha_sin); + let (vx, vy) = (hh * beta_cos, hh * beta_sin); + + let half_extents = Vec2::new(ux.hypot(vx), uy.hypot(vy)); + + Aabb2d { + min: translation - half_extents, + max: translation + half_extents, + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + BoundingCircle::new(translation, self.half_width.max(self.half_height)) + } +} + +impl Bounded2d for Plane2d { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + let normal = Mat2::from_angle(rotation) * *self.normal; + let facing_x = normal == Vec2::X || normal == Vec2::NEG_X; + let facing_y = normal == Vec2::Y || normal == Vec2::NEG_Y; + + // Dividing `f32::MAX` by 2.0 is helpful so that we can do operations + // like growing or shrinking the AABB without breaking things. + let half_width = if facing_x { 0.0 } else { f32::MAX / 2.0 }; + let half_height = if facing_y { 0.0 } else { f32::MAX / 2.0 }; + let half_size = Vec2::new(half_width, half_height); + + Aabb2d { + min: translation - half_size, + max: translation + half_size, + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + BoundingCircle::new(translation, f32::MAX / 2.0) + } +} + +impl Bounded2d for Line2d { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + let direction = Mat2::from_angle(rotation) * *self.direction; + + // Dividing `f32::MAX` by 2.0 is helpful so that we can do operations + // like growing or shrinking the AABB without breaking things. + let max = f32::MAX / 2.0; + let half_width = if direction.x == 0.0 { 0.0 } else { max }; + let half_height = if direction.y == 0.0 { 0.0 } else { max }; + let half_size = Vec2::new(half_width, half_height); + + Aabb2d { + min: translation - half_size, + max: translation + half_size, + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + BoundingCircle::new(translation, f32::MAX / 2.0) + } +} + +impl Bounded2d for Segment2d { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + // Rotate the segment by `rotation` + let direction = Mat2::from_angle(rotation) * *self.direction; + let half_extent = (self.half_length * direction).abs(); + + Aabb2d { + min: translation - half_extent, + max: translation + half_extent, + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + BoundingCircle::new(translation, self.half_length) + } +} + +impl Bounded2d for Polyline2d { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + Aabb2d::from_point_cloud(translation, rotation, &self.vertices) + } + + fn bounding_circle(&self, translation: Vec2, rotation: f32) -> BoundingCircle { + BoundingCircle::from_point_cloud(translation, rotation, &self.vertices) + } +} + +impl Bounded2d for BoxedPolyline2d { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + Aabb2d::from_point_cloud(translation, rotation, &self.vertices) + } + + fn bounding_circle(&self, translation: Vec2, rotation: f32) -> BoundingCircle { + BoundingCircle::from_point_cloud(translation, rotation, &self.vertices) + } +} + +impl Bounded2d for Triangle2d { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + let rotation_mat = Mat2::from_angle(rotation); + let [a, b, c] = self.vertices.map(|vtx| rotation_mat * vtx); + + let min = Vec2::new(a.x.min(b.x).min(c.x), a.y.min(b.y).min(c.y)); + let max = Vec2::new(a.x.max(b.x).max(c.x), a.y.max(b.y).max(c.y)); + + Aabb2d { + min: min + translation, + max: max + translation, + } + } + + fn bounding_circle(&self, translation: Vec2, rotation: f32) -> BoundingCircle { + let rotation_mat = Mat2::from_angle(rotation); + let [a, b, c] = self.vertices; + + // The points of the segment opposite to the obtuse or right angle if one exists + let side_opposite_to_non_acute = if (b - a).dot(c - a) <= 0.0 { + Some((b, c)) + } else if (c - b).dot(a - b) <= 0.0 { + Some((c, a)) + } else if (a - c).dot(b - c) <= 0.0 { + Some((a, b)) + } else { + // The triangle is acute. + None + }; + + // Find the minimum bounding circle. If the triangle is obtuse, the circle passes through two vertices. + // Otherwise, it's the circumcircle and passes through all three. + if let Some((point1, point2)) = side_opposite_to_non_acute { + // The triangle is obtuse or right, so the minimum bounding circle's diameter is equal to the longest side. + // We can compute the minimum bounding circle from the line segment of the longest side. + let (segment, center) = Segment2d::from_points(point1, point2); + segment.bounding_circle(rotation_mat * center + translation, rotation) + } else { + // The triangle is acute, so the smallest bounding circle is the circumcircle. + let (Circle { radius }, circumcenter) = self.circumcircle(); + BoundingCircle::new(rotation_mat * circumcenter + translation, radius) + } + } +} + +impl Bounded2d for Rectangle { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + let half_size = Vec2::new(self.half_width, self.half_height); + + // Compute the AABB of the rotated rectangle by transforming the half-extents + // by an absolute rotation matrix. + let (sin, cos) = rotation.sin_cos(); + let abs_rot_mat = Mat2::from_cols_array(&[cos.abs(), sin.abs(), sin.abs(), cos.abs()]); + let half_extents = abs_rot_mat * half_size; + + Aabb2d { + min: translation - half_extents, + max: translation + half_extents, + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + let radius = self.half_width.hypot(self.half_height); + BoundingCircle::new(translation, radius) + } +} + +impl Bounded2d for Polygon { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + Aabb2d::from_point_cloud(translation, rotation, &self.vertices) + } + + fn bounding_circle(&self, translation: Vec2, rotation: f32) -> BoundingCircle { + BoundingCircle::from_point_cloud(translation, rotation, &self.vertices) + } +} + +impl Bounded2d for BoxedPolygon { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + Aabb2d::from_point_cloud(translation, rotation, &self.vertices) + } + + fn bounding_circle(&self, translation: Vec2, rotation: f32) -> BoundingCircle { + BoundingCircle::from_point_cloud(translation, rotation, &self.vertices) + } +} + +impl Bounded2d for RegularPolygon { + fn aabb_2d(&self, translation: Vec2, rotation: f32) -> Aabb2d { + let mut min = Vec2::ZERO; + let mut max = Vec2::ZERO; + + for vertex in self.vertices(rotation) { + min = min.min(vertex); + max = max.max(vertex); + } + + Aabb2d { + min: min + translation, + max: max + translation, + } + } + + fn bounding_circle(&self, translation: Vec2, _rotation: f32) -> BoundingCircle { + BoundingCircle::new(translation, self.circumcircle.radius) + } +} + +#[cfg(test)] +mod tests { + use glam::Vec2; + + use crate::{ + bounding::Bounded2d, + primitives::{ + Circle, Direction2d, Ellipse, Line2d, Plane2d, Polygon, Polyline2d, Rectangle, + RegularPolygon, Segment2d, Triangle2d, + }, + }; + + #[test] + fn circle() { + let circle = Circle { radius: 1.0 }; + let translation = Vec2::new(2.0, 1.0); + + let aabb = circle.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(1.0, 0.0)); + assert_eq!(aabb.max, Vec2::new(3.0, 2.0)); + + let bounding_circle = circle.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), 1.0); + } + + #[test] + fn ellipse() { + let ellipse = Ellipse { + half_width: 1.0, + half_height: 0.5, + }; + let translation = Vec2::new(2.0, 1.0); + + let aabb = ellipse.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(1.0, 0.5)); + assert_eq!(aabb.max, Vec2::new(3.0, 1.5)); + + let bounding_circle = ellipse.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), 1.0); + } + + #[test] + fn plane() { + let translation = Vec2::new(2.0, 1.0); + + let aabb1 = Plane2d::new(Vec2::X).aabb_2d(translation, 0.0); + assert_eq!(aabb1.min, Vec2::new(2.0, -f32::MAX / 2.0)); + assert_eq!(aabb1.max, Vec2::new(2.0, f32::MAX / 2.0)); + + let aabb2 = Plane2d::new(Vec2::Y).aabb_2d(translation, 0.0); + assert_eq!(aabb2.min, Vec2::new(-f32::MAX / 2.0, 1.0)); + assert_eq!(aabb2.max, Vec2::new(f32::MAX / 2.0, 1.0)); + + let aabb3 = Plane2d::new(Vec2::ONE).aabb_2d(translation, 0.0); + assert_eq!(aabb3.min, Vec2::new(-f32::MAX / 2.0, -f32::MAX / 2.0)); + assert_eq!(aabb3.max, Vec2::new(f32::MAX / 2.0, f32::MAX / 2.0)); + + let bounding_circle = Plane2d::new(Vec2::Y).bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), f32::MAX / 2.0); + } + + #[test] + fn line() { + let translation = Vec2::new(2.0, 1.0); + + let aabb1 = Line2d { + direction: Direction2d::Y, + } + .aabb_2d(translation, 0.0); + assert_eq!(aabb1.min, Vec2::new(2.0, -f32::MAX / 2.0)); + assert_eq!(aabb1.max, Vec2::new(2.0, f32::MAX / 2.0)); + + let aabb2 = Line2d { + direction: Direction2d::X, + } + .aabb_2d(translation, 0.0); + assert_eq!(aabb2.min, Vec2::new(-f32::MAX / 2.0, 1.0)); + assert_eq!(aabb2.max, Vec2::new(f32::MAX / 2.0, 1.0)); + + let aabb3 = Line2d { + direction: Direction2d::from_xy(1.0, 1.0).unwrap(), + } + .aabb_2d(translation, 0.0); + assert_eq!(aabb3.min, Vec2::new(-f32::MAX / 2.0, -f32::MAX / 2.0)); + assert_eq!(aabb3.max, Vec2::new(f32::MAX / 2.0, f32::MAX / 2.0)); + + let bounding_circle = Line2d { + direction: Direction2d::Y, + } + .bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), f32::MAX / 2.0); + } + + #[test] + fn segment() { + let translation = Vec2::new(2.0, 1.0); + let segment = Segment2d::from_points(Vec2::new(-1.0, -0.5), Vec2::new(1.0, 0.5)).0; + + let aabb = segment.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(1.0, 0.5)); + assert_eq!(aabb.max, Vec2::new(3.0, 1.5)); + + let bounding_circle = segment.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), 1.0_f32.hypot(0.5)); + } + + #[test] + fn polyline() { + let polyline = Polyline2d::<4>::new([ + Vec2::ONE, + Vec2::new(-1.0, 1.0), + Vec2::NEG_ONE, + Vec2::new(1.0, -1.0), + ]); + let translation = Vec2::new(2.0, 1.0); + + let aabb = polyline.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(1.0, 0.0)); + assert_eq!(aabb.max, Vec2::new(3.0, 2.0)); + + let bounding_circle = polyline.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), std::f32::consts::SQRT_2); + } + + #[test] + fn acute_triangle() { + let acute_triangle = + Triangle2d::new(Vec2::new(0.0, 1.0), Vec2::NEG_ONE, Vec2::new(1.0, -1.0)); + let translation = Vec2::new(2.0, 1.0); + + let aabb = acute_triangle.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(1.0, 0.0)); + assert_eq!(aabb.max, Vec2::new(3.0, 2.0)); + + // For acute triangles, the center is the circumcenter + let (Circle { radius }, circumcenter) = acute_triangle.circumcircle(); + let bounding_circle = acute_triangle.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, circumcenter + translation); + assert_eq!(bounding_circle.radius(), radius); + } + + #[test] + fn obtuse_triangle() { + let obtuse_triangle = Triangle2d::new( + Vec2::new(0.0, 1.0), + Vec2::new(-10.0, -1.0), + Vec2::new(10.0, -1.0), + ); + let translation = Vec2::new(2.0, 1.0); + + let aabb = obtuse_triangle.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(-8.0, 0.0)); + assert_eq!(aabb.max, Vec2::new(12.0, 2.0)); + + // For obtuse and right triangles, the center is the midpoint of the longest side (diameter of bounding circle) + let bounding_circle = obtuse_triangle.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation - Vec2::Y); + assert_eq!(bounding_circle.radius(), 10.0); + } + + #[test] + fn rectangle() { + let rectangle = Rectangle::new(2.0, 1.0); + let translation = Vec2::new(2.0, 1.0); + + let aabb = rectangle.aabb_2d(translation, std::f32::consts::FRAC_PI_4); + let expected_half_size = Vec2::splat(1.0606601); + assert_eq!(aabb.min, translation - expected_half_size); + assert_eq!(aabb.max, translation + expected_half_size); + + let bounding_circle = rectangle.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), 1.0_f32.hypot(0.5)); + } + + #[test] + fn polygon() { + let polygon = Polygon::<4>::new([ + Vec2::ONE, + Vec2::new(-1.0, 1.0), + Vec2::NEG_ONE, + Vec2::new(1.0, -1.0), + ]); + let translation = Vec2::new(2.0, 1.0); + + let aabb = polygon.aabb_2d(translation, 0.0); + assert_eq!(aabb.min, Vec2::new(1.0, 0.0)); + assert_eq!(aabb.max, Vec2::new(3.0, 2.0)); + + let bounding_circle = polygon.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), std::f32::consts::SQRT_2); + } + + #[test] + fn regular_polygon() { + let regular_polygon = RegularPolygon::new(1.0, 5); + let translation = Vec2::new(2.0, 1.0); + + let aabb = regular_polygon.aabb_2d(translation, 0.0); + assert!((aabb.min - (translation - Vec2::new(0.9510565, 0.8090169))).length() < 1e-6); + assert!((aabb.max - (translation + Vec2::new(0.9510565, 1.0))).length() < 1e-6); + + let bounding_circle = regular_polygon.bounding_circle(translation, 0.0); + assert_eq!(bounding_circle.center, translation); + assert_eq!(bounding_circle.radius(), 1.0); + } +} diff --git a/crates/bevy_math/src/bounding/bounded3d.rs b/crates/bevy_math/src/bounding/bounded3d/mod.rs similarity index 80% rename from crates/bevy_math/src/bounding/bounded3d.rs rename to crates/bevy_math/src/bounding/bounded3d/mod.rs index fb9bd7a58a86d..f46a68ffd45a9 100644 --- a/crates/bevy_math/src/bounding/bounded3d.rs +++ b/crates/bevy_math/src/bounding/bounded3d/mod.rs @@ -1,6 +1,20 @@ +mod primitive_impls; + use super::BoundingVolume; use crate::prelude::{Quat, Vec3}; +/// Computes the geometric center of the given set of points. +#[inline(always)] +fn point_cloud_3d_center(points: &[Vec3]) -> Vec3 { + assert!( + !points.is_empty(), + "cannot compute the center of an empty set of points" + ); + + let denom = 1.0 / points.len() as f32; + points.iter().fold(Vec3::ZERO, |acc, point| acc + *point) * denom +} + /// A trait with methods that return 3D bounded volumes for a shape pub trait Bounded3d { /// Get an axis-aligned bounding box for the shape with the given translation and rotation @@ -18,6 +32,40 @@ pub struct Aabb3d { pub max: Vec3, } +impl Aabb3d { + /// Computes the smallest [`Aabb3d`] containing the given set of points, + /// transformed by `translation` and `rotation`. + /// + /// # Panics + /// + /// Panics if the given set of points is empty. + #[inline(always)] + pub fn from_point_cloud(translation: Vec3, rotation: Quat, points: &[Vec3]) -> Aabb3d { + // Transform all points by rotation + let mut iter = points.iter().map(|point| rotation * *point); + + let first = iter + .next() + .expect("point cloud must contain at least one point for Aabb3d construction"); + + let (min, max) = iter.fold((first, first), |(prev_min, prev_max), point| { + (point.min(prev_min), point.max(prev_max)) + }); + + Aabb3d { + min: min + translation, + max: max + translation, + } + } + + /// Computes the smallest [`BoundingSphere`] containing this [`Aabb3d`]. + #[inline(always)] + pub fn bounding_sphere(&self) -> BoundingSphere { + let radius = self.min.distance(self.max) / 2.0; + BoundingSphere::new(self.center(), radius) + } +} + impl BoundingVolume for Aabb3d { type Position = Vec3; type HalfSize = Vec3; @@ -204,11 +252,40 @@ impl BoundingSphere { } } + /// Computes a [`BoundingSphere`] containing the given set of points, + /// transformed by `translation` and `rotation`. + /// + /// The bounding sphere is not guaranteed to be the smallest possible. + #[inline(always)] + pub fn from_point_cloud(translation: Vec3, rotation: Quat, points: &[Vec3]) -> BoundingSphere { + let center = point_cloud_3d_center(points); + let mut radius_squared = 0.0; + + for point in points { + // Get squared version to avoid unnecessary sqrt calls + let distance_squared = point.distance_squared(center); + if distance_squared > radius_squared { + radius_squared = distance_squared; + } + } + + BoundingSphere::new(rotation * center + translation, radius_squared.sqrt()) + } + /// Get the radius of the bounding sphere #[inline(always)] pub fn radius(&self) -> f32 { self.sphere.radius } + + /// Computes the smallest [`Aabb3d`] containing this [`BoundingSphere`]. + #[inline(always)] + pub fn aabb_3d(&self) -> Aabb3d { + Aabb3d { + min: self.center - Vec3::splat(self.radius()), + max: self.center + Vec3::splat(self.radius()), + } + } } impl BoundingVolume for BoundingSphere { diff --git a/crates/bevy_math/src/bounding/bounded3d/primitive_impls.rs b/crates/bevy_math/src/bounding/bounded3d/primitive_impls.rs new file mode 100644 index 0000000000000..cc1730a919d3e --- /dev/null +++ b/crates/bevy_math/src/bounding/bounded3d/primitive_impls.rs @@ -0,0 +1,567 @@ +//! Contains [`Bounded3d`] implementations for [geometric primitives](crate::primitives). + +use glam::{Mat3, Quat, Vec2, Vec3}; + +use crate::{ + bounding::{Bounded2d, BoundingCircle}, + primitives::{ + BoxedPolyline3d, Capsule, Cone, ConicalFrustum, Cuboid, Cylinder, Direction3d, Line3d, + Plane3d, Polyline3d, Segment3d, Sphere, Torus, Triangle2d, + }, +}; + +use super::{Aabb3d, Bounded3d, BoundingSphere}; + +impl Bounded3d for Sphere { + fn aabb_3d(&self, translation: Vec3, _rotation: Quat) -> Aabb3d { + Aabb3d { + min: translation - Vec3::splat(self.radius), + max: translation + Vec3::splat(self.radius), + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere::new(translation, self.radius) + } +} + +impl Bounded3d for Plane3d { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + let normal = rotation * *self.normal; + let facing_x = normal == Vec3::X || normal == Vec3::NEG_X; + let facing_y = normal == Vec3::Y || normal == Vec3::NEG_Y; + let facing_z = normal == Vec3::Z || normal == Vec3::NEG_Z; + + // Dividing `f32::MAX` by 2.0 is helpful so that we can do operations + // like growing or shrinking the AABB without breaking things. + let half_width = if facing_x { 0.0 } else { f32::MAX / 2.0 }; + let half_height = if facing_y { 0.0 } else { f32::MAX / 2.0 }; + let half_depth = if facing_z { 0.0 } else { f32::MAX / 2.0 }; + let half_size = Vec3::new(half_width, half_height, half_depth); + + Aabb3d { + min: translation - half_size, + max: translation + half_size, + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere::new(translation, f32::MAX / 2.0) + } +} + +impl Bounded3d for Line3d { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + let direction = rotation * *self.direction; + + // Dividing `f32::MAX` by 2.0 is helpful so that we can do operations + // like growing or shrinking the AABB without breaking things. + let max = f32::MAX / 2.0; + let half_width = if direction.x == 0.0 { 0.0 } else { max }; + let half_height = if direction.y == 0.0 { 0.0 } else { max }; + let half_depth = if direction.z == 0.0 { 0.0 } else { max }; + let half_size = Vec3::new(half_width, half_height, half_depth); + + Aabb3d { + min: translation - half_size, + max: translation + half_size, + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere::new(translation, f32::MAX / 2.0) + } +} + +impl Bounded3d for Segment3d { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Rotate the segment by `rotation` + let direction = rotation * *self.direction; + let half_extent = (self.half_length * direction).abs(); + + Aabb3d { + min: translation - half_extent, + max: translation + half_extent, + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere::new(translation, self.half_length) + } +} + +impl Bounded3d for Polyline3d { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + Aabb3d::from_point_cloud(translation, rotation, &self.vertices) + } + + fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere { + BoundingSphere::from_point_cloud(translation, rotation, &self.vertices) + } +} + +impl Bounded3d for BoxedPolyline3d { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + Aabb3d::from_point_cloud(translation, rotation, &self.vertices) + } + + fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere { + BoundingSphere::from_point_cloud(translation, rotation, &self.vertices) + } +} + +impl Bounded3d for Cuboid { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Compute the AABB of the rotated cuboid by transforming the half-extents + // by an absolute rotation matrix. + let rot_mat = Mat3::from_quat(rotation); + let abs_rot_mat = Mat3::from_cols( + rot_mat.x_axis.abs(), + rot_mat.y_axis.abs(), + rot_mat.z_axis.abs(), + ); + + let half_extents = abs_rot_mat * self.half_extents; + + Aabb3d { + min: translation - half_extents, + max: translation + half_extents, + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere { + center: translation, + sphere: Sphere { + radius: self.half_extents.length(), + }, + } + } +} + +impl Bounded3d for Cylinder { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Reference: http://iquilezles.org/articles/diskbbox/ + + let segment_dir = rotation * Vec3::Y; + let top = segment_dir * self.half_height; + let bottom = -top; + + let e = Vec3::ONE - segment_dir * segment_dir; + let half_extents = self.radius * Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt()); + + Aabb3d { + min: translation + (top - half_extents).min(bottom - half_extents), + max: translation + (top + half_extents).max(bottom + half_extents), + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + let radius = self.radius.hypot(self.half_height); + BoundingSphere::new(translation, radius) + } +} + +impl Bounded3d for Capsule { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Get the line segment between the hemispheres of the rotated capsule + let segment = Segment3d { + direction: Direction3d::from_normalized(rotation * Vec3::Y), + half_length: self.half_length, + }; + let (a, b) = (segment.point1(), segment.point2()); + + // Expand the line segment by the capsule radius to get the capsule half-extents + let min = a.min(b) - Vec3::splat(self.radius); + let max = a.max(b) + Vec3::splat(self.radius); + + Aabb3d { + min: min + translation, + max: max + translation, + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere::new(translation, self.radius + self.half_length) + } +} + +impl Bounded3d for Cone { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Reference: http://iquilezles.org/articles/diskbbox/ + + let top = rotation * Vec3::Y * 0.5 * self.height; + let bottom = -top; + let segment = bottom - top; + + let e = 1.0 - segment * segment / segment.length_squared(); + let half_extents = Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt()); + + Aabb3d { + min: translation + top.min(bottom - self.radius * half_extents), + max: translation + top.max(bottom + self.radius * half_extents), + } + } + + fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere { + // Get the triangular cross-section of the cone. + let half_height = 0.5 * self.height; + let triangle = Triangle2d::new( + half_height * Vec2::Y, + Vec2::new(-self.radius, -half_height), + Vec2::new(self.radius, -half_height), + ); + + // Because of circular symmetry, we can use the bounding circle of the triangle + // for the bounding sphere of the cone. + let BoundingCircle { circle, center } = triangle.bounding_circle(Vec2::ZERO, 0.0); + + BoundingSphere::new(rotation * center.extend(0.0) + translation, circle.radius) + } +} + +impl Bounded3d for ConicalFrustum { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Reference: http://iquilezles.org/articles/diskbbox/ + + let top = rotation * Vec3::Y * 0.5 * self.height; + let bottom = -top; + let segment = bottom - top; + + let e = 1.0 - segment * segment / segment.length_squared(); + let half_extents = Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt()); + + Aabb3d { + min: translation + + (top - self.radius_top * half_extents) + .min(bottom - self.radius_bottom * half_extents), + max: translation + + (top + self.radius_top * half_extents) + .max(bottom + self.radius_bottom * half_extents), + } + } + + fn bounding_sphere(&self, translation: Vec3, rotation: Quat) -> BoundingSphere { + let half_height = 0.5 * self.height; + + // To compute the bounding sphere, we'll get the center and radius of the circumcircle + // passing through all four vertices of the trapezoidal cross-section of the conical frustum. + // + // If the circumcenter is inside the trapezoid, we can use that for the bounding sphere. + // Otherwise, we clamp it to the longer parallel side to get a more tightly fitting bounding sphere. + // + // The circumcenter is at the intersection of the bisectors perpendicular to the sides. + // For the isosceles trapezoid, the X coordinate is zero at the center, so a single bisector is enough. + // + // A + // *-------* + // / | \ + // / | \ + // AB / \ | / \ + // / \ | / \ + // / C \ + // *-------------------* + // B + + let a = Vec2::new(-self.radius_top, half_height); + let b = Vec2::new(-self.radius_bottom, -half_height); + let ab = a - b; + let ab_midpoint = b + 0.5 * ab; + let bisector = ab.perp(); + + // Compute intersection between bisector and vertical line at x = 0. + // + // x = ab_midpoint.x + t * bisector.x = 0 + // y = ab_midpoint.y + t * bisector.y = ? + // + // Because ab_midpoint.y = 0 for our conical frustum, we get: + // y = t * bisector.y + // + // Solve x for t: + // t = -ab_midpoint.x / bisector.x + // + // Substitute t to solve for y: + // y = -ab_midpoint.x / bisector.x * bisector.y + let circumcenter_y = -ab_midpoint.x / bisector.x * bisector.y; + + // If the circumcenter is outside the trapezoid, the bounding circle is too large. + // In those cases, we clamp it to the longer parallel side. + let (center, radius) = if circumcenter_y <= -half_height { + (Vec2::new(0.0, -half_height), self.radius_bottom) + } else if circumcenter_y >= half_height { + (Vec2::new(0.0, half_height), self.radius_top) + } else { + let circumcenter = Vec2::new(0.0, circumcenter_y); + // We can use the distance from an arbitrary vertex because they all lie on the circumcircle. + (circumcenter, a.distance(circumcenter)) + }; + + BoundingSphere::new(translation + rotation * center.extend(0.0), radius) + } +} + +impl Bounded3d for Torus { + fn aabb_3d(&self, translation: Vec3, rotation: Quat) -> Aabb3d { + // Compute the AABB of a flat disc with the major radius of the torus. + // Reference: http://iquilezles.org/articles/diskbbox/ + let normal = rotation * Vec3::Y; + let e = 1.0 - normal * normal; + let disc_half_extents = self.major_radius * Vec3::new(e.x.sqrt(), e.y.sqrt(), e.z.sqrt()); + + // Expand the disc by the minor radius to get the torus half extents + let half_extents = disc_half_extents + Vec3::splat(self.minor_radius); + + Aabb3d { + min: translation - half_extents, + max: translation + half_extents, + } + } + + fn bounding_sphere(&self, translation: Vec3, _rotation: Quat) -> BoundingSphere { + BoundingSphere::new(translation, self.outer_radius()) + } +} + +#[cfg(test)] +mod tests { + use glam::{Quat, Vec3}; + + use crate::{ + bounding::Bounded3d, + primitives::{ + Capsule, Cone, ConicalFrustum, Cuboid, Cylinder, Direction3d, Line3d, Plane3d, + Polyline3d, Segment3d, Sphere, Torus, + }, + }; + + #[test] + fn sphere() { + let sphere = Sphere { radius: 1.0 }; + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = sphere.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0)); + assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0)); + + let bounding_sphere = sphere.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.0); + } + + #[test] + fn plane() { + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb1 = Plane3d::new(Vec3::X).aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb1.min, Vec3::new(2.0, -f32::MAX / 2.0, -f32::MAX / 2.0)); + assert_eq!(aabb1.max, Vec3::new(2.0, f32::MAX / 2.0, f32::MAX / 2.0)); + + let aabb2 = Plane3d::new(Vec3::Y).aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb2.min, Vec3::new(-f32::MAX / 2.0, 1.0, -f32::MAX / 2.0)); + assert_eq!(aabb2.max, Vec3::new(f32::MAX / 2.0, 1.0, f32::MAX / 2.0)); + + let aabb3 = Plane3d::new(Vec3::Z).aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb3.min, Vec3::new(-f32::MAX / 2.0, -f32::MAX / 2.0, 0.0)); + assert_eq!(aabb3.max, Vec3::new(f32::MAX / 2.0, f32::MAX / 2.0, 0.0)); + + let aabb4 = Plane3d::new(Vec3::ONE).aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb4.min, Vec3::splat(-f32::MAX / 2.0)); + assert_eq!(aabb4.max, Vec3::splat(f32::MAX / 2.0)); + + let bounding_sphere = Plane3d::new(Vec3::Y).bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), f32::MAX / 2.0); + } + + #[test] + fn line() { + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb1 = Line3d { + direction: Direction3d::Y, + } + .aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb1.min, Vec3::new(2.0, -f32::MAX / 2.0, 0.0)); + assert_eq!(aabb1.max, Vec3::new(2.0, f32::MAX / 2.0, 0.0)); + + let aabb2 = Line3d { + direction: Direction3d::X, + } + .aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb2.min, Vec3::new(-f32::MAX / 2.0, 1.0, 0.0)); + assert_eq!(aabb2.max, Vec3::new(f32::MAX / 2.0, 1.0, 0.0)); + + let aabb3 = Line3d { + direction: Direction3d::Z, + } + .aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb3.min, Vec3::new(2.0, 1.0, -f32::MAX / 2.0)); + assert_eq!(aabb3.max, Vec3::new(2.0, 1.0, f32::MAX / 2.0)); + + let aabb4 = Line3d { + direction: Direction3d::from_xyz(1.0, 1.0, 1.0).unwrap(), + } + .aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb4.min, Vec3::splat(-f32::MAX / 2.0)); + assert_eq!(aabb4.max, Vec3::splat(f32::MAX / 2.0)); + + let bounding_sphere = Line3d { + direction: Direction3d::Y, + } + .bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), f32::MAX / 2.0); + } + + #[test] + fn segment() { + let translation = Vec3::new(2.0, 1.0, 0.0); + let segment = + Segment3d::from_points(Vec3::new(-1.0, -0.5, 0.0), Vec3::new(1.0, 0.5, 0.0)).0; + + let aabb = segment.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(1.0, 0.5, 0.0)); + assert_eq!(aabb.max, Vec3::new(3.0, 1.5, 0.0)); + + let bounding_sphere = segment.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(0.5)); + } + + #[test] + fn polyline() { + let polyline = Polyline3d::<4>::new([ + Vec3::ONE, + Vec3::new(-1.0, 1.0, 1.0), + Vec3::NEG_ONE, + Vec3::new(1.0, -1.0, -1.0), + ]); + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = polyline.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0)); + assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0)); + + let bounding_sphere = polyline.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(1.0).hypot(1.0)); + } + + #[test] + fn cuboid() { + let cuboid = Cuboid::new(2.0, 1.0, 1.0); + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = cuboid.aabb_3d( + translation, + Quat::from_rotation_z(std::f32::consts::FRAC_PI_4), + ); + let expected_half_size = Vec3::new(1.0606601, 1.0606601, 0.5); + assert_eq!(aabb.min, translation - expected_half_size); + assert_eq!(aabb.max, translation + expected_half_size); + + let bounding_sphere = cuboid.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(0.5).hypot(0.5)); + } + + #[test] + fn cylinder() { + let cylinder = Cylinder::new(0.5, 2.0); + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = cylinder.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, translation - Vec3::new(0.5, 1.0, 0.5)); + assert_eq!(aabb.max, translation + Vec3::new(0.5, 1.0, 0.5)); + + let bounding_sphere = cylinder.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.0_f32.hypot(0.5)); + } + + #[test] + fn capsule() { + let capsule = Capsule::new(0.5, 2.0); + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = capsule.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, translation - Vec3::new(0.5, 1.5, 0.5)); + assert_eq!(aabb.max, translation + Vec3::new(0.5, 1.5, 0.5)); + + let bounding_sphere = capsule.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.5); + } + + #[test] + fn cone() { + let cone = Cone { + radius: 1.0, + height: 2.0, + }; + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = cone.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0)); + assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0)); + + let bounding_sphere = cone.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation + Vec3::NEG_Y * 0.25); + assert_eq!(bounding_sphere.radius(), 1.25); + } + + #[test] + fn conical_frustum() { + let conical_frustum = ConicalFrustum { + radius_top: 0.5, + radius_bottom: 1.0, + height: 2.0, + }; + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = conical_frustum.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(1.0, 0.0, -1.0)); + assert_eq!(aabb.max, Vec3::new(3.0, 2.0, 1.0)); + + let bounding_sphere = conical_frustum.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation + Vec3::NEG_Y * 0.1875); + assert_eq!(bounding_sphere.radius(), 1.2884705); + } + + #[test] + fn wide_conical_frustum() { + let conical_frustum = ConicalFrustum { + radius_top: 0.5, + radius_bottom: 5.0, + height: 1.0, + }; + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = conical_frustum.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(-3.0, 0.5, -5.0)); + assert_eq!(aabb.max, Vec3::new(7.0, 1.5, 5.0)); + + // For wide conical frusta like this, the circumcenter can be outside the frustum, + // so the center and radius should be clamped to the longest side. + let bounding_sphere = conical_frustum.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation + Vec3::NEG_Y * 0.5); + assert_eq!(bounding_sphere.radius(), 5.0); + } + + #[test] + fn torus() { + let torus = Torus { + minor_radius: 0.5, + major_radius: 1.0, + }; + let translation = Vec3::new(2.0, 1.0, 0.0); + + let aabb = torus.aabb_3d(translation, Quat::IDENTITY); + assert_eq!(aabb.min, Vec3::new(0.5, 0.5, -1.5)); + assert_eq!(aabb.max, Vec3::new(3.5, 1.5, 1.5)); + + let bounding_sphere = torus.bounding_sphere(translation, Quat::IDENTITY); + assert_eq!(bounding_sphere.center, translation); + assert_eq!(bounding_sphere.radius(), 1.5); + } +} diff --git a/crates/bevy_math/src/primitives/dim2.rs b/crates/bevy_math/src/primitives/dim2.rs index 993097087c8f4..cac59adb9383f 100644 --- a/crates/bevy_math/src/primitives/dim2.rs +++ b/crates/bevy_math/src/primitives/dim2.rs @@ -265,6 +265,41 @@ impl Triangle2d { } } + /// Compute the circle passing through all three vertices of the triangle. + /// The vector in the returned tuple is the circumcenter. + pub fn circumcircle(&self) -> (Circle, Vec2) { + // We treat the triangle as translated so that vertex A is at the origin. This simplifies calculations. + // + // A = (0, 0) + // * + // / \ + // / \ + // / \ + // / \ + // / U \ + // / \ + // *-------------* + // B C + + let a = self.vertices[0]; + let (b, c) = (self.vertices[1] - a, self.vertices[2] - a); + let b_length_sq = b.length_squared(); + let c_length_sq = c.length_squared(); + + // Reference: https://en.wikipedia.org/wiki/Circumcircle#Cartesian_coordinates_2 + let inv_d = (2.0 * (b.x * c.y - b.y * c.x)).recip(); + let ux = inv_d * (c.y * b_length_sq - b.y * c_length_sq); + let uy = inv_d * (b.x * c_length_sq - c.x * b_length_sq); + let u = Vec2::new(ux, uy); + + // Compute true circumcenter and circumradius, adding the tip coordinate so that + // A is translated back to its actual coordinate. + let center = u + a; + let radius = u.length(); + + (Circle { radius }, center) + } + /// Reverse the [`WindingOrder`] of the triangle /// by swapping the second and third vertices pub fn reverse(&mut self) { @@ -353,7 +388,7 @@ impl BoxedPolygon { } } -/// A polygon where all vertices lie on a circle, equally far apart +/// A polygon where all vertices lie on a circle, equally far apart. #[derive(Clone, Copy, Debug)] pub struct RegularPolygon { /// The circumcircle on which all vertices lie @@ -379,6 +414,22 @@ impl RegularPolygon { sides, } } + + /// Returns an iterator over the vertices of the regular polygon, + /// rotated counterclockwise by the given angle in radians. + /// + /// With a rotation of 0, a vertex will be placed at the top `(0.0, circumradius)`. + pub fn vertices(self, rotation: f32) -> impl IntoIterator { + // Add pi/2 so that the polygon has a vertex at the top (sin is 1.0 and cos is 0.0) + let start_angle = rotation + std::f32::consts::FRAC_PI_2; + let step = std::f32::consts::TAU / self.sides as f32; + + (0..self.sides).map(move |i| { + let theta = start_angle + i as f32 * step; + let (sin, cos) = theta.sin_cos(); + Vec2::new(cos, sin) * self.circumcircle.radius + }) + } } #[cfg(test)] @@ -438,4 +489,37 @@ mod tests { ); assert_eq!(invalid_triangle.winding_order(), WindingOrder::Invalid); } + + #[test] + fn triangle_circumcenter() { + let triangle = Triangle2d::new( + Vec2::new(10.0, 2.0), + Vec2::new(-5.0, -3.0), + Vec2::new(2.0, -1.0), + ); + let (Circle { radius }, circumcenter) = triangle.circumcircle(); + + // Calculated with external calculator + assert_eq!(radius, 98.34887); + assert_eq!(circumcenter, Vec2::new(-28.5, 92.5)); + } + + #[test] + fn regular_polygon_vertices() { + let polygon = RegularPolygon::new(1.0, 4); + + // Regular polygons have a vertex at the top by default + let mut vertices = polygon.vertices(0.0).into_iter(); + assert!((vertices.next().unwrap() - Vec2::Y).length() < 1e-7); + + // Rotate by 45 degrees, forming an axis-aligned square + let mut rotated_vertices = polygon.vertices(std::f32::consts::FRAC_PI_4).into_iter(); + + // Distance from the origin to the middle of a side, derived using Pythagorean theorem + let side_sistance = std::f32::consts::FRAC_1_SQRT_2; + assert!( + (rotated_vertices.next().unwrap() - Vec2::new(-side_sistance, side_sistance)).length() + < 1e-7, + ); + } }