use crate::{
euler::EulerRot::{self, *},
nums::{bool32x4, f32x4},
Mat3x4, Mat4x4, UnitQuatx4, UnitVec2x4, UnitVec3x4, Vec3x4, Vec4x4,
};
use crate::{nums::num_traits::*, Quat};
use auto_ops_det::{impl_op_ex, impl_op_ex_commutative};
#[cfg(not(target_arch = "spirv"))]
use core::fmt;
use core::iter::Sum;
use core::ops::{self, Neg};
#[inline]
pub const fn quatx4(x: f32x4, y: f32x4, z: f32x4, w: f32x4) -> Quatx4 {
Quatx4::from_xyzw(x, y, z, w)
}
#[derive(Clone, Copy)]
#[repr(C)]
pub struct Quatx4 {
pub x: f32x4,
pub y: f32x4,
pub z: f32x4,
pub w: f32x4,
}
impl Quatx4 {
const ZERO: Self = Self::from_array([f32x4::ZERO; 4]);
pub const IDENTITY: Self = Self::from_xyzw(f32x4::ZERO, f32x4::ZERO, f32x4::ZERO, f32x4::ONE);
pub const NAN: Self = Self::from_array([f32x4::NAN; 4]);
#[inline]
pub const fn from_xyzw(x: f32x4, y: f32x4, z: f32x4, w: f32x4) -> Self {
Self { x, y, z, w }
}
#[inline]
pub const fn from_array(a: [f32x4; 4]) -> Self {
Self::from_xyzw(a[0], a[1], a[2], a[3])
}
#[inline]
pub fn lane_select(cond: bool32x4, if_true: Self, if_false: Self) -> Self {
Self::from_vec4(Vec4x4::lane_select(cond, if_true.into(), if_false.into()))
}
#[inline]
pub fn splat_soa(q: Quat) -> Self {
Self {
x: f32x4::splat(q.x),
y: f32x4::splat(q.y),
z: f32x4::splat(q.z),
w: f32x4::splat(q.w),
}
}
#[inline]
pub fn extract_lane(&self, i: usize) -> Quat {
Quat::from_xyzw(
self.x.extract(i),
self.y.extract(i),
self.z.extract(i),
self.w.extract(i),
)
}
#[inline]
pub fn replace_lane(&mut self, i: usize, q: Quat) {
self.x.replace(i, q.x);
self.y.replace(i, q.y);
self.z.replace(i, q.z);
self.w.replace(i, q.w);
}
#[inline]
pub fn split_soa(self) -> [Quat; 4] {
[
self.extract_lane(0),
self.extract_lane(1),
self.extract_lane(2),
self.extract_lane(3),
]
}
#[inline]
pub fn compose_soa(a: &[Quat; 4]) -> Quatx4 {
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 fn from_vec4(v: Vec4x4) -> Self {
Self {
x: v.x,
y: v.y,
z: v.z,
w: v.w,
}
}
#[inline]
pub fn from_slice(slice: &[f32x4]) -> Self {
Self::from_xyzw(slice[0], slice[1], slice[2], slice[3])
}
#[inline]
pub fn write_to_slice(self, slice: &mut [f32x4]) {
slice[0] = self.x;
slice[1] = self.y;
slice[2] = self.z;
slice[3] = self.w;
}
#[inline]
pub fn from_axis_angle(axis: UnitVec3x4, angle: f32x4) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * f32x4::const_splat(0.5)).sin_cosf();
let v = axis * s;
Self::from_xyzw(v.x, v.y, v.z, c)
}
#[inline]
pub fn from_scaled_axis(v: Vec3x4) -> Self {
let length = v.length();
let cond = length.eq(f32x4::ZERO);
let v0 = Self::IDENTITY;
#[cfg(any(feature = "glam-assert", feature = "debug-glam-assert"))]
let length = f32x4::select(f32x4::ONE, cond, length);
#[cfg(any(feature = "glam-assert", feature = "debug-glam-assert"))]
let v = Vec3x4::lane_select(cond, Vec3x4::ONE, v);
let v1 = Self::from_axis_angle((v / length).as_unit_vec3x4_unchecked(), length);
Quatx4::lane_select(cond, v0, v1)
}
#[inline]
pub fn from_rotation_x(angle: f32x4) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * f32x4::const_splat(0.5)).sin_cosf();
Self::from_xyzw(s, f32x4::ZERO, f32x4::ZERO, c)
}
#[inline]
pub fn from_rotation_y(angle: f32x4) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * f32x4::const_splat(0.5)).sin_cosf();
Self::from_xyzw(f32x4::ZERO, s, f32x4::ZERO, c)
}
#[inline]
pub fn from_rotation_z(angle: f32x4) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * f32x4::const_splat(0.5)).sin_cosf();
Self::from_xyzw(f32x4::ZERO, f32x4::ZERO, s, c)
}
#[inline]
pub fn from_euler(euler: EulerRot, a: f32x4, b: f32x4, c: f32x4) -> Self {
let (sa, ca) = (a * f32x4::const_splat(0.5)).sin_cosf();
let (sb, cb) = (b * f32x4::const_splat(0.5)).sin_cosf();
let (sc, cc) = (c * f32x4::const_splat(0.5)).sin_cosf();
match euler {
ZYX => Self::from_xyzw(
ca * cb * sc - cc * sa * sb,
ca * cc * sb + cb * sa * sc,
cb * cc * sa - ca * sb * sc,
ca * cb * cc + sa * sb * sc,
),
ZXY => Self::from_xyzw(
ca * cc * sb - cb * sa * sc,
cc * sa * sb + ca * cb * sc,
cb * cc * sa + ca * sb * sc,
ca * cb * cc - sa * sb * sc,
),
YXZ => Self::from_xyzw(
ca * cc * sb + cb * sa * sc,
cb * cc * sa - ca * sb * sc,
ca * cb * sc - cc * sa * sb,
ca * cb * cc + sa * sb * sc,
),
YZX => Self::from_xyzw(
cc * sa * sb + ca * cb * sc,
cb * cc * sa + ca * sb * sc,
ca * cc * sb - cb * sa * sc,
ca * cb * cc - sa * sb * sc,
),
XYZ => Self::from_xyzw(
cb * cc * sa + ca * sb * sc,
ca * cc * sb - cb * sa * sc,
cc * sa * sb + ca * cb * sc,
ca * cb * cc - sa * sb * sc,
),
XZY => Self::from_xyzw(
cb * cc * sa - ca * sb * sc,
ca * cb * sc - cc * sa * sb,
ca * cc * sb + cb * sa * sc,
ca * cb * cc + sa * sb * sc,
),
ZYZ => Self::from_xyzw(
ca * sb * sc - cc * sa * sb,
ca * cc * sb + sa * sb * sc,
cb * cc * sa + ca * cb * sc,
ca * cb * cc - cb * sa * sc,
),
ZXZ => Self::from_xyzw(
ca * cc * sb + sa * sb * sc,
cc * sa * sb - ca * sb * sc,
cb * cc * sa + ca * cb * sc,
ca * cb * cc - cb * sa * sc,
),
YXY => Self::from_xyzw(
ca * cc * sb + sa * sb * sc,
cb * cc * sa + ca * cb * sc,
ca * sb * sc - cc * sa * sb,
ca * cb * cc - cb * sa * sc,
),
YZY => Self::from_xyzw(
cc * sa * sb - ca * sb * sc,
cb * cc * sa + ca * cb * sc,
ca * cc * sb + sa * sb * sc,
ca * cb * cc - cb * sa * sc,
),
XYX => Self::from_xyzw(
cb * cc * sa + ca * cb * sc,
ca * cc * sb + sa * sb * sc,
cc * sa * sb - ca * sb * sc,
ca * cb * cc - cb * sa * sc,
),
XZX => Self::from_xyzw(
cb * cc * sa + ca * cb * sc,
ca * sb * sc - cc * sa * sb,
ca * cc * sb + sa * sb * sc,
ca * cb * cc - cb * sa * sc,
),
}
.normalize()
}
#[inline]
pub fn from_euler_default(a: f32x4, b: f32x4, c: f32x4) -> Self {
Self::from_euler(EulerRot::default(), a, b, c)
}
#[inline]
pub(crate) fn from_rotation_axes(x_axis: Vec3x4, y_axis: Vec3x4, z_axis: Vec3x4) -> Self {
let (m00, m01, m02) = x_axis.into();
let (m10, m11, m12) = y_axis.into();
let (m20, m21, m22) = z_axis.into();
let branch0 = m22.le(f32x4::ZERO);
let dif10 = m11 - m00;
let omm22 = f32x4::ONE - m22;
let branch00 = dif10.le(f32x4::ZERO);
let four_xsq = omm22 - dif10;
let inv4x = f32x4::HALF * four_xsq.sqrtf().recip();
let four_ysq = omm22 + dif10;
let inv4y = f32x4::HALF * four_ysq.sqrtf().recip();
let sum10 = m11 + m00;
let opm22 = f32x4::ONE + m22;
let branch01 = sum10.le(f32x4::ZERO);
let four_zsq = opm22 - sum10;
let inv4z = f32x4::HALF * four_zsq.sqrtf().recip();
let four_wsq = opm22 + sum10;
let inv4w = f32x4::HALF * four_wsq.sqrtf().recip();
Self::from_xyzw(
(four_xsq * inv4x)
.select(branch00, (m01 + m10) * inv4y)
.select(
branch0,
((m02 + m20) * inv4z).select(branch01, (m12 - m21) * inv4w),
),
((m01 + m10) * inv4x)
.select(branch00, four_ysq * inv4y)
.select(
branch0,
((m12 + m21) * inv4z).select(branch01, (m20 - m02) * inv4w),
),
((m02 + m20) * inv4x)
.select(branch00, (m12 + m21) * inv4y)
.select(
branch0,
(four_zsq * inv4z).select(branch01, (m01 - m10) * inv4w),
),
((m12 - m21) * inv4x)
.select(branch00, (m20 - m02) * inv4y)
.select(
branch0,
((m01 - m10) * inv4z).select(branch01, four_wsq * inv4w),
),
)
}
#[inline]
pub fn from_mat3(mat: &Mat3x4) -> Self {
Self::from_rotation_axes(mat.x_axis, mat.y_axis, mat.z_axis)
}
#[inline]
pub fn from_mat4(mat: &Mat4x4) -> Self {
Self::from_rotation_axes(
mat.x_axis.truncate(),
mat.y_axis.truncate(),
mat.z_axis.truncate(),
)
}
#[inline]
pub fn from_affine3(a: &crate::Affine3x4) -> Self {
#[allow(clippy::useless_conversion)]
Self::from_rotation_axes(
a.matrix3.x_axis.into(),
a.matrix3.y_axis.into(),
a.matrix3.z_axis.into(),
)
}
pub fn from_rotation_arc(from: UnitVec3x4, to: UnitVec3x4) -> Self {
let one_minus_eps = f32x4::ONE - f32x4::const_splat(2.0) * f32x4::EPSILON;
let dot = from.dot(to);
let cond1 = dot.gt(one_minus_eps);
let cond2 = dot.lt(-one_minus_eps);
let res1 = Self::IDENTITY;
let res2 = Self::from_axis_angle(from.any_orthogonal_vector(), f32x4::PI);
let c = from.cross(to);
let res3 = Vec4x4::new(c.x, c.y, c.z, f32x4::ONE + dot);
#[cfg(any(feature = "glam-assert", feature = "debug-glam-assert"))]
let res3 = Vec4x4::lane_select(cond2, Vec4x4::X, res3);
let res3 = Self::from_vec4(res3.normalize());
Quatx4::lane_select(cond2, Quatx4::lane_select(cond1, res1, res2), res3)
}
#[inline]
pub fn from_rotation_arc_colinear(from: UnitVec3x4, to: UnitVec3x4) -> Self {
let cond = from.dot(to).le(f32x4::ZERO);
let res1 = Self::from_rotation_arc(from, -to);
let res2 = Self::from_rotation_arc(from, to);
Quatx4::lane_select(cond, res1, res2)
}
pub fn from_rotation_arc_2d(from: UnitVec2x4, to: UnitVec2x4) -> Self {
let one_minus_epsilon = f32x4::ONE - f32x4::const_splat(2.0) * f32x4::EPSILON;
let dot = from.dot(to);
let cond1 = dot.gt(one_minus_epsilon);
let cond2 = dot.lt(-one_minus_epsilon);
let res1 = Self::IDENTITY;
const COS_FRAC_PI_2: f32x4 = f32x4::ZERO;
const SIN_FRAC_PI_2: f32x4 = f32x4::ONE;
let res2 = Self::from_xyzw(f32x4::ZERO, f32x4::ZERO, SIN_FRAC_PI_2, COS_FRAC_PI_2);
let z = from.x * to.y - to.x * from.y;
let w = f32x4::ONE + dot;
let len_rcp = f32x4::ONE / (z * z + w * w).sqrtf();
let res3 = Self::from_xyzw(f32x4::ZERO, f32x4::ZERO, z * len_rcp, w * len_rcp);
Quatx4::lane_select(cond2, Quatx4::lane_select(cond1, res1, res2), res3)
}
#[inline]
pub fn to_array(&self) -> [f32x4; 4] {
[self.x, self.y, self.z, self.w]
}
#[inline]
pub fn xyz(self) -> Vec3x4 {
Vec3x4::new(self.x, self.y, self.z)
}
#[must_use]
#[inline]
pub fn conjugate(self) -> Self {
Self {
x: -self.x,
y: -self.y,
z: -self.z,
w: self.w,
}
}
#[inline]
pub fn dot(self, rhs: Self) -> f32x4 {
Vec4x4::from(self).dot(Vec4x4::from(rhs))
}
#[doc(alias = "magnitude")]
#[inline]
pub fn length(self) -> f32x4 {
Vec4x4::from(self).length()
}
#[doc(alias = "magnitude2")]
#[inline]
pub fn length_squared(self) -> f32x4 {
Vec4x4::from(self).length_squared()
}
#[inline]
pub fn length_recip(self) -> f32x4 {
Vec4x4::from(self).length_recip()
}
#[must_use]
#[inline]
pub fn normalize(self) -> Self {
Self::from_vec4(Vec4x4::from(self).normalize())
}
#[must_use]
#[inline]
pub fn normalize_to_unit(self) -> UnitQuatx4 {
self.normalize().as_unit_quatx4_unchecked()
}
#[inline]
pub fn is_finite(self) -> bool32x4 {
Vec4x4::from(self).is_finite()
}
#[inline]
pub fn is_nan(self) -> bool32x4 {
Vec4x4::from(self).is_nan()
}
#[inline]
pub fn is_normalized(self) -> bool32x4 {
Vec4x4::from(self).is_normalized()
}
#[inline]
pub fn abs_diff_eq(self, rhs: Self, max_abs_diff: f32x4) -> bool32x4 {
Vec4x4::from(self).abs_diff_eq(Vec4x4::from(rhs), max_abs_diff)
}
#[inline]
pub fn mul_quat(self, rhs: Self) -> Self {
let (x0, y0, z0, w0) = self.into();
let (x1, y1, z1, w1) = rhs.into();
Self::from_xyzw(
w0 * x1 + x0 * w1 + y0 * z1 - z0 * y1,
w0 * y1 - x0 * z1 + y0 * w1 + z0 * x1,
w0 * z1 + x0 * y1 - y0 * x1 + z0 * w1,
w0 * w1 - x0 * x1 - y0 * y1 - z0 * z1,
)
}
}
#[cfg(not(target_arch = "spirv"))]
impl fmt::Debug for Quatx4 {
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
fmt.debug_tuple(stringify!(Quatx4))
.field(&self.x)
.field(&self.y)
.field(&self.z)
.field(&self.w)
.finish()
}
}
#[cfg(not(target_arch = "spirv"))]
impl fmt::Display for Quatx4 {
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(fmt, "[{}, {}, {}, {}]", self.x, self.y, self.z, self.w)
}
}
impl_op_ex!(+ |a: &Quatx4, b: &Quatx4| -> Quatx4{
Quatx4::from_vec4(Vec4x4::from(a) + Vec4x4::from(b))
});
impl_op_ex!(-|a: &Quatx4, b: &Quatx4| -> Quatx4 {
Quatx4::from_vec4(Vec4x4::from(a) - Vec4x4::from(b))
});
impl_op_ex!(+= |a: &mut Quatx4, b: &Quatx4| { *a = *a + b });
impl_op_ex!(-= |a: &mut Quatx4, b: &Quatx4| { *a = *a - b });
impl_op_ex_commutative!(*|a: &Quatx4, b: &f32x4| -> Quatx4 {
Quatx4::from_vec4(Vec4x4::from(a) * b)
});
impl_op_ex!(/ |a: &Quatx4, b: &f32x4| -> Quatx4{
Quatx4::from_vec4(Vec4x4::from(a) / b)
});
impl_op_ex!(*|a: &Quatx4, b: &Quatx4| -> Quatx4 { a.mul_quat(*b) });
impl_op_ex!(*= |a: &mut Quatx4, b: &Quatx4| {
*a = a.mul_quat(*b)
});
impl Neg for Quatx4 {
type Output = Self;
#[inline]
fn neg(self) -> Self {
self * f32x4::NEG_ONE
}
}
impl Default for Quatx4 {
#[inline]
fn default() -> Self {
Self::IDENTITY
}
}
impl PartialEq for Quatx4 {
#[inline]
fn eq(&self, rhs: &Self) -> bool {
Vec4x4::from(*self).eq(&Vec4x4::from(*rhs)).all()
}
}
#[cfg(not(target_arch = "spirv"))]
impl AsRef<[f32x4; 4]> for Quatx4 {
#[inline]
fn as_ref(&self) -> &[f32x4; 4] {
unsafe { &*(self as *const Self as *const [f32x4; 4]) }
}
}
impl<'a> Sum<&'a Self> for Quatx4 {
fn sum<I>(iter: I) -> Self
where
I: Iterator<Item = &'a Self>,
{
iter.fold(Self::ZERO, |a, &b| a + b)
}
}
impl From<Quatx4> for Vec4x4 {
#[inline]
fn from(q: Quatx4) -> Self {
Self::new(q.x, q.y, q.z, q.w)
}
}
impl From<Quatx4> for (f32x4, f32x4, f32x4, f32x4) {
#[inline]
fn from(q: Quatx4) -> Self {
(q.x, q.y, q.z, q.w)
}
}
impl From<Quatx4> for [f32x4; 4] {
#[inline]
fn from(q: Quatx4) -> Self {
[q.x, q.y, q.z, q.w]
}
}
impl From<&Quatx4> for Vec4x4 {
#[inline]
fn from(q: &Quatx4) -> Self {
Self::new(q.x, q.y, q.z, q.w)
}
}
impl From<&Quatx4> for (f32x4, f32x4, f32x4, f32x4) {
#[inline]
fn from(q: &Quatx4) -> Self {
(q.x, q.y, q.z, q.w)
}
}
impl From<&Quatx4> for [f32x4; 4] {
#[inline]
fn from(q: &Quatx4) -> Self {
[q.x, q.y, q.z, q.w]
}
}