use crate::{
nums::{bool32x4, f32x4, num_traits::*},
swizzles::*,
EulerRot, Mat2x4, Mat3, Mat4x4, Point2x4, UnitQuatx4, UnitVec3x4, Vec2x4, Vec3x4,
};
use auto_ops_det::{impl_op_ex, impl_op_ex_commutative};
#[cfg(not(target_arch = "spirv"))]
use core::fmt;
use core::iter::{Product, Sum};
use core::ops::{self, Add, Mul, Neg, Sub};
#[inline]
pub const fn mat3x4(x_axis: Vec3x4, y_axis: Vec3x4, z_axis: Vec3x4) -> Mat3x4 {
Mat3x4::from_cols(x_axis, y_axis, z_axis)
}
#[derive(Clone, Copy)]
#[repr(C)]
pub struct Mat3x4 {
pub x_axis: Vec3x4,
pub y_axis: Vec3x4,
pub z_axis: Vec3x4,
}
impl Mat3x4 {
pub const ZERO: Self = Self::from_cols(Vec3x4::ZERO, Vec3x4::ZERO, Vec3x4::ZERO);
pub const IDENTITY: Self = Self::from_cols(Vec3x4::X, Vec3x4::Y, Vec3x4::Z);
pub const NAN: Self = Self::from_cols(Vec3x4::NAN, Vec3x4::NAN, Vec3x4::NAN);
#[allow(clippy::too_many_arguments)]
#[inline]
const fn new(
m00: f32x4,
m01: f32x4,
m02: f32x4,
m10: f32x4,
m11: f32x4,
m12: f32x4,
m20: f32x4,
m21: f32x4,
m22: f32x4,
) -> Self {
Self {
x_axis: Vec3x4::new(m00, m01, m02),
y_axis: Vec3x4::new(m10, m11, m12),
z_axis: Vec3x4::new(m20, m21, m22),
}
}
#[inline]
pub fn lane_select(cond: bool32x4, if_true: Self, if_false: Self) -> Self {
Self {
x_axis: Vec3x4::lane_select(cond, if_true.x_axis, if_false.x_axis),
y_axis: Vec3x4::lane_select(cond, if_true.y_axis, if_false.y_axis),
z_axis: Vec3x4::lane_select(cond, if_true.z_axis, if_false.z_axis),
}
}
#[inline]
pub fn splat_soa(m: Mat3) -> Self {
Self {
x_axis: Vec3x4::splat_soa(m.x_axis),
y_axis: Vec3x4::splat_soa(m.y_axis),
z_axis: Vec3x4::splat_soa(m.z_axis),
}
}
#[inline]
pub fn extract_lane(&self, i: usize) -> Mat3 {
Mat3 {
x_axis: self.x_axis.extract_lane(i),
y_axis: self.y_axis.extract_lane(i),
z_axis: self.z_axis.extract_lane(i),
}
}
#[inline]
pub fn replace_lane(&mut self, i: usize, m: Mat3) {
self.x_axis.replace_lane(i, m.x_axis);
self.y_axis.replace_lane(i, m.y_axis);
self.z_axis.replace_lane(i, m.z_axis);
}
#[inline]
pub fn split_soa(self) -> [Mat3; 4] {
[
self.extract_lane(0),
self.extract_lane(1),
self.extract_lane(2),
self.extract_lane(3),
]
}
#[inline]
pub fn compose_soa(a: &[Mat3; 4]) -> Mat3x4 {
let mut v = Self::default();
v.replace_lane(0, a[0]);
v.replace_lane(1, a[1]);
v.replace_lane(2, a[2]);
v.replace_lane(3, a[3]);
v
}
#[inline]
pub const fn from_cols(x_axis: Vec3x4, y_axis: Vec3x4, z_axis: Vec3x4) -> Self {
Self {
x_axis,
y_axis,
z_axis,
}
}
#[inline]
pub const fn from_cols_array(m: &[f32x4; 9]) -> Self {
Self::new(m[0], m[1], m[2], m[3], m[4], m[5], m[6], m[7], m[8])
}
#[inline]
pub fn to_cols_array(&self) -> [f32x4; 9] {
[
self.x_axis.x,
self.x_axis.y,
self.x_axis.z,
self.y_axis.x,
self.y_axis.y,
self.y_axis.z,
self.z_axis.x,
self.z_axis.y,
self.z_axis.z,
]
}
#[inline]
pub const fn from_cols_array_2d(m: &[[f32x4; 3]; 3]) -> Self {
Self::from_cols(
Vec3x4::from_array(m[0]),
Vec3x4::from_array(m[1]),
Vec3x4::from_array(m[2]),
)
}
#[inline]
pub fn to_cols_array_2d(&self) -> [[f32x4; 3]; 3] {
[
self.x_axis.to_array(),
self.y_axis.to_array(),
self.z_axis.to_array(),
]
}
#[doc(alias = "scale")]
#[inline]
pub fn from_diagonal(diagonal: Vec3x4) -> Self {
Self::new(
diagonal.x,
f32x4::ZERO,
f32x4::ZERO,
f32x4::ZERO,
diagonal.y,
f32x4::ZERO,
f32x4::ZERO,
f32x4::ZERO,
diagonal.z,
)
}
pub fn from_mat4(m: Mat4x4) -> Self {
Self::from_cols(m.x_axis.xyz(), m.y_axis.xyz(), m.z_axis.xyz())
}
#[inline]
pub fn from_quat(rotation: UnitQuatx4) -> Self {
let x2 = rotation.x + rotation.x;
let y2 = rotation.y + rotation.y;
let z2 = rotation.z + rotation.z;
let xx = rotation.x * x2;
let xy = rotation.x * y2;
let xz = rotation.x * z2;
let yy = rotation.y * y2;
let yz = rotation.y * z2;
let zz = rotation.z * z2;
let wx = rotation.w * x2;
let wy = rotation.w * y2;
let wz = rotation.w * z2;
Self::from_cols(
Vec3x4::new(f32x4::ONE - (yy + zz), xy + wz, xz - wy),
Vec3x4::new(xy - wz, f32x4::ONE - (xx + zz), yz + wx),
Vec3x4::new(xz + wy, yz - wx, f32x4::ONE - (xx + yy)),
)
}
#[inline]
pub fn from_axis_angle(axis: UnitVec3x4, angle: f32x4) -> Self {
let (sin, cos) = angle.sin_cosf();
let (xsin, ysin, zsin) = axis.mul(sin).into();
let (x, y, z) = axis.into();
let (x2, y2, z2) = axis.mul(axis).into();
let omc = f32x4::ONE - cos;
let xyomc = x * y * omc;
let xzomc = x * z * omc;
let yzomc = y * z * omc;
Self::from_cols(
Vec3x4::new(x2 * omc + cos, xyomc + zsin, xzomc - ysin),
Vec3x4::new(xyomc - zsin, y2 * omc + cos, yzomc + xsin),
Vec3x4::new(xzomc + ysin, yzomc - xsin, z2 * omc + cos),
)
}
#[inline]
pub fn from_euler(order: EulerRot, a: f32x4, b: f32x4, c: f32x4) -> Self {
let quat = UnitQuatx4::from_euler(order, a, b, c);
Self::from_quat(quat)
}
#[inline]
pub fn from_rotation_x(angle: f32x4) -> Self {
let (sina, cosa) = angle.sin_cosf();
Self::from_cols(
Vec3x4::X,
Vec3x4::new(f32x4::ZERO, cosa, sina),
Vec3x4::new(f32x4::ZERO, -sina, cosa),
)
}
#[inline]
pub fn from_rotation_y(angle: f32x4) -> Self {
let (sina, cosa) = angle.sin_cosf();
Self::from_cols(
Vec3x4::new(cosa, f32x4::ZERO, -sina),
Vec3x4::Y,
Vec3x4::new(sina, f32x4::ZERO, cosa),
)
}
#[inline]
pub fn from_rotation_z(angle: f32x4) -> Self {
let (sina, cosa) = angle.sin_cosf();
Self::from_cols(
Vec3x4::new(cosa, sina, f32x4::ZERO),
Vec3x4::new(-sina, cosa, f32x4::ZERO),
Vec3x4::Z,
)
}
#[inline]
pub fn from_translation(translation: Vec2x4) -> Self {
Self::from_cols(
Vec3x4::X,
Vec3x4::Y,
Vec3x4::new(translation.x, translation.y, f32x4::ONE),
)
}
#[inline]
pub fn from_angle(angle: f32x4) -> Self {
let (sin, cos) = angle.sin_cosf();
Self::from_cols(
Vec3x4::new(cos, sin, f32x4::ZERO),
Vec3x4::new(-sin, cos, f32x4::ZERO),
Vec3x4::Z,
)
}
#[inline]
pub fn from_scale_angle_translation(scale: Vec2x4, angle: f32x4, translation: Vec2x4) -> Self {
let (sin, cos) = angle.sin_cosf();
Self::from_cols(
Vec3x4::new(cos * scale.x, sin * scale.x, f32x4::ZERO),
Vec3x4::new(-sin * scale.y, cos * scale.y, f32x4::ZERO),
Vec3x4::new(translation.x, translation.y, f32x4::ONE),
)
}
#[inline]
pub fn from_scale(scale: Vec2x4) -> Self {
glam_assert!(scale.cmpne(Vec2x4::ZERO).any());
Self::from_cols(
Vec3x4::new(scale.x, f32x4::ZERO, f32x4::ZERO),
Vec3x4::new(f32x4::ZERO, scale.y, f32x4::ZERO),
Vec3x4::Z,
)
}
#[inline]
pub fn from_mat2(m: Mat2x4) -> Self {
Self::from_cols(
(m.x_axis, f32x4::ZERO).into(),
(m.y_axis, f32x4::ZERO).into(),
Vec3x4::Z,
)
}
#[inline]
pub const fn from_cols_slice(slice: &[f32x4]) -> Self {
Self::new(
slice[0], slice[1], slice[2], slice[3], slice[4], slice[5], slice[6], slice[7],
slice[8],
)
}
#[inline]
pub fn write_cols_to_slice(self, slice: &mut [f32x4]) {
slice[0] = self.x_axis.x;
slice[1] = self.x_axis.y;
slice[2] = self.x_axis.z;
slice[3] = self.y_axis.x;
slice[4] = self.y_axis.y;
slice[5] = self.y_axis.z;
slice[6] = self.z_axis.x;
slice[7] = self.z_axis.y;
slice[8] = self.z_axis.z;
}
#[inline]
pub fn col(&self, index: usize) -> Vec3x4 {
match index {
0 => self.x_axis,
1 => self.y_axis,
2 => self.z_axis,
_ => panic!("index out of bounds"),
}
}
#[inline]
pub fn col_mut(&mut self, index: usize) -> &mut Vec3x4 {
match index {
0 => &mut self.x_axis,
1 => &mut self.y_axis,
2 => &mut self.z_axis,
_ => panic!("index out of bounds"),
}
}
#[inline]
pub fn row(&self, index: usize) -> Vec3x4 {
match index {
0 => Vec3x4::new(self.x_axis.x, self.y_axis.x, self.z_axis.x),
1 => Vec3x4::new(self.x_axis.y, self.y_axis.y, self.z_axis.y),
2 => Vec3x4::new(self.x_axis.z, self.y_axis.z, self.z_axis.z),
_ => panic!("index out of bounds"),
}
}
#[inline]
pub fn is_finite(&self) -> bool32x4 {
self.x_axis.is_finite() & self.y_axis.is_finite() & self.z_axis.is_finite()
}
#[inline]
pub fn is_nan(&self) -> bool32x4 {
self.x_axis.is_nan() | self.y_axis.is_nan() | self.z_axis.is_nan()
}
#[must_use]
#[inline]
pub fn transpose(&self) -> Self {
Self {
x_axis: Vec3x4::new(self.x_axis.x, self.y_axis.x, self.z_axis.x),
y_axis: Vec3x4::new(self.x_axis.y, self.y_axis.y, self.z_axis.y),
z_axis: Vec3x4::new(self.x_axis.z, self.y_axis.z, self.z_axis.z),
}
}
#[inline]
pub fn determinant(&self) -> f32x4 {
self.z_axis.dot(self.x_axis.cross(self.y_axis))
}
#[must_use]
#[inline]
pub fn inverse(&self) -> Self {
let tmp0 = self.y_axis.cross(self.z_axis);
let tmp1 = self.z_axis.cross(self.x_axis);
let tmp2 = self.x_axis.cross(self.y_axis);
let det = self.z_axis.dot(tmp2);
glam_assert!(det.ne(f32x4::ZERO));
let inv_det = Vec3x4::splat(det.recip());
Self::from_cols(tmp0.mul(inv_det), tmp1.mul(inv_det), tmp2.mul(inv_det)).transpose()
}
#[inline]
pub fn transform_point2(&self, rhs: Point2x4) -> Point2x4 {
glam_assert!(self.row(2).abs_diff_eq(Vec3x4::Z, f32x4::const_splat(1e-6)));
Point2x4((self.x_axis * rhs.0.x + self.y_axis * rhs.0.y + self.z_axis).xy())
}
#[inline]
pub fn transform_vector2(&self, rhs: Vec2x4) -> Vec2x4 {
glam_assert!(self.row(2).abs_diff_eq(Vec3x4::Z, f32x4::const_splat(1e-6)));
(self.x_axis * rhs.x + self.y_axis * rhs.y).xy()
}
#[inline]
pub fn mul_vec3(&self, rhs: Vec3x4) -> Vec3x4 {
let mut res = self.x_axis.mul(rhs.x);
res = res.add(self.y_axis.mul(rhs.y));
res = res.add(self.z_axis.mul(rhs.z));
res
}
#[inline]
pub fn mul_mat3(&self, rhs: &Self) -> Self {
Self::from_cols(
self.mul(rhs.x_axis),
self.mul(rhs.y_axis),
self.mul(rhs.z_axis),
)
}
#[inline]
pub fn add_mat3(&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),
)
}
#[inline]
pub fn sub_mat3(&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),
)
}
#[inline]
pub fn mul_scalar(&self, rhs: f32x4) -> Self {
Self::from_cols(
self.x_axis.mul(rhs),
self.y_axis.mul(rhs),
self.z_axis.mul(rhs),
)
}
#[inline]
pub fn abs_diff_eq(&self, rhs: Self, max_abs_diff: f32x4) -> bool32x4 {
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)
}
}
impl Default for Mat3x4 {
#[inline]
fn default() -> Self {
Self::IDENTITY
}
}
impl_op_ex!(+ |a: &Mat3x4, b:&Mat3x4| -> Mat3x4 { a.add_mat3(b) });
impl_op_ex!(+= |a: &mut Mat3x4, b:&Mat3x4| { *a = a.add_mat3(b); });
impl_op_ex!(-|a: &Mat3x4, b: &Mat3x4| -> Mat3x4 { a.sub_mat3(b) });
impl_op_ex!(-= |a: &mut Mat3x4, b:&Mat3x4| { *a = a.sub_mat3(b); });
impl Neg for Mat3x4 {
type Output = Self;
#[inline]
fn neg(self) -> Self::Output {
Self::from_cols(self.x_axis.neg(), self.y_axis.neg(), self.z_axis.neg())
}
}
impl_op_ex!(*|a: &Mat3x4, b: &Mat3x4| -> Mat3x4 { a.mul_mat3(b) });
impl_op_ex!(*= |a: &mut Mat3x4, b:&Mat3x4| { *a = a.mul_mat3(b); });
impl_op_ex!(*|a: &Mat3x4, b: &Vec3x4| -> Vec3x4 { a.mul_vec3(*b) });
impl_op_ex!(*|a: &Mat3x4, b: &UnitVec3x4| -> Vec3x4 { a * b.as_vec3x4() });
impl_op_ex!(*|a: &Mat3x4, b: &Point2x4| -> Point2x4 { a.transform_point2(*b) });
impl_op_ex_commutative!(*|a: &Mat3x4, b: f32x4| -> Mat3x4 { a.mul_scalar(b) });
impl_op_ex!(*= |a: &mut Mat3x4, b:f32x4| { *a = a.mul_scalar(b); });
impl<'a> Sum<&'a Self> for Mat3x4 {
fn sum<I>(iter: I) -> Self
where
I: Iterator<Item = &'a Self>,
{
iter.fold(Self::ZERO, |a, &b| Self::add(a, b))
}
}
impl<'a> Product<&'a Self> for Mat3x4 {
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 Mat3x4 {
#[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)
}
}
#[cfg(not(target_arch = "spirv"))]
impl AsRef<[f32x4; 9]> for Mat3x4 {
#[inline]
fn as_ref(&self) -> &[f32x4; 9] {
unsafe { &*(self as *const Self as *const [f32x4; 9]) }
}
}
#[cfg(not(target_arch = "spirv"))]
impl AsMut<[f32x4; 9]> for Mat3x4 {
#[inline]
fn as_mut(&mut self) -> &mut [f32x4; 9] {
unsafe { &mut *(self as *mut Self as *mut [f32x4; 9]) }
}
}
#[cfg(not(target_arch = "spirv"))]
impl fmt::Debug for Mat3x4 {
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
fmt.debug_struct(stringify!(Mat3x4))
.field("x_axis", &self.x_axis)
.field("y_axis", &self.y_axis)
.field("z_axis", &self.z_axis)
.finish()
}
}
#[cfg(not(target_arch = "spirv"))]
impl fmt::Display for Mat3x4 {
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(f, "[{}, {}, {}]", self.x_axis, self.y_axis, self.z_axis)
}
}