use crate::{
euler::{FromEuler, ToEuler},
f32::math,
sse2::*,
swizzles::*,
DMat4, EulerRot, Mat3, Mat3A, Quat, Vec3, Vec3A, Vec4,
};
use core::fmt;
use core::iter::{Product, Sum};
use core::ops::{Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign};
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
#[inline(always)]
#[must_use]
pub const fn mat4(x_axis: Vec4, y_axis: Vec4, z_axis: Vec4, w_axis: Vec4) -> Mat4 {
Mat4::from_cols(x_axis, y_axis, z_axis, w_axis)
}
#[derive(Clone, Copy)]
#[repr(C)]
pub struct Mat4 {
pub x_axis: Vec4,
pub y_axis: Vec4,
pub z_axis: Vec4,
pub w_axis: Vec4,
}
impl Mat4 {
pub const ZERO: Self = Self::from_cols(Vec4::ZERO, Vec4::ZERO, Vec4::ZERO, Vec4::ZERO);
pub const IDENTITY: Self = Self::from_cols(Vec4::X, Vec4::Y, Vec4::Z, Vec4::W);
pub const NAN: Self = Self::from_cols(Vec4::NAN, Vec4::NAN, Vec4::NAN, Vec4::NAN);
#[allow(clippy::too_many_arguments)]
#[inline(always)]
#[must_use]
const fn new(
m00: f32,
m01: f32,
m02: f32,
m03: f32,
m10: f32,
m11: f32,
m12: f32,
m13: f32,
m20: f32,
m21: f32,
m22: f32,
m23: f32,
m30: f32,
m31: f32,
m32: f32,
m33: f32,
) -> Self {
Self {
x_axis: Vec4::new(m00, m01, m02, m03),
y_axis: Vec4::new(m10, m11, m12, m13),
z_axis: Vec4::new(m20, m21, m22, m23),
w_axis: Vec4::new(m30, m31, m32, m33),
}
}
#[inline(always)]
#[must_use]
pub const fn from_cols(x_axis: Vec4, y_axis: Vec4, z_axis: Vec4, w_axis: Vec4) -> Self {
Self {
x_axis,
y_axis,
z_axis,
w_axis,
}
}
#[inline]
#[must_use]
pub const fn from_cols_array(m: &[f32; 16]) -> Self {
Self::new(
m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8], m[9], m[10], m[11], m[12], m[13],
m[14], m[15],
)
}
#[inline]
#[must_use]
pub const fn to_cols_array(&self) -> [f32; 16] {
let [x_axis_x, x_axis_y, x_axis_z, x_axis_w] = self.x_axis.to_array();
let [y_axis_x, y_axis_y, y_axis_z, y_axis_w] = self.y_axis.to_array();
let [z_axis_x, z_axis_y, z_axis_z, z_axis_w] = self.z_axis.to_array();
let [w_axis_x, w_axis_y, w_axis_z, w_axis_w] = self.w_axis.to_array();
[
x_axis_x, x_axis_y, x_axis_z, x_axis_w, y_axis_x, y_axis_y, y_axis_z, y_axis_w,
z_axis_x, z_axis_y, z_axis_z, z_axis_w, w_axis_x, w_axis_y, w_axis_z, w_axis_w,
]
}
#[inline]
#[must_use]
pub const fn from_cols_array_2d(m: &[[f32; 4]; 4]) -> Self {
Self::from_cols(
Vec4::from_array(m[0]),
Vec4::from_array(m[1]),
Vec4::from_array(m[2]),
Vec4::from_array(m[3]),
)
}
#[inline]
#[must_use]
pub const fn to_cols_array_2d(&self) -> [[f32; 4]; 4] {
[
self.x_axis.to_array(),
self.y_axis.to_array(),
self.z_axis.to_array(),
self.w_axis.to_array(),
]
}
#[doc(alias = "scale")]
#[inline]
#[must_use]
pub const fn from_diagonal(diagonal: Vec4) -> Self {
let [x, y, z, w] = diagonal.to_array();
Self::new(
x, 0.0, 0.0, 0.0, 0.0, y, 0.0, 0.0, 0.0, 0.0, z, 0.0, 0.0, 0.0, 0.0, w,
)
}
#[inline]
#[must_use]
fn quat_to_axes(rotation: Quat) -> (Vec4, Vec4, Vec4) {
glam_assert!(rotation.is_normalized());
let (x, y, z, w) = rotation.into();
let x2 = x + x;
let y2 = y + y;
let z2 = z + z;
let xx = x * x2;
let xy = x * y2;
let xz = x * z2;
let yy = y * y2;
let yz = y * z2;
let zz = z * z2;
let wx = w * x2;
let wy = w * y2;
let wz = w * z2;
let x_axis = Vec4::new(1.0 - (yy + zz), xy + wz, xz - wy, 0.0);
let y_axis = Vec4::new(xy - wz, 1.0 - (xx + zz), yz + wx, 0.0);
let z_axis = Vec4::new(xz + wy, yz - wx, 1.0 - (xx + yy), 0.0);
(x_axis, y_axis, z_axis)
}
#[inline]
#[must_use]
pub fn from_scale_rotation_translation(scale: Vec3, rotation: Quat, translation: Vec3) -> Self {
let (x_axis, y_axis, z_axis) = Self::quat_to_axes(rotation);
Self::from_cols(
x_axis.mul(scale.x),
y_axis.mul(scale.y),
z_axis.mul(scale.z),
Vec4::from((translation, 1.0)),
)
}
#[inline]
#[must_use]
pub fn from_rotation_translation(rotation: Quat, translation: Vec3) -> Self {
let (x_axis, y_axis, z_axis) = Self::quat_to_axes(rotation);
Self::from_cols(x_axis, y_axis, z_axis, Vec4::from((translation, 1.0)))
}
#[inline]
#[must_use]
pub fn to_scale_rotation_translation(&self) -> (Vec3, Quat, Vec3) {
let det = self.determinant();
glam_assert!(det != 0.0);
let scale = Vec3::new(
self.x_axis.length() * math::signum(det),
self.y_axis.length(),
self.z_axis.length(),
);
glam_assert!(scale.cmpne(Vec3::ZERO).all());
let inv_scale = scale.recip();
let rotation = Quat::from_rotation_axes(
self.x_axis.mul(inv_scale.x).xyz(),
self.y_axis.mul(inv_scale.y).xyz(),
self.z_axis.mul(inv_scale.z).xyz(),
);
let translation = self.w_axis.xyz();
(scale, rotation, translation)
}
#[inline]
#[must_use]
pub fn from_quat(rotation: Quat) -> Self {
let (x_axis, y_axis, z_axis) = Self::quat_to_axes(rotation);
Self::from_cols(x_axis, y_axis, z_axis, Vec4::W)
}
#[inline]
#[must_use]
pub fn from_mat3(m: Mat3) -> Self {
Self::from_cols(
Vec4::from((m.x_axis, 0.0)),
Vec4::from((m.y_axis, 0.0)),
Vec4::from((m.z_axis, 0.0)),
Vec4::W,
)
}
#[inline]
#[must_use]
pub fn from_mat3_translation(mat3: Mat3, translation: Vec3) -> Self {
Self::from_cols(
Vec4::from((mat3.x_axis, 0.0)),
Vec4::from((mat3.y_axis, 0.0)),
Vec4::from((mat3.z_axis, 0.0)),
Vec4::from((translation, 1.0)),
)
}
#[inline]
#[must_use]
pub fn from_mat3a(m: Mat3A) -> Self {
Self::from_cols(
Vec4::from((m.x_axis, 0.0)),
Vec4::from((m.y_axis, 0.0)),
Vec4::from((m.z_axis, 0.0)),
Vec4::W,
)
}
#[inline]
#[must_use]
pub fn from_translation(translation: Vec3) -> Self {
Self::from_cols(
Vec4::X,
Vec4::Y,
Vec4::Z,
Vec4::new(translation.x, translation.y, translation.z, 1.0),
)
}
#[inline]
#[must_use]
pub fn from_axis_angle(axis: Vec3, angle: f32) -> Self {
glam_assert!(axis.is_normalized());
let (sin, cos) = math::sin_cos(angle);
let axis_sin = axis.mul(sin);
let axis_sq = axis.mul(axis);
let omc = 1.0 - cos;
let xyomc = axis.x * axis.y * omc;
let xzomc = axis.x * axis.z * omc;
let yzomc = axis.y * axis.z * omc;
Self::from_cols(
Vec4::new(
axis_sq.x * omc + cos,
xyomc + axis_sin.z,
xzomc - axis_sin.y,
0.0,
),
Vec4::new(
xyomc - axis_sin.z,
axis_sq.y * omc + cos,
yzomc + axis_sin.x,
0.0,
),
Vec4::new(
xzomc + axis_sin.y,
yzomc - axis_sin.x,
axis_sq.z * omc + cos,
0.0,
),
Vec4::W,
)
}
#[inline]
#[must_use]
pub fn from_euler(order: EulerRot, a: f32, b: f32, c: f32) -> Self {
Self::from_euler_angles(order, a, b, c)
}
#[inline]
#[must_use]
pub fn to_euler(&self, order: EulerRot) -> (f32, f32, f32) {
glam_assert!(
self.x_axis.xyz().is_normalized()
&& self.y_axis.xyz().is_normalized()
&& self.z_axis.xyz().is_normalized()
);
self.to_euler_angles(order)
}
#[inline]
#[must_use]
pub fn from_rotation_x(angle: f32) -> Self {
let (sina, cosa) = math::sin_cos(angle);
Self::from_cols(
Vec4::X,
Vec4::new(0.0, cosa, sina, 0.0),
Vec4::new(0.0, -sina, cosa, 0.0),
Vec4::W,
)
}
#[inline]
#[must_use]
pub fn from_rotation_y(angle: f32) -> Self {
let (sina, cosa) = math::sin_cos(angle);
Self::from_cols(
Vec4::new(cosa, 0.0, -sina, 0.0),
Vec4::Y,
Vec4::new(sina, 0.0, cosa, 0.0),
Vec4::W,
)
}
#[inline]
#[must_use]
pub fn from_rotation_z(angle: f32) -> Self {
let (sina, cosa) = math::sin_cos(angle);
Self::from_cols(
Vec4::new(cosa, sina, 0.0, 0.0),
Vec4::new(-sina, cosa, 0.0, 0.0),
Vec4::Z,
Vec4::W,
)
}
#[inline]
#[must_use]
pub fn from_scale(scale: Vec3) -> Self {
glam_assert!(scale.cmpne(Vec3::ZERO).any());
Self::from_cols(
Vec4::new(scale.x, 0.0, 0.0, 0.0),
Vec4::new(0.0, scale.y, 0.0, 0.0),
Vec4::new(0.0, 0.0, scale.z, 0.0),
Vec4::W,
)
}
#[inline]
#[must_use]
pub const fn from_cols_slice(slice: &[f32]) -> Self {
Self::new(
slice[0], slice[1], slice[2], slice[3], slice[4], slice[5], slice[6], slice[7],
slice[8], slice[9], slice[10], slice[11], slice[12], slice[13], slice[14], slice[15],
)
}
#[inline]
pub fn write_cols_to_slice(self, slice: &mut [f32]) {
slice[0] = self.x_axis.x;
slice[1] = self.x_axis.y;
slice[2] = self.x_axis.z;
slice[3] = self.x_axis.w;
slice[4] = self.y_axis.x;
slice[5] = self.y_axis.y;
slice[6] = self.y_axis.z;
slice[7] = self.y_axis.w;
slice[8] = self.z_axis.x;
slice[9] = self.z_axis.y;
slice[10] = self.z_axis.z;
slice[11] = self.z_axis.w;
slice[12] = self.w_axis.x;
slice[13] = self.w_axis.y;
slice[14] = self.w_axis.z;
slice[15] = self.w_axis.w;
}
#[inline]
#[must_use]
pub fn col(&self, index: usize) -> Vec4 {
match index {
0 => self.x_axis,
1 => self.y_axis,
2 => self.z_axis,
3 => self.w_axis,
_ => panic!("index out of bounds"),
}
}
#[inline]
pub fn col_mut(&mut self, index: usize) -> &mut Vec4 {
match index {
0 => &mut self.x_axis,
1 => &mut self.y_axis,
2 => &mut self.z_axis,
3 => &mut self.w_axis,
_ => panic!("index out of bounds"),
}
}
#[inline]
#[must_use]
pub fn row(&self, index: usize) -> Vec4 {
match index {
0 => Vec4::new(self.x_axis.x, self.y_axis.x, self.z_axis.x, self.w_axis.x),
1 => Vec4::new(self.x_axis.y, self.y_axis.y, self.z_axis.y, self.w_axis.y),
2 => Vec4::new(self.x_axis.z, self.y_axis.z, self.z_axis.z, self.w_axis.z),
3 => Vec4::new(self.x_axis.w, self.y_axis.w, self.z_axis.w, self.w_axis.w),
_ => panic!("index out of bounds"),
}
}
#[inline]
#[must_use]
pub fn is_finite(&self) -> bool {
self.x_axis.is_finite()
&& self.y_axis.is_finite()
&& self.z_axis.is_finite()
&& self.w_axis.is_finite()
}
#[inline]
#[must_use]
pub fn is_nan(&self) -> bool {
self.x_axis.is_nan() || self.y_axis.is_nan() || self.z_axis.is_nan() || self.w_axis.is_nan()
}
#[inline]
#[must_use]
pub fn transpose(&self) -> Self {
unsafe {
let tmp0 = _mm_shuffle_ps(self.x_axis.0, self.y_axis.0, 0b01_00_01_00);
let tmp1 = _mm_shuffle_ps(self.x_axis.0, self.y_axis.0, 0b11_10_11_10);
let tmp2 = _mm_shuffle_ps(self.z_axis.0, self.w_axis.0, 0b01_00_01_00);
let tmp3 = _mm_shuffle_ps(self.z_axis.0, self.w_axis.0, 0b11_10_11_10);
Self {
x_axis: Vec4(_mm_shuffle_ps(tmp0, tmp2, 0b10_00_10_00)),
y_axis: Vec4(_mm_shuffle_ps(tmp0, tmp2, 0b11_01_11_01)),
z_axis: Vec4(_mm_shuffle_ps(tmp1, tmp3, 0b10_00_10_00)),
w_axis: Vec4(_mm_shuffle_ps(tmp1, tmp3, 0b11_01_11_01)),
}
}
}
#[must_use]
pub fn determinant(&self) -> f32 {
unsafe {
let swp2a = _mm_shuffle_ps(self.z_axis.0, self.z_axis.0, 0b00_01_01_10);
let swp3a = _mm_shuffle_ps(self.w_axis.0, self.w_axis.0, 0b11_10_11_11);
let swp2b = _mm_shuffle_ps(self.z_axis.0, self.z_axis.0, 0b11_10_11_11);
let swp3b = _mm_shuffle_ps(self.w_axis.0, self.w_axis.0, 0b00_01_01_10);
let swp2c = _mm_shuffle_ps(self.z_axis.0, self.z_axis.0, 0b00_00_01_10);
let swp3c = _mm_shuffle_ps(self.w_axis.0, self.w_axis.0, 0b01_10_00_00);
let mula = _mm_mul_ps(swp2a, swp3a);
let mulb = _mm_mul_ps(swp2b, swp3b);
let mulc = _mm_mul_ps(swp2c, swp3c);
let sube = _mm_sub_ps(mula, mulb);
let subf = _mm_sub_ps(_mm_movehl_ps(mulc, mulc), mulc);
let subfaca = _mm_shuffle_ps(sube, sube, 0b10_01_00_00);
let swpfaca = _mm_shuffle_ps(self.y_axis.0, self.y_axis.0, 0b00_00_00_01);
let mulfaca = _mm_mul_ps(swpfaca, subfaca);
let subtmpb = _mm_shuffle_ps(sube, subf, 0b00_00_11_01);
let subfacb = _mm_shuffle_ps(subtmpb, subtmpb, 0b11_01_01_00);
let swpfacb = _mm_shuffle_ps(self.y_axis.0, self.y_axis.0, 0b01_01_10_10);
let mulfacb = _mm_mul_ps(swpfacb, subfacb);
let subres = _mm_sub_ps(mulfaca, mulfacb);
let subtmpc = _mm_shuffle_ps(sube, subf, 0b01_00_10_10);
let subfacc = _mm_shuffle_ps(subtmpc, subtmpc, 0b11_11_10_00);
let swpfacc = _mm_shuffle_ps(self.y_axis.0, self.y_axis.0, 0b10_11_11_11);
let mulfacc = _mm_mul_ps(swpfacc, subfacc);
let addres = _mm_add_ps(subres, mulfacc);
let detcof = _mm_mul_ps(addres, _mm_setr_ps(1.0, -1.0, 1.0, -1.0));
dot4(self.x_axis.0, detcof)
}
}
#[must_use]
pub fn inverse(&self) -> Self {
unsafe {
let fac0 = {
let swp0a = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b11_11_11_11);
let swp0b = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b10_10_10_10);
let swp00 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b10_10_10_10);
let swp01 = _mm_shuffle_ps(swp0a, swp0a, 0b10_00_00_00);
let swp02 = _mm_shuffle_ps(swp0b, swp0b, 0b10_00_00_00);
let swp03 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b11_11_11_11);
let mul00 = _mm_mul_ps(swp00, swp01);
let mul01 = _mm_mul_ps(swp02, swp03);
_mm_sub_ps(mul00, mul01)
};
let fac1 = {
let swp0a = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b11_11_11_11);
let swp0b = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b01_01_01_01);
let swp00 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b01_01_01_01);
let swp01 = _mm_shuffle_ps(swp0a, swp0a, 0b10_00_00_00);
let swp02 = _mm_shuffle_ps(swp0b, swp0b, 0b10_00_00_00);
let swp03 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b11_11_11_11);
let mul00 = _mm_mul_ps(swp00, swp01);
let mul01 = _mm_mul_ps(swp02, swp03);
_mm_sub_ps(mul00, mul01)
};
let fac2 = {
let swp0a = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b10_10_10_10);
let swp0b = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b01_01_01_01);
let swp00 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b01_01_01_01);
let swp01 = _mm_shuffle_ps(swp0a, swp0a, 0b10_00_00_00);
let swp02 = _mm_shuffle_ps(swp0b, swp0b, 0b10_00_00_00);
let swp03 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b10_10_10_10);
let mul00 = _mm_mul_ps(swp00, swp01);
let mul01 = _mm_mul_ps(swp02, swp03);
_mm_sub_ps(mul00, mul01)
};
let fac3 = {
let swp0a = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b11_11_11_11);
let swp0b = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b00_00_00_00);
let swp00 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b00_00_00_00);
let swp01 = _mm_shuffle_ps(swp0a, swp0a, 0b10_00_00_00);
let swp02 = _mm_shuffle_ps(swp0b, swp0b, 0b10_00_00_00);
let swp03 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b11_11_11_11);
let mul00 = _mm_mul_ps(swp00, swp01);
let mul01 = _mm_mul_ps(swp02, swp03);
_mm_sub_ps(mul00, mul01)
};
let fac4 = {
let swp0a = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b10_10_10_10);
let swp0b = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b00_00_00_00);
let swp00 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b00_00_00_00);
let swp01 = _mm_shuffle_ps(swp0a, swp0a, 0b10_00_00_00);
let swp02 = _mm_shuffle_ps(swp0b, swp0b, 0b10_00_00_00);
let swp03 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b10_10_10_10);
let mul00 = _mm_mul_ps(swp00, swp01);
let mul01 = _mm_mul_ps(swp02, swp03);
_mm_sub_ps(mul00, mul01)
};
let fac5 = {
let swp0a = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b01_01_01_01);
let swp0b = _mm_shuffle_ps(self.w_axis.0, self.z_axis.0, 0b00_00_00_00);
let swp00 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b00_00_00_00);
let swp01 = _mm_shuffle_ps(swp0a, swp0a, 0b10_00_00_00);
let swp02 = _mm_shuffle_ps(swp0b, swp0b, 0b10_00_00_00);
let swp03 = _mm_shuffle_ps(self.z_axis.0, self.y_axis.0, 0b01_01_01_01);
let mul00 = _mm_mul_ps(swp00, swp01);
let mul01 = _mm_mul_ps(swp02, swp03);
_mm_sub_ps(mul00, mul01)
};
let sign_a = _mm_set_ps(1.0, -1.0, 1.0, -1.0);
let sign_b = _mm_set_ps(-1.0, 1.0, -1.0, 1.0);
let temp0 = _mm_shuffle_ps(self.y_axis.0, self.x_axis.0, 0b00_00_00_00);
let vec0 = _mm_shuffle_ps(temp0, temp0, 0b10_10_10_00);
let temp1 = _mm_shuffle_ps(self.y_axis.0, self.x_axis.0, 0b01_01_01_01);
let vec1 = _mm_shuffle_ps(temp1, temp1, 0b10_10_10_00);
let temp2 = _mm_shuffle_ps(self.y_axis.0, self.x_axis.0, 0b10_10_10_10);
let vec2 = _mm_shuffle_ps(temp2, temp2, 0b10_10_10_00);
let temp3 = _mm_shuffle_ps(self.y_axis.0, self.x_axis.0, 0b11_11_11_11);
let vec3 = _mm_shuffle_ps(temp3, temp3, 0b10_10_10_00);
let mul00 = _mm_mul_ps(vec1, fac0);
let mul01 = _mm_mul_ps(vec2, fac1);
let mul02 = _mm_mul_ps(vec3, fac2);
let sub00 = _mm_sub_ps(mul00, mul01);
let add00 = _mm_add_ps(sub00, mul02);
let inv0 = _mm_mul_ps(sign_b, add00);
let mul03 = _mm_mul_ps(vec0, fac0);
let mul04 = _mm_mul_ps(vec2, fac3);
let mul05 = _mm_mul_ps(vec3, fac4);
let sub01 = _mm_sub_ps(mul03, mul04);
let add01 = _mm_add_ps(sub01, mul05);
let inv1 = _mm_mul_ps(sign_a, add01);
let mul06 = _mm_mul_ps(vec0, fac1);
let mul07 = _mm_mul_ps(vec1, fac3);
let mul08 = _mm_mul_ps(vec3, fac5);
let sub02 = _mm_sub_ps(mul06, mul07);
let add02 = _mm_add_ps(sub02, mul08);
let inv2 = _mm_mul_ps(sign_b, add02);
let mul09 = _mm_mul_ps(vec0, fac2);
let mul10 = _mm_mul_ps(vec1, fac4);
let mul11 = _mm_mul_ps(vec2, fac5);
let sub03 = _mm_sub_ps(mul09, mul10);
let add03 = _mm_add_ps(sub03, mul11);
let inv3 = _mm_mul_ps(sign_a, add03);
let row0 = _mm_shuffle_ps(inv0, inv1, 0b00_00_00_00);
let row1 = _mm_shuffle_ps(inv2, inv3, 0b00_00_00_00);
let row2 = _mm_shuffle_ps(row0, row1, 0b10_00_10_00);
let dot0 = dot4(self.x_axis.0, row2);
glam_assert!(dot0 != 0.0);
let rcp0 = _mm_set1_ps(dot0.recip());
Self {
x_axis: Vec4(_mm_mul_ps(inv0, rcp0)),
y_axis: Vec4(_mm_mul_ps(inv1, rcp0)),
z_axis: Vec4(_mm_mul_ps(inv2, rcp0)),
w_axis: Vec4(_mm_mul_ps(inv3, rcp0)),
}
}
}
#[inline]
#[must_use]
pub fn look_to_lh(eye: Vec3, dir: Vec3, up: Vec3) -> Self {
Self::look_to_rh(eye, -dir, up)
}
#[inline]
#[must_use]
pub fn look_to_rh(eye: Vec3, dir: Vec3, up: Vec3) -> Self {
glam_assert!(dir.is_normalized());
glam_assert!(up.is_normalized());
let f = dir;
let s = f.cross(up).normalize();
let u = s.cross(f);
Self::from_cols(
Vec4::new(s.x, u.x, -f.x, 0.0),
Vec4::new(s.y, u.y, -f.y, 0.0),
Vec4::new(s.z, u.z, -f.z, 0.0),
Vec4::new(-eye.dot(s), -eye.dot(u), eye.dot(f), 1.0),
)
}
#[inline]
#[must_use]
pub fn look_at_lh(eye: Vec3, center: Vec3, up: Vec3) -> Self {
Self::look_to_lh(eye, center.sub(eye).normalize(), up)
}
#[inline]
pub fn look_at_rh(eye: Vec3, center: Vec3, up: Vec3) -> Self {
Self::look_to_rh(eye, center.sub(eye).normalize(), up)
}
#[inline]
#[must_use]
pub fn perspective_rh_gl(
fov_y_radians: f32,
aspect_ratio: f32,
z_near: f32,
z_far: f32,
) -> Self {
let inv_length = 1.0 / (z_near - z_far);
let f = 1.0 / math::tan(0.5 * fov_y_radians);
let a = f / aspect_ratio;
let b = (z_near + z_far) * inv_length;
let c = (2.0 * z_near * z_far) * inv_length;
Self::from_cols(
Vec4::new(a, 0.0, 0.0, 0.0),
Vec4::new(0.0, f, 0.0, 0.0),
Vec4::new(0.0, 0.0, b, -1.0),
Vec4::new(0.0, 0.0, c, 0.0),
)
}
#[inline]
#[must_use]
pub fn perspective_lh(fov_y_radians: f32, aspect_ratio: f32, z_near: f32, z_far: f32) -> Self {
glam_assert!(z_near > 0.0 && z_far > 0.0);
let (sin_fov, cos_fov) = math::sin_cos(0.5 * fov_y_radians);
let h = cos_fov / sin_fov;
let w = h / aspect_ratio;
let r = z_far / (z_far - z_near);
Self::from_cols(
Vec4::new(w, 0.0, 0.0, 0.0),
Vec4::new(0.0, h, 0.0, 0.0),
Vec4::new(0.0, 0.0, r, 1.0),
Vec4::new(0.0, 0.0, -r * z_near, 0.0),
)
}
#[inline]
#[must_use]
pub fn perspective_rh(fov_y_radians: f32, aspect_ratio: f32, z_near: f32, z_far: f32) -> Self {
glam_assert!(z_near > 0.0 && z_far > 0.0);
let (sin_fov, cos_fov) = math::sin_cos(0.5 * fov_y_radians);
let h = cos_fov / sin_fov;
let w = h / aspect_ratio;
let r = z_far / (z_near - z_far);
Self::from_cols(
Vec4::new(w, 0.0, 0.0, 0.0),
Vec4::new(0.0, h, 0.0, 0.0),
Vec4::new(0.0, 0.0, r, -1.0),
Vec4::new(0.0, 0.0, r * z_near, 0.0),
)
}
#[inline]
#[must_use]
pub fn perspective_infinite_lh(fov_y_radians: f32, aspect_ratio: f32, z_near: f32) -> Self {
glam_assert!(z_near > 0.0);
let (sin_fov, cos_fov) = math::sin_cos(0.5 * fov_y_radians);
let h = cos_fov / sin_fov;
let w = h / aspect_ratio;
Self::from_cols(
Vec4::new(w, 0.0, 0.0, 0.0),
Vec4::new(0.0, h, 0.0, 0.0),
Vec4::new(0.0, 0.0, 1.0, 1.0),
Vec4::new(0.0, 0.0, -z_near, 0.0),
)
}
#[inline]
#[must_use]
pub fn perspective_infinite_reverse_lh(
fov_y_radians: f32,
aspect_ratio: f32,
z_near: f32,
) -> Self {
glam_assert!(z_near > 0.0);
let (sin_fov, cos_fov) = math::sin_cos(0.5 * fov_y_radians);
let h = cos_fov / sin_fov;
let w = h / aspect_ratio;
Self::from_cols(
Vec4::new(w, 0.0, 0.0, 0.0),
Vec4::new(0.0, h, 0.0, 0.0),
Vec4::new(0.0, 0.0, 0.0, 1.0),
Vec4::new(0.0, 0.0, z_near, 0.0),
)
}
#[inline]
#[must_use]
pub fn perspective_infinite_rh(fov_y_radians: f32, aspect_ratio: f32, z_near: f32) -> Self {
glam_assert!(z_near > 0.0);
let f = 1.0 / math::tan(0.5 * fov_y_radians);
Self::from_cols(
Vec4::new(f / aspect_ratio, 0.0, 0.0, 0.0),
Vec4::new(0.0, f, 0.0, 0.0),
Vec4::new(0.0, 0.0, -1.0, -1.0),
Vec4::new(0.0, 0.0, -z_near, 0.0),
)
}
#[inline]
#[must_use]
pub fn perspective_infinite_reverse_rh(
fov_y_radians: f32,
aspect_ratio: f32,
z_near: f32,
) -> Self {
glam_assert!(z_near > 0.0);
let f = 1.0 / math::tan(0.5 * fov_y_radians);
Self::from_cols(
Vec4::new(f / aspect_ratio, 0.0, 0.0, 0.0),
Vec4::new(0.0, f, 0.0, 0.0),
Vec4::new(0.0, 0.0, 0.0, -1.0),
Vec4::new(0.0, 0.0, z_near, 0.0),
)
}
#[inline]
#[must_use]
pub fn orthographic_rh_gl(
left: f32,
right: f32,
bottom: f32,
top: f32,
near: f32,
far: f32,
) -> Self {
let a = 2.0 / (right - left);
let b = 2.0 / (top - bottom);
let c = -2.0 / (far - near);
let tx = -(right + left) / (right - left);
let ty = -(top + bottom) / (top - bottom);
let tz = -(far + near) / (far - near);
Self::from_cols(
Vec4::new(a, 0.0, 0.0, 0.0),
Vec4::new(0.0, b, 0.0, 0.0),
Vec4::new(0.0, 0.0, c, 0.0),
Vec4::new(tx, ty, tz, 1.0),
)
}
#[inline]
#[must_use]
pub fn orthographic_lh(
left: f32,
right: f32,
bottom: f32,
top: f32,
near: f32,
far: f32,
) -> Self {
let rcp_width = 1.0 / (right - left);
let rcp_height = 1.0 / (top - bottom);
let r = 1.0 / (far - near);
Self::from_cols(
Vec4::new(rcp_width + rcp_width, 0.0, 0.0, 0.0),
Vec4::new(0.0, rcp_height + rcp_height, 0.0, 0.0),
Vec4::new(0.0, 0.0, r, 0.0),
Vec4::new(
-(left + right) * rcp_width,
-(top + bottom) * rcp_height,
-r * near,
1.0,
),
)
}
#[inline]
#[must_use]
pub fn orthographic_rh(
left: f32,
right: f32,
bottom: f32,
top: f32,
near: f32,
far: f32,
) -> Self {
let rcp_width = 1.0 / (right - left);
let rcp_height = 1.0 / (top - bottom);
let r = 1.0 / (near - far);
Self::from_cols(
Vec4::new(rcp_width + rcp_width, 0.0, 0.0, 0.0),
Vec4::new(0.0, rcp_height + rcp_height, 0.0, 0.0),
Vec4::new(0.0, 0.0, r, 0.0),
Vec4::new(
-(left + right) * rcp_width,
-(top + bottom) * rcp_height,
r * near,
1.0,
),
)
}
#[inline]
#[must_use]
pub fn project_point3(&self, rhs: Vec3) -> Vec3 {
let mut res = self.x_axis.mul(rhs.x);
res = self.y_axis.mul(rhs.y).add(res);
res = self.z_axis.mul(rhs.z).add(res);
res = self.w_axis.add(res);
res = res.div(res.w);
res.xyz()
}
#[inline]
#[must_use]
pub fn transform_point3(&self, rhs: Vec3) -> Vec3 {
glam_assert!(self.row(3).abs_diff_eq(Vec4::W, 1e-6));
let mut res = self.x_axis.mul(rhs.x);
res = self.y_axis.mul(rhs.y).add(res);
res = self.z_axis.mul(rhs.z).add(res);
res = self.w_axis.add(res);
res.xyz()
}
#[inline]
#[must_use]
pub fn transform_vector3(&self, rhs: Vec3) -> Vec3 {
glam_assert!(self.row(3).abs_diff_eq(Vec4::W, 1e-6));
let mut res = self.x_axis.mul(rhs.x);
res = self.y_axis.mul(rhs.y).add(res);
res = self.z_axis.mul(rhs.z).add(res);
res.xyz()
}
#[inline]
#[must_use]
pub fn project_point3a(&self, rhs: Vec3A) -> Vec3A {
let mut res = self.x_axis.mul(rhs.xxxx());
res = self.y_axis.mul(rhs.yyyy()).add(res);
res = self.z_axis.mul(rhs.zzzz()).add(res);
res = self.w_axis.add(res);
res = res.div(res.wwww());
Vec3A::from_vec4(res)
}
#[inline]
#[must_use]
pub fn transform_point3a(&self, rhs: Vec3A) -> Vec3A {
glam_assert!(self.row(3).abs_diff_eq(Vec4::W, 1e-6));
let mut res = self.x_axis.mul(rhs.xxxx());
res = self.y_axis.mul(rhs.yyyy()).add(res);
res = self.z_axis.mul(rhs.zzzz()).add(res);
res = self.w_axis.add(res);
Vec3A::from_vec4(res)
}
#[inline]
#[must_use]
pub fn transform_vector3a(&self, rhs: Vec3A) -> Vec3A {
glam_assert!(self.row(3).abs_diff_eq(Vec4::W, 1e-6));
let mut res = self.x_axis.mul(rhs.xxxx());
res = self.y_axis.mul(rhs.yyyy()).add(res);
res = self.z_axis.mul(rhs.zzzz()).add(res);
Vec3A::from_vec4(res)
}
#[inline]
#[must_use]
pub fn mul_vec4(&self, rhs: Vec4) -> Vec4 {
let mut res = self.x_axis.mul(rhs.xxxx());
res = res.add(self.y_axis.mul(rhs.yyyy()));
res = res.add(self.z_axis.mul(rhs.zzzz()));
res = res.add(self.w_axis.mul(rhs.wwww()));
res
}
#[inline]
#[must_use]
pub fn mul_mat4(&self, rhs: &Self) -> Self {
Self::from_cols(
self.mul(rhs.x_axis),
self.mul(rhs.y_axis),
self.mul(rhs.z_axis),
self.mul(rhs.w_axis),
)
}
#[inline]
#[must_use]
pub fn add_mat4(&self, rhs: &Self) -> Self {
Self::from_cols(
self.x_axis.add(rhs.x_axis),
self.y_axis.add(rhs.y_axis),
self.z_axis.add(rhs.z_axis),
self.w_axis.add(rhs.w_axis),
)
}
#[inline]
#[must_use]
pub fn sub_mat4(&self, rhs: &Self) -> Self {
Self::from_cols(
self.x_axis.sub(rhs.x_axis),
self.y_axis.sub(rhs.y_axis),
self.z_axis.sub(rhs.z_axis),
self.w_axis.sub(rhs.w_axis),
)
}
#[inline]
#[must_use]
pub fn mul_scalar(&self, rhs: f32) -> Self {
Self::from_cols(
self.x_axis.mul(rhs),
self.y_axis.mul(rhs),
self.z_axis.mul(rhs),
self.w_axis.mul(rhs),
)
}
#[inline]
#[must_use]
pub fn div_scalar(&self, rhs: f32) -> Self {
let rhs = Vec4::splat(rhs);
Self::from_cols(
self.x_axis.div(rhs),
self.y_axis.div(rhs),
self.z_axis.div(rhs),
self.w_axis.div(rhs),
)
}
#[inline]
#[must_use]
pub fn abs_diff_eq(&self, rhs: Self, max_abs_diff: f32) -> bool {
self.x_axis.abs_diff_eq(rhs.x_axis, max_abs_diff)
&& self.y_axis.abs_diff_eq(rhs.y_axis, max_abs_diff)
&& self.z_axis.abs_diff_eq(rhs.z_axis, max_abs_diff)
&& self.w_axis.abs_diff_eq(rhs.w_axis, max_abs_diff)
}
#[inline]
#[must_use]
pub fn abs(&self) -> Self {
Self::from_cols(
self.x_axis.abs(),
self.y_axis.abs(),
self.z_axis.abs(),
self.w_axis.abs(),
)
}
#[inline]
pub fn as_dmat4(&self) -> DMat4 {
DMat4::from_cols(
self.x_axis.as_dvec4(),
self.y_axis.as_dvec4(),
self.z_axis.as_dvec4(),
self.w_axis.as_dvec4(),
)
}
}
impl Default for Mat4 {
#[inline]
fn default() -> Self {
Self::IDENTITY
}
}
impl Add<Mat4> for Mat4 {
type Output = Self;
#[inline]
fn add(self, rhs: Self) -> Self::Output {
self.add_mat4(&rhs)
}
}
impl AddAssign<Mat4> for Mat4 {
#[inline]
fn add_assign(&mut self, rhs: Self) {
*self = self.add_mat4(&rhs);
}
}
impl Sub<Mat4> for Mat4 {
type Output = Self;
#[inline]
fn sub(self, rhs: Self) -> Self::Output {
self.sub_mat4(&rhs)
}
}
impl SubAssign<Mat4> for Mat4 {
#[inline]
fn sub_assign(&mut self, rhs: Self) {
*self = self.sub_mat4(&rhs);
}
}
impl Neg for Mat4 {
type Output = Self;
#[inline]
fn neg(self) -> Self::Output {
Self::from_cols(
self.x_axis.neg(),
self.y_axis.neg(),
self.z_axis.neg(),
self.w_axis.neg(),
)
}
}
impl Mul<Mat4> for Mat4 {
type Output = Self;
#[inline]
fn mul(self, rhs: Self) -> Self::Output {
self.mul_mat4(&rhs)
}
}
impl MulAssign<Mat4> for Mat4 {
#[inline]
fn mul_assign(&mut self, rhs: Self) {
*self = self.mul_mat4(&rhs);
}
}
impl Mul<Vec4> for Mat4 {
type Output = Vec4;
#[inline]
fn mul(self, rhs: Vec4) -> Self::Output {
self.mul_vec4(rhs)
}
}
impl Mul<Mat4> for f32 {
type Output = Mat4;
#[inline]
fn mul(self, rhs: Mat4) -> Self::Output {
rhs.mul_scalar(self)
}
}
impl Mul<f32> for Mat4 {
type Output = Self;
#[inline]
fn mul(self, rhs: f32) -> Self::Output {
self.mul_scalar(rhs)
}
}
impl MulAssign<f32> for Mat4 {
#[inline]
fn mul_assign(&mut self, rhs: f32) {
*self = self.mul_scalar(rhs);
}
}
impl Div<Mat4> for f32 {
type Output = Mat4;
#[inline]
fn div(self, rhs: Mat4) -> Self::Output {
rhs.div_scalar(self)
}
}
impl Div<f32> for Mat4 {
type Output = Self;
#[inline]
fn div(self, rhs: f32) -> Self::Output {
self.div_scalar(rhs)
}
}
impl DivAssign<f32> for Mat4 {
#[inline]
fn div_assign(&mut self, rhs: f32) {
*self = self.div_scalar(rhs);
}
}
impl Sum<Self> for Mat4 {
fn sum<I>(iter: I) -> Self
where
I: Iterator<Item = Self>,
{
iter.fold(Self::ZERO, Self::add)
}
}
impl<'a> Sum<&'a Self> for Mat4 {
fn sum<I>(iter: I) -> Self
where
I: Iterator<Item = &'a Self>,
{
iter.fold(Self::ZERO, |a, &b| Self::add(a, b))
}
}
impl Product for Mat4 {
fn product<I>(iter: I) -> Self
where
I: Iterator<Item = Self>,
{
iter.fold(Self::IDENTITY, Self::mul)
}
}
impl<'a> Product<&'a Self> for Mat4 {
fn product<I>(iter: I) -> Self
where
I: Iterator<Item = &'a Self>,
{
iter.fold(Self::IDENTITY, |a, &b| Self::mul(a, b))
}
}
impl PartialEq for Mat4 {
#[inline]
fn eq(&self, rhs: &Self) -> bool {
self.x_axis.eq(&rhs.x_axis)
&& self.y_axis.eq(&rhs.y_axis)
&& self.z_axis.eq(&rhs.z_axis)
&& self.w_axis.eq(&rhs.w_axis)
}
}
#[cfg(not(target_arch = "spirv"))]
impl AsRef<[f32; 16]> for Mat4 {
#[inline]
fn as_ref(&self) -> &[f32; 16] {
unsafe { &*(self as *const Self as *const [f32; 16]) }
}
}
#[cfg(not(target_arch = "spirv"))]
impl AsMut<[f32; 16]> for Mat4 {
#[inline]
fn as_mut(&mut self) -> &mut [f32; 16] {
unsafe { &mut *(self as *mut Self as *mut [f32; 16]) }
}
}
impl fmt::Debug for Mat4 {
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
fmt.debug_struct(stringify!(Mat4))
.field("x_axis", &self.x_axis)
.field("y_axis", &self.y_axis)
.field("z_axis", &self.z_axis)
.field("w_axis", &self.w_axis)
.finish()
}
}
impl fmt::Display for Mat4 {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
if let Some(p) = f.precision() {
write!(
f,
"[{:.*}, {:.*}, {:.*}, {:.*}]",
p, self.x_axis, p, self.y_axis, p, self.z_axis, p, self.w_axis
)
} else {
write!(
f,
"[{}, {}, {}, {}]",
self.x_axis, self.y_axis, self.z_axis, self.w_axis
)
}
}
}