foundation: add vector, quaternion, and matrix types
This commit is contained in:
ApsisRS contributors
parent
af3b1c77d4
commit
2afb62bd14
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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<row><column>` 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<Self> {
|
||||
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<f64> {
|
||||
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<Vec3> {
|
||||
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<Vec3> {
|
||||
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<Vec3> 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<f64> 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<Mat3> 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));
|
||||
}
|
||||
}
|
||||
|
|
@ -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<Self> {
|
||||
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<Self> {
|
||||
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<Self> {
|
||||
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<Self> {
|
||||
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<Vec3> {
|
||||
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());
|
||||
}
|
||||
}
|
||||
|
|
@ -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<f64> {
|
||||
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<Self> {
|
||||
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<f64> {
|
||||
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<f64> for Vec3 {
|
||||
type Output = Self;
|
||||
|
||||
fn mul(self, rhs: f64) -> Self {
|
||||
Self::new(self.x * rhs, self.y * rhs, self.z * rhs)
|
||||
}
|
||||
}
|
||||
|
||||
impl Mul<Vec3> for f64 {
|
||||
type Output = Vec3;
|
||||
|
||||
fn mul(self, rhs: Vec3) -> Vec3 {
|
||||
rhs * self
|
||||
}
|
||||
}
|
||||
|
||||
impl Div<f64> 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<f64> for Vec3 {
|
||||
fn mul_assign(&mut self, rhs: f64) {
|
||||
*self = *self * rhs;
|
||||
}
|
||||
}
|
||||
|
||||
impl DivAssign<f64> 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)");
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue