aboutsummaryrefslogtreecommitdiff
path: root/src/f32/sse2/quat.rs
diff options
context:
space:
mode:
Diffstat (limited to 'src/f32/sse2/quat.rs')
-rw-r--r--src/f32/sse2/quat.rs123
1 files changed, 84 insertions, 39 deletions
diff --git a/src/f32/sse2/quat.rs b/src/f32/sse2/quat.rs
index f7fbd8d..ce032fe 100644
--- a/src/f32/sse2/quat.rs
+++ b/src/f32/sse2/quat.rs
@@ -2,14 +2,11 @@
use crate::{
euler::{EulerFromQuaternion, EulerRot, EulerToQuaternion},
+ f32::math,
sse2::*,
- DQuat, FloatEx, Mat3, Mat3A, Mat4, Vec2, Vec3, Vec3A, Vec4,
+ DQuat, Mat3, Mat3A, Mat4, Vec2, Vec3, Vec3A, Vec4,
};
-#[cfg(feature = "libm")]
-#[allow(unused_imports)]
-use num_traits::Float;
-
#[cfg(target_arch = "x86")]
use core::arch::x86::*;
#[cfg(target_arch = "x86_64")]
@@ -20,6 +17,7 @@ use core::fmt;
use core::iter::{Product, Sum};
use core::ops::{Add, Deref, DerefMut, Div, Mul, MulAssign, Neg, Sub};
+#[repr(C)]
union UnionCast {
a: [f32; 4],
v: Quat,
@@ -30,6 +28,7 @@ union UnionCast {
/// This should generally not be called manually unless you know what you are doing. Use
/// one of the other constructors instead such as `identity` or `from_axis_angle`.
#[inline]
+#[must_use]
pub const fn quat(x: f32, y: f32, z: f32, w: f32) -> Quat {
Quat::from_xyzw(x, y, z, w)
}
@@ -40,6 +39,8 @@ pub const fn quat(x: f32, y: f32, z: f32, w: f32) -> Quat {
/// floating point "error creep" which can occur when successive quaternion
/// operations are applied.
///
+/// SIMD vector types are used for storage on supported platforms.
+///
/// This type is 16 byte aligned.
#[derive(Clone, Copy)]
#[repr(transparent)]
@@ -67,6 +68,7 @@ impl Quat {
/// This function does not check if the input is normalized, it is up to the user to
/// provide normalized input or to normalized the resulting quaternion.
#[inline(always)]
+ #[must_use]
pub const fn from_xyzw(x: f32, y: f32, z: f32, w: f32) -> Self {
unsafe { UnionCast { a: [x, y, z, w] }.v }
}
@@ -78,6 +80,7 @@ impl Quat {
/// This function does not check if the input is normalized, it is up to the user to
/// provide normalized input or to normalized the resulting quaternion.
#[inline]
+ #[must_use]
pub const fn from_array(a: [f32; 4]) -> Self {
Self::from_xyzw(a[0], a[1], a[2], a[3])
}
@@ -89,7 +92,8 @@ impl Quat {
/// This function does not check if the input is normalized, it is up to the user to
/// provide normalized input or to normalized the resulting quaternion.
#[inline]
- pub fn from_vec4(v: Vec4) -> Self {
+ #[must_use]
+ pub const fn from_vec4(v: Vec4) -> Self {
Self(v.0)
}
@@ -104,6 +108,7 @@ impl Quat {
///
/// Panics if `slice` length is less than 4.
#[inline]
+ #[must_use]
pub fn from_slice(slice: &[f32]) -> Self {
assert!(slice.len() >= 4);
Self(unsafe { _mm_loadu_ps(slice.as_ptr()) })
@@ -121,15 +126,17 @@ impl Quat {
}
/// Create a quaternion for a normalized rotation `axis` and `angle` (in radians).
- /// The axis must be normalized (unit-length).
+ ///
+ /// The axis must be a unit vector.
///
/// # Panics
///
/// Will panic if `axis` is not normalized when `glam_assert` is enabled.
#[inline]
+ #[must_use]
pub fn from_axis_angle(axis: Vec3, angle: f32) -> Self {
glam_assert!(axis.is_normalized());
- let (s, c) = (angle * 0.5).sin_cos();
+ let (s, c) = math::sin_cos(angle * 0.5);
let v = axis * s;
Self::from_xyzw(v.x, v.y, v.z, c)
}
@@ -138,6 +145,7 @@ impl Quat {
///
/// `from_scaled_axis(Vec3::ZERO)` results in the identity quaternion.
#[inline]
+ #[must_use]
pub fn from_scaled_axis(v: Vec3) -> Self {
let length = v.length();
if length == 0.0 {
@@ -149,33 +157,38 @@ impl Quat {
/// Creates a quaternion from the `angle` (in radians) around the x axis.
#[inline]
+ #[must_use]
pub fn from_rotation_x(angle: f32) -> Self {
- let (s, c) = (angle * 0.5).sin_cos();
+ let (s, c) = math::sin_cos(angle * 0.5);
Self::from_xyzw(s, 0.0, 0.0, c)
}
/// Creates a quaternion from the `angle` (in radians) around the y axis.
#[inline]
+ #[must_use]
pub fn from_rotation_y(angle: f32) -> Self {
- let (s, c) = (angle * 0.5).sin_cos();
+ let (s, c) = math::sin_cos(angle * 0.5);
Self::from_xyzw(0.0, s, 0.0, c)
}
/// Creates a quaternion from the `angle` (in radians) around the z axis.
#[inline]
+ #[must_use]
pub fn from_rotation_z(angle: f32) -> Self {
- let (s, c) = (angle * 0.5).sin_cos();
+ let (s, c) = math::sin_cos(angle * 0.5);
Self::from_xyzw(0.0, 0.0, s, c)
}
- #[inline]
/// Creates a quaternion from the given Euler rotation sequence and the angles (in radians).
+ #[inline]
+ #[must_use]
pub fn from_euler(euler: EulerRot, a: f32, b: f32, c: f32) -> Self {
euler.new_quat(a, b, c)
}
/// From the columns of a 3x3 rotation matrix.
#[inline]
+ #[must_use]
pub(crate) fn from_rotation_axes(x_axis: Vec3, y_axis: Vec3, z_axis: Vec3) -> Self {
// Based on https://github.com/microsoft/DirectXMath `XM$quaternionRotationMatrix`
let (m00, m01, m02) = x_axis.into();
@@ -188,7 +201,7 @@ impl Quat {
if dif10 <= 0.0 {
// x^2 >= y^2
let four_xsq = omm22 - dif10;
- let inv4x = 0.5 / four_xsq.sqrt();
+ let inv4x = 0.5 / math::sqrt(four_xsq);
Self::from_xyzw(
four_xsq * inv4x,
(m01 + m10) * inv4x,
@@ -198,7 +211,7 @@ impl Quat {
} else {
// y^2 >= x^2
let four_ysq = omm22 + dif10;
- let inv4y = 0.5 / four_ysq.sqrt();
+ let inv4y = 0.5 / math::sqrt(four_ysq);
Self::from_xyzw(
(m01 + m10) * inv4y,
four_ysq * inv4y,
@@ -213,7 +226,7 @@ impl Quat {
if sum10 <= 0.0 {
// z^2 >= w^2
let four_zsq = opm22 - sum10;
- let inv4z = 0.5 / four_zsq.sqrt();
+ let inv4z = 0.5 / math::sqrt(four_zsq);
Self::from_xyzw(
(m02 + m20) * inv4z,
(m12 + m21) * inv4z,
@@ -223,7 +236,7 @@ impl Quat {
} else {
// w^2 >= z^2
let four_wsq = opm22 + sum10;
- let inv4w = 0.5 / four_wsq.sqrt();
+ let inv4w = 0.5 / math::sqrt(four_wsq);
Self::from_xyzw(
(m12 - m21) * inv4w,
(m20 - m02) * inv4w,
@@ -236,18 +249,21 @@ impl Quat {
/// Creates a quaternion from a 3x3 rotation matrix.
#[inline]
+ #[must_use]
pub fn from_mat3(mat: &Mat3) -> Self {
Self::from_rotation_axes(mat.x_axis, mat.y_axis, mat.z_axis)
}
/// Creates a quaternion from a 3x3 SIMD aligned rotation matrix.
#[inline]
+ #[must_use]
pub fn from_mat3a(mat: &Mat3A) -> Self {
Self::from_rotation_axes(mat.x_axis.into(), mat.y_axis.into(), mat.z_axis.into())
}
/// Creates a quaternion from a 3x3 rotation matrix inside a homogeneous 4x4 matrix.
#[inline]
+ #[must_use]
pub fn from_mat4(mat: &Mat4) -> Self {
Self::from_rotation_axes(
mat.x_axis.truncate(),
@@ -259,7 +275,7 @@ impl Quat {
/// Gets the minimal rotation for transforming `from` to `to`. The rotation is in the
/// plane spanned by the two vectors. Will rotate at most 180 degrees.
///
- /// The input vectors must be normalized (unit-length).
+ /// The inputs must be unit vectors.
///
/// `from_rotation_arc(from, to) * from ≈ to`.
///
@@ -269,6 +285,7 @@ impl Quat {
/// # Panics
///
/// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
+ #[must_use]
pub fn from_rotation_arc(from: Vec3, to: Vec3) -> Self {
glam_assert!(from.is_normalized());
glam_assert!(to.is_normalized());
@@ -294,7 +311,7 @@ impl Quat {
/// The rotation is in the plane spanned by the two vectors. Will rotate at most 90
/// degrees.
///
- /// The input vectors must be normalized (unit-length).
+ /// The inputs must be unit vectors.
///
/// `to.dot(from_rotation_arc_colinear(from, to) * from).abs() ≈ 1`.
///
@@ -302,6 +319,7 @@ impl Quat {
///
/// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
#[inline]
+ #[must_use]
pub fn from_rotation_arc_colinear(from: Vec3, to: Vec3) -> Self {
if from.dot(to) < 0.0 {
Self::from_rotation_arc(from, -to)
@@ -313,7 +331,7 @@ impl Quat {
/// Gets the minimal rotation for transforming `from` to `to`. The resulting rotation is
/// around the z axis. Will rotate at most 180 degrees.
///
- /// The input vectors must be normalized (unit-length).
+ /// The inputs must be unit vectors.
///
/// `from_rotation_arc_2d(from, to) * from ≈ to`.
///
@@ -323,6 +341,7 @@ impl Quat {
/// # Panics
///
/// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
+ #[must_use]
pub fn from_rotation_arc_2d(from: Vec2, to: Vec2) -> Self {
glam_assert!(from.is_normalized());
glam_assert!(to.is_normalized());
@@ -343,31 +362,30 @@ impl Quat {
let z = from.x * to.y - to.x * from.y;
let w = 1.0 + dot;
// calculate length with x=0 and y=0 to normalize
- let len_rcp = 1.0 / (z * z + w * w).sqrt();
+ let len_rcp = 1.0 / math::sqrt(z * z + w * w);
Self::from_xyzw(0.0, 0.0, z * len_rcp, w * len_rcp)
}
}
- /// Returns the rotation axis and angle (in radians) of `self`.
+ /// Returns the rotation axis (normalized) and angle (in radians) of `self`.
#[inline]
+ #[must_use]
pub fn to_axis_angle(self) -> (Vec3, f32) {
const EPSILON: f32 = 1.0e-8;
- const EPSILON_SQUARED: f32 = EPSILON * EPSILON;
- let w = self.w;
- let angle = w.acos_approx() * 2.0;
- let scale_sq = f32::max(1.0 - w * w, 0.0);
- if scale_sq >= EPSILON_SQUARED {
- (
- Vec3::new(self.x, self.y, self.z) * scale_sq.sqrt().recip(),
- angle,
- )
+ let v = Vec3::new(self.x, self.y, self.z);
+ let length = v.length();
+ if length >= EPSILON {
+ let angle = 2.0 * math::atan2(length, self.w);
+ let axis = v / length;
+ (axis, angle)
} else {
- (Vec3::X, angle)
+ (Vec3::X, 0.0)
}
}
/// Returns the rotation axis scaled by the rotation in radians.
#[inline]
+ #[must_use]
pub fn to_scaled_axis(self) -> Vec3 {
let (axis, angle) = self.to_axis_angle();
axis * angle
@@ -375,26 +393,29 @@ impl Quat {
/// Returns the rotation angles for the given euler rotation sequence.
#[inline]
+ #[must_use]
pub fn to_euler(self, euler: EulerRot) -> (f32, f32, f32) {
euler.convert_quat(self)
}
/// `[x, y, z, w]`
#[inline]
+ #[must_use]
pub fn to_array(&self) -> [f32; 4] {
[self.x, self.y, self.z, self.w]
}
/// Returns the vector part of the quaternion.
#[inline]
+ #[must_use]
pub fn xyz(self) -> Vec3 {
Vec3::new(self.x, self.y, self.z)
}
/// Returns the quaternion conjugate of `self`. For a unit quaternion the
/// conjugate is also the inverse.
- #[must_use]
#[inline]
+ #[must_use]
pub fn conjugate(self) -> Self {
const SIGN: __m128 = m128_from_f32x4([-0.0, -0.0, -0.0, 0.0]);
Self(unsafe { _mm_xor_ps(self.0, SIGN) })
@@ -409,8 +430,8 @@ impl Quat {
/// # Panics
///
/// Will panic if `self` is not normalized when `glam_assert` is enabled.
- #[must_use]
#[inline]
+ #[must_use]
pub fn inverse(self) -> Self {
glam_assert!(self.is_normalized());
self.conjugate()
@@ -419,6 +440,7 @@ impl Quat {
/// Computes the dot product of `self` and `rhs`. The dot product is
/// equal to the cosine of the angle between two quaternion rotations.
#[inline]
+ #[must_use]
pub fn dot(self, rhs: Self) -> f32 {
Vec4::from(self).dot(Vec4::from(rhs))
}
@@ -426,6 +448,7 @@ impl Quat {
/// Computes the length of `self`.
#[doc(alias = "magnitude")]
#[inline]
+ #[must_use]
pub fn length(self) -> f32 {
Vec4::from(self).length()
}
@@ -436,6 +459,7 @@ impl Quat {
/// root operation.
#[doc(alias = "magnitude2")]
#[inline]
+ #[must_use]
pub fn length_squared(self) -> f32 {
Vec4::from(self).length_squared()
}
@@ -444,6 +468,7 @@ impl Quat {
///
/// For valid results, `self` must _not_ be of length zero.
#[inline]
+ #[must_use]
pub fn length_recip(self) -> f32 {
Vec4::from(self).length_recip()
}
@@ -455,8 +480,8 @@ impl Quat {
/// Panics
///
/// Will panic if `self` is zero length when `glam_assert` is enabled.
- #[must_use]
#[inline]
+ #[must_use]
pub fn normalize(self) -> Self {
Self::from_vec4(Vec4::from(self).normalize())
}
@@ -464,11 +489,13 @@ impl Quat {
/// Returns `true` if, and only if, all elements are finite.
/// If any element is either `NaN`, positive or negative infinity, this will return `false`.
#[inline]
+ #[must_use]
pub fn is_finite(self) -> bool {
Vec4::from(self).is_finite()
}
#[inline]
+ #[must_use]
pub fn is_nan(self) -> bool {
Vec4::from(self).is_nan()
}
@@ -477,11 +504,13 @@ impl Quat {
///
/// Uses a precision threshold of `1e-6`.
#[inline]
+ #[must_use]
pub fn is_normalized(self) -> bool {
Vec4::from(self).is_normalized()
}
#[inline]
+ #[must_use]
pub fn is_near_identity(self) -> bool {
// Based on https://github.com/nfrechette/rtm `rtm::quat_near_identity`
let threshold_angle = 0.002_847_144_6;
@@ -498,7 +527,7 @@ impl Quat {
// If the quat.w is close to -1.0, the angle will be near 2*PI which is close to
// a negative 0 rotation. By forcing quat.w to be positive, we'll end up with
// the shortest path.
- let positive_w_angle = self.w.abs().acos_approx() * 2.0;
+ let positive_w_angle = math::acos_approx(math::abs(self.w)) * 2.0;
positive_w_angle < threshold_angle
}
@@ -511,9 +540,10 @@ impl Quat {
///
/// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
#[inline]
+ #[must_use]
pub fn angle_between(self, rhs: Self) -> f32 {
glam_assert!(self.is_normalized() && rhs.is_normalized());
- self.dot(rhs).abs().acos_approx() * 2.0
+ math::acos_approx(math::abs(self.dot(rhs))) * 2.0
}
/// Returns true if the absolute difference of all elements between `self` and `rhs`
@@ -526,6 +556,7 @@ impl Quat {
/// For more see
/// [comparing floating point numbers](https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/).
#[inline]
+ #[must_use]
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)
}
@@ -539,8 +570,9 @@ impl Quat {
/// # Panics
///
/// Will panic if `self` or `end` are not normalized when `glam_assert` is enabled.
- #[inline]
#[doc(alias = "mix")]
+ #[inline]
+ #[must_use]
pub fn lerp(self, end: Self, s: f32) -> Self {
glam_assert!(self.is_normalized());
glam_assert!(end.is_normalized());
@@ -571,6 +603,7 @@ impl Quat {
///
/// Will panic if `self` or `end` are not normalized when `glam_assert` is enabled.
#[inline]
+ #[must_use]
pub fn slerp(self, mut end: Self, s: f32) -> Self {
// http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
glam_assert!(self.is_normalized());
@@ -594,7 +627,7 @@ impl Quat {
// assumes lerp returns a normalized quaternion
self.lerp(end, s)
} else {
- let theta = dot.acos_approx();
+ let theta = math::acos_approx(dot);
let x = 1.0 - s;
let y = s;
@@ -622,6 +655,7 @@ impl Quat {
///
/// Will panic if `self` is not normalized when `glam_assert` is enabled.
#[inline]
+ #[must_use]
pub fn mul_vec3(self, rhs: Vec3) -> Vec3 {
glam_assert!(self.is_normalized());
@@ -637,6 +671,7 @@ impl Quat {
///
/// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
#[inline]
+ #[must_use]
pub fn mul_quat(self, rhs: Self) -> Self {
glam_assert!(self.is_normalized());
glam_assert!(rhs.is_normalized());
@@ -680,6 +715,7 @@ impl Quat {
/// Creates a quaternion from a 3x3 rotation matrix inside a 3D affine transform.
#[inline]
+ #[must_use]
pub fn from_affine3(a: &crate::Affine3A) -> Self {
#[allow(clippy::useless_conversion)]
Self::from_rotation_axes(
@@ -691,6 +727,7 @@ impl Quat {
/// Multiplies a quaternion and a 3D vector, returning the rotated vector.
#[inline]
+ #[must_use]
pub fn mul_vec3a(self, rhs: Vec3A) -> Vec3A {
unsafe {
const TWO: __m128 = m128_from_f32x4([2.0; 4]);
@@ -708,9 +745,17 @@ impl Quat {
}
#[inline]
- pub fn as_f64(self) -> DQuat {
+ #[must_use]
+ pub fn as_dquat(self) -> DQuat {
DQuat::from_xyzw(self.x as f64, self.y as f64, self.z as f64, self.w as f64)
}
+
+ #[inline]
+ #[must_use]
+ #[deprecated(since = "0.24.2", note = "Use as_dquat() instead")]
+ pub fn as_f64(self) -> DQuat {
+ self.as_dquat()
+ }
}
#[cfg(not(target_arch = "spirv"))]
generated by cgit v1.2.3 (git 2.25.1) at 2024-06-01 23:25:26 +0000