1500 lines
51 KiB
Text
1500 lines
51 KiB
Text
{% import "macros.rs.tera" as macros %}
|
|
|
|
// Generated from {{template_path}} template. Edit the template, not the generated file.
|
|
|
|
{% if not is_scalar %}
|
|
{% set is_simd = true %}
|
|
{% if is_sse2 %}
|
|
{% set simd_t = "__m128" %}
|
|
{% elif is_wasm32 %}
|
|
{% set simd_t = "v128" %}
|
|
{% elif is_coresimd %}
|
|
{% set simd_t = "f32x4" %}
|
|
{% elif is_neon %}
|
|
{% set simd_t = "float32x4_t" %}
|
|
{% endif %}
|
|
{% endif %}
|
|
|
|
{% if scalar_t == "f32" %}
|
|
{% set self_t = "Quat" %}
|
|
{% set affine3_t = "Affine3A" %}
|
|
{% set vec2_t = "Vec2" %}
|
|
{% set vec3_t = "Vec3" %}
|
|
{% set vec4_t = "Vec4" %}
|
|
{% set mat3_t = "Mat3" %}
|
|
{% set mat4_t = "Mat4" %}
|
|
{% elif scalar_t == "f64" %}
|
|
{% set self_t = "DQuat" %}
|
|
{% set affine3_t = "DAffine3" %}
|
|
{% set vec2_t = "DVec2" %}
|
|
{% set vec3_t = "DVec3" %}
|
|
{% set vec4_t = "DVec4" %}
|
|
{% set mat3_t = "DMat3" %}
|
|
{% set mat4_t = "DMat4" %}
|
|
{% endif %}
|
|
|
|
use crate::{
|
|
{{ scalar_t }}::math,
|
|
euler::{EulerRot, FromEuler, ToEuler},
|
|
{% if scalar_t == "f32" %}
|
|
DQuat, Mat3, Mat3A, Mat4, Vec2, Vec3, Vec3A, Vec4,
|
|
{% elif scalar_t == "f64" %}
|
|
DMat3, DMat4, DVec2, DVec3, DVec4, Quat,
|
|
{% endif %}
|
|
{% if is_sse2 %}
|
|
sse2::*,
|
|
{% elif is_wasm32 %}
|
|
wasm32::*,
|
|
{% elif is_coresimd %}
|
|
coresimd::*,
|
|
{% elif is_neon %}
|
|
neon::*,
|
|
{% endif %}
|
|
};
|
|
|
|
{% if is_sse2 %}
|
|
#[cfg(target_arch = "x86")]
|
|
use core::arch::x86::*;
|
|
#[cfg(target_arch = "x86_64")]
|
|
use core::arch::x86_64::*;
|
|
{% elif is_wasm32 %}
|
|
use core::arch::wasm32::*;
|
|
{% elif is_coresimd %}
|
|
use core::simd::*;
|
|
{% elif is_neon %}
|
|
use core::arch::aarch64::*;
|
|
{% endif %}
|
|
|
|
use core::fmt;
|
|
use core::iter::{Product, Sum};
|
|
use core::ops::{
|
|
{% if not is_scalar %}
|
|
Deref, DerefMut,
|
|
{% endif %}
|
|
Add, AddAssign, Div, DivAssign, Mul, MulAssign, Neg, Sub, SubAssign
|
|
};
|
|
|
|
{% if not is_coresimd %}
|
|
#[cfg(feature = "zerocopy")]
|
|
use zerocopy_derive::*;
|
|
{% endif %}
|
|
|
|
{% if is_sse2 or is_neon %}
|
|
#[repr(C)]
|
|
union UnionCast {
|
|
a: [f32; 4],
|
|
v: {{ self_t }}
|
|
}
|
|
{% endif %}
|
|
|
|
/// Creates a quaternion from `x`, `y`, `z` and `w` values.
|
|
///
|
|
/// 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 {{ self_t | lower }}(x: {{ scalar_t }}, y: {{ scalar_t }}, z: {{ scalar_t }}, w: {{ scalar_t }}) -> {{ self_t }} {
|
|
{{ self_t }}::from_xyzw(x, y, z, w)
|
|
}
|
|
|
|
/// A quaternion representing an orientation.
|
|
///
|
|
/// This quaternion is intended to be of unit length but may denormalize due to
|
|
/// floating point "error creep" which can occur when successive quaternion
|
|
/// operations are applied.
|
|
{%- if is_simd %}
|
|
///
|
|
/// SIMD vector types are used for storage on supported platforms.
|
|
///
|
|
/// This type is 16 byte aligned.
|
|
{%- endif %}
|
|
#[derive(Clone, Copy)]
|
|
#[cfg_attr(feature = "bytemuck", derive(bytemuck::Pod, bytemuck::Zeroable))]
|
|
{%- if not is_coresimd %}
|
|
#[cfg_attr(feature = "zerocopy", derive(FromBytes, Immutable, IntoBytes, KnownLayout))]
|
|
{%- endif %}
|
|
{%- if is_scalar %}
|
|
{%- if scalar_t == "f32" %}
|
|
#[cfg_attr(not(feature = "scalar-math"), repr(align(16)))]
|
|
{%- endif %}
|
|
#[repr(C)]
|
|
#[cfg_attr(target_arch = "spirv", rust_gpu::vector::v1)]
|
|
pub struct {{ self_t }}{
|
|
pub x: {{ scalar_t }},
|
|
pub y: {{ scalar_t }},
|
|
pub z: {{ scalar_t }},
|
|
pub w: {{ scalar_t }},
|
|
}
|
|
{%- else %}
|
|
#[repr(transparent)]
|
|
pub struct {{ self_t }}(pub(crate) {{ simd_t }});
|
|
{%- endif %}
|
|
|
|
impl {{ self_t }} {
|
|
/// All zeros.
|
|
const ZERO: Self = Self::from_array([0.0; 4]);
|
|
|
|
/// The identity quaternion. Corresponds to no rotation.
|
|
pub const IDENTITY: Self = Self::from_xyzw(0.0, 0.0, 0.0, 1.0);
|
|
|
|
/// All NANs.
|
|
pub const NAN: Self = Self::from_array([{{ scalar_t }}::NAN; 4]);
|
|
|
|
/// Creates a new rotation quaternion.
|
|
///
|
|
/// 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`.
|
|
///
|
|
/// `from_xyzw` is mostly used by unit tests and `serde` deserialization.
|
|
///
|
|
/// # Preconditions
|
|
///
|
|
/// 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: {{ scalar_t }}, y: {{ scalar_t }}, z: {{ scalar_t }}, w: {{ scalar_t }}) -> Self {
|
|
{% if is_scalar %}
|
|
Self { x, y, z, w }
|
|
{% elif is_sse2 %}
|
|
unsafe { UnionCast { a: [x, y, z, w] }.v }
|
|
{% elif is_wasm32 %}
|
|
Self(f32x4(x, y, z, w))
|
|
{% elif is_coresimd %}
|
|
Self(f32x4::from_array([x, y, z, w]))
|
|
{% elif is_neon %}
|
|
unsafe { UnionCast { a: [x, y, z, w] }.v }
|
|
{% else %}
|
|
unimplemented!()
|
|
{% endif %}
|
|
}
|
|
|
|
/// Creates a rotation quaternion from an array.
|
|
///
|
|
/// # Preconditions
|
|
///
|
|
/// 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: [{{ scalar_t }}; 4]) -> Self {
|
|
{% if is_coresimd %}
|
|
Self(f32x4::from_array(a))
|
|
{% else %}
|
|
Self::from_xyzw(a[0], a[1], a[2], a[3])
|
|
{% endif %}
|
|
}
|
|
|
|
/// Creates a new rotation quaternion from a 4D vector.
|
|
///
|
|
/// # Preconditions
|
|
///
|
|
/// 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_vec4(v: {{ vec4_t }}) -> Self {
|
|
{% if is_scalar %}
|
|
Self { x: v.x, y: v.y, z: v.z, w: v.w }
|
|
{% else %}
|
|
Self(v.0)
|
|
{% endif %}
|
|
}
|
|
|
|
/// Creates a rotation quaternion from a slice.
|
|
///
|
|
/// # Preconditions
|
|
///
|
|
/// 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.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Panics if `slice` length is less than 4.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_slice(slice: &[{{ scalar_t }}]) -> Self {
|
|
{% if is_sse2 %}
|
|
assert!(slice.len() >= 4);
|
|
Self(unsafe { _mm_loadu_ps(slice.as_ptr()) })
|
|
{% elif is_neon %}
|
|
assert!(slice.len() >= 4);
|
|
Self(unsafe { vld1q_f32(slice.as_ptr()) })
|
|
{% else %}
|
|
Self::from_xyzw(slice[0], slice[1], slice[2], slice[3])
|
|
{% endif %}
|
|
}
|
|
|
|
/// Writes the quaternion to an unaligned slice.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Panics if `slice` length is less than 4.
|
|
#[inline]
|
|
pub fn write_to_slice(self, slice: &mut [{{ scalar_t }}]) {
|
|
{% if is_sse2 %}
|
|
assert!(slice.len() >= 4);
|
|
unsafe { _mm_storeu_ps(slice.as_mut_ptr(), self.0) }
|
|
{% elif is_neon %}
|
|
assert!(slice.len() >= 4);
|
|
unsafe { vst1q_f32(slice.as_mut_ptr(), self.0) }
|
|
{% else %}
|
|
slice[0] = self.x;
|
|
slice[1] = self.y;
|
|
slice[2] = self.z;
|
|
slice[3] = self.w;
|
|
{% endif %}
|
|
}
|
|
|
|
/// Create a quaternion for a normalized rotation `axis` and `angle` (in radians).
|
|
///
|
|
/// 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_t }}, angle: {{ scalar_t }}) -> Self {
|
|
glam_assert!(axis.is_normalized());
|
|
let (s, c) = math::sin_cos(angle * 0.5);
|
|
let v = axis * s;
|
|
Self::from_xyzw(v.x, v.y, v.z, c)
|
|
}
|
|
|
|
/// Create a quaternion that rotates `v.length()` radians around `v.normalize()`.
|
|
///
|
|
/// `from_scaled_axis(Vec3::ZERO)` results in the identity quaternion.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_scaled_axis(v: {{ vec3_t }}) -> Self {
|
|
let length = v.length();
|
|
if length == 0.0 {
|
|
Self::IDENTITY
|
|
} else {
|
|
Self::from_axis_angle(v / length, length)
|
|
}
|
|
}
|
|
|
|
/// Creates a quaternion from the `angle` (in radians) around the x axis.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_rotation_x(angle: {{ scalar_t }}) -> Self {
|
|
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: {{ scalar_t }}) -> Self {
|
|
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: {{ scalar_t }}) -> Self {
|
|
let (s, c) = math::sin_cos(angle * 0.5);
|
|
Self::from_xyzw(0.0, 0.0, s, c)
|
|
}
|
|
|
|
/// Creates a quaternion from the given Euler rotation sequence and the angles (in radians).
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_euler(euler: EulerRot, a: {{ scalar_t }}, b: {{ scalar_t }}, c: {{ scalar_t }}) -> Self {
|
|
Self::from_euler_angles(euler, a, b, c)
|
|
}
|
|
|
|
/// From the columns of a 3x3 rotation matrix.
|
|
///
|
|
/// Note if the input axes contain scales, shears, or other non-rotation transformations then
|
|
/// the output of this function is ill-defined.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if any axis is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub(crate) fn from_rotation_axes(x_axis: {{ vec3_t }}, y_axis: {{ vec3_t }}, z_axis: {{ vec3_t }}) -> Self {
|
|
glam_assert!(x_axis.is_normalized() && y_axis.is_normalized() && z_axis.is_normalized());
|
|
// Based on https://github.com/microsoft/DirectXMath `XMQuaternionRotationMatrix`
|
|
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 {
|
|
// x^2 + y^2 >= z^2 + w^2
|
|
let dif10 = m11 - m00;
|
|
let omm22 = 1.0 - m22;
|
|
if dif10 <= 0.0 {
|
|
// x^2 >= y^2
|
|
let four_xsq = omm22 - dif10;
|
|
let inv4x = 0.5 / math::sqrt(four_xsq);
|
|
Self::from_xyzw(
|
|
four_xsq * inv4x,
|
|
(m01 + m10) * inv4x,
|
|
(m02 + m20) * inv4x,
|
|
(m12 - m21) * inv4x,
|
|
)
|
|
} else {
|
|
// y^2 >= x^2
|
|
let four_ysq = omm22 + dif10;
|
|
let inv4y = 0.5 / math::sqrt(four_ysq);
|
|
Self::from_xyzw(
|
|
(m01 + m10) * inv4y,
|
|
four_ysq * inv4y,
|
|
(m12 + m21) * inv4y,
|
|
(m20 - m02) * inv4y,
|
|
)
|
|
}
|
|
} else {
|
|
// z^2 + w^2 >= x^2 + y^2
|
|
let sum10 = m11 + m00;
|
|
let opm22 = 1.0 + m22;
|
|
if sum10 <= 0.0 {
|
|
// z^2 >= w^2
|
|
let four_zsq = opm22 - sum10;
|
|
let inv4z = 0.5 / math::sqrt(four_zsq);
|
|
Self::from_xyzw(
|
|
(m02 + m20) * inv4z,
|
|
(m12 + m21) * inv4z,
|
|
four_zsq * inv4z,
|
|
(m01 - m10) * inv4z,
|
|
)
|
|
} else {
|
|
// w^2 >= z^2
|
|
let four_wsq = opm22 + sum10;
|
|
let inv4w = 0.5 / math::sqrt(four_wsq);
|
|
Self::from_xyzw(
|
|
(m12 - m21) * inv4w,
|
|
(m20 - m02) * inv4w,
|
|
(m01 - m10) * inv4w,
|
|
four_wsq * inv4w,
|
|
)
|
|
}
|
|
}
|
|
}
|
|
|
|
/// Creates a quaternion from a 3x3 rotation matrix.
|
|
///
|
|
/// Note if the input matrix contain scales, shears, or other non-rotation transformations then
|
|
/// the resulting quaternion will be ill-defined.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if any input matrix column is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_mat3(mat: &{{ mat3_t }}) -> Self {
|
|
Self::from_rotation_axes(
|
|
mat.x_axis,
|
|
mat.y_axis,
|
|
mat.z_axis,
|
|
)
|
|
}
|
|
|
|
{% if scalar_t == "f32" %}
|
|
/// Creates a quaternion from a 3x3 SIMD aligned rotation matrix.
|
|
///
|
|
/// Note if the input matrix contain scales, shears, or other non-rotation transformations then
|
|
/// the resulting quaternion will be ill-defined.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if any input matrix column is not normalized when `glam_assert` is enabled.
|
|
#[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(),
|
|
)
|
|
}
|
|
{% endif %}
|
|
|
|
/// Creates a quaternion from the upper 3x3 rotation matrix inside a homogeneous 4x4 matrix.
|
|
///
|
|
/// Note if the upper 3x3 matrix contain scales, shears, or other non-rotation transformations
|
|
/// then the resulting quaternion will be ill-defined.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if any column of the upper 3x3 rotation matrix is not normalized when
|
|
/// `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_mat4(mat: &{{ mat4_t }}) -> Self {
|
|
Self::from_rotation_axes(
|
|
mat.x_axis.truncate(),
|
|
mat.y_axis.truncate(),
|
|
mat.z_axis.truncate(),
|
|
)
|
|
}
|
|
|
|
/// 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 inputs must be unit vectors.
|
|
///
|
|
/// `from_rotation_arc(from, to) * from ≈ to`.
|
|
///
|
|
/// For near-singular cases (from≈to and from≈-to) the current implementation
|
|
/// is only accurate to about 0.001 (for `f32`).
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `from` or `to` are not normalized when `glam_assert` is enabled.
|
|
#[must_use]
|
|
pub fn from_rotation_arc(from: {{ vec3_t }}, to: {{ vec3_t }}) -> Self {
|
|
glam_assert!(from.is_normalized());
|
|
glam_assert!(to.is_normalized());
|
|
|
|
const ONE_MINUS_EPS: {{ scalar_t }} = 1.0 - 2.0 * {{ scalar_t }}::EPSILON;
|
|
let dot = from.dot(to);
|
|
if dot > ONE_MINUS_EPS {
|
|
// 0° singularity: from ≈ to
|
|
Self::IDENTITY
|
|
} else if dot < -ONE_MINUS_EPS {
|
|
// 180° singularity: from ≈ -to
|
|
use core::{{ scalar_t }}::consts::PI; // half a turn = 𝛕/2 = 180°
|
|
Self::from_axis_angle(from.any_orthonormal_vector(), PI)
|
|
} else {
|
|
let c = from.cross(to);
|
|
Self::from_xyzw(c.x, c.y, c.z, 1.0 + dot).normalize()
|
|
}
|
|
}
|
|
|
|
/// Gets the minimal rotation for transforming `from` to either `to` or `-to`. This means
|
|
/// that the resulting quaternion will rotate `from` so that it is colinear with `to`.
|
|
///
|
|
/// The rotation is in the plane spanned by the two vectors. Will rotate at most 90
|
|
/// degrees.
|
|
///
|
|
/// The inputs must be unit vectors.
|
|
///
|
|
/// `to.dot(from_rotation_arc_colinear(from, to) * from).abs() ≈ 1`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// 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_t }}, to: {{ vec3_t }}) -> Self {
|
|
if from.dot(to) < 0.0 {
|
|
Self::from_rotation_arc(from, -to)
|
|
} else {
|
|
Self::from_rotation_arc(from, to)
|
|
}
|
|
}
|
|
|
|
/// Gets the minimal rotation for transforming `from` to `to`. The resulting rotation is
|
|
/// around the z axis. Will rotate at most 180 degrees.
|
|
///
|
|
/// The inputs must be unit vectors.
|
|
///
|
|
/// `from_rotation_arc_2d(from, to) * from ≈ to`.
|
|
///
|
|
/// For near-singular cases (from≈to and from≈-to) the current implementation
|
|
/// is only accurate to about 0.001 (for `f32`).
|
|
///
|
|
/// # 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_t }}, to: {{ vec2_t }}) -> Self {
|
|
glam_assert!(from.is_normalized());
|
|
glam_assert!(to.is_normalized());
|
|
|
|
const ONE_MINUS_EPSILON: {{ scalar_t }} = 1.0 - 2.0 * {{ scalar_t }}::EPSILON;
|
|
let dot = from.dot(to);
|
|
if dot > ONE_MINUS_EPSILON {
|
|
// 0° singularity: from ≈ to
|
|
Self::IDENTITY
|
|
} else if dot < -ONE_MINUS_EPSILON {
|
|
// 180° singularity: from ≈ -to
|
|
const COS_FRAC_PI_2: {{ scalar_t }} = 0.0;
|
|
const SIN_FRAC_PI_2: {{ scalar_t }} = 1.0;
|
|
// rotation around z by PI radians
|
|
Self::from_xyzw(0.0, 0.0, SIN_FRAC_PI_2, COS_FRAC_PI_2)
|
|
} else {
|
|
// vector3 cross where z=0
|
|
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 / math::sqrt(z * z + w * w);
|
|
Self::from_xyzw(0.0, 0.0, z * len_rcp, w * len_rcp)
|
|
}
|
|
}
|
|
|
|
/// Creates a quaterion rotation from a facing direction and an up direction.
|
|
///
|
|
/// For a left-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=forward`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `up` is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn look_to_lh(dir: {{ vec3_t }}, up: {{ vec3_t }}) -> Self {
|
|
Self::look_to_rh(-dir, up)
|
|
}
|
|
|
|
/// Creates a quaterion rotation from facing direction and an up direction.
|
|
///
|
|
/// For a right-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=back`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `dir` and `up` are not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn look_to_rh(dir: {{ vec3_t }}, up: {{ vec3_t }}) -> 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_rotation_axes(
|
|
{{ vec3_t }}::new(s.x, u.x, -f.x),
|
|
{{ vec3_t }}::new(s.y, u.y, -f.y),
|
|
{{ vec3_t }}::new(s.z, u.z, -f.z),
|
|
)
|
|
}
|
|
|
|
/// Creates a left-handed view matrix using a camera position, a focal point, and an up
|
|
/// direction.
|
|
///
|
|
/// For a left-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=forward`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `up` is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn look_at_lh(eye: {{ vec3_t }}, center: {{ vec3_t }}, up: {{ vec3_t }}) -> Self {
|
|
Self::look_to_lh(center.sub(eye).normalize(), up)
|
|
}
|
|
|
|
/// Creates a right-handed view matrix using a camera position, an up direction, and a focal
|
|
/// point.
|
|
///
|
|
/// For a right-handed view coordinate system with `+X=right`, `+Y=up` and `+Z=back`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `up` is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn look_at_rh(eye: {{ vec3_t }}, center: {{ vec3_t }}, up: {{ vec3_t }}) -> Self {
|
|
Self::look_to_rh(center.sub(eye).normalize(), up)
|
|
}
|
|
|
|
/// Returns the rotation axis (normalized) and angle (in radians) of `self`.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn to_axis_angle(self) -> ({{ vec3_t }}, {{ scalar_t }}) {
|
|
const EPSILON: {{ scalar_t }} = 1.0e-8;
|
|
let v = {{ vec3_t }}::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_t }}::X, 0.0)
|
|
}
|
|
}
|
|
|
|
/// Returns the rotation axis scaled by the rotation in radians.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn to_scaled_axis(self) -> {{ vec3_t }} {
|
|
let (axis, angle) = self.to_axis_angle();
|
|
axis * angle
|
|
}
|
|
|
|
/// Returns the rotation angles for the given euler rotation sequence.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn to_euler(self, order: EulerRot) -> ({{ scalar_t }}, {{ scalar_t }}, {{ scalar_t }}) {
|
|
self.to_euler_angles(order)
|
|
}
|
|
|
|
/// `[x, y, z, w]`
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn to_array(&self) -> [{{ scalar_t }}; 4] {
|
|
[self.x, self.y, self.z, self.w]
|
|
}
|
|
|
|
/// Returns the vector part of the quaternion.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn xyz(self) -> {{ vec3_t }} {
|
|
{{ vec3_t }}::new(self.x, self.y, self.z)
|
|
}
|
|
|
|
/// Returns the quaternion conjugate of `self`. For a unit quaternion the
|
|
/// conjugate is also the inverse.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn conjugate(self) -> Self {
|
|
{% if is_scalar %}
|
|
Self { x: -self.x, y: -self.y, z: -self.z, w: self.w }
|
|
{% elif is_sse2 %}
|
|
const SIGN: __m128 = m128_from_f32x4([-0.0, -0.0, -0.0, 0.0]);
|
|
Self(unsafe { _mm_xor_ps(self.0, SIGN) })
|
|
{% elif is_wasm32 %}
|
|
const SIGN: v128 = v128_from_f32x4([-1.0, -1.0, -1.0, 1.0]);
|
|
Self(f32x4_mul(self.0, SIGN))
|
|
{% elif is_coresimd %}
|
|
const SIGN: f32x4 = f32x4::from_array([-1.0, -1.0, -1.0, 1.0]);
|
|
Self(self.0.mul(SIGN))
|
|
{% elif is_neon %}
|
|
const SIGN: float32x4_t = f32x4_from_array([-1.0, -1.0, -1.0, 1.0]);
|
|
Self(unsafe { vmulq_f32(self.0, SIGN) })
|
|
{% else %}
|
|
unimplemented!()
|
|
{% endif %}
|
|
}
|
|
|
|
/// Returns the inverse of a normalized quaternion.
|
|
///
|
|
/// Typically quaternion inverse returns the conjugate of a normalized quaternion.
|
|
/// Because `self` is assumed to already be unit length this method *does not* normalize
|
|
/// before returning the conjugate.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn inverse(self) -> Self {
|
|
glam_assert!(self.is_normalized());
|
|
self.conjugate()
|
|
}
|
|
|
|
/// 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) -> {{ scalar_t }} {
|
|
{{ vec4_t }}::from(self).dot({{ vec4_t }}::from(rhs))
|
|
}
|
|
|
|
/// Computes the length of `self`.
|
|
#[doc(alias = "magnitude")]
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn length(self) -> {{ scalar_t }} {
|
|
{{ vec4_t }}::from(self).length()
|
|
}
|
|
|
|
/// Computes the squared length of `self`.
|
|
///
|
|
/// This is generally faster than `length()` as it avoids a square
|
|
/// root operation.
|
|
#[doc(alias = "magnitude2")]
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn length_squared(self) -> {{ scalar_t }} {
|
|
{{ vec4_t }}::from(self).length_squared()
|
|
}
|
|
|
|
/// Computes `1.0 / length()`.
|
|
///
|
|
/// For valid results, `self` must _not_ be of length zero.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn length_recip(self) -> {{ scalar_t }} {
|
|
{{ vec4_t }}::from(self).length_recip()
|
|
}
|
|
|
|
/// Returns `self` normalized to length 1.0.
|
|
///
|
|
/// For valid results, `self` must _not_ be of length zero.
|
|
///
|
|
/// Panics
|
|
///
|
|
/// Will panic if `self` is zero length when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn normalize(self) -> Self {
|
|
Self::from_vec4({{ vec4_t }}::from(self).normalize())
|
|
}
|
|
|
|
/// 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_t }}::from(self).is_finite()
|
|
}
|
|
|
|
/// Returns `true` if any elements are `NAN`.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn is_nan(self) -> bool {
|
|
{{ vec4_t }}::from(self).is_nan()
|
|
}
|
|
|
|
/// Returns whether `self` of length `1.0` or not.
|
|
///
|
|
/// Uses a precision threshold of `1e-6`.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn is_normalized(self) -> bool {
|
|
{{ vec4_t }}::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`
|
|
// Because of floating point precision, we cannot represent very small rotations.
|
|
// The closest f32 to 1.0 that is not 1.0 itself yields:
|
|
// 0.99999994.acos() * 2.0 = 0.000690533954 rad
|
|
//
|
|
// An error threshold of 1.e-6 is used by default.
|
|
// (1.0 - 1.e-6).acos() * 2.0 = 0.00284714461 rad
|
|
// (1.0 - 1.e-7).acos() * 2.0 = 0.00097656250 rad
|
|
//
|
|
// We don't really care about the angle value itself, only if it's close to 0.
|
|
// This will happen whenever quat.w is close to 1.0.
|
|
// 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.
|
|
//
|
|
// For f64 we're using a threshhold of
|
|
// (1.0 - 1e-14).acos() * 2.0
|
|
{%- if scalar_t == 'f32' %}
|
|
const THRESHOLD_ANGLE: f32 = 0.002_847_144_6;
|
|
{%- elif scalar_t == 'f64' %}
|
|
const THRESHOLD_ANGLE: f64 = 2.827_296_549_232_347_4e-7;
|
|
{%- endif %}
|
|
let positive_w_angle = math::acos_approx(math::abs(self.w)) * 2.0;
|
|
positive_w_angle < THRESHOLD_ANGLE
|
|
}
|
|
|
|
/// Returns the angle (in radians) for the minimal rotation
|
|
/// for transforming this quaternion into another.
|
|
///
|
|
/// Both quaternions must be normalized.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn angle_between(self, rhs: Self) -> {{ scalar_t }} {
|
|
glam_assert!(self.is_normalized() && rhs.is_normalized());
|
|
math::acos_approx(math::abs(self.dot(rhs))) * 2.0
|
|
}
|
|
|
|
/// Rotates towards `rhs` up to `max_angle` (in radians).
|
|
///
|
|
/// When `max_angle` is `0.0`, the result will be equal to `self`. When `max_angle` is equal to
|
|
/// `self.angle_between(rhs)`, the result will be equal to `rhs`. If `max_angle` is negative,
|
|
/// rotates towards the exact opposite of `rhs`. Will not go past the target.
|
|
///
|
|
/// Both quaternions must be normalized.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn rotate_towards(&self, rhs: Self, max_angle: {{ scalar_t }}) -> Self {
|
|
glam_assert!(self.is_normalized() && rhs.is_normalized());
|
|
let angle = self.angle_between(rhs);
|
|
if angle <= 1e-4 {
|
|
return rhs;
|
|
}
|
|
let s = (max_angle / angle).clamp(-1.0, 1.0);
|
|
self.slerp(rhs, s)
|
|
}
|
|
|
|
/// Returns true if the absolute difference of all elements between `self` and `rhs`
|
|
/// is less than or equal to `max_abs_diff`.
|
|
///
|
|
/// This can be used to compare if two quaternions contain similar elements. It works
|
|
/// best when comparing with a known value. The `max_abs_diff` that should be used used
|
|
/// depends on the values being compared against.
|
|
///
|
|
/// 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: {{ scalar_t }}) -> bool {
|
|
{{ vec4_t }}::from(self).abs_diff_eq({{ vec4_t }}::from(rhs), max_abs_diff)
|
|
}
|
|
|
|
#[inline(always)]
|
|
#[must_use]
|
|
fn lerp_impl(self, end: Self, s: {{ scalar_t }}) -> Self {
|
|
(self * (1.0 - s) + end * s).normalize()
|
|
}
|
|
|
|
/// Performs a linear interpolation between `self` and `rhs` based on
|
|
/// the value `s`.
|
|
///
|
|
/// When `s` is `0.0`, the result will be equal to `self`. When `s`
|
|
/// is `1.0`, the result will be equal to `rhs`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` or `end` are not normalized when `glam_assert` is enabled.
|
|
#[doc(alias = "mix")]
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn lerp(self, end: Self, s: {{ scalar_t }}) -> Self {
|
|
glam_assert!(self.is_normalized());
|
|
glam_assert!(end.is_normalized());
|
|
|
|
{% if is_scalar %}
|
|
let dot = self.dot(end);
|
|
let bias = if dot >= 0.0 { 1.0 } else { -1.0 };
|
|
self.lerp_impl(end * bias, s)
|
|
{% elif is_sse2 %}
|
|
const NEG_ZERO: __m128 = m128_from_f32x4([-0.0; 4]);
|
|
unsafe {
|
|
let dot = dot4_into_m128(self.0, end.0);
|
|
// Calculate the bias, if the dot product is positive or zero, there is no bias
|
|
// but if it is negative, we want to flip the 'end' rotation XYZW components
|
|
let bias = _mm_and_ps(dot, NEG_ZERO);
|
|
self.lerp_impl(Self(_mm_xor_ps(end.0, bias)), s)
|
|
}
|
|
{% elif is_wasm32 %}
|
|
const NEG_ZERO: v128 = v128_from_f32x4([-0.0; 4]);
|
|
let dot = dot4_into_v128(self.0, end.0);
|
|
// Calculate the bias, if the dot product is positive or zero, there is no bias
|
|
// but if it is negative, we want to flip the 'end' rotation XYZW components
|
|
let bias = v128_and(dot, NEG_ZERO);
|
|
self.lerp_impl(Self(v128_xor(end.0, bias)), s)
|
|
{% elif is_coresimd %}
|
|
const NEG_ZERO: f32x4 = f32x4::from_array([-0.0; 4]);
|
|
let dot = dot4_into_f32x4(self.0, end.0);
|
|
// Calculate the bias, if the dot product is positive or zero, there is no bias
|
|
// but if it is negative, we want to flip the 'end' rotation XYZW components
|
|
let bias = f32x4_bitand(dot, NEG_ZERO);
|
|
self.lerp_impl(Self(f32x4_bitxor(end.0, bias)), s)
|
|
{% elif is_neon %}
|
|
const NEG_ZERO: float32x4_t = f32x4_from_array([-0.0; 4]);
|
|
unsafe {
|
|
let dot = dot4_into_f32x4(self.0, end.0);
|
|
// Calculate the bias, if the dot product is positive or zero, there is no bias
|
|
// but if it is negative, we want to flip the 'end' rotation XYZW components
|
|
let bias = vandq_u32(vreinterpretq_u32_f32(dot), vreinterpretq_u32_f32(NEG_ZERO));
|
|
self.lerp_impl(
|
|
Self(vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(end.0), bias))), s)
|
|
}
|
|
{% else %}
|
|
unimplemented!()
|
|
{% endif %}
|
|
}
|
|
|
|
/// Performs a spherical linear interpolation between `self` and `end`
|
|
/// based on the value `s`.
|
|
///
|
|
/// When `s` is `0.0`, the result will be equal to `self`. When `s`
|
|
/// is `1.0`, the result will be equal to `end`.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// 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: {{ scalar_t }}) -> Self {
|
|
// http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
|
|
glam_assert!(self.is_normalized());
|
|
glam_assert!(end.is_normalized());
|
|
|
|
// Note that a rotation can be represented by two quaternions: `q` and
|
|
// `-q`. The slerp path between `q` and `end` will be different from the
|
|
// path between `-q` and `end`. One path will take the long way around and
|
|
// one will take the short way. In order to correct for this, the `dot`
|
|
// product between `self` and `end` should be positive. If the `dot`
|
|
// product is negative, slerp between `self` and `-end`.
|
|
let mut dot = self.dot(end);
|
|
if dot < 0.0 {
|
|
end = -end;
|
|
dot = -dot;
|
|
}
|
|
|
|
const DOT_THRESHOLD: {{ scalar_t }} = 1.0 - {{ scalar_t }}::EPSILON;
|
|
if dot > DOT_THRESHOLD {
|
|
// if above threshold perform linear interpolation to avoid divide by zero
|
|
self.lerp_impl(end, s)
|
|
} else {
|
|
let theta = math::acos_approx(dot);
|
|
{% if is_sse2 %}
|
|
let x = 1.0 - s;
|
|
let y = s;
|
|
let z = 1.0;
|
|
|
|
unsafe {
|
|
let tmp = _mm_mul_ps(_mm_set_ps1(theta), _mm_set_ps(0.0, z, y, x));
|
|
let tmp = m128_sin(tmp);
|
|
|
|
let scale1 = _mm_shuffle_ps(tmp, tmp, 0b00_00_00_00);
|
|
let scale2 = _mm_shuffle_ps(tmp, tmp, 0b01_01_01_01);
|
|
let theta_sin = _mm_shuffle_ps(tmp, tmp, 0b10_10_10_10);
|
|
|
|
Self(_mm_div_ps(
|
|
_mm_add_ps(_mm_mul_ps(self.0, scale1), _mm_mul_ps(end.0, scale2)),
|
|
theta_sin,
|
|
))
|
|
}
|
|
{% else %}
|
|
let scale1 = math::sin(theta * (1.0 - s));
|
|
let scale2 = math::sin(theta * s);
|
|
let theta_sin = math::sin(theta);
|
|
((self * scale1) + (end * scale2)) * (1.0 / theta_sin)
|
|
{% endif %}
|
|
}
|
|
}
|
|
|
|
/// Multiplies a quaternion and a 3D vector, returning the rotated vector.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn mul_vec3(self, rhs: {{ vec3_t }}) -> {{ vec3_t }} {
|
|
glam_assert!(self.is_normalized());
|
|
{% if is_scalar %}
|
|
let w = self.w;
|
|
let b = {{ vec3_t }}::new(self.x, self.y, self.z);
|
|
let b2 = b.dot(b);
|
|
rhs
|
|
.mul(w * w - b2)
|
|
.add(b.mul(rhs.dot(b) * 2.0))
|
|
.add(b.cross(rhs).mul(w * 2.0))
|
|
{% else %}
|
|
self.mul_vec3a(rhs.into()).into()
|
|
{% endif %}
|
|
}
|
|
|
|
/// Multiplies two quaternions. If they each represent a rotation, the result will
|
|
/// represent the combined rotation.
|
|
///
|
|
/// Note that due to floating point rounding the result may not be perfectly normalized.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// 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 {
|
|
{% if is_scalar %}
|
|
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,
|
|
)
|
|
{% elif is_sse2 %}
|
|
// Based on https://github.com/nfrechette/rtm `rtm::quat_mul`
|
|
const CONTROL_WZYX: __m128 = m128_from_f32x4([1.0, -1.0, 1.0, -1.0]);
|
|
const CONTROL_ZWXY: __m128 = m128_from_f32x4([1.0, 1.0, -1.0, -1.0]);
|
|
const CONTROL_YXWZ: __m128 = 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))
|
|
}
|
|
{% elif is_wasm32 %}
|
|
let lhs = self.0;
|
|
let rhs = rhs.0;
|
|
|
|
const CONTROL_WZYX: v128 = v128_from_f32x4([1.0, -1.0, 1.0, -1.0]);
|
|
const CONTROL_ZWXY: v128 = v128_from_f32x4([1.0, 1.0, -1.0, -1.0]);
|
|
const CONTROL_YXWZ: v128 = v128_from_f32x4([-1.0, 1.0, 1.0, -1.0]);
|
|
|
|
let r_xxxx = i32x4_shuffle::<0, 0, 4, 4>(lhs, lhs);
|
|
let r_yyyy = i32x4_shuffle::<1, 1, 5, 5>(lhs, lhs);
|
|
let r_zzzz = i32x4_shuffle::<2, 2, 6, 6>(lhs, lhs);
|
|
let r_wwww = i32x4_shuffle::<3, 3, 7, 7>(lhs, lhs);
|
|
|
|
let lxrw_lyrw_lzrw_lwrw = f32x4_mul(r_wwww, rhs);
|
|
let l_wzyx = i32x4_shuffle::<3, 2, 5, 4>(rhs, rhs);
|
|
|
|
let lwrx_lzrx_lyrx_lxrx = f32x4_mul(r_xxxx, l_wzyx);
|
|
let l_zwxy = i32x4_shuffle::<1, 0, 7, 6>(l_wzyx, l_wzyx);
|
|
|
|
let lwrx_nlzrx_lyrx_nlxrx = f32x4_mul(lwrx_lzrx_lyrx_lxrx, CONTROL_WZYX);
|
|
|
|
let lzry_lwry_lxry_lyry = f32x4_mul(r_yyyy, l_zwxy);
|
|
let l_yxwz = i32x4_shuffle::<3, 2, 5, 4>(l_zwxy, l_zwxy);
|
|
|
|
let lzry_lwry_nlxry_nlyry = f32x4_mul(lzry_lwry_lxry_lyry, CONTROL_ZWXY);
|
|
|
|
let lyrz_lxrz_lwrz_lzrz = f32x4_mul(r_zzzz, l_yxwz);
|
|
let result0 = f32x4_add(lxrw_lyrw_lzrw_lwrw, lwrx_nlzrx_lyrx_nlxrx);
|
|
|
|
let nlyrz_lxrz_lwrz_wlzrz = f32x4_mul(lyrz_lxrz_lwrz_lzrz, CONTROL_YXWZ);
|
|
let result1 = f32x4_add(lzry_lwry_nlxry_nlyry, nlyrz_lxrz_lwrz_wlzrz);
|
|
|
|
Self(f32x4_add(result0, result1))
|
|
{% elif is_coresimd %}
|
|
let lhs = self.0;
|
|
let rhs = rhs.0;
|
|
|
|
const CONTROL_WZYX: f32x4 = f32x4::from_array([1.0, -1.0, 1.0, -1.0]);
|
|
const CONTROL_ZWXY: f32x4 = f32x4::from_array([1.0, 1.0, -1.0, -1.0]);
|
|
const CONTROL_YXWZ: f32x4 = f32x4::from_array([-1.0, 1.0, 1.0, -1.0]);
|
|
|
|
let r_xxxx = simd_swizzle!(lhs, [0, 0, 0, 0]);
|
|
let r_yyyy = simd_swizzle!(lhs, [1, 1, 1, 1]);
|
|
let r_zzzz = simd_swizzle!(lhs, [2, 2, 2, 2]);
|
|
let r_wwww = simd_swizzle!(lhs, [3, 3, 3, 3]);
|
|
|
|
let lxrw_lyrw_lzrw_lwrw = r_wwww * rhs;
|
|
let l_wzyx = simd_swizzle!(rhs, [3, 2, 1, 0]);
|
|
|
|
let lwrx_lzrx_lyrx_lxrx = r_xxxx * l_wzyx;
|
|
let l_zwxy = simd_swizzle!(l_wzyx, [1, 0, 3, 2]);
|
|
|
|
let lwrx_nlzrx_lyrx_nlxrx = lwrx_lzrx_lyrx_lxrx * CONTROL_WZYX;
|
|
|
|
let lzry_lwry_lxry_lyry = r_yyyy * l_zwxy;
|
|
let l_yxwz = simd_swizzle!(l_zwxy, [3, 2, 1, 0]);
|
|
|
|
let lzry_lwry_nlxry_nlyry = lzry_lwry_lxry_lyry * CONTROL_ZWXY;
|
|
|
|
let lyrz_lxrz_lwrz_lzrz = r_zzzz * l_yxwz;
|
|
let result0 = lxrw_lyrw_lzrw_lwrw + lwrx_nlzrx_lyrx_nlxrx;
|
|
|
|
let nlyrz_lxrz_lwrz_wlzrz = lyrz_lxrz_lwrz_lzrz * CONTROL_YXWZ;
|
|
let result1 = lzry_lwry_nlxry_nlyry + nlyrz_lxrz_lwrz_wlzrz;
|
|
Self(result0 + result1)
|
|
{% elif is_neon %}
|
|
unsafe {
|
|
let lhs = self.0;
|
|
let rhs = rhs.0;
|
|
|
|
const CONTROL_WZYX: float32x4_t = f32x4_from_array([1.0, -1.0, 1.0, -1.0]);
|
|
const CONTROL_ZWXY: float32x4_t = f32x4_from_array([1.0, 1.0, -1.0, -1.0]);
|
|
const CONTROL_YXWZ: float32x4_t = f32x4_from_array([-1.0, 1.0, 1.0, -1.0]);
|
|
|
|
let r_xxxx = vdupq_laneq_f32(lhs, 0);
|
|
let r_yyyy = vdupq_laneq_f32(lhs, 1);
|
|
let r_zzzz = vdupq_laneq_f32(lhs, 2);
|
|
let r_wwww = vdupq_laneq_f32(lhs, 3);
|
|
|
|
let lxrw_lyrw_lzrw_lwrw = vmulq_f32(r_wwww, rhs);
|
|
//let l_wzyx = simd_swizzle!(rhs, [3, 2, 1, 0]);
|
|
let l_wzyx = vrev64q_f32(rhs);
|
|
let l_wzyx = vextq_f32(l_wzyx, l_wzyx, 2);
|
|
|
|
let lwrx_lzrx_lyrx_lxrx = vmulq_f32(r_xxxx, l_wzyx);
|
|
//let l_zwxy = simd_swizzle!(l_wzyx, [1, 0, 3, 2]);
|
|
let l_zwxy = vrev64q_f32(l_wzyx);
|
|
|
|
let lwrx_nlzrx_lyrx_nlxrx = vmulq_f32(lwrx_lzrx_lyrx_lxrx, CONTROL_WZYX);
|
|
|
|
let lzry_lwry_lxry_lyry = vmulq_f32(r_yyyy, l_zwxy);
|
|
// let l_yxwz = simd_swizzle!(l_zwxy, [3, 2, 1, 0]);
|
|
let l_yxwz = vrev64q_f32(l_zwxy);
|
|
let l_yxwz = vextq_f32(l_yxwz, l_yxwz, 2);
|
|
|
|
let lzry_lwry_nlxry_nlyry = vmulq_f32(lzry_lwry_lxry_lyry, CONTROL_ZWXY);
|
|
|
|
let lyrz_lxrz_lwrz_lzrz = vmulq_f32(r_zzzz, l_yxwz);
|
|
let result0 = vaddq_f32(lxrw_lyrw_lzrw_lwrw, lwrx_nlzrx_lyrx_nlxrx);
|
|
|
|
let nlyrz_lxrz_lwrz_wlzrz = vmulq_f32(lyrz_lxrz_lwrz_lzrz, CONTROL_YXWZ);
|
|
let result1 = vaddq_f32(lzry_lwry_nlxry_nlyry, nlyrz_lxrz_lwrz_wlzrz);
|
|
Self(vaddq_f32(result0, result1))
|
|
}
|
|
{% else %}
|
|
unimplemented!()
|
|
{% endif %}
|
|
}
|
|
|
|
/// Creates a quaternion from a 3x3 rotation matrix inside a 3D affine transform.
|
|
///
|
|
/// Note if the input affine matrix contain scales, shears, or other non-rotation
|
|
/// transformations then the resulting quaternion will be ill-defined.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if any input affine matrix column is not normalized when `glam_assert` is
|
|
/// enabled.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn from_affine3(a: &crate::{{ affine3_t }}) -> Self {
|
|
#[allow(clippy::useless_conversion)]
|
|
Self::from_rotation_axes(
|
|
a.matrix3.x_axis.into(),
|
|
a.matrix3.y_axis.into(),
|
|
a.matrix3.z_axis.into(),
|
|
)
|
|
}
|
|
|
|
{% if scalar_t == "f32" %}
|
|
/// Multiplies a quaternion and a 3D vector, returning the rotated vector.
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn mul_vec3a(self, rhs: Vec3A) -> Vec3A {
|
|
{% if is_scalar %}
|
|
self.mul_vec3(rhs.into()).into()
|
|
{% elif is_sse2 %}
|
|
unsafe {
|
|
const TWO: __m128 = m128_from_f32x4([2.0; 4]);
|
|
let w = _mm_shuffle_ps(self.0, self.0, 0b11_11_11_11);
|
|
let b = self.0;
|
|
let b2 = dot3_into_m128(b, b);
|
|
Vec3A(_mm_add_ps(
|
|
_mm_add_ps(
|
|
_mm_mul_ps(rhs.0, _mm_sub_ps(_mm_mul_ps(w, w), b2)),
|
|
_mm_mul_ps(b, _mm_mul_ps(dot3_into_m128(rhs.0, b), TWO)),
|
|
),
|
|
_mm_mul_ps(Vec3A(b).cross(rhs).into(), _mm_mul_ps(w, TWO)),
|
|
))
|
|
}
|
|
{% elif is_wasm32 %}
|
|
const TWO: v128 = v128_from_f32x4([2.0; 4]);
|
|
let w = i32x4_shuffle::<3, 3, 7, 7>(self.0, self.0);
|
|
let b = self.0;
|
|
let b2 = dot3_into_v128(b, b);
|
|
Vec3A(f32x4_add(
|
|
f32x4_add(
|
|
f32x4_mul(rhs.0, f32x4_sub(f32x4_mul(w, w), b2)),
|
|
f32x4_mul(b, f32x4_mul(dot3_into_v128(rhs.0, b), TWO)),
|
|
),
|
|
f32x4_mul(Vec3A(b).cross(rhs).into(), f32x4_mul(w, TWO)),
|
|
))
|
|
{% elif is_coresimd %}
|
|
const TWO: f32x4 = f32x4::from_array([2.0; 4]);
|
|
let w = simd_swizzle!(self.0, [3, 3, 3, 3]);
|
|
let b = self.0;
|
|
let b2 = dot3_into_f32x4(b, b);
|
|
Vec3A(
|
|
rhs.0
|
|
.mul(w.mul(w).sub(b2))
|
|
.add(b.mul(dot3_into_f32x4(rhs.0, b).mul(TWO)))
|
|
.add(Vec3A(b).cross(rhs).0.mul(w.mul(TWO))),
|
|
)
|
|
{% elif is_neon %}
|
|
unsafe {
|
|
let w = self.w;
|
|
let b = Vec3A::from(self.0);
|
|
let b2 = b.length_squared();
|
|
Vec3A(vaddq_f32(
|
|
vaddq_f32(
|
|
vmulq_n_f32(rhs.0, (w * w) - b2),
|
|
vmulq_n_f32(b.0, rhs.dot(b) * 2.0),
|
|
),
|
|
vmulq_n_f32(b.cross(rhs).0, w * 2.0),
|
|
))
|
|
}
|
|
{% else %}
|
|
unimplemented!()
|
|
{% endif %}
|
|
}
|
|
|
|
#[inline]
|
|
#[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)
|
|
}
|
|
{% elif scalar_t == "f64" %}
|
|
#[inline]
|
|
#[must_use]
|
|
pub fn as_quat(self) -> Quat {
|
|
Quat::from_xyzw(self.x as f32, self.y as f32, self.z as f32, self.w as f32)
|
|
}
|
|
{% endif %}
|
|
}
|
|
|
|
impl fmt::Debug for {{ self_t }} {
|
|
fn fmt(&self, fmt: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
fmt.debug_tuple(stringify!({{ self_t }}))
|
|
.field(&self.x)
|
|
.field(&self.y)
|
|
.field(&self.z)
|
|
.field(&self.w)
|
|
.finish()
|
|
}
|
|
}
|
|
|
|
impl fmt::Display for {{ self_t }} {
|
|
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
|
|
if let Some(p) = f.precision() {
|
|
write!(f, "[{:.*}, {:.*}, {:.*}, {:.*}]", p, self.x, p, self.y, p, self.z, p, self.w)
|
|
} else {
|
|
write!(f, "[{}, {}, {}, {}]", self.x, self.y, self.z, self.w)
|
|
}
|
|
}
|
|
}
|
|
|
|
impl Add for {{ self_t }} {
|
|
type Output = Self;
|
|
/// Adds two quaternions.
|
|
///
|
|
/// The sum is not guaranteed to be normalized.
|
|
///
|
|
/// Note that addition is not the same as combining the rotations represented by the
|
|
/// two quaternions! That corresponds to multiplication.
|
|
#[inline]
|
|
fn add(self, rhs: Self) -> Self {
|
|
Self::from_vec4({{ vec4_t }}::from(self) + {{ vec4_t }}::from(rhs))
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Add", self_t=self_t, rhs_t=self_t, output_t=self_t) }}
|
|
|
|
{{ macros::impl_op_assign(imp="Add", self_t=self_t, rhs_t=self_t) }}
|
|
|
|
impl Sub for {{ self_t }} {
|
|
type Output = Self;
|
|
/// Subtracts the `rhs` quaternion from `self`.
|
|
///
|
|
/// The difference is not guaranteed to be normalized.
|
|
#[inline]
|
|
fn sub(self, rhs: Self) -> Self {
|
|
Self::from_vec4({{ vec4_t }}::from(self) - {{ vec4_t }}::from(rhs))
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Sub", self_t=self_t, rhs_t=self_t, output_t=self_t) }}
|
|
|
|
{{ macros::impl_op_assign(imp="Sub", self_t=self_t, rhs_t=self_t) }}
|
|
|
|
impl Mul<{{ scalar_t }}> for {{ self_t }} {
|
|
type Output = Self;
|
|
/// Multiplies a quaternion by a scalar value.
|
|
///
|
|
/// The product is not guaranteed to be normalized.
|
|
#[inline]
|
|
fn mul(self, rhs: {{ scalar_t }}) -> Self {
|
|
Self::from_vec4({{ vec4_t }}::from(self) * rhs)
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Mul", self_t=self_t, rhs_t=scalar_t, output_t=self_t) }}
|
|
|
|
{{ macros::impl_op_assign(imp="Mul", self_t=self_t, rhs_t=scalar_t) }}
|
|
|
|
impl Div<{{ scalar_t }}> for {{ self_t }} {
|
|
type Output = Self;
|
|
/// Divides a quaternion by a scalar value.
|
|
/// The quotient is not guaranteed to be normalized.
|
|
#[inline]
|
|
fn div(self, rhs: {{ scalar_t }}) -> Self {
|
|
Self::from_vec4({{ vec4_t }}::from(self) / rhs)
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Div", self_t=self_t, rhs_t=scalar_t, output_t=self_t) }}
|
|
|
|
{{ macros::impl_op_assign(imp="Div", self_t=self_t, rhs_t=scalar_t) }}
|
|
|
|
impl Mul for {{ self_t }} {
|
|
type Output = Self;
|
|
/// Multiplies two quaternions. If they each represent a rotation, the result will
|
|
/// represent the combined rotation.
|
|
///
|
|
/// Note that due to floating point rounding the result may not be perfectly
|
|
/// normalized.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` or `rhs` are not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
fn mul(self, rhs: Self) -> Self {
|
|
self.mul_quat(rhs)
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Mul", self_t=self_t, rhs_t=self_t, output_t=self_t) }}
|
|
|
|
{{ macros::impl_op_assign(imp="Mul", self_t=self_t, rhs_t=self_t) }}
|
|
|
|
impl Mul<{{ vec3_t }}> for {{ self_t }} {
|
|
type Output = {{ vec3_t }};
|
|
/// Multiplies a quaternion and a 3D vector, returning the rotated vector.
|
|
///
|
|
/// # Panics
|
|
///
|
|
/// Will panic if `self` is not normalized when `glam_assert` is enabled.
|
|
#[inline]
|
|
fn mul(self, rhs: {{ vec3_t }}) -> Self::Output {
|
|
self.mul_vec3(rhs)
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Mul", self_t=self_t, rhs_t=vec3_t, output_t=vec3_t) }}
|
|
|
|
{% if scalar_t == "f32" %}
|
|
impl Mul<Vec3A> for Quat {
|
|
type Output = Vec3A;
|
|
#[inline]
|
|
fn mul(self, rhs: Vec3A) -> Self::Output {
|
|
self.mul_vec3a(rhs)
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_binop(imp="Mul", self_t=self_t, rhs_t="Vec3A", output_t="Vec3A") }}
|
|
{% endif %}
|
|
|
|
impl Neg for {{ self_t }} {
|
|
type Output = Self;
|
|
#[inline]
|
|
fn neg(self) -> Self {
|
|
self * -1.0
|
|
}
|
|
}
|
|
|
|
{{ macros::impl_ref_unop(imp="Neg", self_t=self_t, output_t=self_t) }}
|
|
|
|
impl Default for {{ self_t }} {
|
|
#[inline]
|
|
fn default() -> Self {
|
|
Self::IDENTITY
|
|
}
|
|
}
|
|
|
|
impl PartialEq for {{ self_t }} {
|
|
#[inline]
|
|
fn eq(&self, rhs: &Self) -> bool {
|
|
{{ vec4_t }}::from(*self).eq(&{{ vec4_t }}::from(*rhs))
|
|
}
|
|
}
|
|
|
|
impl AsRef<[{{ scalar_t }}; 4]> for {{ self_t }} {
|
|
#[inline]
|
|
fn as_ref(&self) -> &[{{ scalar_t }}; 4] {
|
|
unsafe { &*(self as *const Self as *const [{{ scalar_t }}; 4]) }
|
|
}
|
|
}
|
|
|
|
impl Sum<Self> for {{ self_t }} {
|
|
fn sum<I>(iter: I) -> Self
|
|
where
|
|
I: Iterator<Item = Self>,
|
|
{
|
|
iter.fold(Self::ZERO, Self::add)
|
|
}
|
|
}
|
|
|
|
impl<'a> Sum<&'a Self> for {{ self_t }} {
|
|
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 {{ self_t }} {
|
|
fn product<I>(iter: I) -> Self
|
|
where
|
|
I: Iterator<Item = Self>,
|
|
{
|
|
iter.fold(Self::IDENTITY, Self::mul)
|
|
}
|
|
}
|
|
|
|
impl<'a> Product<&'a Self> for {{ self_t }} {
|
|
fn product<I>(iter: I) -> Self
|
|
where
|
|
I: Iterator<Item = &'a Self>,
|
|
{
|
|
iter.fold(Self::IDENTITY, |a, &b| Self::mul(a, b))
|
|
}
|
|
}
|
|
|
|
impl From<{{ self_t }}> for {{ vec4_t }} {
|
|
#[inline]
|
|
fn from(q: {{ self_t }}) -> Self {
|
|
{% if is_scalar %}
|
|
Self::new(q.x, q.y, q.z, q.w)
|
|
{% else %}
|
|
Self(q.0)
|
|
{% endif %}
|
|
}
|
|
}
|
|
|
|
impl From<{{ self_t }}> for ({{ scalar_t }}, {{ scalar_t }}, {{ scalar_t }}, {{ scalar_t }}) {
|
|
#[inline]
|
|
fn from(q: {{ self_t }}) -> Self {
|
|
{% if is_scalar %}
|
|
(q.x, q.y, q.z, q.w)
|
|
{% else %}
|
|
{{ vec4_t }}::from(q).into()
|
|
{% endif %}
|
|
}
|
|
}
|
|
|
|
impl From<{{ self_t }}> for [{{ scalar_t }}; 4] {
|
|
#[inline]
|
|
fn from(q: {{ self_t }}) -> Self {
|
|
{% if is_scalar %}
|
|
[q.x, q.y, q.z, q.w]
|
|
{% else %}
|
|
{{ vec4_t }}::from(q).into()
|
|
{% endif %}
|
|
}
|
|
}
|
|
|
|
{% if not is_scalar %}
|
|
impl From<{{ self_t }}> for {{ simd_t }} {
|
|
#[inline]
|
|
fn from(q: {{ self_t }}) -> Self {
|
|
{% if is_scalar %}
|
|
Self { x: q.x, y: q.y, z: q.z, w: q.w }
|
|
{% else %}
|
|
q.0
|
|
{% endif %}
|
|
}
|
|
}
|
|
|
|
impl Deref for {{ self_t }} {
|
|
type Target = crate::deref::Vec4<{{ scalar_t }}>;
|
|
#[inline]
|
|
fn deref(&self) -> &Self::Target {
|
|
unsafe { &*(self as *const Self).cast() }
|
|
}
|
|
}
|
|
|
|
impl DerefMut for {{ self_t }} {
|
|
#[inline]
|
|
fn deref_mut(&mut self) -> &mut Self::Target {
|
|
unsafe { &mut *(self as *mut Self).cast() }
|
|
}
|
|
}
|
|
{% endif %}
|