fabrilis/crates/core/src/vec/rotation.rs
loki5512344 878d144b77 Initial commit: Fabrilis docs, rules and deterministic core
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.
2026-11-16 23:06:29 +01:00

226 lines
5.3 KiB
Rust
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

//! 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)
}
}