Docs (architecture, decisions, coupling, determinism, UI, references), code rules enforced by tools/limits, deterministic core with units, time, math, sched, snapshot, vec and rng, plus smoke tests. ref/ stays out of git.
226 lines
5.3 KiB
Rust
226 lines
5.3 KiB
Rust
//! 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<Self> {
|
||
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<Vec3> 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<Mat3> 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<Self> {
|
||
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<Self> {
|
||
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<Self> {
|
||
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<f64> 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)
|
||
}
|
||
}
|