use crate::f32::simd_alias::{Mat4, UnitQuat, Vec4};
use crate::{
euler::EulerRot::{self, *},
sse2, DQuat, Mat3, UnitVec2, UnitVec3, Vec3,
};
use crate::nums::*;
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
use core::arch::x86_64::*;
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, Deref, DerefMut, Neg};
union UnionCast {
a: [f32; 4],
v: Quat,
}
#[inline]
pub const fn quat(x: f32, y: f32, z: f32, w: f32) -> Quat {
Quat::from_xyzw(x, y, z, w)
}
#[derive(Clone, Copy)]
#[repr(transparent)]
pub struct Quat(pub(crate) __m128);
impl Quat {
const ZERO: Self = Self::from_array([0.0_f32; 4]);
pub const IDENTITY: Self = Self::from_xyzw(0.0_f32, 0.0_f32, 0.0_f32, 1.0_f32);
pub const NAN: Self = Self::from_array([f32::NAN; 4]);
#[inline]
pub const fn from_xyzw(x: f32, y: f32, z: f32, w: f32) -> Self {
unsafe { UnionCast { a: [x, y, z, w] }.v }
}
#[inline]
pub const fn from_array(a: [f32; 4]) -> Self {
Self::from_xyzw(a[0], a[1], a[2], a[3])
}
#[inline]
pub fn from_vec4(v: Vec4) -> Self {
Self(v.0)
}
#[inline]
pub fn from_slice(slice: &[f32]) -> Self {
assert!(slice.len() >= 4);
Self(unsafe { _mm_loadu_ps(slice.as_ptr()) })
}
#[inline]
pub fn write_to_slice(self, slice: &mut [f32]) {
assert!(slice.len() >= 4);
unsafe { _mm_storeu_ps(slice.as_mut_ptr(), self.0) }
}
#[inline]
pub fn from_axis_angle(axis: UnitVec3, angle: f32) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * 0.5_f32).sin_cosf();
let v = axis * s;
Self::from_xyzw(v.x, v.y, v.z, c)
}
#[inline]
pub fn from_scaled_axis(v: Vec3) -> Self {
let length = v.length();
if length == 0.0_f32 {
Self::IDENTITY
} else {
Self::from_axis_angle((v / length).as_unit_vec3_unchecked(), length)
}
}
#[inline]
pub fn from_rotation_x(angle: f32) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * 0.5_f32).sin_cosf();
Self::from_xyzw(s, 0.0_f32, 0.0_f32, c)
}
#[inline]
pub fn from_rotation_y(angle: f32) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * 0.5_f32).sin_cosf();
Self::from_xyzw(0.0_f32, s, 0.0_f32, c)
}
#[inline]
pub fn from_rotation_z(angle: f32) -> Self {
glam_assert!(angle.is_finite(), "angle {:?} is not finite.", angle);
let (s, c) = (angle * 0.5_f32).sin_cosf();
Self::from_xyzw(0.0_f32, 0.0_f32, s, c)
}
#[inline]
pub fn from_euler(euler: EulerRot, a: f32, b: f32, c: f32) -> Self {
let (sa, ca) = (a * 0.5_f32).sin_cosf();
let (sb, cb) = (b * 0.5_f32).sin_cosf();
let (sc, cc) = (c * 0.5_f32).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: f32, b: f32, c: f32) -> Self {
Self::from_euler(EulerRot::default(), a, b, c)
}
#[inline]
pub(crate) fn from_rotation_axes(x_axis: Vec3, y_axis: Vec3, z_axis: Vec3) -> Self {
let (m00, m01, m02) = x_axis.into();
let (m10, m11, m12) = y_axis.into();
let (m20, m21, m22) = z_axis.into();
if m22 <= 0.0 {
let dif10 = m11 - m00;
let omm22 = 1.0 - m22;
if dif10 <= 0.0 {
let four_xsq = omm22 - dif10;
let inv4x = 0.5 * four_xsq.sqrtf().recip();
Self::from_xyzw(
four_xsq * inv4x,
(m01 + m10) * inv4x,
(m02 + m20) * inv4x,
(m12 - m21) * inv4x,
)
} else {
let four_ysq = omm22 + dif10;
let inv4y = 0.5 * four_ysq.sqrtf().recip();
Self::from_xyzw(
(m01 + m10) * inv4y,
four_ysq * inv4y,
(m12 + m21) * inv4y,
(m20 - m02) * inv4y,
)
}
} else {
let sum10 = m11 + m00;
let opm22 = 1.0 + m22;
if sum10 <= 0.0 {
let four_zsq = opm22 - sum10;
let inv4z = 0.5 * four_zsq.sqrtf().recip();
Self::from_xyzw(
(m02 + m20) * inv4z,
(m12 + m21) * inv4z,
four_zsq * inv4z,
(m01 - m10) * inv4z,
)
} else {
let four_wsq = opm22 + sum10;
let inv4w = 0.5 * four_wsq.sqrtf().recip();
Self::from_xyzw(
(m12 - m21) * inv4w,
(m20 - m02) * inv4w,
(m01 - m10) * inv4w,
four_wsq * inv4w,
)
}
}
}
#[inline]
pub fn from_mat3(mat: &Mat3) -> Self {
Self::from_rotation_axes(mat.x_axis, mat.y_axis, mat.z_axis)
}
#[inline]
pub fn from_mat4(mat: &Mat4) -> Self {
Self::from_rotation_axes(
mat.x_axis.truncate(),
mat.y_axis.truncate(),
mat.z_axis.truncate(),
)
}
#[inline]
pub fn from_affine3(a: &crate::Affine3A) -> 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: UnitVec3, to: UnitVec3) -> Self {
const ONE_MINUS_EPS: f32 = 1.0_f32 - 2.0_f32 * f32::EPSILON;
let dot = from.dot(to);
if dot > ONE_MINUS_EPS {
Self::IDENTITY
} else if dot < -ONE_MINUS_EPS {
use core::f32::consts::PI; Self::from_axis_angle(from.any_orthogonal_vector(), PI)
} else {
let c = from.cross(to);
Quat::from_xyzw(c.x, c.y, c.z, 1.0 + dot).normalize()
}
}
#[inline]
pub fn from_rotation_arc_colinear(from: UnitVec3, to: UnitVec3) -> Self {
if from.dot(to) < 0.0_f32 {
Self::from_rotation_arc(from, -to)
} else {
Self::from_rotation_arc(from, to)
}
}
pub fn from_rotation_arc_2d(from: UnitVec2, to: UnitVec2) -> Self {
const ONE_MINUS_EPSILON: f32 = 1.0 - 2.0 * f32::EPSILON;
let dot = from.dot(to);
if dot > ONE_MINUS_EPSILON {
Self::IDENTITY
} else if dot < -ONE_MINUS_EPSILON {
const COS_FRAC_PI_2: f32 = 0.0;
const SIN_FRAC_PI_2: f32 = 1.0;
Self::from_xyzw(0.0, 0.0, SIN_FRAC_PI_2, COS_FRAC_PI_2)
} else {
let z = from.x * to.y - to.x * from.y;
let w = 1.0 + dot;
let len_rcp = 1.0 / (z * z + w * w).sqrtf();
Self::from_xyzw(0.0, 0.0, z * len_rcp, w * len_rcp)
}
}
#[inline]
pub fn to_array(&self) -> [f32; 4] {
[self.x, self.y, self.z, self.w]
}
#[inline]
pub fn xyz(self) -> Vec3 {
Vec3::new(self.x, self.y, self.z)
}
#[must_use]
#[inline]
pub fn conjugate(self) -> Self {
const SIGN: __m128 = sse2::m128_from_f32x4([-0.0, -0.0, -0.0, 0.0]);
Self(unsafe { _mm_xor_ps(self.0, SIGN) })
}
#[inline]
pub fn dot(self, rhs: Self) -> f32 {
Vec4::from(self).dot(Vec4::from(rhs))
}
#[doc(alias = "magnitude")]
#[inline]
pub fn length(self) -> f32 {
Vec4::from(self).length()
}
#[doc(alias = "magnitude2")]
#[inline]
pub fn length_squared(self) -> f32 {
Vec4::from(self).length_squared()
}
#[inline]
pub fn length_recip(self) -> f32 {
Vec4::from(self).length_recip()
}
#[must_use]
#[inline]
pub fn normalize(self) -> Self {
Self::from_vec4(Vec4::from(self).normalize())
}
#[must_use]
#[inline]
pub fn normalize_to_unit(self) -> UnitQuat {
self.normalize().as_unit_quat_unchecked()
}
#[inline]
pub fn is_finite(self) -> bool {
Vec4::from(self).is_finite()
}
#[inline]
pub fn is_nan(self) -> bool {
Vec4::from(self).is_nan()
}
#[inline]
pub fn is_normalized(self) -> bool {
Vec4::from(self).is_normalized()
}
#[inline]
pub fn abs_diff_eq(self, rhs: Self, max_abs_diff: f32) -> bool {
Vec4::from(self).abs_diff_eq(Vec4::from(rhs), max_abs_diff)
}
#[inline]
pub fn mul_quat(self, rhs: Self) -> Self {
const CONTROL_WZYX: __m128 = sse2::m128_from_f32x4([1.0, -1.0, 1.0, -1.0]);
const CONTROL_ZWXY: __m128 = sse2::m128_from_f32x4([1.0, 1.0, -1.0, -1.0]);
const CONTROL_YXWZ: __m128 = sse2::m128_from_f32x4([-1.0, 1.0, 1.0, -1.0]);
let lhs = self.0;
let rhs = rhs.0;
unsafe {
let r_xxxx = _mm_shuffle_ps(lhs, lhs, 0b00_00_00_00);
let r_yyyy = _mm_shuffle_ps(lhs, lhs, 0b01_01_01_01);
let r_zzzz = _mm_shuffle_ps(lhs, lhs, 0b10_10_10_10);
let r_wwww = _mm_shuffle_ps(lhs, lhs, 0b11_11_11_11);
let lxrw_lyrw_lzrw_lwrw = _mm_mul_ps(r_wwww, rhs);
let l_wzyx = _mm_shuffle_ps(rhs, rhs, 0b00_01_10_11);
let lwrx_lzrx_lyrx_lxrx = _mm_mul_ps(r_xxxx, l_wzyx);
let l_zwxy = _mm_shuffle_ps(l_wzyx, l_wzyx, 0b10_11_00_01);
let lwrx_nlzrx_lyrx_nlxrx = _mm_mul_ps(lwrx_lzrx_lyrx_lxrx, CONTROL_WZYX);
let lzry_lwry_lxry_lyry = _mm_mul_ps(r_yyyy, l_zwxy);
let l_yxwz = _mm_shuffle_ps(l_zwxy, l_zwxy, 0b00_01_10_11);
let lzry_lwry_nlxry_nlyry = _mm_mul_ps(lzry_lwry_lxry_lyry, CONTROL_ZWXY);
let lyrz_lxrz_lwrz_lzrz = _mm_mul_ps(r_zzzz, l_yxwz);
let result0 = _mm_add_ps(lxrw_lyrw_lzrw_lwrw, lwrx_nlzrx_lyrx_nlxrx);
let nlyrz_lxrz_lwrz_wlzrz = _mm_mul_ps(lyrz_lxrz_lwrz_lzrz, CONTROL_YXWZ);
let result1 = _mm_add_ps(lzry_lwry_nlxry_nlyry, nlyrz_lxrz_lwrz_wlzrz);
Self(_mm_add_ps(result0, result1))
}
}
#[inline]
pub fn as_f64(self) -> DQuat {
DQuat::from_xyzw(self.x as f64, self.y as f64, self.z as f64, self.w as f64)
}
}
#[cfg(not(target_arch = "spirv"))]
impl fmt::Debug for Quat {
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
fmt.debug_tuple(stringify!(Quat))
.field(&self.x)
.field(&self.y)
.field(&self.z)
.field(&self.w)
.finish()
}
}
#[cfg(not(target_arch = "spirv"))]
impl fmt::Display for Quat {
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
write!(fmt, "[{}, {}, {}, {}]", self.x, self.y, self.z, self.w)
}
}
impl_op_ex!(+ |a: &Quat, b: &Quat| -> Quat{
Quat::from_vec4(Vec4::from(a) + Vec4::from(b))
});
impl_op_ex!(-|a: &Quat, b: &Quat| -> Quat { Quat::from_vec4(Vec4::from(a) - Vec4::from(b)) });
impl_op_ex!(+= |a: &mut Quat, b: &Quat| { *a = *a + b });
impl_op_ex!(-= |a: &mut Quat, b: &Quat| { *a = *a - b });
impl_op_ex_commutative!(*|a: &Quat, b: &f32| -> Quat { Quat::from_vec4(Vec4::from(a) * b) });
impl_op_ex!(/ |a: &Quat, b: &f32| -> Quat{
Quat::from_vec4(Vec4::from(a) / b)
});
impl_op_ex!(*|a: &Quat, b: &Quat| -> Quat { a.mul_quat(*b) });
impl_op_ex!(*= |a: &mut Quat, b: &Quat| {
*a = a.mul_quat(*b)
});
impl Neg for Quat {
type Output = Self;
#[inline]
fn neg(self) -> Self {
self * -1.0_f32
}
}
impl Default for Quat {
#[inline]
fn default() -> Self {
Self::IDENTITY
}
}
impl PartialEq for Quat {
#[inline]
fn eq(&self, rhs: &Self) -> bool {
Vec4::from(*self).eq(&Vec4::from(*rhs))
}
}
#[cfg(not(target_arch = "spirv"))]
impl AsRef<[f32; 4]> for Quat {
#[inline]
fn as_ref(&self) -> &[f32; 4] {
unsafe { &*(self as *const Self as *const [f32; 4]) }
}
}
impl<'a> Sum<&'a Self> for Quat {
fn sum<I>(iter: I) -> Self
where
I: Iterator<Item = &'a Self>,
{
iter.fold(Self::ZERO, |a, &b| a + b)
}
}
impl From<Quat> for Vec4 {
#[inline]
fn from(q: Quat) -> Self {
Self(q.0)
}
}
impl From<Quat> for (f32, f32, f32, f32) {
#[inline]
fn from(q: Quat) -> Self {
Vec4::from(q).into()
}
}
impl From<Quat> for [f32; 4] {
#[inline]
fn from(q: Quat) -> Self {
Vec4::from(q).into()
}
}
impl From<&Quat> for Vec4 {
#[inline]
fn from(q: &Quat) -> Self {
Self(q.0)
}
}
impl From<&Quat> for (f32, f32, f32, f32) {
#[inline]
fn from(q: &Quat) -> Self {
Vec4::from(q).into()
}
}
impl From<&Quat> for [f32; 4] {
#[inline]
fn from(q: &Quat) -> Self {
Vec4::from(q).into()
}
}
impl From<Quat> for __m128 {
#[inline]
fn from(q: Quat) -> Self {
q.0
}
}
impl Deref for Quat {
type Target = crate::deref::Vec4<f32>;
#[inline]
fn deref(&self) -> &Self::Target {
unsafe { &*(self as *const Self).cast() }
}
}
impl DerefMut for Quat {
#[inline]
fn deref_mut(&mut self) -> &mut Self::Target {
unsafe { &mut *(self as *mut Self).cast() }
}
}