diff --git a/.idea/rg3d-core.iml b/.idea/rg3d-core.iml index 4abb88750..9962f94a1 100644 --- a/.idea/rg3d-core.iml +++ b/.idea/rg3d-core.iml @@ -34,6 +34,7 @@ + diff --git a/Cargo.toml b/Cargo.toml index cc22829a5..b3ed45794 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -28,7 +28,7 @@ members = [ "editor-standalone", "template", "fyrox-graph" -] +, "fyrox-math"] [profile.dev] opt-level = 0 diff --git a/editor/src/animation/command/mod.rs b/editor/src/animation/command/mod.rs index 1c742afc8..d44e6a248 100644 --- a/editor/src/animation/command/mod.rs +++ b/editor/src/animation/command/mod.rs @@ -6,8 +6,8 @@ use crate::{ }; use fyrox::{ core::{ - curve::Curve, log::Log, + math::curve::Curve, pool::{ErasedHandle, Handle, Ticket}, uuid::Uuid, variable::InheritableVariable, diff --git a/editor/src/curve_editor.rs b/editor/src/curve_editor.rs index 042bfdae8..df1d1b0c8 100644 --- a/editor/src/curve_editor.rs +++ b/editor/src/curve_editor.rs @@ -7,7 +7,7 @@ use crate::{ use fyrox::{ asset::{untyped::ResourceKind, Resource}, core::{ - color::Color, curve::Curve, futures::executor::block_on, pool::Handle, + color::Color, futures::executor::block_on, math::curve::Curve, pool::Handle, type_traits::prelude::*, visitor::prelude::*, visitor::Visitor, }, engine::Engine, diff --git a/fyrox-animation/src/container.rs b/fyrox-animation/src/container.rs index 060173969..1c6d0096c 100644 --- a/fyrox-animation/src/container.rs +++ b/fyrox-animation/src/container.rs @@ -4,7 +4,7 @@ use crate::{ core::{ algebra::{Vector2, Vector3, Vector4}, - curve::Curve, + math::curve::Curve, math::{quat_from_euler, RotationOrder}, reflect::prelude::*, visitor::prelude::*, diff --git a/fyrox-animation/src/lib.rs b/fyrox-animation/src/lib.rs index ee05fd69b..5d7eb9dea 100644 --- a/fyrox-animation/src/lib.rs +++ b/fyrox-animation/src/lib.rs @@ -121,7 +121,7 @@ pub mod value; /// value::ValueBinding, /// Animation, /// core::{ -/// curve::{Curve, CurveKey, CurveKeyKind}, +/// math::curve::{Curve, CurveKey, CurveKeyKind}, /// pool::Handle, /// }, /// }; diff --git a/fyrox-core-derive/src/lib.rs b/fyrox-core-derive/src/lib.rs index bc1f5d02e..3237dc8f3 100644 --- a/fyrox-core-derive/src/lib.rs +++ b/fyrox-core-derive/src/lib.rs @@ -46,6 +46,12 @@ pub fn impl_reflect(input: TokenStream) -> TokenStream { TokenStream::from(reflect_impl) } +#[proc_macro] +pub fn impl_visit(input: TokenStream) -> TokenStream { + let ast = parse_macro_input!(input as DeriveInput); + TokenStream::from(visit::impl_visit(ast)) +} + /// Implements `TypeUuidProvider` trait /// /// User has to import `TypeUuidProvider` trait to use this macro. diff --git a/fyrox-core/Cargo.toml b/fyrox-core/Cargo.toml index e955989f7..c53a33439 100644 --- a/fyrox-core/Cargo.toml +++ b/fyrox-core/Cargo.toml @@ -17,7 +17,7 @@ rust-version = "1.72" [dependencies] fyrox-core-derive = { path = "../fyrox-core-derive", version = "0.22.0" } - +fyrox-math = { path = "../fyrox-math", version = "0.1.0" } base64 = "0.21.0" byteorder = "1.4.3" rand = "0.8.4" @@ -36,7 +36,6 @@ once_cell = "1.17.1" notify = "6" serde = { version = "1", features = ["derive"] } bincode = "1.3.3" -rectutils = "0.1.0" [target.'cfg(target_arch = "wasm32")'.dependencies] web-sys = { version = "0.3.53", features = ["Request", "Window", "Response", "AudioContext", "AudioBuffer", "AudioContextOptions", "AudioNode", "AudioBufferSourceNode", "AudioDestinationNode"] } diff --git a/fyrox-core/src/lib.rs b/fyrox-core/src/lib.rs index d8ea93ae9..3510dbd53 100644 --- a/fyrox-core/src/lib.rs +++ b/fyrox-core/src/lib.rs @@ -31,13 +31,11 @@ use std::{ pub mod color; pub mod color_gradient; -pub mod curve; pub mod io; pub mod log; pub mod math; pub mod net; pub mod numeric_range; -pub mod octree; pub mod pool; pub mod profiler; pub mod quadtree; diff --git a/fyrox-core/src/math/mod.rs b/fyrox-core/src/math/mod.rs index 3e7510caf..e82b63a9c 100644 --- a/fyrox-core/src/math/mod.rs +++ b/fyrox-core/src/math/mod.rs @@ -1,29 +1,21 @@ // Clippy complains about normal mathematical symbols like A, B, C for quadratic equation. #![allow(clippy::many_single_char_names)] -pub mod aabb; -pub mod frustum; -pub mod plane; -pub mod ray; -pub mod triangulator; +pub use fyrox_math::*; +use crate::math::curve::Curve; +use crate::math::curve::CurveKey; +use crate::math::curve::CurveKeyKind; +use crate::Uuid; use crate::{ - algebra::{ - Matrix3, Matrix4, RealField, Scalar, SimdRealField, UnitQuaternion, Vector2, Vector3, - }, - math::ray::IntersectionResult, + algebra::Scalar, + math::{aabb::AxisAlignedBoundingBox, frustum::Frustum, plane::Plane}, num_traits::NumAssign, reflect::prelude::*, visitor::prelude::*, }; -use fyrox_core_derive::impl_reflect; -use std::{ - fmt::Debug, - hash::{Hash, Hasher}, - ops::{Index, IndexMut}, -}; - -pub use rectutils::Rect; +use fyrox_core_derive::{impl_reflect, impl_visit}; +use std::fmt::Debug; impl_reflect!( pub struct Rect {} @@ -45,443 +37,6 @@ where } } -#[derive(Copy, Clone)] -pub enum PlaneClass { - XY, - YZ, - XZ, -} - -#[inline] -#[allow(clippy::useless_let_if_seq)] -pub fn classify_plane(normal: Vector3) -> PlaneClass { - let mut longest = 0.0f32; - let mut class = PlaneClass::XY; - - if normal.x.abs() > longest { - longest = normal.x.abs(); - class = PlaneClass::YZ; - } - - if normal.y.abs() > longest { - longest = normal.y.abs(); - class = PlaneClass::XZ; - } - - if normal.z.abs() > longest { - class = PlaneClass::XY; - } - - class -} - -#[inline] -pub fn get_polygon_normal(polygon: &[Vector3]) -> Result, &'static str> { - let mut normal = Vector3::default(); - - for (i, current) in polygon.iter().enumerate() { - let next = polygon[(i + 1) % polygon.len()]; - normal.x += (current.y - next.y) * (current.z + next.z); - normal.y += (current.z - next.z) * (current.x + next.x); - normal.z += (current.x - next.x) * (current.y + next.y); - } - - normal - .try_normalize(f32::EPSILON) - .ok_or("Unable to get normal of degenerated polygon!") -} - -#[inline] -pub fn get_signed_triangle_area(v1: Vector2, v2: Vector2, v3: Vector2) -> f32 { - 0.5 * (v1.x * (v3.y - v2.y) + v2.x * (v1.y - v3.y) + v3.x * (v2.y - v1.y)) -} - -#[inline] -pub fn vec3_to_vec2_by_plane( - plane_class: PlaneClass, - normal: Vector3, - point: Vector3, -) -> Vector2 { - match plane_class { - PlaneClass::XY => { - if normal.z < 0.0 { - Vector2::new(point.y, point.x) - } else { - Vector2::new(point.x, point.y) - } - } - PlaneClass::XZ => { - if normal.y < 0.0 { - Vector2::new(point.x, point.z) - } else { - Vector2::new(point.z, point.x) - } - } - PlaneClass::YZ => { - if normal.x < 0.0 { - Vector2::new(point.z, point.y) - } else { - Vector2::new(point.y, point.z) - } - } - } -} - -#[inline] -pub fn is_point_inside_2d_triangle( - point: Vector2, - pt_a: Vector2, - pt_b: Vector2, - pt_c: Vector2, -) -> bool { - let ba = pt_b - pt_a; - let ca = pt_c - pt_a; - - let vp = point - pt_a; - - let ba_dot_ba = ba.dot(&ba); - let ca_dot_ba = ca.dot(&ba); - let ca_dot_ca = ca.dot(&ca); - - let dot_02 = ca.dot(&vp); - let dot_12 = ba.dot(&vp); - - let inv_denom = 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba.powi(2)); - - // calculate barycentric coordinates - let u = (ba_dot_ba * dot_02 - ca_dot_ba * dot_12) * inv_denom; - let v = (ca_dot_ca * dot_12 - ca_dot_ba * dot_02) * inv_denom; - - (u >= 0.0) && (v >= 0.0) && (u + v < 1.0) -} - -#[inline] -pub fn wrap_angle(angle: f32) -> f32 { - let two_pi = 2.0 * std::f32::consts::PI; - - if angle > 0.0 { - angle % two_pi - } else { - (angle + two_pi) % two_pi - } -} - -/// There are two versions of remainder, the standard `%` operator which does `x - (x/y).trunc()*y` and IEEE remainder which does `x - (x/y).round()*y`. -#[inline] -pub fn ieee_remainder(x: f32, y: f32) -> f32 { - x - (x / y).round() * y -} - -#[inline] -pub fn round_to_step(x: f32, step: f32) -> f32 { - x - ieee_remainder(x, step) -} - -#[inline] -pub fn wrapf(mut n: f32, mut min_limit: f32, mut max_limit: f32) -> f32 { - if n >= min_limit && n <= max_limit { - return n; - } - - if max_limit == 0.0 && min_limit == 0.0 { - return 0.0; - } - - max_limit -= min_limit; - - let offset = min_limit; - min_limit = 0.0; - n -= offset; - - let num_of_max = (n / max_limit).abs().floor(); - - if n >= max_limit { - n -= num_of_max * max_limit; - } else if n < min_limit { - n += (num_of_max + 1.0) * max_limit; - } - - n + offset -} - -#[inline(always)] -pub fn lerpf(a: f32, b: f32, t: f32) -> f32 { - a + (b - a) * t -} - -// https://en.wikipedia.org/wiki/Cubic_Hermite_spline -#[inline] -pub fn cubicf(p0: f32, p1: f32, t: f32, m0: f32, m1: f32) -> f32 { - let t2 = t * t; - let t3 = t2 * t; - let scale = (p1 - p0).abs(); - - (2.0 * t3 - 3.0 * t2 + 1.0) * p0 - + (t3 - 2.0 * t2 + t) * m0 * scale - + (-2.0 * t3 + 3.0 * t2) * p1 - + (t3 - t2) * m1 * scale -} - -#[inline] -pub fn cubicf_derivative(p0: f32, p1: f32, t: f32, m0: f32, m1: f32) -> f32 { - let t2 = t * t; - let scale = (p1 - p0).abs(); - - (6.0 * t2 - 6.0 * t) * p0 - + (3.0 * t2 - 4.0 * t + 1.0) * m0 * scale - + (6.0 * t - 6.0 * t2) * p1 - + (3.0 * t2 - 2.0 * t) * m1 * scale -} - -#[inline] -pub fn inf_sup_cubicf(p0: f32, p1: f32, m0: f32, m1: f32) -> (f32, f32) { - // Find two `t`s where derivative of cubicf is zero - these will be - // extreme points of the spline. Then get the values at those `t`s - let d = -(9.0 * p0 * p0 + 6.0 * p0 * (-3.0 * p1 + m1 + m0) + 9.0 * p1 * p1 - - 6.0 * p1 * (m1 + m0) - + m1 * m1 - + m1 * m0 - + m0 * m0) - .sqrt(); - let k = 3.0 * (2.0 * p0 - 2.0 * p1 + m1 + m0); - let v = 3.0 * p0 - 3.0 * p1 + m1 + 2.0 * m0; - let t0 = (-d + v) / k; - let t1 = (d + v) / k; - (cubicf(p0, p1, t0, m0, m1), cubicf(p0, p1, t1, m0, m1)) -} - -#[inline] -pub fn get_farthest_point(points: &[Vector3], dir: Vector3) -> Vector3 { - let mut n_farthest = 0; - let mut max_dot = -f32::MAX; - for (i, point) in points.iter().enumerate() { - let dot = dir.dot(point); - if dot > max_dot { - n_farthest = i; - max_dot = dot - } - } - points[n_farthest] -} - -#[inline] -pub fn get_barycentric_coords( - p: &Vector3, - a: &Vector3, - b: &Vector3, - c: &Vector3, -) -> (f32, f32, f32) { - let v0 = *b - *a; - let v1 = *c - *a; - let v2 = *p - *a; - - let d00 = v0.dot(&v0); - let d01 = v0.dot(&v1); - let d11 = v1.dot(&v1); - let d20 = v2.dot(&v0); - let d21 = v2.dot(&v1); - let denom = d00 * d11 - d01.powi(2); - - let v = (d11 * d20 - d01 * d21) / denom; - let w = (d00 * d21 - d01 * d20) / denom; - let u = 1.0 - v - w; - - (u, v, w) -} - -#[inline] -pub fn get_barycentric_coords_2d( - p: Vector2, - a: Vector2, - b: Vector2, - c: Vector2, -) -> (f32, f32, f32) { - let v0 = b - a; - let v1 = c - a; - let v2 = p - a; - - let d00 = v0.dot(&v0); - let d01 = v0.dot(&v1); - let d11 = v1.dot(&v1); - let d20 = v2.dot(&v0); - let d21 = v2.dot(&v1); - let inv_denom = 1.0 / (d00 * d11 - d01.powi(2)); - - let v = (d11 * d20 - d01 * d21) * inv_denom; - let w = (d00 * d21 - d01 * d20) * inv_denom; - let u = 1.0 - v - w; - - (u, v, w) -} - -#[inline] -pub fn barycentric_to_world( - bary: (f32, f32, f32), - pa: Vector3, - pb: Vector3, - pc: Vector3, -) -> Vector3 { - pa.scale(bary.0) + pb.scale(bary.1) + pc.scale(bary.2) -} - -#[inline] -pub fn barycentric_is_inside(bary: (f32, f32, f32)) -> bool { - (bary.0 >= 0.0) && (bary.1 >= 0.0) && (bary.0 + bary.1 < 1.0) -} - -#[inline] -pub fn is_point_inside_triangle(p: &Vector3, vertices: &[Vector3; 3]) -> bool { - let ba = vertices[1] - vertices[0]; - let ca = vertices[2] - vertices[0]; - let vp = *p - vertices[0]; - - let ba_dot_ba = ba.dot(&ba); - let ca_dot_ba = ca.dot(&ba); - let ca_dot_ca = ca.dot(&ca); - - let dot02 = ca.dot(&vp); - let dot12 = ba.dot(&vp); - - let inv_denom = 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba.powi(2)); - - // Calculate barycentric coordinates - let u = (ba_dot_ba * dot02 - ca_dot_ba * dot12) * inv_denom; - let v = (ca_dot_ca * dot12 - ca_dot_ba * dot02) * inv_denom; - - (u >= 0.0) && (v >= 0.0) && (u + v < 1.0) -} - -#[inline] -pub fn triangle_area(a: Vector3, b: Vector3, c: Vector3) -> f32 { - (b - a).cross(&(c - a)).norm() * 0.5 -} - -#[inline] -pub fn solve_quadratic(a: f32, b: f32, c: f32) -> Option<[f32; 2]> { - let discriminant = b * b - 4.0 * a * c; - if discriminant < 0.0 { - // No real roots - None - } else { - // Dont care if quadratic equation has only one root (discriminant == 0), this is edge-case - // which requires additional branching instructions which is not good for branch-predictor in CPU. - let _2a = 2.0 * a; - let discr_root = discriminant.sqrt(); - let r1 = (-b + discr_root) / _2a; - let r2 = (-b - discr_root) / _2a; - Some([r1, r2]) - } -} - -#[inline] -pub fn spherical_to_cartesian(azimuth: f32, elevation: f32, radius: f32) -> Vector3 { - let x = radius * elevation.sin() * azimuth.sin(); - let y = radius * elevation.cos(); - let z = -radius * elevation.sin() * azimuth.cos(); - Vector3::new(x, y, z) -} - -#[inline] -pub fn ray_rect_intersection( - rect: Rect, - origin: Vector2, - dir: Vector2, -) -> Option { - let min = rect.left_top_corner(); - let max = rect.right_bottom_corner(); - - let (mut tmin, mut tmax) = if dir.x >= 0.0 { - ((min.x - origin.x) / dir.x, (max.x - origin.x) / dir.x) - } else { - ((max.x - origin.x) / dir.x, (min.x - origin.x) / dir.x) - }; - - let (tymin, tymax) = if dir.y >= 0.0 { - ((min.y - origin.y) / dir.y, (max.y - origin.y) / dir.y) - } else { - ((max.y - origin.y) / dir.y, (min.y - origin.y) / dir.y) - }; - - if tmin > tymax || tymin > tmax { - return None; - } - if tymin > tmin { - tmin = tymin; - } - if tymax < tmax { - tmax = tymax; - } - if tmin <= 1.0 && tmax >= 0.0 { - Some(IntersectionResult { - min: tmin, - max: tmax, - }) - } else { - None - } -} - -#[derive(Clone, Copy, Debug, Default)] -#[repr(C)] -pub struct TriangleEdge { - pub a: u32, - pub b: u32, -} - -impl PartialEq for TriangleEdge { - fn eq(&self, other: &Self) -> bool { - self.a == other.a && self.b == other.b || self.a == other.b && self.b == other.a - } -} - -impl Eq for TriangleEdge {} - -impl Hash for TriangleEdge { - fn hash(&self, state: &mut H) { - // Direction-agnostic hash. - (self.a as u64 + self.b as u64).hash(state) - } -} - -#[derive(Clone, Copy, PartialEq, Eq, Debug, Default, Hash, Reflect)] -#[repr(C)] -pub struct TriangleDefinition(pub [u32; 3]); - -impl TriangleDefinition { - #[inline] - pub fn indices(&self) -> &[u32] { - self.as_ref() - } - - #[inline] - pub fn indices_mut(&mut self) -> &mut [u32] { - self.as_mut() - } - - #[inline] - pub fn edges(&self) -> [TriangleEdge; 3] { - [ - TriangleEdge { - a: self.0[0], - b: self.0[1], - }, - TriangleEdge { - a: self.0[1], - b: self.0[2], - }, - TriangleEdge { - a: self.0[2], - b: self.0[0], - }, - ] - } - - #[inline] - pub fn add(&self, i: u32) -> Self { - Self([self.0[0] + i, self.0[1] + i, self.0[2] + i]) - } -} - impl Visit for TriangleDefinition { fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult { let mut region = visitor.enter_region(name)?; @@ -494,1086 +49,109 @@ impl Visit for TriangleDefinition { } } -impl Index for TriangleDefinition { - type Output = u32; +impl Visit for AxisAlignedBoundingBox { + fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult { + let mut region = visitor.enter_region(name)?; - #[inline] - fn index(&self, index: usize) -> &Self::Output { - &self.0[index] + self.min.visit("Min", &mut region)?; + self.max.visit("Max", &mut region)?; + + Ok(()) } } -impl IndexMut for TriangleDefinition { - #[inline] - fn index_mut(&mut self, index: usize) -> &mut Self::Output { - &mut self.0[index] +impl Visit for Frustum { + fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult { + let mut region = visitor.enter_region(name)?; + + self.planes[0].visit("Left", &mut region)?; + self.planes[1].visit("Right", &mut region)?; + self.planes[2].visit("Top", &mut region)?; + self.planes[3].visit("Bottom", &mut region)?; + self.planes[4].visit("Far", &mut region)?; + self.planes[5].visit("Near", &mut region)?; + + Ok(()) } } -pub trait PositionProvider: Sized { - fn position(&self) -> Vector3; -} +impl Visit for Plane { + fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult { + let mut region = visitor.enter_region(name)?; -impl PositionProvider for Vector3 { - #[inline] - fn position(&self) -> Vector3 { - *self + self.normal.visit("Normal", &mut region)?; + self.d.visit("D", &mut region)?; + + Ok(()) } } -impl AsRef<[u32]> for TriangleDefinition { - #[inline] - fn as_ref(&self) -> &[u32] { - &self.0 - } -} - -impl AsMut<[u32]> for TriangleDefinition { - #[inline] - fn as_mut(&mut self) -> &mut [u32] { - &mut self.0 - } -} - -/// Tries to find a point closest to given point. -/// -/// # Notes -/// -/// O(n) complexity. -#[inline] -pub fn get_closest_point(points: &[P], point: Vector3) -> Option { - let mut closest_sqr_distance = f32::MAX; - let mut closest_index = None; - for (i, vertex) in points.iter().enumerate() { - let sqr_distance = (vertex.position() - point).norm_squared(); - if sqr_distance < closest_sqr_distance { - closest_sqr_distance = sqr_distance; - closest_index = Some(i); - } - } - closest_index -} - -/// Returns a tuple of (point index; triangle index) closest to the given point. -#[inline] -pub fn get_closest_point_triangles

( - points: &[P], - triangles: &[TriangleDefinition], - triangle_indices: impl Iterator, - point: Vector3, -) -> Option<(usize, usize)> -where - P: PositionProvider, -{ - let mut closest_sqr_distance = f32::MAX; - let mut closest_index = None; - for triangle_index in triangle_indices { - let triangle = triangles.get(triangle_index).unwrap(); - for point_index in triangle.0.iter() { - let vertex = points.get(*point_index as usize).unwrap(); - let sqr_distance = (vertex.position() - point).norm_squared(); - if sqr_distance < closest_sqr_distance { - closest_sqr_distance = sqr_distance; - closest_index = Some((*point_index as usize, triangle_index)); - } - } - } - closest_index -} - -/// Returns a tuple of (point index; triangle index) closest to the given point. -#[inline] -pub fn get_closest_point_triangle_set

( - points: &[P], - triangles: &[TriangleDefinition], - point: Vector3, -) -> Option<(usize, usize)> -where - P: PositionProvider, -{ - let mut closest_sqr_distance = f32::MAX; - let mut closest_index = None; - for (triangle_index, triangle) in triangles.iter().enumerate() { - for point_index in triangle.0.iter() { - let vertex = points.get(*point_index as usize).unwrap(); - let sqr_distance = (vertex.position() - point).norm_squared(); - if sqr_distance < closest_sqr_distance { - closest_sqr_distance = sqr_distance; - closest_index = Some((*point_index as usize, triangle_index)); - } - } - } - closest_index -} - -#[derive(Debug, PartialEq, Visit, Reflect, Clone)] -pub struct SmoothAngle { - /// Current angle in radians. - pub angle: f32, - - /// Target angle in radians. - pub target: f32, - - /// Turn speed in radians per second (rad/s) - pub speed: f32, -} - -impl SmoothAngle { - #[inline] - pub fn set_target(&mut self, angle: f32) -> &mut Self { - self.target = angle; - self - } - - #[inline] - pub fn update(&mut self, dt: f32) -> &mut Self { - self.target = wrap_angle(self.target); - self.angle = wrap_angle(self.angle); - if !self.at_target() { - let delta = self.speed * dt; - if self.distance().abs() > delta { - self.angle += self.turn_direction() * delta; - } else { - self.angle = self.target; - } - } - self - } - - #[inline] - pub fn set_speed(&mut self, speed: f32) -> &mut Self { - self.speed = speed; - self - } - - #[inline] - pub fn set_angle(&mut self, angle: f32) -> &mut Self { - self.angle = angle; - self - } - - #[inline] - pub fn angle(&self) -> f32 { - self.angle - } - - #[inline] - pub fn at_target(&self) -> bool { - (self.target - self.angle).abs() <= f32::EPSILON - } - - #[inline] - pub fn distance(&self) -> f32 { - let diff = (self.target - self.angle + std::f32::consts::PI) % std::f32::consts::TAU - - std::f32::consts::PI; - if diff < -std::f32::consts::PI { - diff + std::f32::consts::TAU - } else { - diff - } - } - - #[inline] - fn turn_direction(&self) -> f32 { - let distance = self.distance(); - - if distance < 0.0 { - if distance < -std::f32::consts::PI { - 1.0 - } else { - -1.0 - } - } else if distance > std::f32::consts::PI { - -1.0 - } else { - 1.0 - } - } -} - -impl Default for SmoothAngle { - fn default() -> Self { - Self { - angle: 0.0, - target: 0.0, - speed: 1.0, - } - } -} - -#[derive(Copy, Clone, Hash, PartialOrd, PartialEq, Ord, Eq)] -pub enum RotationOrder { - XYZ, - XZY, - YZX, - YXZ, - ZXY, - ZYX, -} - -#[inline] -pub fn quat_from_euler( - euler_radians: Vector3, - order: RotationOrder, -) -> UnitQuaternion { - let qx = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), euler_radians.x); - let qy = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), euler_radians.y); - let qz = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), euler_radians.z); - match order { - RotationOrder::XYZ => qz * qy * qx, - RotationOrder::XZY => qy * qz * qx, - RotationOrder::YZX => qx * qz * qy, - RotationOrder::YXZ => qz * qx * qy, - RotationOrder::ZXY => qy * qx * qz, - RotationOrder::ZYX => qx * qy * qz, - } -} - -pub trait Matrix4Ext { - fn side(&self) -> Vector3; - fn up(&self) -> Vector3; - fn look(&self) -> Vector3; - fn position(&self) -> Vector3; - fn basis(&self) -> Matrix3; -} - -impl Matrix4Ext for Matrix4 { - #[inline] - fn side(&self) -> Vector3 { - Vector3::new(self[0], self[1], self[2]) - } - - #[inline] - fn up(&self) -> Vector3 { - Vector3::new(self[4], self[5], self[6]) - } - - #[inline] - fn look(&self) -> Vector3 { - Vector3::new(self[8], self[9], self[10]) - } - - #[inline] - fn position(&self) -> Vector3 { - Vector3::new(self[12], self[13], self[14]) - } - - #[inline] - fn basis(&self) -> Matrix3 { - self.fixed_resize::<3, 3>(T::default()) - } -} - -pub trait Matrix3Ext { - fn side(&self) -> Vector3; - fn up(&self) -> Vector3; - fn look(&self) -> Vector3; -} - -impl Matrix3Ext for Matrix3 { - #[inline] - fn side(&self) -> Vector3 { - Vector3::new(self[0], self[1], self[2]) - } - - #[inline] - fn up(&self) -> Vector3 { - Vector3::new(self[3], self[4], self[5]) - } - - #[inline] - fn look(&self) -> Vector3 { - Vector3::new(self[6], self[7], self[8]) - } -} - -pub trait Vector3Ext { - fn follow(&mut self, other: &Self, fraction: f32); - - fn sqr_distance(&self, other: &Self) -> f32; - - fn non_uniform_scale(&self, other: &Self) -> Self; -} - -impl Vector3Ext for Vector3 { - #[inline] - fn follow(&mut self, other: &Self, fraction: f32) { - self.x += (other.x - self.x) * fraction; - self.y += (other.y - self.y) * fraction; - self.z += (other.z - self.z) * fraction; - } - - #[inline] - fn sqr_distance(&self, other: &Self) -> f32 { - (self - other).norm_squared() - } - - #[inline] - fn non_uniform_scale(&self, other: &Self) -> Self { - Self::new(self.x * other.x, self.y * other.y, self.z * other.z) - } -} - -pub trait Vector2Ext { - fn follow(&mut self, other: &Self, fraction: f32); - - fn per_component_min(&self, other: &Self) -> Self; - fn per_component_max(&self, other: &Self) -> Self; -} - -impl Vector2Ext for Vector2 { - #[inline] - fn follow(&mut self, other: &Self, fraction: f32) { - self.x += (other.x - self.x) * fraction; - self.y += (other.y - self.y) * fraction; - } - - #[inline] - fn per_component_min(&self, other: &Self) -> Self { - Self::new(self.x.min(other.x), self.y.min(other.y)) - } - - #[inline] - fn per_component_max(&self, other: &Self) -> Self { - Self::new(self.x.max(other.x), self.y.max(other.y)) - } -} - -/// Returns rotation quaternion that represents rotation basis with Z axis aligned on `vec`. -/// This function handles singularities for you. -#[inline] -pub fn vector_to_quat(vec: Vector3) -> UnitQuaternion { - let dot = vec.normalize().dot(&Vector3::y()); - - if dot.abs() > 1.0 - 10.0 * f32::EPSILON { - // Handle singularity when vector is collinear with Y axis. - UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -dot.signum() * 90.0f32.to_radians()) - } else { - UnitQuaternion::face_towards(&vec, &Vector3::y()) - } -} - -#[inline] -pub fn m4x4_approx_eq(a: &Matrix4, b: &Matrix4) -> bool { - a.iter() - .zip(b.iter()) - .all(|(a, b)| (*a - *b).abs() <= 0.001) -} - -#[cfg(test)] -mod test { - use nalgebra::{Matrix3, Matrix4, UnitQuaternion, Vector3}; - use num_traits::Zero; - - use super::{ - barycentric_is_inside, barycentric_to_world, cubicf_derivative, get_barycentric_coords, - get_barycentric_coords_2d, get_closest_point, get_closest_point_triangle_set, - get_closest_point_triangles, get_farthest_point, get_signed_triangle_area, ieee_remainder, - inf_sup_cubicf, m4x4_approx_eq, quat_from_euler, round_to_step, spherical_to_cartesian, - triangle_area, wrap_angle, wrapf, Matrix3Ext, Matrix4Ext, PositionProvider, Rect, - RotationOrder, SmoothAngle, TriangleDefinition, TriangleEdge, Vector2Ext, Vector3Ext, - }; - use crate::algebra::Vector2; - - use crate::visitor::{Visit, Visitor}; - - #[test] - fn ray_rect_intersection() { - let rect = Rect::new(0.0, 0.0, 10.0, 10.0); - - // Edge-case: Horizontal ray. - assert!(super::ray_rect_intersection( - rect, - Vector2::new(-1.0, 5.0), - Vector2::new(1.0, 0.0) - ) - .is_some()); - - // Edge-case: Vertical ray. - assert!(super::ray_rect_intersection( - rect, - Vector2::new(5.0, -1.0), - Vector2::new(0.0, 1.0) - ) - .is_some()); - } - - #[test] - fn smooth_angle() { - let mut angle = SmoothAngle { - angle: 290.0f32.to_radians(), - target: 90.0f32.to_radians(), - speed: 100.0f32.to_radians(), - }; - - while !angle.at_target() { - println!("{}", angle.update(1.0).angle().to_degrees()); - } - } - - #[test] - fn default_for_rect() { - assert_eq!( - Rect::::default(), - Rect { - position: Vector2::new(Zero::zero(), Zero::zero()), - size: Vector2::new(Zero::zero(), Zero::zero()), - } - ); - } - - #[test] - fn rect_with_position() { - let rect = Rect::new(0, 0, 1, 1); - - assert_eq!( - rect.with_position(Vector2::new(1, 1)), - Rect::new(1, 1, 1, 1) - ); - } - - #[test] - fn rect_with_size() { - let rect = Rect::new(0, 0, 1, 1); - - assert_eq!( - rect.with_size(Vector2::new(10, 10)), - Rect::new(0, 0, 10, 10) - ); - } - - #[test] - fn rect_inflate() { - let rect = Rect::new(0, 0, 1, 1); - - assert_eq!(rect.inflate(5, 5), Rect::new(-5, -5, 11, 11)); - } - - #[test] - fn rect_deflate() { - let rect = Rect::new(-5, -5, 11, 11); - - assert_eq!(rect.deflate(5, 5), Rect::new(0, 0, 1, 1)); - } - - #[test] - fn rect_contains() { - let rect = Rect::new(0, 0, 10, 10); - - assert!(rect.contains(Vector2::new(0, 0))); - assert!(rect.contains(Vector2::new(0, 10))); - assert!(rect.contains(Vector2::new(10, 0))); - assert!(rect.contains(Vector2::new(10, 10))); - assert!(rect.contains(Vector2::new(5, 5))); - - assert!(!rect.contains(Vector2::new(0, 20))); - } - - #[test] - fn rect_center() { - let rect = Rect::new(0, 0, 10, 10); - - assert_eq!(rect.center(), Vector2::new(5, 5)); - } - - #[test] - fn rect_push() { - let mut rect = Rect::new(10, 10, 11, 11); - - rect.push(Vector2::new(0, 0)); - assert_eq!(rect, Rect::new(0, 0, 11, 11)); - - rect.push(Vector2::new(0, 20)); - assert_eq!(rect, Rect::new(0, 0, 11, 20)); - - rect.push(Vector2::new(20, 20)); - assert_eq!(rect, Rect::new(0, 0, 20, 20)); - - rect.push(Vector2::new(30, 30)); - assert_eq!(rect, Rect::new(0, 0, 30, 30)); - } - - #[test] - fn rect_getters() { - let rect = Rect::new(0, 0, 1, 1); - - assert_eq!(rect.left_top_corner(), Vector2::new(0, 0)); - assert_eq!(rect.left_bottom_corner(), Vector2::new(0, 1)); - assert_eq!(rect.right_top_corner(), Vector2::new(1, 0)); - assert_eq!(rect.right_bottom_corner(), Vector2::new(1, 1)); - - assert_eq!(rect.x(), 0); - assert_eq!(rect.y(), 0); - assert_eq!(rect.w(), 1); - assert_eq!(rect.h(), 1); - } - - #[test] - fn rect_clip_by() { - let rect = Rect::new(0, 0, 10, 10); - - assert_eq!(rect.clip_by(Rect::new(2, 2, 1, 1)), Rect::new(2, 2, 1, 1)); - assert_eq!( - rect.clip_by(Rect::new(0, 0, 15, 15)), - Rect::new(0, 0, 10, 10) - ); - - // When there is no intersection. - assert_eq!( - rect.clip_by(Rect::new(-2, 1, 1, 1)), - Rect::new(0, 0, 10, 10) - ); - assert_eq!( - rect.clip_by(Rect::new(11, 1, 1, 1)), - Rect::new(0, 0, 10, 10) - ); - assert_eq!( - rect.clip_by(Rect::new(1, -2, 1, 1)), - Rect::new(0, 0, 10, 10) - ); - assert_eq!( - rect.clip_by(Rect::new(1, 11, 1, 1)), - Rect::new(0, 0, 10, 10) - ); - } - - #[test] - fn rect_translate() { - let rect = Rect::new(0, 0, 10, 10); - - assert_eq!(rect.translate(Vector2::new(5, 5)), Rect::new(5, 5, 10, 10)); - } - - #[test] - fn rect_intersects_circle() { - let rect = Rect::new(0.0, 0.0, 1.0, 1.0); - - assert!(!rect.intersects_circle(Vector2::new(5.0, 5.0), 1.0)); - assert!(rect.intersects_circle(Vector2::new(0.0, 0.0), 1.0)); - assert!(rect.intersects_circle(Vector2::new(-0.5, -0.5), 1.0)); - } - - #[test] - fn rect_extend_to_contain() { - let mut rect = Rect::new(0.0, 0.0, 1.0, 1.0); - - rect.extend_to_contain(Rect::new(1.0, 1.0, 1.0, 1.0)); - assert_eq!(rect, Rect::new(0.0, 0.0, 2.0, 2.0)); - - rect.extend_to_contain(Rect::new(-1.0, -1.0, 1.0, 1.0)); - assert_eq!(rect, Rect::new(-1.0, -1.0, 2.0, 2.0)); - } - - #[test] - fn rect_transform() { - let rect = Rect::new(0.0, 0.0, 1.0, 1.0); - - assert_eq!( - rect.transform(&Matrix3::new( - 1.0, 0.0, 0.0, // - 0.0, 1.0, 0.0, // - 0.0, 0.0, 1.0, - )), - rect, - ); - - assert_eq!( - rect.transform(&Matrix3::new( - 2.0, 0.0, 0.0, // - 0.0, 2.0, 0.0, // - 0.0, 0.0, 2.0, - )), - Rect::new(0.0, 0.0, 2.0, 2.0), - ); - } - - #[test] - fn visit_for_rect() { - let mut rect = Rect::new(0.0, 0.0, 1.0, 1.0); - let mut visitor = Visitor::default(); - - assert!(rect.visit("name", &mut visitor).is_ok()); - } - - #[test] - fn test_get_signed_triangle_area() { - assert_eq!( - get_signed_triangle_area( - Vector2::new(0.0, 0.0), - Vector2::new(0.0, 1.0), - Vector2::new(1.0, 0.0) - ), - 0.5 - ); - assert_eq!( - get_signed_triangle_area( - Vector2::new(1.0, 1.0), - Vector2::new(0.0, 1.0), - Vector2::new(1.0, 0.0) - ), - -0.5 - ); - } - - #[test] - fn test_wrap_angle() { - let angle = 0.5 * std::f32::consts::PI; - assert_eq!(wrap_angle(angle), angle); - assert_eq!(wrap_angle(-angle), 3.0 * angle); - } - - #[test] - fn test_ieee_remainder() { - assert_eq!(ieee_remainder(1.0, 2.0), -1.0); - assert_eq!(ieee_remainder(3.0, 2.0), -1.0); - - assert_eq!(ieee_remainder(1.0, 3.0), 1.0); - assert_eq!(ieee_remainder(4.0, 3.0), 1.0); - - assert_eq!(ieee_remainder(-1.0, 2.0), 1.0); - assert_eq!(ieee_remainder(-3.0, 2.0), 1.0); - } - - #[test] - fn test_round_to_step() { - assert_eq!(round_to_step(1.0, 2.0), 2.0); - assert_eq!(round_to_step(3.0, 2.0), 4.0); - - assert_eq!(round_to_step(-1.0, 2.0), -2.0); - assert_eq!(round_to_step(-3.0, 2.0), -4.0); - } - - #[test] - fn test_wrapf() { - assert_eq!(wrapf(5.0, 0.0, 10.0), 5.0); - assert_eq!(wrapf(5.0, 0.0, 0.0), 0.0); - assert_eq!(wrapf(2.0, 5.0, 10.0), 7.0); - assert_eq!(wrapf(12.0, 5.0, 10.0), 7.0); - } - - #[test] - fn test_cubicf_derivative() { - assert_eq!(cubicf_derivative(1.0, 1.0, 1.0, 1.0, 1.0), 0.0); - assert_eq!(cubicf_derivative(2.0, 1.0, 1.0, 1.0, 1.0), 1.0); - } - - #[test] - fn test_inf_sup_cubicf() { - assert_eq!(inf_sup_cubicf(1.0, 1.0, 1.0, 1.0), (1.0, 1.0)); - assert_eq!(inf_sup_cubicf(2.0, 2.0, 1.0, 1.0), (2.0, 2.0)); - } - - #[test] - fn test_get_farthest_point() { - let points = [ - Vector3::new(0.0, 0.0, 0.0), - Vector3::new(1.0, 0.0, 0.0), - Vector3::new(0.0, 1.0, 0.0), - Vector3::new(0.0, 0.0, 1.0), - Vector3::new(1.0, 1.0, 1.0), - ]; - - assert_eq!( - get_farthest_point(&points, Vector3::new(1.0, 0.0, 0.0)), - Vector3::new(1.0, 0.0, 0.0) - ); - assert_eq!( - get_farthest_point(&points, Vector3::new(10.0, 0.0, 0.0)), - Vector3::new(1.0, 0.0, 0.0) - ); - assert_eq!( - get_farthest_point(&points, Vector3::new(1.0, 1.0, 0.0)), - Vector3::new(1.0, 1.0, 1.0) - ); - } - - #[test] - fn test_get_barycentric_coords() { - assert_eq!( - get_barycentric_coords( - &Vector3::new(0.0, 0.0, 0.0), - &Vector3::new(1.0, 0.0, 0.0), - &Vector3::new(0.0, 1.0, 0.0), - &Vector3::new(0.0, 0.0, 1.0), - ), - (0.33333328, 0.33333334, 0.33333334) - ); - } - - #[test] - fn test_get_barycentric_coords_2d() { - assert_eq!( - get_barycentric_coords_2d( - Vector2::new(0.0, 0.0), - Vector2::new(1.0, 0.0), - Vector2::new(0.0, 1.0), - Vector2::new(0.0, 0.0), - ), - (0.0, 0.0, 1.0) - ); - } - - #[test] - fn test_barycentric_to_world() { - assert_eq!( - barycentric_to_world( - (2.0, 2.0, 2.0), - Vector3::new(1.0, 0.0, 0.0), - Vector3::new(0.0, 1.0, 0.0), - Vector3::new(0.0, 0.0, 1.0), - ), - Vector3::new(2.0, 2.0, 2.0) - ); - } - - #[test] - fn test_barycentric_is_inside() { - assert!(barycentric_is_inside((0.0, 0.0, 0.0))); - assert!(barycentric_is_inside((0.5, 0.49, 0.0))); - - assert!(!barycentric_is_inside((0.5, 0.5, 0.0))); - assert!(!barycentric_is_inside((-0.5, 0.49, 0.0))); - assert!(!barycentric_is_inside((0.5, -0.49, 0.0))); - assert!(!barycentric_is_inside((-0.5, -0.49, 0.0))); - } - - #[test] - fn test_triangle_area() { - assert_eq!( - triangle_area( - Vector3::new(0.0, 0.0, 0.0), - Vector3::new(0.0, 1.0, 0.0), - Vector3::new(0.0, 0.0, 1.0), - ), - 0.5 - ); - } - - #[test] - fn test_spherical_to_cartesian() { - assert_eq!( - spherical_to_cartesian(0.0, 0.0, 1.0), - Vector3::new(0.0, 1.0, 0.0) - ); - } - - #[test] - fn partial_eq_for_triangle_edge() { - let te = TriangleEdge { a: 2, b: 5 }; - let te2 = TriangleEdge { a: 2, b: 5 }; - let te3 = TriangleEdge { a: 5, b: 2 }; - - assert_eq!(te, te2); - assert_eq!(te, te3); - } - - #[test] - fn triangle_definition_indices() { - assert_eq!(TriangleDefinition([0, 0, 0]).indices(), &[0, 0, 0]); - } - - #[test] - fn triangle_definition_indices_mut() { - assert_eq!(TriangleDefinition([0, 0, 0]).indices_mut(), &mut [0, 0, 0]); - } - - #[test] - fn triangle_definition_edges() { - let t = TriangleDefinition([0, 1, 2]); - assert_eq!( - t.edges(), - [ - TriangleEdge { a: 0, b: 1 }, - TriangleEdge { a: 1, b: 2 }, - TriangleEdge { a: 2, b: 0 } - ] - ); - } - - #[test] - fn visit_for_triangle_definition() { - let mut t = TriangleDefinition([0, 1, 2]); - let mut visitor = Visitor::default(); - - assert!(t.visit("name", &mut visitor).is_ok()); - } - - #[test] - fn index_for_triangle_definition() { - let t = TriangleDefinition([0, 1, 2]); - - assert_eq!(t[0], 0); - assert_eq!(t[1], 1); - assert_eq!(t[2], 2); - } - - #[test] - fn index_mut_for_triangle_definition() { - let mut t = TriangleDefinition([5, 5, 5]); - t[0] = 0; - t[1] = 1; - t[2] = 2; - - assert_eq!(t[0], 0); - assert_eq!(t[1], 1); - assert_eq!(t[2], 2); - } - - #[test] - fn position_provider_for_vector3() { - let v = Vector3::new(0.0, 1.0, 2.0); - - assert_eq!(v.position(), v); - } - - #[test] - fn as_ref_for_triangle_definition() { - let t = TriangleDefinition([0, 1, 2]); - - assert_eq!(t.as_ref(), &[0, 1, 2]); - } - - #[test] - fn as_mut_for_triangle_definition() { - let mut t = TriangleDefinition([0, 1, 2]); - - assert_eq!(t.as_mut(), &mut [0, 1, 2]); - } - - #[test] - fn test_get_closest_point() { - let points = [ - Vector3::new(1.0, 0.0, 0.0), - Vector3::new(0.0, 1.0, 0.0), - Vector3::new(0.0, 0.0, 1.0), - ]; - - assert_eq!( - get_closest_point(&points, Vector3::new(0.0, 0.0, 0.0)), - Some(0), - ); - assert_eq!( - get_closest_point(&points, Vector3::new(0.0, 1.0, 1.0)), - Some(1), - ); - assert_eq!( - get_closest_point(&points, Vector3::new(0.0, 0.0, 10.0)), - Some(2), - ); - } - - #[test] - fn test_get_closest_point_triangles() { - let points = [ - Vector3::new(0.0, 0.0, 0.0), - Vector3::new(1.0, 0.0, 0.0), - Vector3::new(0.0, 1.0, 0.0), - Vector3::new(0.0, 0.0, 1.0), - ]; - let triangles = [TriangleDefinition([0, 1, 2]), TriangleDefinition([1, 2, 3])]; - - assert_eq!( - get_closest_point_triangles( - &points, - &triangles, - [0, 1].into_iter(), - Vector3::new(1.0, 1.0, 1.0) - ), - Some((1, 0)) - ); - } - - #[test] - fn test_get_closest_point_triangle_set() { - let points = [ - Vector3::new(0.0, 0.0, 0.0), - Vector3::new(1.0, 0.0, 0.0), - Vector3::new(0.0, 1.0, 0.0), - Vector3::new(0.0, 0.0, 1.0), - ]; - let triangles = [TriangleDefinition([0, 1, 2]), TriangleDefinition([1, 2, 3])]; - - assert_eq!( - get_closest_point_triangle_set(&points, &triangles, Vector3::new(1.0, 1.0, 1.0)), - Some((1, 0)) - ); - } - - #[test] - fn smooth_angle_setters() { - let mut sa = SmoothAngle { - angle: 0.0, - speed: 0.0, - target: 0.0, - }; - - assert_eq!(sa.angle(), 0.0); - - sa.set_angle(std::f32::consts::PI); - assert_eq!(sa.angle(), std::f32::consts::PI); - - sa.set_target(std::f32::consts::PI); - assert_eq!(sa.target, std::f32::consts::PI); - - sa.set_speed(1.0); - assert_eq!(sa.speed, 1.0); - } - - #[test] - fn smooth_angle_turn_direction() { - assert_eq!( - SmoothAngle { - angle: 0.0, - speed: 0.0, - target: std::f32::consts::PI * 1.1, - } - .turn_direction(), - -1.0 - ); - - assert_eq!( - SmoothAngle { - angle: 0.0, - speed: 0.0, - target: -std::f32::consts::PI * 1.1, - } - .turn_direction(), - 1.0 - ); - - assert_eq!( - SmoothAngle { - angle: 0.0, - speed: 0.0, - target: -std::f32::consts::PI * 0.9, - } - .turn_direction(), - -1.0 - ); - - assert_eq!( - SmoothAngle { - angle: 0.0, - speed: 0.0, - target: std::f32::consts::PI * 0.9, - } - .turn_direction(), - 1.0 - ); - } - - #[test] - fn default_for_smooth_angle() { - let sa = SmoothAngle::default(); - - assert_eq!(sa.angle, 0.0); - assert_eq!(sa.target, 0.0); - assert_eq!(sa.speed, 1.0); - } - - #[test] - fn test_quat_from_euler() { - assert_eq!( - quat_from_euler( - Vector3::new( - std::f32::consts::PI, - std::f32::consts::PI, - std::f32::consts::PI - ), - RotationOrder::XYZ - ), - UnitQuaternion::from_euler_angles( - std::f32::consts::PI, - std::f32::consts::PI, - std::f32::consts::PI - ) - ); - } - - #[test] - fn matrix4_ext_for_matrix4() { - let m = Matrix4::new( - 1.0, 0.0, 0.0, 0.0, // - 0.0, 1.0, 0.0, 0.0, // - 0.0, 0.0, 1.0, 0.0, // - 0.0, 0.0, 0.0, 1.0, - ); - - assert_eq!(m.side(), Vector3::new(1.0, 0.0, 0.0)); - assert_eq!(m.up(), Vector3::new(0.0, 1.0, 0.0)); - assert_eq!(m.look(), Vector3::new(0.0, 0.0, 1.0)); - assert_eq!(m.position(), Vector3::new(0.0, 0.0, 0.0)); - assert_eq!( - m.basis(), - Matrix3::new( - 1.0, 0.0, 0.0, // - 0.0, 1.0, 0.0, // - 0.0, 0.0, 1.0, - ) - ); - } - - #[test] - fn matrix3_ext_for_matrix3() { - let m = Matrix3::new( - 1.0, 0.0, 0.0, // - 0.0, 1.0, 0.0, // - 0.0, 0.0, 1.0, - ); - - assert_eq!(m.side(), Vector3::new(1.0, 0.0, 0.0)); - assert_eq!(m.up(), Vector3::new(0.0, 1.0, 0.0)); - assert_eq!(m.look(), Vector3::new(0.0, 0.0, 1.0)); - } - - #[test] - fn vector3_ext_for_vector3() { - let mut v = Vector3::new(2.0, 2.0, 2.0); - - assert_eq!(v.sqr_distance(&Vector3::new(0.0, 1.0, 1.0)), 6.0); - - assert_eq!( - v.non_uniform_scale(&Vector3::new(3.0, 3.0, 3.0)), - Vector3::new(6.0, 6.0, 6.0) - ); - - v.follow(&Vector3::new(0.5, 0.5, 0.5), 2.0); - assert_eq!(v, Vector3::new(-1.0, -1.0, -1.0)); - } - - #[test] - fn vector2_ext_for_vector2() { - let mut v = Vector2::new(2.0, 2.0); - - assert_eq!( - v.per_component_min(&Vector2::new(0.0, 4.0)), - Vector2::new(0.0, 2.0) - ); - - assert_eq!( - v.per_component_max(&Vector2::new(0.0, 4.0)), - Vector2::new(2.0, 4.0) - ); - - v.follow(&Vector2::new(0.5, 0.5), 2.0); - assert_eq!(v, Vector2::new(-1.0, -1.0)); - } - - #[test] - fn test_m4x4_approx_eq() { - assert!(m4x4_approx_eq( - &Matrix4::new( - 1.0, 0.0, 0.0, 0.0, // - 0.0, 1.0, 0.0, 0.0, // - 0.0, 0.0, 1.0, 0.0, // - 0.0, 0.0, 0.0, 1.0, - ), - &Matrix4::new( - 1.0001, 0.0001, 0.0001, 0.0001, // - 0.0001, 1.0001, 0.0001, 0.0001, // - 0.0001, 0.0001, 1.0001, 0.0001, // - 0.0001, 0.0001, 0.0001, 1.0001, - ) - ),); - } -} +impl_reflect!( + pub struct TriangleDefinition(pub [u32; 3]); +); + +impl_visit!( + pub struct SmoothAngle { + angle: f32, + target: f32, + speed: f32, + } +); + +impl_reflect!( + pub struct SmoothAngle { + angle: f32, + target: f32, + speed: f32, + } +); + +impl_reflect!( + pub enum CurveKeyKind { + Constant, + Linear, + Cubic { + left_tangent: f32, + right_tangent: f32, + }, + } +); + +impl_visit!( + pub enum CurveKeyKind { + Constant, + Linear, + Cubic { + left_tangent: f32, + right_tangent: f32, + }, + } +); + +impl_visit!( + pub struct CurveKey { + pub id: Uuid, + location: f32, + pub value: f32, + pub kind: CurveKeyKind, + } +); + +impl_reflect!( + #[reflect(hide_all)] + pub struct Curve { + pub id: Uuid, + pub name: String, + pub keys: Vec, + } +); + +impl_visit!( + pub struct Curve { + #[visit(optional)] // Backward compatibility + pub id: Uuid, + #[visit(optional)] // Backward compatibility + pub name: String, + pub keys: Vec, + } +); diff --git a/fyrox-core/src/quadtree.rs b/fyrox-core/src/quadtree.rs index ba75680cf..66c0e9dd4 100644 --- a/fyrox-core/src/quadtree.rs +++ b/fyrox-core/src/quadtree.rs @@ -1 +1 @@ -pub use rectutils::quadtree::*; +pub use crate::math::quadtree::*; diff --git a/fyrox-core/src/rectpack.rs b/fyrox-core/src/rectpack.rs index db16472db..6ed36996a 100644 --- a/fyrox-core/src/rectpack.rs +++ b/fyrox-core/src/rectpack.rs @@ -1 +1 @@ -pub use rectutils::pack::RectPacker; +pub use crate::math::pack::RectPacker; diff --git a/fyrox-math/Cargo.toml b/fyrox-math/Cargo.toml new file mode 100644 index 000000000..1c1411a0a --- /dev/null +++ b/fyrox-math/Cargo.toml @@ -0,0 +1,11 @@ +[package] +name = "fyrox-math" +version = "0.1.0" +edition = "2021" + +[dependencies] +rectutils = "0.1.0" +nalgebra = "0.32.3" +arrayvec = "0.7.4" +num-traits = "0.2.18" +uuid = "1.7.0" \ No newline at end of file diff --git a/fyrox-core/src/math/aabb.rs b/fyrox-math/src/aabb.rs similarity index 98% rename from fyrox-core/src/math/aabb.rs rename to fyrox-math/src/aabb.rs index 465f68789..62542e7c5 100644 --- a/fyrox-core/src/math/aabb.rs +++ b/fyrox-math/src/aabb.rs @@ -1,10 +1,7 @@ -use crate::{ - algebra::{Matrix4, Vector3}, - math::Matrix4Ext, - visitor::{Visit, VisitResult, Visitor}, -}; +use crate::Matrix4Ext; +use nalgebra::{Matrix4, Vector3}; -#[derive(Copy, Clone, Debug, Visit)] +#[derive(Copy, Clone, Debug)] pub struct AxisAlignedBoundingBox { pub min: Vector3, pub max: Vector3, @@ -305,8 +302,8 @@ impl AxisAlignedBoundingBox { #[cfg(test)] mod test { - use crate::algebra::{Matrix4, Vector3}; - use crate::math::aabb::AxisAlignedBoundingBox; + use crate::aabb::AxisAlignedBoundingBox; + use nalgebra::{Matrix4, Vector3}; #[test] fn test_aabb_transform() { diff --git a/fyrox-core/src/curve.rs b/fyrox-math/src/curve.rs similarity index 96% rename from fyrox-core/src/curve.rs rename to fyrox-math/src/curve.rs index d6e0b6743..06ef25875 100644 --- a/fyrox-core/src/curve.rs +++ b/fyrox-math/src/curve.rs @@ -1,8 +1,4 @@ -use crate::{ - math::{cubicf, inf_sup_cubicf, lerpf, Rect}, - reflect::prelude::*, - visitor::prelude::*, -}; +use crate::{cubicf, inf_sup_cubicf, lerpf, Rect}; use std::cmp::Ordering; use uuid::Uuid; @@ -14,7 +10,7 @@ fn stepf(p0: f32, p1: f32, t: f32) -> f32 { } } -#[derive(Visit, Default, Clone, Debug, PartialEq, Reflect)] +#[derive(Default, Clone, Debug, PartialEq)] pub enum CurveKeyKind { #[default] Constant, @@ -37,10 +33,10 @@ impl CurveKeyKind { } } -#[derive(Visit, Clone, Default, Debug, PartialEq)] +#[derive(Clone, Default, Debug, PartialEq)] pub struct CurveKey { pub id: Uuid, - location: f32, + pub location: f32, pub value: f32, pub kind: CurveKeyKind, } @@ -111,16 +107,11 @@ impl CurveKey { } } -#[derive(Visit, Reflect, Clone, Debug, PartialEq)] -#[reflect(hide_all)] +#[derive(Clone, Debug, PartialEq)] pub struct Curve { - #[visit(optional)] // Backward compatibility - id: Uuid, - - #[visit(optional)] // Backward compatibility - name: String, - - keys: Vec, + pub id: Uuid, + pub name: String, + pub keys: Vec, } impl Default for Curve { diff --git a/fyrox-core/src/math/frustum.rs b/fyrox-math/src/frustum.rs similarity index 93% rename from fyrox-core/src/math/frustum.rs rename to fyrox-math/src/frustum.rs index 856484310..e03502329 100644 --- a/fyrox-core/src/math/frustum.rs +++ b/fyrox-math/src/frustum.rs @@ -1,15 +1,12 @@ -use crate::{ - algebra::{Matrix4, Vector3}, - math::{aabb::AxisAlignedBoundingBox, plane::Plane}, - visitor::{Visit, VisitResult, Visitor}, -}; +use crate::{aabb::AxisAlignedBoundingBox, plane::Plane}; use nalgebra::Point3; +use nalgebra::{Matrix4, Vector3}; #[derive(Copy, Clone, Debug, PartialEq)] pub struct Frustum { /// 0 - left, 1 - right, 2 - top, 3 - bottom, 4 - far, 5 - near - planes: [Plane; 6], - corners: [Vector3; 8], + pub planes: [Plane; 6], + pub corners: [Vector3; 8], } impl Default for Frustum { @@ -330,29 +327,12 @@ impl Frustum { } } -impl Visit for Frustum { - fn visit(&mut self, name: &str, visitor: &mut Visitor) -> VisitResult { - let mut region = visitor.enter_region(name)?; - - self.planes[0].visit("Left", &mut region)?; - self.planes[1].visit("Right", &mut region)?; - self.planes[2].visit("Top", &mut region)?; - self.planes[3].visit("Bottom", &mut region)?; - self.planes[4].visit("Far", &mut region)?; - self.planes[5].visit("Near", &mut region)?; - - Ok(()) - } -} - #[cfg(test)] mod test { + use crate::aabb::AxisAlignedBoundingBox; + use crate::{frustum::Frustum, plane::Plane}; use nalgebra::{Matrix4, Vector3}; - use crate::math::aabb::AxisAlignedBoundingBox; - use crate::math::{frustum::Frustum, plane::Plane}; - use crate::visitor::{Visit, Visitor}; - #[test] fn test_default_for_frustum() { assert_eq!( @@ -550,18 +530,4 @@ mod test { assert!(f.is_intersects_sphere(Vector3::new(0.0, 0.0, 0.0), 2.0)); assert!(!f.is_intersects_sphere(Vector3::new(10.0, 10.0, 10.0), 1.0)); } - - #[test] - fn test_visit_for_frustum() { - let mut f = Frustum::from_view_projection_matrix(Matrix4::new( - 1.0, 0.0, 0.0, 0.0, // - 0.0, 1.0, 0.0, 0.0, // - 0.0, 0.0, 1.0, 0.0, // - 0.0, 0.0, 0.0, 1.0, - )) - .unwrap(); - let mut visitor = Visitor::default(); - - assert!(f.visit("name", &mut visitor).is_ok()); - } } diff --git a/fyrox-math/src/lib.rs b/fyrox-math/src/lib.rs new file mode 100644 index 000000000..146c0aee9 --- /dev/null +++ b/fyrox-math/src/lib.rs @@ -0,0 +1,1525 @@ +// Clippy complains about normal mathematical symbols like A, B, C for quadratic equation. +#![allow(clippy::many_single_char_names)] + +pub mod aabb; +pub mod curve; +pub mod frustum; +pub mod octree; +pub mod plane; +pub mod ray; +pub mod triangulator; + +use crate::ray::IntersectionResult; +use nalgebra::{ + Matrix3, Matrix4, RealField, Scalar, SimdRealField, UnitQuaternion, Vector2, Vector3, +}; +use std::{ + fmt::Debug, + hash::{Hash, Hasher}, + ops::{Index, IndexMut}, +}; + +pub use rectutils::*; + +#[derive(Copy, Clone)] +pub enum PlaneClass { + XY, + YZ, + XZ, +} + +#[inline] +#[allow(clippy::useless_let_if_seq)] +pub fn classify_plane(normal: Vector3) -> PlaneClass { + let mut longest = 0.0f32; + let mut class = PlaneClass::XY; + + if normal.x.abs() > longest { + longest = normal.x.abs(); + class = PlaneClass::YZ; + } + + if normal.y.abs() > longest { + longest = normal.y.abs(); + class = PlaneClass::XZ; + } + + if normal.z.abs() > longest { + class = PlaneClass::XY; + } + + class +} + +#[inline] +pub fn get_polygon_normal(polygon: &[Vector3]) -> Result, &'static str> { + let mut normal = Vector3::default(); + + for (i, current) in polygon.iter().enumerate() { + let next = polygon[(i + 1) % polygon.len()]; + normal.x += (current.y - next.y) * (current.z + next.z); + normal.y += (current.z - next.z) * (current.x + next.x); + normal.z += (current.x - next.x) * (current.y + next.y); + } + + normal + .try_normalize(f32::EPSILON) + .ok_or("Unable to get normal of degenerated polygon!") +} + +#[inline] +pub fn get_signed_triangle_area(v1: Vector2, v2: Vector2, v3: Vector2) -> f32 { + 0.5 * (v1.x * (v3.y - v2.y) + v2.x * (v1.y - v3.y) + v3.x * (v2.y - v1.y)) +} + +#[inline] +pub fn vec3_to_vec2_by_plane( + plane_class: PlaneClass, + normal: Vector3, + point: Vector3, +) -> Vector2 { + match plane_class { + PlaneClass::XY => { + if normal.z < 0.0 { + Vector2::new(point.y, point.x) + } else { + Vector2::new(point.x, point.y) + } + } + PlaneClass::XZ => { + if normal.y < 0.0 { + Vector2::new(point.x, point.z) + } else { + Vector2::new(point.z, point.x) + } + } + PlaneClass::YZ => { + if normal.x < 0.0 { + Vector2::new(point.z, point.y) + } else { + Vector2::new(point.y, point.z) + } + } + } +} + +#[inline] +pub fn is_point_inside_2d_triangle( + point: Vector2, + pt_a: Vector2, + pt_b: Vector2, + pt_c: Vector2, +) -> bool { + let ba = pt_b - pt_a; + let ca = pt_c - pt_a; + + let vp = point - pt_a; + + let ba_dot_ba = ba.dot(&ba); + let ca_dot_ba = ca.dot(&ba); + let ca_dot_ca = ca.dot(&ca); + + let dot_02 = ca.dot(&vp); + let dot_12 = ba.dot(&vp); + + let inv_denom = 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba.powi(2)); + + // calculate barycentric coordinates + let u = (ba_dot_ba * dot_02 - ca_dot_ba * dot_12) * inv_denom; + let v = (ca_dot_ca * dot_12 - ca_dot_ba * dot_02) * inv_denom; + + (u >= 0.0) && (v >= 0.0) && (u + v < 1.0) +} + +#[inline] +pub fn wrap_angle(angle: f32) -> f32 { + let two_pi = 2.0 * std::f32::consts::PI; + + if angle > 0.0 { + angle % two_pi + } else { + (angle + two_pi) % two_pi + } +} + +/// There are two versions of remainder, the standard `%` operator which does `x - (x/y).trunc()*y` and IEEE remainder which does `x - (x/y).round()*y`. +#[inline] +pub fn ieee_remainder(x: f32, y: f32) -> f32 { + x - (x / y).round() * y +} + +#[inline] +pub fn round_to_step(x: f32, step: f32) -> f32 { + x - ieee_remainder(x, step) +} + +#[inline] +pub fn wrapf(mut n: f32, mut min_limit: f32, mut max_limit: f32) -> f32 { + if n >= min_limit && n <= max_limit { + return n; + } + + if max_limit == 0.0 && min_limit == 0.0 { + return 0.0; + } + + max_limit -= min_limit; + + let offset = min_limit; + min_limit = 0.0; + n -= offset; + + let num_of_max = (n / max_limit).abs().floor(); + + if n >= max_limit { + n -= num_of_max * max_limit; + } else if n < min_limit { + n += (num_of_max + 1.0) * max_limit; + } + + n + offset +} + +#[inline(always)] +pub fn lerpf(a: f32, b: f32, t: f32) -> f32 { + a + (b - a) * t +} + +// https://en.wikipedia.org/wiki/Cubic_Hermite_spline +#[inline] +pub fn cubicf(p0: f32, p1: f32, t: f32, m0: f32, m1: f32) -> f32 { + let t2 = t * t; + let t3 = t2 * t; + let scale = (p1 - p0).abs(); + + (2.0 * t3 - 3.0 * t2 + 1.0) * p0 + + (t3 - 2.0 * t2 + t) * m0 * scale + + (-2.0 * t3 + 3.0 * t2) * p1 + + (t3 - t2) * m1 * scale +} + +#[inline] +pub fn cubicf_derivative(p0: f32, p1: f32, t: f32, m0: f32, m1: f32) -> f32 { + let t2 = t * t; + let scale = (p1 - p0).abs(); + + (6.0 * t2 - 6.0 * t) * p0 + + (3.0 * t2 - 4.0 * t + 1.0) * m0 * scale + + (6.0 * t - 6.0 * t2) * p1 + + (3.0 * t2 - 2.0 * t) * m1 * scale +} + +#[inline] +pub fn inf_sup_cubicf(p0: f32, p1: f32, m0: f32, m1: f32) -> (f32, f32) { + // Find two `t`s where derivative of cubicf is zero - these will be + // extreme points of the spline. Then get the values at those `t`s + let d = -(9.0 * p0 * p0 + 6.0 * p0 * (-3.0 * p1 + m1 + m0) + 9.0 * p1 * p1 + - 6.0 * p1 * (m1 + m0) + + m1 * m1 + + m1 * m0 + + m0 * m0) + .sqrt(); + let k = 3.0 * (2.0 * p0 - 2.0 * p1 + m1 + m0); + let v = 3.0 * p0 - 3.0 * p1 + m1 + 2.0 * m0; + let t0 = (-d + v) / k; + let t1 = (d + v) / k; + (cubicf(p0, p1, t0, m0, m1), cubicf(p0, p1, t1, m0, m1)) +} + +#[inline] +pub fn get_farthest_point(points: &[Vector3], dir: Vector3) -> Vector3 { + let mut n_farthest = 0; + let mut max_dot = -f32::MAX; + for (i, point) in points.iter().enumerate() { + let dot = dir.dot(point); + if dot > max_dot { + n_farthest = i; + max_dot = dot + } + } + points[n_farthest] +} + +#[inline] +pub fn get_barycentric_coords( + p: &Vector3, + a: &Vector3, + b: &Vector3, + c: &Vector3, +) -> (f32, f32, f32) { + let v0 = *b - *a; + let v1 = *c - *a; + let v2 = *p - *a; + + let d00 = v0.dot(&v0); + let d01 = v0.dot(&v1); + let d11 = v1.dot(&v1); + let d20 = v2.dot(&v0); + let d21 = v2.dot(&v1); + let denom = d00 * d11 - d01.powi(2); + + let v = (d11 * d20 - d01 * d21) / denom; + let w = (d00 * d21 - d01 * d20) / denom; + let u = 1.0 - v - w; + + (u, v, w) +} + +#[inline] +pub fn get_barycentric_coords_2d( + p: Vector2, + a: Vector2, + b: Vector2, + c: Vector2, +) -> (f32, f32, f32) { + let v0 = b - a; + let v1 = c - a; + let v2 = p - a; + + let d00 = v0.dot(&v0); + let d01 = v0.dot(&v1); + let d11 = v1.dot(&v1); + let d20 = v2.dot(&v0); + let d21 = v2.dot(&v1); + let inv_denom = 1.0 / (d00 * d11 - d01.powi(2)); + + let v = (d11 * d20 - d01 * d21) * inv_denom; + let w = (d00 * d21 - d01 * d20) * inv_denom; + let u = 1.0 - v - w; + + (u, v, w) +} + +#[inline] +pub fn barycentric_to_world( + bary: (f32, f32, f32), + pa: Vector3, + pb: Vector3, + pc: Vector3, +) -> Vector3 { + pa.scale(bary.0) + pb.scale(bary.1) + pc.scale(bary.2) +} + +#[inline] +pub fn barycentric_is_inside(bary: (f32, f32, f32)) -> bool { + (bary.0 >= 0.0) && (bary.1 >= 0.0) && (bary.0 + bary.1 < 1.0) +} + +#[inline] +pub fn is_point_inside_triangle(p: &Vector3, vertices: &[Vector3; 3]) -> bool { + let ba = vertices[1] - vertices[0]; + let ca = vertices[2] - vertices[0]; + let vp = *p - vertices[0]; + + let ba_dot_ba = ba.dot(&ba); + let ca_dot_ba = ca.dot(&ba); + let ca_dot_ca = ca.dot(&ca); + + let dot02 = ca.dot(&vp); + let dot12 = ba.dot(&vp); + + let inv_denom = 1.0 / (ca_dot_ca * ba_dot_ba - ca_dot_ba.powi(2)); + + // Calculate barycentric coordinates + let u = (ba_dot_ba * dot02 - ca_dot_ba * dot12) * inv_denom; + let v = (ca_dot_ca * dot12 - ca_dot_ba * dot02) * inv_denom; + + (u >= 0.0) && (v >= 0.0) && (u + v < 1.0) +} + +#[inline] +pub fn triangle_area(a: Vector3, b: Vector3, c: Vector3) -> f32 { + (b - a).cross(&(c - a)).norm() * 0.5 +} + +#[inline] +pub fn solve_quadratic(a: f32, b: f32, c: f32) -> Option<[f32; 2]> { + let discriminant = b * b - 4.0 * a * c; + if discriminant < 0.0 { + // No real roots + None + } else { + // Dont care if quadratic equation has only one root (discriminant == 0), this is edge-case + // which requires additional branching instructions which is not good for branch-predictor in CPU. + let _2a = 2.0 * a; + let discr_root = discriminant.sqrt(); + let r1 = (-b + discr_root) / _2a; + let r2 = (-b - discr_root) / _2a; + Some([r1, r2]) + } +} + +#[inline] +pub fn spherical_to_cartesian(azimuth: f32, elevation: f32, radius: f32) -> Vector3 { + let x = radius * elevation.sin() * azimuth.sin(); + let y = radius * elevation.cos(); + let z = -radius * elevation.sin() * azimuth.cos(); + Vector3::new(x, y, z) +} + +#[inline] +pub fn ray_rect_intersection( + rect: Rect, + origin: Vector2, + dir: Vector2, +) -> Option { + let min = rect.left_top_corner(); + let max = rect.right_bottom_corner(); + + let (mut tmin, mut tmax) = if dir.x >= 0.0 { + ((min.x - origin.x) / dir.x, (max.x - origin.x) / dir.x) + } else { + ((max.x - origin.x) / dir.x, (min.x - origin.x) / dir.x) + }; + + let (tymin, tymax) = if dir.y >= 0.0 { + ((min.y - origin.y) / dir.y, (max.y - origin.y) / dir.y) + } else { + ((max.y - origin.y) / dir.y, (min.y - origin.y) / dir.y) + }; + + if tmin > tymax || tymin > tmax { + return None; + } + if tymin > tmin { + tmin = tymin; + } + if tymax < tmax { + tmax = tymax; + } + if tmin <= 1.0 && tmax >= 0.0 { + Some(IntersectionResult { + min: tmin, + max: tmax, + }) + } else { + None + } +} + +#[derive(Clone, Copy, Debug, Default)] +#[repr(C)] +pub struct TriangleEdge { + pub a: u32, + pub b: u32, +} + +impl PartialEq for TriangleEdge { + fn eq(&self, other: &Self) -> bool { + self.a == other.a && self.b == other.b || self.a == other.b && self.b == other.a + } +} + +impl Eq for TriangleEdge {} + +impl Hash for TriangleEdge { + fn hash(&self, state: &mut H) { + // Direction-agnostic hash. + (self.a as u64 + self.b as u64).hash(state) + } +} + +#[derive(Clone, Copy, PartialEq, Eq, Debug, Default, Hash)] +#[repr(C)] +pub struct TriangleDefinition(pub [u32; 3]); + +impl TriangleDefinition { + #[inline] + pub fn indices(&self) -> &[u32] { + self.as_ref() + } + + #[inline] + pub fn indices_mut(&mut self) -> &mut [u32] { + self.as_mut() + } + + #[inline] + pub fn edges(&self) -> [TriangleEdge; 3] { + [ + TriangleEdge { + a: self.0[0], + b: self.0[1], + }, + TriangleEdge { + a: self.0[1], + b: self.0[2], + }, + TriangleEdge { + a: self.0[2], + b: self.0[0], + }, + ] + } + + #[inline] + pub fn add(&self, i: u32) -> Self { + Self([self.0[0] + i, self.0[1] + i, self.0[2] + i]) + } +} + +impl Index for TriangleDefinition { + type Output = u32; + + #[inline] + fn index(&self, index: usize) -> &Self::Output { + &self.0[index] + } +} + +impl IndexMut for TriangleDefinition { + #[inline] + fn index_mut(&mut self, index: usize) -> &mut Self::Output { + &mut self.0[index] + } +} + +pub trait PositionProvider: Sized { + fn position(&self) -> Vector3; +} + +impl PositionProvider for Vector3 { + #[inline] + fn position(&self) -> Vector3 { + *self + } +} + +impl AsRef<[u32]> for TriangleDefinition { + #[inline] + fn as_ref(&self) -> &[u32] { + &self.0 + } +} + +impl AsMut<[u32]> for TriangleDefinition { + #[inline] + fn as_mut(&mut self) -> &mut [u32] { + &mut self.0 + } +} + +/// Tries to find a point closest to given point. +/// +/// # Notes +/// +/// O(n) complexity. +#[inline] +pub fn get_closest_point(points: &[P], point: Vector3) -> Option { + let mut closest_sqr_distance = f32::MAX; + let mut closest_index = None; + for (i, vertex) in points.iter().enumerate() { + let sqr_distance = (vertex.position() - point).norm_squared(); + if sqr_distance < closest_sqr_distance { + closest_sqr_distance = sqr_distance; + closest_index = Some(i); + } + } + closest_index +} + +/// Returns a tuple of (point index; triangle index) closest to the given point. +#[inline] +pub fn get_closest_point_triangles

( + points: &[P], + triangles: &[TriangleDefinition], + triangle_indices: impl Iterator, + point: Vector3, +) -> Option<(usize, usize)> +where + P: PositionProvider, +{ + let mut closest_sqr_distance = f32::MAX; + let mut closest_index = None; + for triangle_index in triangle_indices { + let triangle = triangles.get(triangle_index).unwrap(); + for point_index in triangle.0.iter() { + let vertex = points.get(*point_index as usize).unwrap(); + let sqr_distance = (vertex.position() - point).norm_squared(); + if sqr_distance < closest_sqr_distance { + closest_sqr_distance = sqr_distance; + closest_index = Some((*point_index as usize, triangle_index)); + } + } + } + closest_index +} + +/// Returns a tuple of (point index; triangle index) closest to the given point. +#[inline] +pub fn get_closest_point_triangle_set

( + points: &[P], + triangles: &[TriangleDefinition], + point: Vector3, +) -> Option<(usize, usize)> +where + P: PositionProvider, +{ + let mut closest_sqr_distance = f32::MAX; + let mut closest_index = None; + for (triangle_index, triangle) in triangles.iter().enumerate() { + for point_index in triangle.0.iter() { + let vertex = points.get(*point_index as usize).unwrap(); + let sqr_distance = (vertex.position() - point).norm_squared(); + if sqr_distance < closest_sqr_distance { + closest_sqr_distance = sqr_distance; + closest_index = Some((*point_index as usize, triangle_index)); + } + } + } + closest_index +} + +#[derive(Debug, PartialEq, Clone)] +pub struct SmoothAngle { + /// Current angle in radians. + pub angle: f32, + + /// Target angle in radians. + pub target: f32, + + /// Turn speed in radians per second (rad/s) + pub speed: f32, +} + +impl SmoothAngle { + #[inline] + pub fn set_target(&mut self, angle: f32) -> &mut Self { + self.target = angle; + self + } + + #[inline] + pub fn update(&mut self, dt: f32) -> &mut Self { + self.target = wrap_angle(self.target); + self.angle = wrap_angle(self.angle); + if !self.at_target() { + let delta = self.speed * dt; + if self.distance().abs() > delta { + self.angle += self.turn_direction() * delta; + } else { + self.angle = self.target; + } + } + self + } + + #[inline] + pub fn set_speed(&mut self, speed: f32) -> &mut Self { + self.speed = speed; + self + } + + #[inline] + pub fn set_angle(&mut self, angle: f32) -> &mut Self { + self.angle = angle; + self + } + + #[inline] + pub fn angle(&self) -> f32 { + self.angle + } + + #[inline] + pub fn at_target(&self) -> bool { + (self.target - self.angle).abs() <= f32::EPSILON + } + + #[inline] + pub fn distance(&self) -> f32 { + let diff = (self.target - self.angle + std::f32::consts::PI) % std::f32::consts::TAU + - std::f32::consts::PI; + if diff < -std::f32::consts::PI { + diff + std::f32::consts::TAU + } else { + diff + } + } + + #[inline] + fn turn_direction(&self) -> f32 { + let distance = self.distance(); + + if distance < 0.0 { + if distance < -std::f32::consts::PI { + 1.0 + } else { + -1.0 + } + } else if distance > std::f32::consts::PI { + -1.0 + } else { + 1.0 + } + } +} + +impl Default for SmoothAngle { + fn default() -> Self { + Self { + angle: 0.0, + target: 0.0, + speed: 1.0, + } + } +} + +#[derive(Copy, Clone, Hash, PartialOrd, PartialEq, Ord, Eq)] +pub enum RotationOrder { + XYZ, + XZY, + YZX, + YXZ, + ZXY, + ZYX, +} + +#[inline] +pub fn quat_from_euler( + euler_radians: Vector3, + order: RotationOrder, +) -> UnitQuaternion { + let qx = UnitQuaternion::from_axis_angle(&Vector3::x_axis(), euler_radians.x); + let qy = UnitQuaternion::from_axis_angle(&Vector3::y_axis(), euler_radians.y); + let qz = UnitQuaternion::from_axis_angle(&Vector3::z_axis(), euler_radians.z); + match order { + RotationOrder::XYZ => qz * qy * qx, + RotationOrder::XZY => qy * qz * qx, + RotationOrder::YZX => qx * qz * qy, + RotationOrder::YXZ => qz * qx * qy, + RotationOrder::ZXY => qy * qx * qz, + RotationOrder::ZYX => qx * qy * qz, + } +} + +pub trait Matrix4Ext { + fn side(&self) -> Vector3; + fn up(&self) -> Vector3; + fn look(&self) -> Vector3; + fn position(&self) -> Vector3; + fn basis(&self) -> Matrix3; +} + +impl Matrix4Ext for Matrix4 { + #[inline] + fn side(&self) -> Vector3 { + Vector3::new(self[0], self[1], self[2]) + } + + #[inline] + fn up(&self) -> Vector3 { + Vector3::new(self[4], self[5], self[6]) + } + + #[inline] + fn look(&self) -> Vector3 { + Vector3::new(self[8], self[9], self[10]) + } + + #[inline] + fn position(&self) -> Vector3 { + Vector3::new(self[12], self[13], self[14]) + } + + #[inline] + fn basis(&self) -> Matrix3 { + self.fixed_resize::<3, 3>(T::default()) + } +} + +pub trait Matrix3Ext { + fn side(&self) -> Vector3; + fn up(&self) -> Vector3; + fn look(&self) -> Vector3; +} + +impl Matrix3Ext for Matrix3 { + #[inline] + fn side(&self) -> Vector3 { + Vector3::new(self[0], self[1], self[2]) + } + + #[inline] + fn up(&self) -> Vector3 { + Vector3::new(self[3], self[4], self[5]) + } + + #[inline] + fn look(&self) -> Vector3 { + Vector3::new(self[6], self[7], self[8]) + } +} + +pub trait Vector3Ext { + fn follow(&mut self, other: &Self, fraction: f32); + + fn sqr_distance(&self, other: &Self) -> f32; + + fn non_uniform_scale(&self, other: &Self) -> Self; +} + +impl Vector3Ext for Vector3 { + #[inline] + fn follow(&mut self, other: &Self, fraction: f32) { + self.x += (other.x - self.x) * fraction; + self.y += (other.y - self.y) * fraction; + self.z += (other.z - self.z) * fraction; + } + + #[inline] + fn sqr_distance(&self, other: &Self) -> f32 { + (self - other).norm_squared() + } + + #[inline] + fn non_uniform_scale(&self, other: &Self) -> Self { + Self::new(self.x * other.x, self.y * other.y, self.z * other.z) + } +} + +pub trait Vector2Ext { + fn follow(&mut self, other: &Self, fraction: f32); + + fn per_component_min(&self, other: &Self) -> Self; + fn per_component_max(&self, other: &Self) -> Self; +} + +impl Vector2Ext for Vector2 { + #[inline] + fn follow(&mut self, other: &Self, fraction: f32) { + self.x += (other.x - self.x) * fraction; + self.y += (other.y - self.y) * fraction; + } + + #[inline] + fn per_component_min(&self, other: &Self) -> Self { + Self::new(self.x.min(other.x), self.y.min(other.y)) + } + + #[inline] + fn per_component_max(&self, other: &Self) -> Self { + Self::new(self.x.max(other.x), self.y.max(other.y)) + } +} + +/// Returns rotation quaternion that represents rotation basis with Z axis aligned on `vec`. +/// This function handles singularities for you. +#[inline] +pub fn vector_to_quat(vec: Vector3) -> UnitQuaternion { + let dot = vec.normalize().dot(&Vector3::y()); + + if dot.abs() > 1.0 - 10.0 * f32::EPSILON { + // Handle singularity when vector is collinear with Y axis. + UnitQuaternion::from_axis_angle(&Vector3::x_axis(), -dot.signum() * 90.0f32.to_radians()) + } else { + UnitQuaternion::face_towards(&vec, &Vector3::y()) + } +} + +#[inline] +pub fn m4x4_approx_eq(a: &Matrix4, b: &Matrix4) -> bool { + a.iter() + .zip(b.iter()) + .all(|(a, b)| (*a - *b).abs() <= 0.001) +} + +#[cfg(test)] +mod test { + use nalgebra::{Matrix3, Matrix4, UnitQuaternion, Vector3}; + use num_traits::Zero; + + use super::{ + barycentric_is_inside, barycentric_to_world, cubicf_derivative, get_barycentric_coords, + get_barycentric_coords_2d, get_closest_point, get_closest_point_triangle_set, + get_closest_point_triangles, get_farthest_point, get_signed_triangle_area, ieee_remainder, + inf_sup_cubicf, quat_from_euler, round_to_step, spherical_to_cartesian, triangle_area, + wrap_angle, wrapf, Matrix3Ext, Matrix4Ext, PositionProvider, Rect, RotationOrder, + SmoothAngle, TriangleDefinition, TriangleEdge, Vector2Ext, Vector3Ext, + }; + use nalgebra::Vector2; + + #[test] + fn ray_rect_intersection() { + let rect = Rect::new(0.0, 0.0, 10.0, 10.0); + + // Edge-case: Horizontal ray. + assert!(super::ray_rect_intersection( + rect, + Vector2::new(-1.0, 5.0), + Vector2::new(1.0, 0.0) + ) + .is_some()); + + // Edge-case: Vertical ray. + assert!(super::ray_rect_intersection( + rect, + Vector2::new(5.0, -1.0), + Vector2::new(0.0, 1.0) + ) + .is_some()); + } + + #[test] + fn smooth_angle() { + let mut angle = SmoothAngle { + angle: 290.0f32.to_radians(), + target: 90.0f32.to_radians(), + speed: 100.0f32.to_radians(), + }; + + while !angle.at_target() { + println!("{}", angle.update(1.0).angle().to_degrees()); + } + } + + #[test] + fn default_for_rect() { + assert_eq!( + Rect::::default(), + Rect { + position: Vector2::new(Zero::zero(), Zero::zero()), + size: Vector2::new(Zero::zero(), Zero::zero()), + } + ); + } + + #[test] + fn rect_with_position() { + let rect = Rect::new(0, 0, 1, 1); + + assert_eq!( + rect.with_position(Vector2::new(1, 1)), + Rect::new(1, 1, 1, 1) + ); + } + + #[test] + fn rect_with_size() { + let rect = Rect::new(0, 0, 1, 1); + + assert_eq!( + rect.with_size(Vector2::new(10, 10)), + Rect::new(0, 0, 10, 10) + ); + } + + #[test] + fn rect_inflate() { + let rect = Rect::new(0, 0, 1, 1); + + assert_eq!(rect.inflate(5, 5), Rect::new(-5, -5, 11, 11)); + } + + #[test] + fn rect_deflate() { + let rect = Rect::new(-5, -5, 11, 11); + + assert_eq!(rect.deflate(5, 5), Rect::new(0, 0, 1, 1)); + } + + #[test] + fn rect_contains() { + let rect = Rect::new(0, 0, 10, 10); + + assert!(rect.contains(Vector2::new(0, 0))); + assert!(rect.contains(Vector2::new(0, 10))); + assert!(rect.contains(Vector2::new(10, 0))); + assert!(rect.contains(Vector2::new(10, 10))); + assert!(rect.contains(Vector2::new(5, 5))); + + assert!(!rect.contains(Vector2::new(0, 20))); + } + + #[test] + fn rect_center() { + let rect = Rect::new(0, 0, 10, 10); + + assert_eq!(rect.center(), Vector2::new(5, 5)); + } + + #[test] + fn rect_push() { + let mut rect = Rect::new(10, 10, 11, 11); + + rect.push(Vector2::new(0, 0)); + assert_eq!(rect, Rect::new(0, 0, 11, 11)); + + rect.push(Vector2::new(0, 20)); + assert_eq!(rect, Rect::new(0, 0, 11, 20)); + + rect.push(Vector2::new(20, 20)); + assert_eq!(rect, Rect::new(0, 0, 20, 20)); + + rect.push(Vector2::new(30, 30)); + assert_eq!(rect, Rect::new(0, 0, 30, 30)); + } + + #[test] + fn rect_getters() { + let rect = Rect::new(0, 0, 1, 1); + + assert_eq!(rect.left_top_corner(), Vector2::new(0, 0)); + assert_eq!(rect.left_bottom_corner(), Vector2::new(0, 1)); + assert_eq!(rect.right_top_corner(), Vector2::new(1, 0)); + assert_eq!(rect.right_bottom_corner(), Vector2::new(1, 1)); + + assert_eq!(rect.x(), 0); + assert_eq!(rect.y(), 0); + assert_eq!(rect.w(), 1); + assert_eq!(rect.h(), 1); + } + + #[test] + fn rect_clip_by() { + let rect = Rect::new(0, 0, 10, 10); + + assert_eq!(rect.clip_by(Rect::new(2, 2, 1, 1)), Rect::new(2, 2, 1, 1)); + assert_eq!( + rect.clip_by(Rect::new(0, 0, 15, 15)), + Rect::new(0, 0, 10, 10) + ); + + // When there is no intersection. + assert_eq!( + rect.clip_by(Rect::new(-2, 1, 1, 1)), + Rect::new(0, 0, 10, 10) + ); + assert_eq!( + rect.clip_by(Rect::new(11, 1, 1, 1)), + Rect::new(0, 0, 10, 10) + ); + assert_eq!( + rect.clip_by(Rect::new(1, -2, 1, 1)), + Rect::new(0, 0, 10, 10) + ); + assert_eq!( + rect.clip_by(Rect::new(1, 11, 1, 1)), + Rect::new(0, 0, 10, 10) + ); + } + + #[test] + fn rect_translate() { + let rect = Rect::new(0, 0, 10, 10); + + assert_eq!(rect.translate(Vector2::new(5, 5)), Rect::new(5, 5, 10, 10)); + } + + #[test] + fn rect_intersects_circle() { + let rect = Rect::new(0.0, 0.0, 1.0, 1.0); + + assert!(!rect.intersects_circle(Vector2::new(5.0, 5.0), 1.0)); + assert!(rect.intersects_circle(Vector2::new(0.0, 0.0), 1.0)); + assert!(rect.intersects_circle(Vector2::new(-0.5, -0.5), 1.0)); + } + + #[test] + fn rect_extend_to_contain() { + let mut rect = Rect::new(0.0, 0.0, 1.0, 1.0); + + rect.extend_to_contain(Rect::new(1.0, 1.0, 1.0, 1.0)); + assert_eq!(rect, Rect::new(0.0, 0.0, 2.0, 2.0)); + + rect.extend_to_contain(Rect::new(-1.0, -1.0, 1.0, 1.0)); + assert_eq!(rect, Rect::new(-1.0, -1.0, 2.0, 2.0)); + } + + #[test] + fn rect_transform() { + let rect = Rect::new(0.0, 0.0, 1.0, 1.0); + + assert_eq!( + rect.transform(&Matrix3::new( + 1.0, 0.0, 0.0, // + 0.0, 1.0, 0.0, // + 0.0, 0.0, 1.0, + )), + rect, + ); + + assert_eq!( + rect.transform(&Matrix3::new( + 2.0, 0.0, 0.0, // + 0.0, 2.0, 0.0, // + 0.0, 0.0, 2.0, + )), + Rect::new(0.0, 0.0, 2.0, 2.0), + ); + } + + #[test] + fn test_get_signed_triangle_area() { + assert_eq!( + get_signed_triangle_area( + Vector2::new(0.0, 0.0), + Vector2::new(0.0, 1.0), + Vector2::new(1.0, 0.0) + ), + 0.5 + ); + assert_eq!( + get_signed_triangle_area( + Vector2::new(1.0, 1.0), + Vector2::new(0.0, 1.0), + Vector2::new(1.0, 0.0) + ), + -0.5 + ); + } + + #[test] + fn test_wrap_angle() { + let angle = 0.5 * std::f32::consts::PI; + assert_eq!(wrap_angle(angle), angle); + assert_eq!(wrap_angle(-angle), 3.0 * angle); + } + + #[test] + fn test_ieee_remainder() { + assert_eq!(ieee_remainder(1.0, 2.0), -1.0); + assert_eq!(ieee_remainder(3.0, 2.0), -1.0); + + assert_eq!(ieee_remainder(1.0, 3.0), 1.0); + assert_eq!(ieee_remainder(4.0, 3.0), 1.0); + + assert_eq!(ieee_remainder(-1.0, 2.0), 1.0); + assert_eq!(ieee_remainder(-3.0, 2.0), 1.0); + } + + #[test] + fn test_round_to_step() { + assert_eq!(round_to_step(1.0, 2.0), 2.0); + assert_eq!(round_to_step(3.0, 2.0), 4.0); + + assert_eq!(round_to_step(-1.0, 2.0), -2.0); + assert_eq!(round_to_step(-3.0, 2.0), -4.0); + } + + #[test] + fn test_wrapf() { + assert_eq!(wrapf(5.0, 0.0, 10.0), 5.0); + assert_eq!(wrapf(5.0, 0.0, 0.0), 0.0); + assert_eq!(wrapf(2.0, 5.0, 10.0), 7.0); + assert_eq!(wrapf(12.0, 5.0, 10.0), 7.0); + } + + #[test] + fn test_cubicf_derivative() { + assert_eq!(cubicf_derivative(1.0, 1.0, 1.0, 1.0, 1.0), 0.0); + assert_eq!(cubicf_derivative(2.0, 1.0, 1.0, 1.0, 1.0), 1.0); + } + + #[test] + fn test_inf_sup_cubicf() { + assert_eq!(inf_sup_cubicf(1.0, 1.0, 1.0, 1.0), (1.0, 1.0)); + assert_eq!(inf_sup_cubicf(2.0, 2.0, 1.0, 1.0), (2.0, 2.0)); + } + + #[test] + fn test_get_farthest_point() { + let points = [ + Vector3::new(0.0, 0.0, 0.0), + Vector3::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + Vector3::new(1.0, 1.0, 1.0), + ]; + + assert_eq!( + get_farthest_point(&points, Vector3::new(1.0, 0.0, 0.0)), + Vector3::new(1.0, 0.0, 0.0) + ); + assert_eq!( + get_farthest_point(&points, Vector3::new(10.0, 0.0, 0.0)), + Vector3::new(1.0, 0.0, 0.0) + ); + assert_eq!( + get_farthest_point(&points, Vector3::new(1.0, 1.0, 0.0)), + Vector3::new(1.0, 1.0, 1.0) + ); + } + + #[test] + fn test_get_barycentric_coords() { + assert_eq!( + get_barycentric_coords( + &Vector3::new(0.0, 0.0, 0.0), + &Vector3::new(1.0, 0.0, 0.0), + &Vector3::new(0.0, 1.0, 0.0), + &Vector3::new(0.0, 0.0, 1.0), + ), + (0.33333328, 0.33333334, 0.33333334) + ); + } + + #[test] + fn test_get_barycentric_coords_2d() { + assert_eq!( + get_barycentric_coords_2d( + Vector2::new(0.0, 0.0), + Vector2::new(1.0, 0.0), + Vector2::new(0.0, 1.0), + Vector2::new(0.0, 0.0), + ), + (0.0, 0.0, 1.0) + ); + } + + #[test] + fn test_barycentric_to_world() { + assert_eq!( + barycentric_to_world( + (2.0, 2.0, 2.0), + Vector3::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ), + Vector3::new(2.0, 2.0, 2.0) + ); + } + + #[test] + fn test_barycentric_is_inside() { + assert!(barycentric_is_inside((0.0, 0.0, 0.0))); + assert!(barycentric_is_inside((0.5, 0.49, 0.0))); + + assert!(!barycentric_is_inside((0.5, 0.5, 0.0))); + assert!(!barycentric_is_inside((-0.5, 0.49, 0.0))); + assert!(!barycentric_is_inside((0.5, -0.49, 0.0))); + assert!(!barycentric_is_inside((-0.5, -0.49, 0.0))); + } + + #[test] + fn test_triangle_area() { + assert_eq!( + triangle_area( + Vector3::new(0.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ), + 0.5 + ); + } + + #[test] + fn test_spherical_to_cartesian() { + assert_eq!( + spherical_to_cartesian(0.0, 0.0, 1.0), + Vector3::new(0.0, 1.0, 0.0) + ); + } + + #[test] + fn partial_eq_for_triangle_edge() { + let te = TriangleEdge { a: 2, b: 5 }; + let te2 = TriangleEdge { a: 2, b: 5 }; + let te3 = TriangleEdge { a: 5, b: 2 }; + + assert_eq!(te, te2); + assert_eq!(te, te3); + } + + #[test] + fn triangle_definition_indices() { + assert_eq!(TriangleDefinition([0, 0, 0]).indices(), &[0, 0, 0]); + } + + #[test] + fn triangle_definition_indices_mut() { + assert_eq!(TriangleDefinition([0, 0, 0]).indices_mut(), &mut [0, 0, 0]); + } + + #[test] + fn triangle_definition_edges() { + let t = TriangleDefinition([0, 1, 2]); + assert_eq!( + t.edges(), + [ + TriangleEdge { a: 0, b: 1 }, + TriangleEdge { a: 1, b: 2 }, + TriangleEdge { a: 2, b: 0 } + ] + ); + } + + #[test] + fn index_for_triangle_definition() { + let t = TriangleDefinition([0, 1, 2]); + + assert_eq!(t[0], 0); + assert_eq!(t[1], 1); + assert_eq!(t[2], 2); + } + + #[test] + fn index_mut_for_triangle_definition() { + let mut t = TriangleDefinition([5, 5, 5]); + t[0] = 0; + t[1] = 1; + t[2] = 2; + + assert_eq!(t[0], 0); + assert_eq!(t[1], 1); + assert_eq!(t[2], 2); + } + + #[test] + fn position_provider_for_vector3() { + let v = Vector3::new(0.0, 1.0, 2.0); + + assert_eq!(v.position(), v); + } + + #[test] + fn as_ref_for_triangle_definition() { + let t = TriangleDefinition([0, 1, 2]); + + assert_eq!(t.as_ref(), &[0, 1, 2]); + } + + #[test] + fn as_mut_for_triangle_definition() { + let mut t = TriangleDefinition([0, 1, 2]); + + assert_eq!(t.as_mut(), &mut [0, 1, 2]); + } + + #[test] + fn test_get_closest_point() { + let points = [ + Vector3::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ]; + + assert_eq!( + get_closest_point(&points, Vector3::new(0.0, 0.0, 0.0)), + Some(0), + ); + assert_eq!( + get_closest_point(&points, Vector3::new(0.0, 1.0, 1.0)), + Some(1), + ); + assert_eq!( + get_closest_point(&points, Vector3::new(0.0, 0.0, 10.0)), + Some(2), + ); + } + + #[test] + fn test_get_closest_point_triangles() { + let points = [ + Vector3::new(0.0, 0.0, 0.0), + Vector3::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ]; + let triangles = [TriangleDefinition([0, 1, 2]), TriangleDefinition([1, 2, 3])]; + + assert_eq!( + get_closest_point_triangles( + &points, + &triangles, + [0, 1].into_iter(), + Vector3::new(1.0, 1.0, 1.0) + ), + Some((1, 0)) + ); + } + + #[test] + fn test_get_closest_point_triangle_set() { + let points = [ + Vector3::new(0.0, 0.0, 0.0), + Vector3::new(1.0, 0.0, 0.0), + Vector3::new(0.0, 1.0, 0.0), + Vector3::new(0.0, 0.0, 1.0), + ]; + let triangles = [TriangleDefinition([0, 1, 2]), TriangleDefinition([1, 2, 3])]; + + assert_eq!( + get_closest_point_triangle_set(&points, &triangles, Vector3::new(1.0, 1.0, 1.0)), + Some((1, 0)) + ); + } + + #[test] + fn smooth_angle_setters() { + let mut sa = SmoothAngle { + angle: 0.0, + speed: 0.0, + target: 0.0, + }; + + assert_eq!(sa.angle(), 0.0); + + sa.set_angle(std::f32::consts::PI); + assert_eq!(sa.angle(), std::f32::consts::PI); + + sa.set_target(std::f32::consts::PI); + assert_eq!(sa.target, std::f32::consts::PI); + + sa.set_speed(1.0); + assert_eq!(sa.speed, 1.0); + } + + #[test] + fn smooth_angle_turn_direction() { + assert_eq!( + SmoothAngle { + angle: 0.0, + speed: 0.0, + target: std::f32::consts::PI * 1.1, + } + .turn_direction(), + -1.0 + ); + + assert_eq!( + SmoothAngle { + angle: 0.0, + speed: 0.0, + target: -std::f32::consts::PI * 1.1, + } + .turn_direction(), + 1.0 + ); + + assert_eq!( + SmoothAngle { + angle: 0.0, + speed: 0.0, + target: -std::f32::consts::PI * 0.9, + } + .turn_direction(), + -1.0 + ); + + assert_eq!( + SmoothAngle { + angle: 0.0, + speed: 0.0, + target: std::f32::consts::PI * 0.9, + } + .turn_direction(), + 1.0 + ); + } + + #[test] + fn default_for_smooth_angle() { + let sa = SmoothAngle::default(); + + assert_eq!(sa.angle, 0.0); + assert_eq!(sa.target, 0.0); + assert_eq!(sa.speed, 1.0); + } + + #[test] + fn test_quat_from_euler() { + assert_eq!( + quat_from_euler( + Vector3::new( + std::f32::consts::PI, + std::f32::consts::PI, + std::f32::consts::PI + ), + RotationOrder::XYZ + ), + UnitQuaternion::from_euler_angles( + std::f32::consts::PI, + std::f32::consts::PI, + std::f32::consts::PI + ) + ); + } + + #[test] + fn matrix4_ext_for_matrix4() { + let m = Matrix4::new( + 1.0, 0.0, 0.0, 0.0, // + 0.0, 1.0, 0.0, 0.0, // + 0.0, 0.0, 1.0, 0.0, // + 0.0, 0.0, 0.0, 1.0, + ); + + assert_eq!(m.side(), Vector3::new(1.0, 0.0, 0.0)); + assert_eq!(m.up(), Vector3::new(0.0, 1.0, 0.0)); + assert_eq!(m.look(), Vector3::new(0.0, 0.0, 1.0)); + assert_eq!(m.position(), Vector3::new(0.0, 0.0, 0.0)); + assert_eq!( + m.basis(), + Matrix3::new( + 1.0, 0.0, 0.0, // + 0.0, 1.0, 0.0, // + 0.0, 0.0, 1.0, + ) + ); + } + + #[test] + fn matrix3_ext_for_matrix3() { + let m = Matrix3::new( + 1.0, 0.0, 0.0, // + 0.0, 1.0, 0.0, // + 0.0, 0.0, 1.0, + ); + + assert_eq!(m.side(), Vector3::new(1.0, 0.0, 0.0)); + assert_eq!(m.up(), Vector3::new(0.0, 1.0, 0.0)); + assert_eq!(m.look(), Vector3::new(0.0, 0.0, 1.0)); + } + + #[test] + fn vector3_ext_for_vector3() { + let mut v = Vector3::new(2.0, 2.0, 2.0); + + assert_eq!(v.sqr_distance(&Vector3::new(0.0, 1.0, 1.0)), 6.0); + + assert_eq!( + v.non_uniform_scale(&Vector3::new(3.0, 3.0, 3.0)), + Vector3::new(6.0, 6.0, 6.0) + ); + + v.follow(&Vector3::new(0.5, 0.5, 0.5), 2.0); + assert_eq!(v, Vector3::new(-1.0, -1.0, -1.0)); + } + + #[test] + fn vector2_ext_for_vector2() { + let mut v = Vector2::new(2.0, 2.0); + + assert_eq!( + v.per_component_min(&Vector2::new(0.0, 4.0)), + Vector2::new(0.0, 2.0) + ); + + assert_eq!( + v.per_component_max(&Vector2::new(0.0, 4.0)), + Vector2::new(2.0, 4.0) + ); + + v.follow(&Vector2::new(0.5, 0.5), 2.0); + assert_eq!(v, Vector2::new(-1.0, -1.0)); + } + + #[test] + fn test_m4x4_approx_eq() { + assert!(crate::m4x4_approx_eq( + &Matrix4::new( + 1.0, 0.0, 0.0, 0.0, // + 0.0, 1.0, 0.0, 0.0, // + 0.0, 0.0, 1.0, 0.0, // + 0.0, 0.0, 0.0, 1.0, + ), + &Matrix4::new( + 1.0001, 0.0001, 0.0001, 0.0001, // + 0.0001, 1.0001, 0.0001, 0.0001, // + 0.0001, 0.0001, 1.0001, 0.0001, // + 0.0001, 0.0001, 0.0001, 1.0001, + ) + ),); + } +} diff --git a/fyrox-core/src/octree.rs b/fyrox-math/src/octree.rs similarity index 99% rename from fyrox-core/src/octree.rs rename to fyrox-math/src/octree.rs index 4cc4e468c..22625cef1 100644 --- a/fyrox-core/src/octree.rs +++ b/fyrox-math/src/octree.rs @@ -1,6 +1,6 @@ -use crate::algebra::Vector3; -use crate::math::{aabb::AxisAlignedBoundingBox, ray::Ray}; +use crate::{aabb::AxisAlignedBoundingBox, ray::Ray}; use arrayvec::ArrayVec; +use nalgebra::Vector3; #[derive(Clone, Debug)] pub enum OctreeNode { diff --git a/fyrox-core/src/math/plane.rs b/fyrox-math/src/plane.rs similarity index 97% rename from fyrox-core/src/math/plane.rs rename to fyrox-math/src/plane.rs index 02d3eaecf..354c1f2b7 100644 --- a/fyrox-core/src/math/plane.rs +++ b/fyrox-math/src/plane.rs @@ -1,9 +1,6 @@ -use crate::{ - algebra::Vector3, - visitor::{Visit, VisitResult, Visitor}, -}; +use nalgebra::Vector3; -#[derive(Copy, Clone, Debug, PartialEq, Visit)] +#[derive(Copy, Clone, Debug, PartialEq)] pub struct Plane { pub normal: Vector3, pub d: f32, @@ -87,10 +84,9 @@ impl Plane { #[cfg(test)] mod test { + use crate::plane::Plane; use nalgebra::Vector3; - use crate::math::plane::Plane; - #[test] fn plane_sanity_tests() { // Computation test diff --git a/fyrox-core/src/math/ray.rs b/fyrox-math/src/ray.rs similarity index 98% rename from fyrox-core/src/math/ray.rs rename to fyrox-math/src/ray.rs index 8a669188f..ce8163d4e 100644 --- a/fyrox-core/src/math/ray.rs +++ b/fyrox-math/src/ray.rs @@ -1,9 +1,9 @@ // Clippy complains about normal mathematical symbols like A, B, C for quadratic equation. #![allow(clippy::many_single_char_names)] -use crate::algebra::{Matrix4, Point3, Vector3}; -use crate::math::aabb::AxisAlignedBoundingBox; -use crate::math::{is_point_inside_triangle, plane::Plane, solve_quadratic}; +use crate::aabb::AxisAlignedBoundingBox; +use crate::{is_point_inside_triangle, plane::Plane, solve_quadratic}; +use nalgebra::{Matrix4, Point3, Vector3}; #[derive(Copy, Clone, Debug)] pub struct Ray { @@ -439,10 +439,12 @@ impl Ray { mod test { use nalgebra::Matrix4; - use crate::math::aabb::AxisAlignedBoundingBox; - use crate::math::plane::Plane; - use crate::math::ray::{CylinderKind, Ray}; - use crate::math::Vector3; + use crate::{ + aabb::AxisAlignedBoundingBox, + plane::Plane, + ray::{CylinderKind, Ray}, + Vector3, + }; use super::IntersectionResult; diff --git a/fyrox-core/src/math/triangulator.rs b/fyrox-math/src/triangulator.rs similarity index 95% rename from fyrox-core/src/math/triangulator.rs rename to fyrox-math/src/triangulator.rs index ffbcaa29a..37e3cbe0a 100644 --- a/fyrox-core/src/math/triangulator.rs +++ b/fyrox-math/src/triangulator.rs @@ -1,4 +1,3 @@ -use crate::math; use nalgebra::{Vector2, Vector3}; use std::fmt; @@ -74,7 +73,7 @@ fn is_ear(poly: &Polygon, prev: &Vertex, ear: &Vertex, next: &Vertex) -> bool { if i != prev.index && i != ear.index && i != next.index - && math::is_point_inside_2d_triangle( + && crate::is_point_inside_2d_triangle( vertex.position, prev.position, ear.position, @@ -126,14 +125,14 @@ pub fn triangulate(vertices: &[Vector3], out_triangles: &mut Vec<[usize; 3] } else { // Ear-clipping for arbitrary polygon (requires one additional memory allocation, so // relatively slow) - if let Ok(normal) = math::get_polygon_normal(vertices) { - let plane_class = math::classify_plane(normal); + if let Ok(normal) = crate::get_polygon_normal(vertices) { + let plane_class = crate::classify_plane(normal); let mut polygon = Polygon { vertices: vertices .iter() .enumerate() .map(|(i, point)| Vertex { - position: math::vec3_to_vec2_by_plane(plane_class, normal, *point), + position: crate::vec3_to_vec2_by_plane(plane_class, normal, *point), index: i, prev: if i == 0 { vertices.len() - 1 } else { i - 1 }, next: if i == vertices.len() - 1 { 0 } else { i + 1 }, @@ -172,8 +171,8 @@ pub fn triangulate(vertices: &[Vector3], out_triangles: &mut Vec<[usize; 3] mod test { use nalgebra::Vector2; - use crate::algebra::{Point3, Unit, UnitQuaternion, Vector3}; - use crate::math::triangulator::triangulate; + use crate::triangulator::triangulate; + use nalgebra::{Point3, Unit, UnitQuaternion, Vector3}; use super::{Polygon, Vertex}; diff --git a/fyrox-resource/src/untyped.rs b/fyrox-resource/src/untyped.rs index 28ae8f835..2f1f209c1 100644 --- a/fyrox-resource/src/untyped.rs +++ b/fyrox-resource/src/untyped.rs @@ -11,7 +11,7 @@ use crate::{ Resource, ResourceData, ResourceLoadError, TypedResourceData, CURVE_RESOURCE_UUID, MODEL_RESOURCE_UUID, SHADER_RESOURCE_UUID, SOUND_BUFFER_RESOURCE_UUID, TEXTURE_RESOURCE_UUID, }; -use fyrox_core::curve::Curve; +use fyrox_core::math::curve::Curve; use fyrox_core::visitor::RegionGuard; use std::ffi::OsStr; use std::fmt::Display; diff --git a/fyrox-scripts/src/camera.rs b/fyrox-scripts/src/camera.rs index c7808f52b..623a98973 100644 --- a/fyrox-scripts/src/camera.rs +++ b/fyrox-scripts/src/camera.rs @@ -4,8 +4,8 @@ use fyrox::{ core::{ algebra::{UnitQuaternion, UnitVector3, Vector3}, - curve::{Curve, CurveKey, CurveKeyKind}, impl_component_provider, + math::curve::{Curve, CurveKey, CurveKeyKind}, math::Vector3Ext, reflect::prelude::*, uuid_provider, diff --git a/fyrox-ui/src/curve/key.rs b/fyrox-ui/src/curve/key.rs index e3afdd5c3..9f806f233 100644 --- a/fyrox-ui/src/curve/key.rs +++ b/fyrox-ui/src/curve/key.rs @@ -1,6 +1,6 @@ use crate::core::{ algebra::Vector2, - curve::{Curve, CurveKey, CurveKeyKind}, + math::curve::{Curve, CurveKey, CurveKeyKind}, reflect::prelude::*, uuid::Uuid, visitor::prelude::*, diff --git a/fyrox-ui/src/curve/mod.rs b/fyrox-ui/src/curve/mod.rs index 8d9a9f0ca..ad0630f76 100644 --- a/fyrox-ui/src/curve/mod.rs +++ b/fyrox-ui/src/curve/mod.rs @@ -3,7 +3,7 @@ use crate::{ core::{ algebra::{Matrix3, Point2, SimdPartialOrd, Vector2, Vector3}, color::Color, - curve::{Curve, CurveKeyKind}, + math::curve::{Curve, CurveKeyKind}, math::{cubicf, lerpf, wrap_angle, Rect}, pool::Handle, type_traits::prelude::*, diff --git a/fyrox-ui/src/inspector/editors/curve.rs b/fyrox-ui/src/inspector/editors/curve.rs index 88516ad11..80af7c84c 100644 --- a/fyrox-ui/src/inspector/editors/curve.rs +++ b/fyrox-ui/src/inspector/editors/curve.rs @@ -1,5 +1,5 @@ use crate::{ - core::{algebra::Vector2, curve::Curve}, + core::{algebra::Vector2, math::curve::Curve}, curve::{CurveEditorBuilder, CurveEditorMessage}, inspector::{ editors::{ diff --git a/fyrox-ui/src/inspector/editors/mod.rs b/fyrox-ui/src/inspector/editors/mod.rs index e77c508ad..a03129271 100644 --- a/fyrox-ui/src/inspector/editors/mod.rs +++ b/fyrox-ui/src/inspector/editors/mod.rs @@ -10,7 +10,7 @@ use crate::{ algebra::{UnitQuaternion, Vector2, Vector3, Vector4}, color::Color, color_gradient::ColorGradient, - curve::Curve, + math::curve::Curve, math::{Rect, SmoothAngle}, parking_lot::{RwLock, RwLockReadGuard}, pool::Handle, diff --git a/src/resource/curve/mod.rs b/src/resource/curve/mod.rs index 0e20ddbcb..df8536067 100644 --- a/src/resource/curve/mod.rs +++ b/src/resource/curve/mod.rs @@ -3,8 +3,8 @@ use crate::{ asset::{io::ResourceIo, Resource, ResourceData, CURVE_RESOURCE_UUID}, core::{ - curve::Curve, io::FileLoadError, reflect::prelude::*, uuid::Uuid, visitor::prelude::*, - TypeUuidProvider, + io::FileLoadError, math::curve::Curve, reflect::prelude::*, uuid::Uuid, + visitor::prelude::*, TypeUuidProvider, }, }; use std::error::Error; diff --git a/src/resource/fbx/mod.rs b/src/resource/fbx/mod.rs index d45b8d791..62604ca26 100644 --- a/src/resource/fbx/mod.rs +++ b/src/resource/fbx/mod.rs @@ -15,9 +15,9 @@ use crate::{ asset::manager::ResourceManager, core::{ algebra::{Matrix4, Point3, UnitQuaternion, Vector2, Vector3, Vector4}, - curve::{CurveKey, CurveKeyKind}, instant::Instant, log::{Log, MessageKind}, + math::curve::{CurveKey, CurveKeyKind}, math::{self, triangulator::triangulate, RotationOrder}, pool::Handle, sstorage::ImmutableString, diff --git a/src/scene/animation/mod.rs b/src/scene/animation/mod.rs index 02e03d9e6..f89159752 100644 --- a/src/scene/animation/mod.rs +++ b/src/scene/animation/mod.rs @@ -174,7 +174,7 @@ impl BoundValueCollectionExt for BoundValueCollection { /// ```rust /// use fyrox::{ /// core::{ -/// curve::{Curve, CurveKey, CurveKeyKind}, +/// math::curve::{Curve, CurveKey, CurveKeyKind}, /// pool::Handle, /// }, /// scene::{animation::prelude::*, base::BaseBuilder, graph::Graph, node::Node}, diff --git a/src/utils/lightmap.rs b/src/utils/lightmap.rs index 48185d559..3c240746c 100644 --- a/src/utils/lightmap.rs +++ b/src/utils/lightmap.rs @@ -12,7 +12,6 @@ use crate::{ algebra::{Matrix3, Matrix4, Point3, Vector2, Vector3, Vector4}, arrayvec::ArrayVec, math::{self, ray::Ray, Matrix4Ext, Rect, TriangleDefinition, Vector2Ext}, - octree::{Octree, OctreeNode}, pool::Handle, reflect::prelude::*, sstorage::ImmutableString, @@ -37,6 +36,7 @@ use crate::{ utils::{uvgen, uvgen::SurfaceDataPatch}, }; use fxhash::FxHashMap; +use fyrox_core::math::octree::{Octree, OctreeNode}; use rayon::prelude::*; use std::{ fmt::{Display, Formatter}, diff --git a/src/utils/navmesh.rs b/src/utils/navmesh.rs index 87283e4d1..d42a2cccc 100644 --- a/src/utils/navmesh.rs +++ b/src/utils/navmesh.rs @@ -10,7 +10,6 @@ use crate::{ algebra::{Point3, Vector3}, arrayvec::ArrayVec, math::{self, plane::Plane, ray::Ray, PositionProvider, TriangleDefinition, Vector3Ext}, - octree::{Octree, OctreeNode}, reflect::prelude::*, visitor::{Visit, VisitResult, Visitor}, }, @@ -24,6 +23,7 @@ use crate::{ }, }; use fxhash::{FxBuildHasher, FxHashMap}; +use fyrox_core::math::octree::{Octree, OctreeNode}; use std::ops::{Deref, DerefMut}; #[derive(Clone, Debug, Default, Visit)]