//! Rotations: a 3×3 matrix and a unit quaternion. //! //! Both are `f64` and use [`crate::math`] for trigonometry, so results do not depend //! on the operating system. See docs/DETERMINISM.md. use core::ops::{Div, Mul}; use super::Vec3; use crate::math; use crate::units::Radians; /// A 3×3 matrix; column `i` is the image of the basis vector `i`. #[derive(Debug, Clone, Copy, PartialEq)] pub struct Mat3 { cols: [Vec3; 3], } impl Mat3 { /// The identity matrix. pub const IDENTITY: Self = Self::from_cols(Vec3::X, Vec3::Y, Vec3::Z); /// Builds a matrix from its columns. #[inline] #[must_use] pub const fn from_cols(x: Vec3, y: Vec3, z: Vec3) -> Self { Self { cols: [x, y, z] } } /// A diagonal matrix. #[inline] #[must_use] pub const fn from_diagonal(diagonal: Vec3) -> Self { Self::from_cols( Vec3::new(diagonal.x, 0.0, 0.0), Vec3::new(0.0, diagonal.y, 0.0), Vec3::new(0.0, 0.0, diagonal.z), ) } /// The columns. #[inline] #[must_use] pub const fn cols(self) -> [Vec3; 3] { self.cols } /// The transposed matrix. #[inline] #[must_use] pub fn transpose(self) -> Self { let [x, y, z] = self.cols; Self::from_cols( Vec3::new(x.x, y.x, z.x), Vec3::new(x.y, y.y, z.y), Vec3::new(x.z, y.z, z.z), ) } /// The determinant. #[inline] #[must_use] pub fn determinant(self) -> f64 { let [a, b, c] = self.cols; a.dot(b.cross(c)) } /// The inverse, or `None` for a singular or non-finite matrix. #[inline] #[must_use] pub fn inverse(self) -> Option { let [a, b, c] = self.cols; let det = self.determinant(); if det == 0.0 || !det.is_finite() { return None; } let inv_det = 1.0 / det; let r0 = b.cross(c); let r1 = c.cross(a); let r2 = a.cross(b); Some(Self::from_cols( Vec3::new(r0.x, r1.x, r2.x) * inv_det, Vec3::new(r0.y, r1.y, r2.y) * inv_det, Vec3::new(r0.z, r1.z, r2.z) * inv_det, )) } } impl Mul for Mat3 { type Output = Vec3; #[inline] fn mul(self, rhs: Vec3) -> Vec3 { let [a, b, c] = self.cols; a * rhs.x + b * rhs.y + c * rhs.z } } impl Mul for Mat3 { type Output = Self; #[inline] fn mul(self, rhs: Mat3) -> Self { let [x, y, z] = rhs.cols; Self::from_cols(self * x, self * y, self * z) } } /// A rotation as a quaternion `w + xi + yj + zk`. #[derive(Debug, Clone, Copy, PartialEq)] pub struct Quat { pub w: f64, pub x: f64, pub y: f64, pub z: f64, } impl Quat { /// The identity rotation. pub const IDENTITY: Self = Self::new(1.0, 0.0, 0.0, 0.0); /// Builds a quaternion from its components; it is not normalized here. #[inline] #[must_use] pub const fn new(w: f64, x: f64, y: f64, z: f64) -> Self { Self { w, x, y, z } } /// A rotation of `angle` around `axis`, or `None` for a zero axis. #[inline] #[must_use] pub fn from_axis_angle(axis: Vec3, angle: Radians) -> Option { let axis = axis.normalized()?; let (sin, cos) = math::sin_cos(angle.get() * 0.5); Some(Self::new(cos, axis.x * sin, axis.y * sin, axis.z * sin)) } /// The squared norm; `1.0` for a unit quaternion. #[inline] #[must_use] pub fn norm_squared(self) -> f64 { self.w * self.w + self.x * self.x + self.y * self.y + self.z * self.z } /// The negation of the vector part, which is the inverse for a unit quaternion. #[inline] #[must_use] pub fn conjugate(self) -> Self { Self::new(self.w, -self.x, -self.y, -self.z) } /// The inverse, or `None` for the zero quaternion. #[inline] #[must_use] pub fn inverse(self) -> Option { let norm_squared = self.norm_squared(); if norm_squared > 0.0 && norm_squared.is_finite() { Some(self.conjugate() / norm_squared) } else { None } } /// The unit quaternion, or `None` for the zero quaternion. #[inline] #[must_use] pub fn normalize(self) -> Option { let norm_squared = self.norm_squared(); if norm_squared > 0.0 && norm_squared.is_finite() { Some(self / math::sqrt(norm_squared)) } else { None } } /// Rotates a vector; a non-unit quaternion is normalized first. #[inline] #[must_use] pub fn rotate(self, v: Vec3) -> Vec3 { let Some(unit) = self.normalize() else { return v; }; let qv = Vec3::new(unit.x, unit.y, unit.z); let t = qv.cross(v) * 2.0; v + t * unit.w + qv.cross(t) } /// The rotation matrix equivalent to this rotation. #[inline] #[must_use] pub fn to_mat3(self) -> Mat3 { let Some(q) = self.normalize() else { return Mat3::IDENTITY; }; let (w, x, y, z) = (q.w, q.x, q.y, q.z); let (xx, yy, zz) = (x * x, y * y, z * z); let (xy, xz, yz) = (x * y, x * z, y * z); let (wx, wy, wz) = (w * x, w * y, w * z); Mat3::from_cols( Vec3::new(1.0 - 2.0 * (yy + zz), 2.0 * (xy + wz), 2.0 * (xz - wy)), Vec3::new(2.0 * (xy - wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz + wx)), Vec3::new(2.0 * (xz + wy), 2.0 * (yz - wx), 1.0 - 2.0 * (xx + yy)), ) } } impl Mul for Quat { type Output = Self; /// Composition: `(a * b).rotate(v) == a.rotate(b.rotate(v))`. #[inline] fn mul(self, rhs: Self) -> Self { let a = Vec3::new(self.x, self.y, self.z); let b = Vec3::new(rhs.x, rhs.y, rhs.z); let vector = b * self.w + a * rhs.w + a.cross(b); Self::new(self.w * rhs.w - a.dot(b), vector.x, vector.y, vector.z) } } impl Div for Quat { type Output = Self; #[inline] fn div(self, rhs: f64) -> Self { Self::new(self.w / rhs, self.x / rhs, self.y / rhs, self.z / rhs) } }