diff --git a/crates/foundation/apsis-math/src/lib.rs b/crates/foundation/apsis-math/src/lib.rs index 94f462c..86782ba 100644 --- a/crates/foundation/apsis-math/src/lib.rs +++ b/crates/foundation/apsis-math/src/lib.rs @@ -1 +1,37 @@ -//! Dependency-free vector, quaternion, and matrix types used across the ApsisRS workspace. +//! Dependency-free linear algebra for the ApsisRS workspace. +//! +//! The crate provides three types: +//! +//! - [`Vec3`], a three component column vector over `f64` +//! - [`Quat`], a unit quaternion used for rotations +//! - [`Mat3`], a three by three matrix stored in column-major order +//! +//! The types are plain data with no interior invariants beyond those +//! documented on each method, so their fields are public and a value can be +//! built by struct literal expression. Operations that are mathematically +//! undefined for some inputs, such as normalizing the zero vector, return +//! [`Option`] instead of panicking or producing a non-finite result. +//! +//! All rotations use the right-hand rule about the given axis, and all +//! angles are in radians. Angles are not wrapped on input; methods that +//! require a canonical range document that requirement. +//! +//! ``` +//! use apsis_math::Vec3; +//! +//! let x = Vec3::new(1.0, 0.0, 0.0); +//! let y = Vec3::new(0.0, 1.0, 0.0); +//! let z = x.cross(y); +//! assert_eq!(z, Vec3::new(0.0, 0.0, 1.0)); +//! ``` + +#![forbid(unsafe_code)] +#![deny(missing_docs)] + +mod mat3; +mod quat; +mod vec3; + +pub use mat3::Mat3; +pub use quat::Quat; +pub use vec3::Vec3; diff --git a/crates/foundation/apsis-math/src/mat3.rs b/crates/foundation/apsis-math/src/mat3.rs new file mode 100644 index 0000000..c9c17c7 --- /dev/null +++ b/crates/foundation/apsis-math/src/mat3.rs @@ -0,0 +1,532 @@ +//! Three by three matrices over `f64`. + +use crate::quat::Quat; +use crate::vec3::Vec3; +use core::fmt; +use core::ops::Mul; + +/// A three by three matrix stored in column-major order. +/// +/// The field names follow the `m` convention, so `m01` is the +/// entry in the first row and second column. The fields are listed in +/// column-major order, and the same order is used by [`Mat3::from_columns`] +/// and [`Mat3::column`]. +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct Mat3 { + /// First row, first column. + pub m00: f64, + /// Second row, first column. + pub m10: f64, + /// Third row, first column. + pub m20: f64, + /// First row, second column. + pub m01: f64, + /// Second row, second column. + pub m11: f64, + /// Third row, second column. + pub m21: f64, + /// First row, third column. + pub m02: f64, + /// Second row, third column. + pub m12: f64, + /// Third row, third column. + pub m22: f64, +} + +impl Mat3 { + /// The zero matrix. + pub const ZERO: Self = Self::from_rows_raw([0.0, 0.0, 0.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0]); + + /// The identity matrix. + pub const IDENTITY: Self = + Self::from_rows_raw([1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]); + + /// Creates a matrix from its nine entries in row-major order. + /// + /// The arguments are grouped by row: `row0`, then `row1`, then `row2`. + pub const fn from_rows(row0: [f64; 3], row1: [f64; 3], row2: [f64; 3]) -> Self { + Self::from_rows_raw(row0, row1, row2) + } + + /// Creates a matrix from three column vectors. + pub const fn from_columns(c0: Vec3, c1: Vec3, c2: Vec3) -> Self { + Self { + m00: c0.x, + m10: c0.y, + m20: c0.z, + m01: c1.x, + m11: c1.y, + m21: c1.z, + m02: c2.x, + m12: c2.y, + m22: c2.z, + } + } + + /// Creates a diagonal matrix from a vector of diagonal entries. + pub const fn from_diagonal(diagonal: Vec3) -> Self { + Self::from_rows_raw( + [diagonal.x, 0.0, 0.0], + [0.0, diagonal.y, 0.0], + [0.0, 0.0, diagonal.z], + ) + } + + /// Creates a rotation matrix from a unit quaternion. + /// + /// The quaternion is assumed to have unit length. A non-unit quaternion + /// produces a scaled matrix, which [`Mat3::is_rotation`] then rejects. + pub fn from_quat(q: Quat) -> Self { + let (w, x, y, z) = (q.w, q.x, q.y, q.z); + Self::from_rows( + [ + 1.0 - 2.0 * (y * y + z * z), + 2.0 * (x * y - w * z), + 2.0 * (x * z + w * y), + ], + [ + 2.0 * (x * y + w * z), + 1.0 - 2.0 * (x * x + z * z), + 2.0 * (y * z - w * x), + ], + [ + 2.0 * (x * z - w * y), + 2.0 * (y * z + w * x), + 1.0 - 2.0 * (x * x + y * y), + ], + ) + } + + /// Creates a rotation matrix of `angle` radians about the unit axis + /// `axis`. + /// + /// Returns `None` when the axis has zero or non-finite length. + pub fn from_axis_angle(axis: Vec3, angle: f64) -> Option { + Quat::from_axis_angle(axis, angle).map(Self::from_quat) + } + + /// Creates a rotation matrix of `angle` radians about the first axis. + pub fn from_rotation_x(angle: f64) -> Self { + Self::from_quat(Quat::from_rotation_x(angle)) + } + + /// Creates a rotation matrix of `angle` radians about the second axis. + pub fn from_rotation_y(angle: f64) -> Self { + Self::from_quat(Quat::from_rotation_y(angle)) + } + + /// Creates a rotation matrix of `angle` radians about the third axis. + pub fn from_rotation_z(angle: f64) -> Self { + Self::from_quat(Quat::from_rotation_z(angle)) + } + + /// Returns the entry in row `row` and column `column`. + /// + /// Returns `None` when either index is outside `0..=2`. + pub const fn get(self, row: usize, column: usize) -> Option { + match (row, column) { + (0, 0) => Some(self.m00), + (0, 1) => Some(self.m01), + (0, 2) => Some(self.m02), + (1, 0) => Some(self.m10), + (1, 1) => Some(self.m11), + (1, 2) => Some(self.m12), + (2, 0) => Some(self.m20), + (2, 1) => Some(self.m21), + (2, 2) => Some(self.m22), + _ => None, + } + } + + /// Returns column `index`. + /// + /// Returns `None` when `index` is outside `0..=2`. + pub const fn column(self, index: usize) -> Option { + match index { + 0 => Some(Vec3::new(self.m00, self.m10, self.m20)), + 1 => Some(Vec3::new(self.m01, self.m11, self.m21)), + 2 => Some(Vec3::new(self.m02, self.m12, self.m22)), + _ => None, + } + } + + /// Returns row `index`. + /// + /// Returns `None` when `index` is outside `0..=2`. + pub const fn row(self, index: usize) -> Option { + match index { + 0 => Some(Vec3::new(self.m00, self.m01, self.m02)), + 1 => Some(Vec3::new(self.m10, self.m11, self.m12)), + 2 => Some(Vec3::new(self.m20, self.m21, self.m22)), + _ => None, + } + } + + /// Returns the transpose. + pub const fn transpose(self) -> Self { + Self { + m00: self.m00, + m10: self.m01, + m20: self.m02, + m01: self.m10, + m11: self.m11, + m21: self.m12, + m02: self.m20, + m12: self.m21, + m22: self.m22, + } + } + + /// Returns the sum of the diagonal entries. + pub const fn trace(self) -> f64 { + self.m00 + self.m11 + self.m22 + } + + /// Returns the determinant. + pub fn determinant(self) -> f64 { + self.m00 * (self.m11 * self.m22 - self.m12 * self.m21) + - self.m01 * (self.m10 * self.m22 - self.m12 * self.m20) + + self.m02 * (self.m10 * self.m21 - self.m11 * self.m20) + } + + /// Returns `true` when this matrix is a rotation within `tolerance`. + /// + /// A matrix is a rotation when it is orthonormal with determinant one. + /// The check compares `M^T M` to the identity entry by entry and then + /// compares the determinant to one. + pub fn is_rotation(self, tolerance: f64) -> bool { + let product = self.transpose() * self; + let identity = Self::IDENTITY; + let orthonormal = (0..3).all(|row| { + (0..3).all( + |column| match (product.get(row, column), identity.get(row, column)) { + (Some(lhs), Some(rhs)) => (lhs - rhs).abs() <= tolerance, + _ => false, + }, + ) + }); + orthonormal && (self.determinant() - 1.0).abs() <= tolerance + } + + /// Returns `true` when all nine entries are finite. + pub fn is_finite(self) -> bool { + [ + self.m00, self.m10, self.m20, self.m01, self.m11, self.m21, self.m02, self.m12, + self.m22, + ] + .iter() + .all(|entry| entry.is_finite()) + } + + /// Returns a copy of this matrix with one entry replaced. + const fn with_entry(self, row: usize, column: usize, value: f64) -> Self { + let mut entries = [ + [self.m00, self.m01, self.m02], + [self.m10, self.m11, self.m12], + [self.m20, self.m21, self.m22], + ]; + entries[row][column] = value; + Self::from_rows_raw(entries[0], entries[1], entries[2]) + } + + /// Const constructor used by the associated constants. + const fn from_rows_raw(row0: [f64; 3], row1: [f64; 3], row2: [f64; 3]) -> Self { + Self { + m00: row0[0], + m10: row1[0], + m20: row2[0], + m01: row0[1], + m11: row1[1], + m21: row2[1], + m02: row0[2], + m12: row1[2], + m22: row2[2], + } + } +} + +impl Mul for Mat3 { + type Output = Self; + + fn mul(self, rhs: Self) -> Self { + let mut column = 0; + let mut result = Self::ZERO; + while column < 3 { + let mut row = 0; + while row < 3 { + let sum = match (self.row(row), rhs.column(column)) { + (Some(lhs), Some(rhs)) => lhs.dot(rhs), + _ => 0.0, + }; + result = result.with_entry(row, column, sum); + row += 1; + } + column += 1; + } + result + } +} + +impl Mul for Mat3 { + type Output = Vec3; + + fn mul(self, rhs: Vec3) -> Vec3 { + Vec3::new( + self.m00 * rhs.x + self.m01 * rhs.y + self.m02 * rhs.z, + self.m10 * rhs.x + self.m11 * rhs.y + self.m12 * rhs.z, + self.m20 * rhs.x + self.m21 * rhs.y + self.m22 * rhs.z, + ) + } +} + +impl Mul for Mat3 { + type Output = Self; + + fn mul(self, rhs: f64) -> Self { + Self { + m00: self.m00 * rhs, + m10: self.m10 * rhs, + m20: self.m20 * rhs, + m01: self.m01 * rhs, + m11: self.m11 * rhs, + m21: self.m21 * rhs, + m02: self.m02 * rhs, + m12: self.m12 * rhs, + m22: self.m22 * rhs, + } + } +} + +impl Mul for f64 { + type Output = Mat3; + + fn mul(self, rhs: Mat3) -> Mat3 { + rhs * self + } +} + +impl fmt::Display for Mat3 { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!( + f, + "[{}, {}, {}; {}, {}, {}; {}, {}, {}]", + self.m00, + self.m01, + self.m02, + self.m10, + self.m11, + self.m12, + self.m20, + self.m21, + self.m22 + ) + } +} + +#[cfg(test)] +mod tests { + use super::Mat3; + use crate::quat::Quat; + use crate::vec3::Vec3; + use core::f64::consts::FRAC_PI_2; + + const EPS: f64 = 1e-12; + + fn assert_mat_close(lhs: Mat3, rhs: Mat3) { + for row in 0..3 { + for column in 0..3 { + let (lhs, rhs) = ( + lhs.get(row, column).expect("indices are in range"), + rhs.get(row, column).expect("indices are in range"), + ); + assert!( + (lhs - rhs).abs() < EPS, + "at ({row}, {column}): {lhs} vs {rhs}" + ); + } + } + } + + #[test] + fn constructors_place_entries_in_the_right_slots() { + let m = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]); + assert_eq!(m.get(0, 0), Some(1.0)); + assert_eq!(m.get(0, 1), Some(2.0)); + assert_eq!(m.get(0, 2), Some(3.0)); + assert_eq!(m.get(1, 0), Some(4.0)); + assert_eq!(m.get(2, 2), Some(9.0)); + assert_eq!(m.get(3, 0), None); + assert_eq!(m.get(0, 3), None); + + assert_eq!(m.row(1), Some(Vec3::new(4.0, 5.0, 6.0))); + assert_eq!(m.column(1), Some(Vec3::new(2.0, 5.0, 8.0))); + assert_eq!(m.row(3), None); + assert_eq!(m.column(3), None); + } + + #[test] + fn from_columns_matches_from_rows() { + let c0 = Vec3::new(1.0, 4.0, 7.0); + let c1 = Vec3::new(2.0, 5.0, 8.0); + let c2 = Vec3::new(3.0, 6.0, 9.0); + let expected = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]); + assert_eq!(Mat3::from_columns(c0, c1, c2), expected); + } + + #[test] + fn identity_is_the_multiplicative_unit() { + let m = Mat3::from_rotation_z(FRAC_PI_2); + assert_eq!(Mat3::IDENTITY * m, m); + assert_eq!(m * Mat3::IDENTITY, m); + assert_eq!(Mat3::IDENTITY.determinant(), 1.0); + assert_eq!(Mat3::default(), Mat3::ZERO); + } + + #[test] + fn zero_annihilates() { + assert_eq!(Mat3::ZERO * Vec3::new(1.0, 2.0, 3.0), Vec3::ZERO); + assert_eq!(Mat3::ZERO * Mat3::IDENTITY, Mat3::ZERO); + assert_eq!(Mat3::ZERO.determinant(), 0.0); + } + + #[test] + fn transpose_swaps_rows_and_columns() { + let m = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]); + let t = m.transpose(); + assert_eq!(t.row(0), m.column(0)); + assert_eq!(t.row(1), m.column(1)); + assert_eq!(t.row(2), m.column(2)); + assert_eq!(t.transpose(), m); + } + + #[test] + fn determinant_matches_hand_computation() { + let m = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 10.0]); + assert!((m.determinant() + 3.0).abs() < EPS); + assert_eq!( + Mat3::from_diagonal(Vec3::new(2.0, 3.0, 4.0)).determinant(), + 24.0 + ); + } + + #[test] + fn trace_sums_the_diagonal() { + let m = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]); + assert_eq!(m.trace(), 15.0); + } + + #[test] + fn matrix_vector_product_uses_rows() { + let m = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]); + let v = Vec3::new(1.0, 1.0, 1.0); + assert_eq!(m * v, Vec3::new(6.0, 15.0, 24.0)); + } + + #[test] + fn matrix_product_is_associative() { + let a = Mat3::from_rows([1.0, 2.0, 0.0], [0.0, 1.0, 3.0], [1.0, 0.0, 1.0]); + let b = Mat3::from_rows([0.0, 1.0, 2.0], [1.0, 0.0, 1.0], [2.0, 1.0, 0.0]); + let c = Mat3::from_rows([1.0, 1.0, 1.0], [2.0, 2.0, 2.0], [3.0, 3.0, 3.0]); + assert_eq!((a * b) * c, a * (b * c)); + } + + #[test] + fn matrix_product_preserves_the_determinant_product() { + let a = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 10.0]); + let b = Mat3::from_rows([2.0, 0.0, 1.0], [1.0, 3.0, 2.0], [0.0, 1.0, 4.0]); + let expected = a.determinant() * b.determinant(); + assert!(((a * b).determinant() - expected).abs() < 1e-9); + } + + #[test] + fn scalar_product_scales_every_entry() { + let m = Mat3::from_rows([1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]); + let scaled = m * 2.0; + assert_eq!(scaled.get(0, 0), Some(2.0)); + assert_eq!(scaled.get(2, 2), Some(18.0)); + assert_eq!(2.0 * m, scaled); + } + + #[test] + fn rotation_matrices_satisfy_the_rotation_predicate() { + let m = Mat3::from_rotation_z(FRAC_PI_2); + assert!(m.is_rotation(1e-12)); + assert!((m.determinant() - 1.0).abs() < EPS); + assert_mat_close(m * m.transpose(), Mat3::IDENTITY); + } + + #[test] + fn is_rotation_rejects_non_rotations() { + assert!(!Mat3::ZERO.is_rotation(1e-9)); + assert!(!Mat3::from_diagonal(Vec3::splat(2.0)).is_rotation(1e-9)); + // A reflection has determinant minus one. + assert!(!Mat3::from_diagonal(Vec3::new(1.0, 1.0, -1.0)).is_rotation(1e-9)); + } + + #[test] + fn axis_angle_matches_the_quaternion_rotation() { + let axis = Vec3::new(0.5, -2.0, 1.0); + let m = Mat3::from_axis_angle(axis, 0.9).expect("non-zero axis"); + let q = Quat::from_axis_angle(axis, 0.9).expect("non-zero axis"); + let v = Vec3::new(1.0, -2.0, 3.0); + assert!((m * v - q.rotate(v)).norm() < EPS); + assert_eq!(Mat3::from_axis_angle(Vec3::ZERO, 1.0), None); + } + + #[test] + fn rotation_maps_axes_as_expected() { + let m = Mat3::from_rotation_z(FRAC_PI_2); + assert!((m * Vec3::X - Vec3::Y).norm() < EPS); + assert!((m * Vec3::Y + Vec3::X).norm() < EPS); + assert!((m * Vec3::Z - Vec3::Z).norm() < EPS); + } + + #[test] + fn per_axis_constructors_agree_with_the_axis_angle_form() { + let angle = 0.6; + let about_x = Mat3::from_rotation_x(angle); + assert_eq!( + about_x, + Mat3::from_axis_angle(Vec3::X, angle).expect("unit axis") + ); + assert_eq!(about_x * Vec3::X, Vec3::X); + + let about_y = Mat3::from_rotation_y(angle); + assert_eq!( + about_y, + Mat3::from_axis_angle(Vec3::Y, angle).expect("unit axis") + ); + assert_eq!(about_y * Vec3::Y, Vec3::Y); + + let about_z = Mat3::from_rotation_z(angle); + assert_eq!( + about_z, + Mat3::from_axis_angle(Vec3::Z, angle).expect("unit axis") + ); + assert_eq!(about_z * Vec3::Z, Vec3::Z); + assert!((about_z.determinant() - 1.0).abs() < EPS); + } + + #[test] + fn finiteness_is_reported() { + assert!(Mat3::IDENTITY.is_finite()); + let mut m = Mat3::IDENTITY; + m.m01 = f64::NAN; + assert!(!m.is_finite()); + } + + #[test] + fn display_uses_row_major_groups() { + let m = Mat3::from_diagonal(Vec3::new(1.0, 2.0, 3.0)); + assert_eq!(m.to_string(), "[1, 0, 0; 0, 2, 0; 0, 0, 3]"); + } + + #[test] + fn from_quat_is_consistent_with_quaternion_rotation() { + let q = Quat::from_axis_angle(Vec3::new(1.0, 1.0, 0.0), 1.3).expect("non-zero axis"); + let m = Mat3::from_quat(q); + let v = Vec3::new(2.0, -1.0, 0.5); + assert!((m * v - q.rotate(v)).norm() < EPS); + assert!(m.is_rotation(1e-12)); + } +} diff --git a/crates/foundation/apsis-math/src/quat.rs b/crates/foundation/apsis-math/src/quat.rs new file mode 100644 index 0000000..2b9ff8e --- /dev/null +++ b/crates/foundation/apsis-math/src/quat.rs @@ -0,0 +1,496 @@ +//! Unit quaternions used for rotations. + +use crate::vec3::Vec3; +use core::fmt; +use core::ops::Mul; + +/// A quaternion `w + xi + yj + zk`. +/// +/// Methods that rotate vectors assume the quaternion has unit length. +/// Multiplication and addition do not, because arbitrary quaternion +/// arithmetic is well defined without that assumption. Use +/// [`Quat::normalize`] to enforce unit length, and +/// [`Quat::is_normalized`] to check it. +/// +/// A rotation by angle `theta` about a unit axis `u` is +/// `w = cos(theta / 2)`, `(x, y, z) = u * sin(theta / 2)`. Composition uses +/// the same convention as matrix multiplication: `(a * b)` applied to a +/// vector applies `b` first, then `a`. +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct Quat { + /// Scalar part. + pub w: f64, + /// First component of the vector part. + pub x: f64, + /// Second component of the vector part. + pub y: f64, + /// Third component of the vector part. + pub z: f64, +} + +impl Quat { + /// The identity rotation. + pub const IDENTITY: Self = Self::new(1.0, 0.0, 0.0, 0.0); + + /// Creates a quaternion from its four components. + pub const fn new(w: f64, x: f64, y: f64, z: f64) -> Self { + Self { w, x, y, z } + } + + /// Creates a rotation of `angle` radians about the unit axis `axis`. + /// + /// The axis is normalized internally. Returns `None` when the axis has + /// zero or non-finite length. + pub fn from_axis_angle(axis: Vec3, angle: f64) -> Option { + let axis = axis.normalize()?; + let half = angle / 2.0; + let (sin, cos) = half.sin_cos(); + Some(Self::new(cos, axis.x * sin, axis.y * sin, axis.z * sin)) + } + + /// Creates a rotation of `angle` radians about the first axis. + pub fn from_rotation_x(angle: f64) -> Self { + let (sin, cos) = (angle / 2.0).sin_cos(); + Self::new(cos, sin, 0.0, 0.0) + } + + /// Creates a rotation of `angle` radians about the second axis. + pub fn from_rotation_y(angle: f64) -> Self { + let (sin, cos) = (angle / 2.0).sin_cos(); + Self::new(cos, 0.0, sin, 0.0) + } + + /// Creates a rotation of `angle` radians about the third axis. + pub fn from_rotation_z(angle: f64) -> Self { + let (sin, cos) = (angle / 2.0).sin_cos(); + Self::new(cos, 0.0, 0.0, sin) + } + + /// Creates a quaternion from the rotation matrix `m`. + /// + /// The algorithm picks the largest diagonal entry to divide by, which + /// keeps the square root well conditioned for every input rotation. + /// Returns `None` when `m` is not a rotation matrix, that is when it is + /// not orthonormal with determinant one within a tolerance of `1e-9`. + pub fn from_mat3(m: crate::mat3::Mat3) -> Option { + if !m.is_rotation(1e-9) { + return None; + } + let (m00, m01, m02) = (m.m00, m.m01, m.m02); + let (m10, m11, m12) = (m.m10, m.m11, m.m12); + let (m20, m21, m22) = (m.m20, m.m21, m.m22); + let trace = m00 + m11 + m22; + let quaternion = if trace > 0.0 { + let s = (trace + 1.0).sqrt() * 2.0; + Self::new(0.25 * s, (m21 - m12) / s, (m02 - m20) / s, (m10 - m01) / s) + } else if m00 > m11 && m00 > m22 { + let s = (1.0 + m00 - m11 - m22).sqrt() * 2.0; + Self::new((m21 - m12) / s, 0.25 * s, (m01 + m10) / s, (m02 + m20) / s) + } else if m11 > m22 { + let s = (1.0 + m11 - m00 - m22).sqrt() * 2.0; + Self::new((m02 - m20) / s, (m01 + m10) / s, 0.25 * s, (m12 + m21) / s) + } else { + let s = (1.0 + m22 - m00 - m11).sqrt() * 2.0; + Self::new((m10 - m01) / s, (m02 + m20) / s, (m12 + m21) / s, 0.25 * s) + }; + Some(quaternion) + } + + /// Returns the vector part `(x, y, z)`. + pub const fn vector(self) -> Vec3 { + Vec3::new(self.x, self.y, self.z) + } + + /// Returns the squared length of this quaternion. + pub fn norm_squared(self) -> f64 { + self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z + } + + /// Returns the length of this quaternion. + pub fn norm(self) -> f64 { + self.norm_squared().sqrt() + } + + /// Returns this quaternion scaled to unit length. + /// + /// Returns `None` when the length is zero or not finite. + pub fn normalize(self) -> Option { + let norm = self.norm(); + if norm > 0.0 && norm.is_finite() { + Some(Self::new( + self.w / norm, + self.x / norm, + self.y / norm, + self.z / norm, + )) + } else { + None + } + } + + /// Returns the conjugate `w - xi - yj - zk`. + /// + /// For a unit quaternion this is the inverse rotation. + pub const fn conjugate(self) -> Self { + Self::new(self.w, -self.x, -self.y, -self.z) + } + + /// Returns the inverse rotation. + /// + /// Returns `None` when the length is zero or not finite. + pub fn inverse(self) -> Option { + let norm_squared = self.norm_squared(); + if norm_squared > 0.0 && norm_squared.is_finite() { + let conjugate = self.conjugate(); + Some(Self::new( + conjugate.w / norm_squared, + conjugate.x / norm_squared, + conjugate.y / norm_squared, + conjugate.z / norm_squared, + )) + } else { + None + } + } + + /// Returns the dot product of `self` and `rhs`, treating both as four + /// component vectors. + pub fn dot(self, rhs: Self) -> f64 { + self.w * rhs.w + self.x * rhs.x + self.y * rhs.y + self.z * rhs.z + } + + /// Rotates `v` by this rotation. + /// + /// Assumes this quaternion has unit length. The rotation is computed as + /// `v + 2 * cross(vector, cross(vector, v) + w * v)`, which avoids + /// forming the quaternion product matrix twice. + pub fn rotate(self, v: Vec3) -> Vec3 { + let q = self.vector(); + let t = q.cross(v) * 2.0; + v + t * self.w + q.cross(t) + } + + /// Returns the rotation angle in radians, in `[0, pi]`. + /// + /// Assumes this quaternion has unit length. + pub fn angle(self) -> f64 { + 2.0 * self.w.clamp(-1.0, 1.0).acos() + } + + /// Returns the rotation axis as a unit vector. + /// + /// Returns `None` for the identity rotation, where no axis is defined. + /// Assumes this quaternion has unit length. + pub fn axis(self) -> Option { + self.vector().normalize() + } + + /// Returns the linear interpolation from `self` to `rhs`. + /// + /// This is a chord interpolation, so the result is not a unit + /// quaternion and the angular speed is not constant. Use + /// [`Quat::slerp`] when the intermediate rotations must be uniform. + pub fn lerp(self, rhs: Self, t: f64) -> Self { + let w = self.w + (rhs.w - self.w) * t; + let x = self.x + (rhs.x - self.x) * t; + let y = self.y + (rhs.y - self.y) * t; + let z = self.z + (rhs.z - self.z) * t; + Self::new(w, x, y, z) + } + + /// Returns the spherical linear interpolation from `self` to `rhs`. + /// + /// The shortest of the two arcs is taken, so the result follows the + /// path a rotation would take if the angle were kept minimal. Falls + /// back to [`Quat::lerp`] when the inputs are nearly parallel, where + /// the sine in the denominator would otherwise lose precision. + pub fn slerp(self, rhs: Self, t: f64) -> Self { + let mut end = rhs; + let mut dot = self.dot(rhs); + if dot < 0.0 { + // Negating one endpoint selects the shorter arc, because `q` + // and `-q` describe the same rotation. + end = Self::new(-rhs.w, -rhs.x, -rhs.y, -rhs.z); + dot = -dot; + } + if dot > 0.999_5 { + return self.lerp(end, t); + } + let theta = dot.clamp(-1.0, 1.0).acos(); + let sin_theta = theta.sin(); + if sin_theta == 0.0 { + return self; + } + let scale_start = ((1.0 - t) * theta).sin() / sin_theta; + let scale_end = (t * theta).sin() / sin_theta; + Self::new( + self.w * scale_start + end.w * scale_end, + self.x * scale_start + end.x * scale_end, + self.y * scale_start + end.y * scale_end, + self.z * scale_start + end.z * scale_end, + ) + } + + /// Returns `true` when the length is one within a tolerance of `1e-9`. + pub fn is_normalized(self) -> bool { + (self.norm_squared() - 1.0).abs() < 1e-9 + } + + /// Returns `true` when all four components are finite. + pub fn is_finite(self) -> bool { + self.w.is_finite() && self.x.is_finite() && self.y.is_finite() && self.z.is_finite() + } + + /// Returns a rotation of a quarter turn about the first axis. + pub fn quarter_turn_x() -> Self { + Self::from_rotation_x(core::f64::consts::FRAC_PI_2) + } + + /// Returns a rotation of a half turn about the third axis. + pub fn half_turn_z() -> Self { + Self::from_rotation_z(core::f64::consts::PI) + } +} + +impl Mul for Quat { + type Output = Self; + + fn mul(self, rhs: Self) -> Self { + // Hamilton product, matching the convention that `(a * b)` + // applies `b` first. + Self::new( + self.w * rhs.w - self.x * rhs.x - self.y * rhs.y - self.z * rhs.z, + self.w * rhs.x + self.x * rhs.w + self.y * rhs.z - self.z * rhs.y, + self.w * rhs.y - self.x * rhs.z + self.y * rhs.w + self.z * rhs.x, + self.w * rhs.z + self.x * rhs.y - self.y * rhs.x + self.z * rhs.w, + ) + } +} + +impl fmt::Display for Quat { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, "({}, {}, {}, {})", self.w, self.x, self.y, self.z) + } +} + +#[cfg(test)] +mod tests { + use super::Quat; + use crate::mat3::Mat3; + use crate::vec3::Vec3; + use core::f64::consts::{FRAC_PI_2, PI}; + + const EPS: f64 = 1e-12; + + fn assert_close(lhs: Vec3, rhs: Vec3) { + assert!((lhs - rhs).norm() < EPS, "left {lhs}, right {rhs}"); + } + + fn assert_quat_close(lhs: Quat, rhs: Quat) { + let difference = Quat::new(lhs.w - rhs.w, lhs.x - rhs.x, lhs.y - rhs.y, lhs.z - rhs.z); + assert!(difference.norm() < EPS, "left {lhs}, right {rhs}"); + } + + #[test] + fn identity_rotates_nothing() { + let v = Vec3::new(1.0, 2.0, 3.0); + assert_close(Quat::IDENTITY.rotate(v), v); + assert_eq!(Quat::IDENTITY.angle(), 0.0); + assert_eq!(Quat::IDENTITY.axis(), None); + } + + #[test] + fn axis_angle_round_trips() { + let axis = Vec3::new(1.0, 2.0, -2.0) + .normalize() + .expect("non-zero axis"); + let q = Quat::from_axis_angle(axis, 0.7).expect("non-zero axis"); + assert!((q.angle() - 0.7).abs() < EPS); + assert_close(q.axis().expect("rotation has an axis"), axis); + assert!(q.is_normalized()); + } + + #[test] + fn axis_angle_rejects_undefined_axes() { + assert_eq!(Quat::from_axis_angle(Vec3::ZERO, 1.0), None); + assert_eq!(Quat::from_axis_angle(Vec3::splat(f64::NAN), 1.0), None); + } + + #[test] + fn quarter_turn_about_z_maps_x_to_y() { + let q = Quat::from_rotation_z(FRAC_PI_2); + assert_close(q.rotate(Vec3::X), Vec3::Y); + assert_close(q.rotate(Vec3::Y), -Vec3::X); + assert_close(q.rotate(Vec3::Z), Vec3::Z); + } + + #[test] + fn quarter_turn_about_x_maps_y_to_z() { + let q = Quat::from_rotation_x(FRAC_PI_2); + assert_close(q.rotate(Vec3::Y), Vec3::Z); + assert_close(q.rotate(Vec3::Z), -Vec3::Y); + } + + #[test] + fn quarter_turn_about_y_maps_z_to_x() { + let q = Quat::from_rotation_y(FRAC_PI_2); + assert_close(q.rotate(Vec3::Z), Vec3::X); + assert_close(q.rotate(Vec3::X), -Vec3::Z); + } + + #[test] + fn rotation_preserves_length() { + let q = Quat::from_axis_angle(Vec3::new(1.0, -1.0, 0.5), 1.234).expect("non-zero axis"); + let v = Vec3::new(3.0, -4.0, 12.0); + assert!((q.rotate(v).norm() - v.norm()).abs() < EPS); + } + + #[test] + fn conjugate_undoes_the_rotation() { + let q = Quat::from_axis_angle(Vec3::new(0.0, 1.0, 1.0), 0.9).expect("non-zero axis"); + let v = Vec3::new(1.0, -2.0, 0.5); + assert_close(q.conjugate().rotate(q.rotate(v)), v); + } + + #[test] + fn inverse_undoes_the_rotation() { + let q = Quat::from_axis_angle(Vec3::new(-0.5, 1.0, 0.25), 2.0).expect("non-zero axis"); + let v = Vec3::new(2.0, 0.0, -1.0); + let inverse = q.inverse().expect("unit quaternion is invertible"); + assert_close(inverse.rotate(q.rotate(v)), v); + } + + #[test] + fn inverse_rejects_degenerate_quaternions() { + assert_eq!(Quat::new(0.0, 0.0, 0.0, 0.0).inverse(), None); + assert_eq!(Quat::new(f64::NAN, 0.0, 0.0, 0.0).inverse(), None); + } + + #[test] + fn multiplication_composes_right_to_left() { + let first = Quat::from_rotation_z(FRAC_PI_2); + let second = Quat::from_rotation_x(FRAC_PI_2); + let v = Vec3::new(1.0, 0.0, 0.0); + // `second * first` applies `first` first, then `second`. + assert_close((second * first).rotate(v), second.rotate(first.rotate(v))); + } + + #[test] + fn identity_is_the_multiplicative_unit() { + let q = Quat::from_rotation_y(0.4); + assert_quat_close(q * Quat::IDENTITY, q); + assert_quat_close(Quat::IDENTITY * q, q); + } + + #[test] + fn multiplication_matches_matrix_composition() { + let a = Quat::from_axis_angle(Vec3::new(1.0, 0.5, -0.25), 0.8).expect("non-zero axis"); + let b = Quat::from_axis_angle(Vec3::new(-0.5, 1.0, 0.75), 1.6).expect("non-zero axis"); + let composed = a * b; + let v = Vec3::new(1.0, 2.0, 3.0); + let via_matrices = Mat3::from_quat(a) * (Mat3::from_quat(b) * v); + assert_close(composed.rotate(v), via_matrices); + } + + #[test] + fn normalize_produces_unit_length() { + let q = Quat::new(2.0, 0.0, 0.0, 0.0); + let unit = q.normalize().expect("non-zero quaternion"); + assert!(unit.is_normalized()); + assert!((unit.w - 1.0).abs() < EPS); + } + + #[test] + fn normalize_rejects_degenerate_quaternions() { + assert_eq!(Quat::new(0.0, 0.0, 0.0, 0.0).normalize(), None); + assert_eq!(Quat::new(f64::NAN, 0.0, 0.0, 0.0).normalize(), None); + } + + #[test] + fn from_mat3_recovers_the_original_rotation() { + let q = Quat::from_axis_angle(Vec3::new(0.3, -0.4, 0.5), 1.1).expect("non-zero axis"); + let recovered = Quat::from_mat3(Mat3::from_quat(q)).expect("rotation matrix is accepted"); + // `q` and `-q` are the same rotation, so compare through their effect. + let v = Vec3::new(1.0, -2.0, 3.0); + assert_close(recovered.rotate(v), q.rotate(v)); + } + + #[test] + fn from_mat3_covers_every_branch() { + // These angles exercise the trace branch and each of the three + // largest-diagonal branches. + for (axis, angle) in [ + (Vec3::new(1.0, 0.0, 0.0), 0.2), + (Vec3::new(1.0, 0.0, 0.0), PI - 0.1), + (Vec3::new(0.0, 1.0, 0.0), PI - 0.1), + (Vec3::new(0.0, 0.0, 1.0), PI - 0.1), + ] { + let q = Quat::from_axis_angle(axis, angle).expect("non-zero axis"); + let recovered = + Quat::from_mat3(Mat3::from_quat(q)).expect("rotation matrix is accepted"); + let v = Vec3::new(0.5, 1.5, -2.5); + assert_close(recovered.rotate(v), q.rotate(v)); + } + } + + #[test] + fn from_mat3_rejects_non_rotations() { + let scaled = Mat3::from_diagonal(Vec3::splat(2.0)); + assert_eq!(Quat::from_mat3(scaled), None); + assert_eq!(Quat::from_mat3(Mat3::ZERO), None); + } + + #[test] + fn slerp_walks_the_short_arc_uniformly() { + let start = Quat::IDENTITY; + let end = Quat::from_rotation_z(PI); + let half = start.slerp(end, 0.5); + assert!((half.angle() - FRAC_PI_2).abs() < 1e-9); + assert_close(half.rotate(Vec3::X), Vec3::Y); + } + + #[test] + fn slerp_selects_the_short_arc_when_endpoints_are_negated() { + let start = Quat::IDENTITY; + let end = Quat::new(-1.0, 0.0, 0.0, 0.0); + // The two quaternions describe the same rotation, so any + // interpolation between them must stay at the identity. + let middle = start.slerp(end, 0.5); + assert_close(middle.rotate(Vec3::X), Vec3::X); + } + + #[test] + fn slerp_hits_both_endpoints() { + let start = Quat::from_rotation_x(0.3); + let end = Quat::from_rotation_y(1.2); + assert_quat_close(start.slerp(end, 0.0), start); + assert_quat_close(start.slerp(end, 1.0), end); + } + + #[test] + fn slerp_handles_nearly_parallel_inputs() { + let start = Quat::IDENTITY; + let end = Quat::from_rotation_z(1e-9); + let middle = start.slerp(end, 0.5); + assert!(middle.is_normalized()); + } + + #[test] + fn display_lists_the_components() { + assert_eq!(Quat::new(1.0, 0.0, 0.0, 0.0).to_string(), "(1, 0, 0, 0)"); + } + + #[test] + fn finiteness_is_reported() { + assert!(Quat::IDENTITY.is_finite()); + assert!(!Quat::new(f64::INFINITY, 0.0, 0.0, 0.0).is_finite()); + } + + #[test] + fn named_turns_behave_as_documented() { + assert_close(Quat::quarter_turn_x().rotate(Vec3::Y), Vec3::Z); + assert_close(Quat::half_turn_z().rotate(Vec3::X), -Vec3::X); + assert!(Quat::quarter_turn_x().is_normalized()); + assert!(Quat::half_turn_z().is_normalized()); + } +} diff --git a/crates/foundation/apsis-math/src/vec3.rs b/crates/foundation/apsis-math/src/vec3.rs new file mode 100644 index 0000000..e596dcf --- /dev/null +++ b/crates/foundation/apsis-math/src/vec3.rs @@ -0,0 +1,416 @@ +//! Three component vectors over `f64`. + +use core::fmt; +use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign}; + +/// A three component column vector. +/// +/// The components are named `x`, `y`, and `z`. No meaning is attached to +/// them here; the reference frame is defined by the crate that uses the +/// vector. +#[derive(Debug, Clone, Copy, PartialEq, Default)] +pub struct Vec3 { + /// First component. + pub x: f64, + /// Second component. + pub y: f64, + /// Third component. + pub z: f64, +} + +impl Vec3 { + /// The zero vector. + pub const ZERO: Self = Self::new(0.0, 0.0, 0.0); + + /// The unit vector along the first axis. + pub const X: Self = Self::new(1.0, 0.0, 0.0); + + /// The unit vector along the second axis. + pub const Y: Self = Self::new(0.0, 1.0, 0.0); + + /// The unit vector along the third axis. + pub const Z: Self = Self::new(0.0, 0.0, 1.0); + + /// Creates a vector from its three components. + pub const fn new(x: f64, y: f64, z: f64) -> Self { + Self { x, y, z } + } + + /// Creates a vector whose three components all equal `value`. + pub const fn splat(value: f64) -> Self { + Self::new(value, value, value) + } + + /// Returns the first component. + pub const fn x(self) -> f64 { + self.x + } + + /// Returns the second component. + pub const fn y(self) -> f64 { + self.y + } + + /// Returns the third component. + pub const fn z(self) -> f64 { + self.z + } + + /// Returns the component selected by `index`, where `0`, `1`, and `2` + /// select `x`, `y`, and `z` respectively. + /// + /// Returns `None` for an index outside `0..=2`. + pub const fn get(self, index: usize) -> Option { + match index { + 0 => Some(self.x), + 1 => Some(self.y), + 2 => Some(self.z), + _ => None, + } + } + + /// Returns the dot product of `self` and `rhs`. + pub fn dot(self, rhs: Self) -> f64 { + self.x * rhs.x + self.y * rhs.y + self.z * rhs.z + } + + /// Returns the cross product `self x rhs`. + /// + /// The result is perpendicular to both inputs and follows the + /// right-hand rule, so `Vec3::X.cross(Vec3::Y)` equals `Vec3::Z`. + pub fn cross(self, rhs: Self) -> Self { + Self::new( + self.y * rhs.z - self.z * rhs.y, + self.z * rhs.x - self.x * rhs.z, + self.x * rhs.y - self.y * rhs.x, + ) + } + + /// Returns the squared Euclidean length of this vector. + /// + /// Prefer this over [`Vec3::norm`] when comparing lengths, because it + /// avoids a square root. + pub fn norm_squared(self) -> f64 { + self.dot(self) + } + + /// Returns the Euclidean length of this vector. + pub fn norm(self) -> f64 { + self.norm_squared().sqrt() + } + + /// Returns this vector scaled to unit length. + /// + /// Returns `None` when the length is zero or not finite, in which case + /// no direction is defined. + pub fn normalize(self) -> Option { + let norm = self.norm(); + if norm > 0.0 && norm.is_finite() { + Some(self / norm) + } else { + None + } + } + + /// Returns the Euclidean distance between `self` and `rhs`. + pub fn distance(self, rhs: Self) -> f64 { + (self - rhs).norm() + } + + /// Returns the squared Euclidean distance between `self` and `rhs`. + pub fn distance_squared(self, rhs: Self) -> f64 { + (self - rhs).norm_squared() + } + + /// Returns the angle between `self` and `rhs`, in radians. + /// + /// The result lies in `[0, pi]`. Returns `None` when either vector has + /// zero or non-finite length, or when either vector has a non-finite + /// component. + pub fn angle_between(self, rhs: Self) -> Option { + let denominator = self.norm() * rhs.norm(); + if denominator > 0.0 && denominator.is_finite() { + // The cosine can exceed one in magnitude through rounding, so + // it is clamped to keep the arc cosine defined. + Some((self.dot(rhs) / denominator).clamp(-1.0, 1.0).acos()) + } else { + None + } + } + + /// Returns the linear interpolation `self + t * (rhs - self)`. + /// + /// `t = 0` returns `self` and `t = 1` returns `rhs`. Values outside + /// `[0, 1]` extrapolate. + pub fn lerp(self, rhs: Self, t: f64) -> Self { + self + (rhs - self) * t + } + + /// Returns a vector with each component replaced by its absolute value. + pub fn abs(self) -> Self { + Self::new(self.x.abs(), self.y.abs(), self.z.abs()) + } + + /// Returns the largest component. + pub fn max_component(self) -> f64 { + self.x.max(self.y).max(self.z) + } + + /// Returns the smallest component. + pub fn min_component(self) -> f64 { + self.x.min(self.y).min(self.z) + } + + /// Returns `true` when all three components are finite. + pub fn is_finite(self) -> bool { + self.x.is_finite() && self.y.is_finite() && self.z.is_finite() + } +} + +impl Add for Vec3 { + type Output = Self; + + fn add(self, rhs: Self) -> Self { + Self::new(self.x + rhs.x, self.y + rhs.y, self.z + rhs.z) + } +} + +impl Sub for Vec3 { + type Output = Self; + + fn sub(self, rhs: Self) -> Self { + Self::new(self.x - rhs.x, self.y - rhs.y, self.z - rhs.z) + } +} + +impl Neg for Vec3 { + type Output = Self; + + fn neg(self) -> Self { + Self::new(-self.x, -self.y, -self.z) + } +} + +impl Mul for Vec3 { + type Output = Self; + + fn mul(self, rhs: f64) -> Self { + Self::new(self.x * rhs, self.y * rhs, self.z * rhs) + } +} + +impl Mul for f64 { + type Output = Vec3; + + fn mul(self, rhs: Vec3) -> Vec3 { + rhs * self + } +} + +impl Div for Vec3 { + type Output = Self; + + fn div(self, rhs: f64) -> Self { + Self::new(self.x / rhs, self.y / rhs, self.z / rhs) + } +} + +impl AddAssign for Vec3 { + fn add_assign(&mut self, rhs: Self) { + *self = *self + rhs; + } +} + +impl SubAssign for Vec3 { + fn sub_assign(&mut self, rhs: Self) { + *self = *self - rhs; + } +} + +impl MulAssign for Vec3 { + fn mul_assign(&mut self, rhs: f64) { + *self = *self * rhs; + } +} + +impl DivAssign for Vec3 { + fn div_assign(&mut self, rhs: f64) { + *self = *self / rhs; + } +} + +impl fmt::Display for Vec3 { + fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result { + write!(f, "({}, {}, {})", self.x, self.y, self.z) + } +} + +#[cfg(test)] +mod tests { + use super::Vec3; + + const EPS: f64 = 1e-12; + + fn assert_close(lhs: Vec3, rhs: Vec3) { + assert!((lhs - rhs).norm() < EPS, "left {lhs}, right {rhs}"); + } + + #[test] + fn constructors_set_the_components() { + assert_eq!( + Vec3::new(1.0, 2.0, 3.0), + Vec3 { + x: 1.0, + y: 2.0, + z: 3.0 + } + ); + assert_eq!(Vec3::splat(2.0), Vec3::new(2.0, 2.0, 2.0)); + assert_eq!(Vec3::default(), Vec3::ZERO); + } + + #[test] + fn get_selects_by_index_and_rejects_out_of_range() { + let v = Vec3::new(1.0, 2.0, 3.0); + assert_eq!(v.get(0), Some(1.0)); + assert_eq!(v.get(1), Some(2.0)); + assert_eq!(v.get(2), Some(3.0)); + assert_eq!(v.get(3), None); + } + + #[test] + fn dot_product_is_symmetric() { + let a = Vec3::new(1.0, 2.0, 3.0); + let b = Vec3::new(-4.0, 5.0, 0.5); + assert!((a.dot(b) - b.dot(a)).abs() < EPS); + assert!((a.dot(b) - 7.5).abs() < EPS); + } + + #[test] + fn cross_product_follows_the_right_hand_rule() { + assert_eq!(Vec3::X.cross(Vec3::Y), Vec3::Z); + assert_eq!(Vec3::Y.cross(Vec3::Z), Vec3::X); + assert_eq!(Vec3::Z.cross(Vec3::X), Vec3::Y); + // The cross product of parallel vectors vanishes. + assert_eq!(Vec3::X.cross(Vec3::X), Vec3::ZERO); + } + + #[test] + fn cross_product_is_orthogonal_to_its_inputs() { + let a = Vec3::new(1.0, -2.0, 3.5); + let b = Vec3::new(0.5, 4.0, -1.0); + let c = a.cross(b); + assert!(c.dot(a).abs() < EPS); + assert!(c.dot(b).abs() < EPS); + } + + #[test] + fn cross_product_is_antisymmetric() { + let a = Vec3::new(1.0, 2.0, 3.0); + let b = Vec3::new(4.0, 5.0, 6.0); + assert_eq!(a.cross(b), -b.cross(a)); + } + + #[test] + fn norm_matches_the_pythagorean_length() { + let v = Vec3::new(3.0, 4.0, 12.0); + assert!((v.norm_squared() - 169.0).abs() < EPS); + assert!((v.norm() - 13.0).abs() < EPS); + } + + #[test] + fn normalize_produces_a_unit_vector() { + let v = Vec3::new(-3.0, 0.0, 4.0); + let unit = v.normalize().expect("non-zero vector has a direction"); + assert!((unit.norm() - 1.0).abs() < EPS); + assert_close(unit, Vec3::new(-0.6, 0.0, 0.8)); + } + + #[test] + fn normalize_rejects_undefined_directions() { + assert_eq!(Vec3::ZERO.normalize(), None); + assert_eq!(Vec3::splat(f64::INFINITY).normalize(), None); + assert_eq!(Vec3::splat(f64::NAN).normalize(), None); + } + + #[test] + fn distance_measures_separation() { + let a = Vec3::new(1.0, 1.0, 1.0); + let b = Vec3::new(1.0, 1.0, 4.0); + assert!((a.distance(b) - 3.0).abs() < EPS); + assert!((a.distance_squared(b) - 9.0).abs() < EPS); + } + + #[test] + fn angle_between_uses_the_right_hand_geometry() { + let angle = Vec3::X + .angle_between(Vec3::Y) + .expect("both axes have unit length"); + assert!((angle - core::f64::consts::FRAC_PI_2).abs() < EPS); + let opposite = Vec3::X + .angle_between(-Vec3::X) + .expect("both vectors have unit length"); + assert!((opposite - core::f64::consts::PI).abs() < EPS); + let parallel = Vec3::X + .angle_between(Vec3::X) + .expect("both vectors have unit length"); + assert!(parallel.abs() < EPS); + } + + #[test] + fn angle_between_rejects_undefined_directions() { + assert_eq!(Vec3::ZERO.angle_between(Vec3::X), None); + assert_eq!(Vec3::X.angle_between(Vec3::ZERO), None); + } + + #[test] + fn lerp_hits_both_endpoints() { + let a = Vec3::new(0.0, 0.0, 0.0); + let b = Vec3::new(2.0, 4.0, 6.0); + assert_eq!(a.lerp(b, 0.0), a); + assert_eq!(a.lerp(b, 1.0), b); + assert_eq!(a.lerp(b, 0.5), Vec3::new(1.0, 2.0, 3.0)); + } + + #[test] + fn component_reductions_report_extremes() { + let v = Vec3::new(-2.0, 5.0, 1.0); + assert!((v.max_component() - 5.0).abs() < EPS); + assert!((v.min_component() + 2.0).abs() < EPS); + assert_close(v.abs(), Vec3::new(2.0, 5.0, 1.0)); + } + + #[test] + fn finiteness_is_reported() { + assert!(Vec3::new(1.0, 2.0, 3.0).is_finite()); + assert!(!Vec3::new(1.0, f64::NAN, 3.0).is_finite()); + assert!(!Vec3::splat(f64::INFINITY).is_finite()); + } + + #[test] + fn arithmetic_operators_match_component_wise_results() { + let a = Vec3::new(1.0, 2.0, 3.0); + let b = Vec3::new(4.0, 5.0, 6.0); + assert_eq!(a + b, Vec3::new(5.0, 7.0, 9.0)); + assert_eq!(b - a, Vec3::new(3.0, 3.0, 3.0)); + assert_eq!(-a, Vec3::new(-1.0, -2.0, -3.0)); + assert_eq!(a * 2.0, Vec3::new(2.0, 4.0, 6.0)); + assert_eq!(2.0 * a, a * 2.0); + assert_eq!(b / 2.0, Vec3::new(2.0, 2.5, 3.0)); + + let mut acc = a; + acc += b; + assert_eq!(acc, a + b); + acc -= b; + assert_eq!(acc, a); + acc *= 3.0; + assert_eq!(acc, a * 3.0); + acc /= 3.0; + assert_eq!(acc, a); + } + + #[test] + fn display_lists_the_components() { + assert_eq!(Vec3::new(1.0, 2.5, -3.0).to_string(), "(1, 2.5, -3)"); + } +}