From: Joshua Simmons Date: Fri, 28 Oct 2022 21:20:09 +0000 (+0200) Subject: Fill in affine2, affine3, and quat functions X-Git-Url: https://git.nega.tv//gitweb.cgi?a=commitdiff_plain;h=8053f8aed3bea19cea0cc1778012e17523669f52;p=josh%2Fnarcissus Fill in affine2, affine3, and quat functions --- diff --git a/narcissus-maths/src/affine2.rs b/narcissus-maths/src/affine2.rs index b3b6b22..cca48f7 100644 --- a/narcissus-maths/src/affine2.rs +++ b/narcissus-maths/src/affine2.rs @@ -4,8 +4,8 @@ use crate::{Mat2, Point2, Vec2}; #[derive(Clone, Copy, PartialEq)] #[repr(C)] pub struct Affine2 { - matrix: Mat2, - translate: Vec2, + pub matrix: Mat2, + pub translate: Vec2, } impl Affine2 { @@ -19,11 +19,52 @@ impl Affine2 { translate: Vec2::ZERO, }; - pub fn mul_vec2(&self, vec: Vec2) -> Vec2 { - self.matrix * vec + self.translate + pub fn mul_affine2(&self, rhs: Affine2) -> Affine2 { + Self { + matrix: self.matrix * rhs.matrix, + translate: self.translate + rhs.translate, + } } - pub fn mul_point2(&self, point: Point2) -> Point2 { + pub fn transform_vec2(&self, vec: Vec2) -> Vec2 { + self.matrix * vec + } + + pub fn transform_point2(&self, point: Point2) -> Point2 { self.matrix * point + self.translate } } + +impl std::ops::Mul for Affine2 { + type Output = Affine2; + + #[inline(always)] + fn mul(self, rhs: Self) -> Self::Output { + self.mul_affine2(rhs) + } +} + +impl std::ops::MulAssign for Affine2 { + #[inline(always)] + fn mul_assign(&mut self, rhs: Self) { + *self = *self * rhs + } +} + +impl std::ops::Mul for Affine2 { + type Output = Vec2; + + #[inline(always)] + fn mul(self, rhs: Vec2) -> Self::Output { + self.transform_vec2(rhs) + } +} + +impl std::ops::Mul for Affine2 { + type Output = Point2; + + #[inline(always)] + fn mul(self, rhs: Point2) -> Self::Output { + self.transform_point2(rhs) + } +} diff --git a/narcissus-maths/src/affine3.rs b/narcissus-maths/src/affine3.rs index b2b7c7c..e96c949 100644 --- a/narcissus-maths/src/affine3.rs +++ b/narcissus-maths/src/affine3.rs @@ -4,8 +4,8 @@ use crate::{Mat3, Point3, Vec3}; #[derive(Clone, Copy, PartialEq)] #[repr(C)] pub struct Affine3 { - matrix: Mat3, - translate: Vec3, + pub matrix: Mat3, + pub translate: Vec3, } impl Affine3 { @@ -19,20 +19,6 @@ impl Affine3 { translate: Vec3::ZERO, }; - pub const fn from_scale(scale: Vec3) -> Affine3 { - Self { - matrix: Mat3::from_scale(scale), - translate: Vec3::ZERO, - } - } - - pub const fn from_translation(translate: Vec3) -> Affine3 { - Self { - matrix: Mat3::IDENTITY, - translate, - } - } - pub fn mul_affine3(&self, rhs: Affine3) -> Affine3 { Self { matrix: self.matrix * rhs.matrix, @@ -40,11 +26,11 @@ impl Affine3 { } } - pub fn mul_vec3(&self, vec: Vec3) -> Vec3 { - self.matrix * vec + self.translate + pub fn transform_vec3(&self, vec: Vec3) -> Vec3 { + self.matrix * vec } - pub fn mul_point3(&self, point: Point3) -> Point3 { + pub fn transform_point3(&self, point: Point3) -> Point3 { self.matrix * point + self.translate } } @@ -70,7 +56,7 @@ impl std::ops::Mul for Affine3 { #[inline(always)] fn mul(self, rhs: Vec3) -> Self::Output { - self.mul_vec3(rhs) + self.transform_vec3(rhs) } } @@ -79,6 +65,6 @@ impl std::ops::Mul for Affine3 { #[inline(always)] fn mul(self, rhs: Point3) -> Self::Output { - self.mul_point3(rhs) + self.transform_point3(rhs) } } diff --git a/narcissus-maths/src/mat2.rs b/narcissus-maths/src/mat2.rs index afab4bb..c6d1b1b 100644 --- a/narcissus-maths/src/mat2.rs +++ b/narcissus-maths/src/mat2.rs @@ -48,6 +48,35 @@ impl Mat2 { Mat2::from_diagonal(scale) } + /// Returns `true` if all elements are finite. + /// + /// If any element is `NaN`, positive infinity, or negative infinity, returns `false`. + pub fn is_finite(&self) -> bool { + let mut is_finite = true; + for x in self.0 { + is_finite &= x.is_finite(); + } + is_finite + } + + /// Returns `true` if any element is positive infinity, or negative infinity, and `false` otherwise. + pub fn is_infinite(&self) -> bool { + let mut is_infinite = false; + for x in self.0 { + is_infinite |= x.is_infinite(); + } + is_infinite + } + + /// Returns `true` if any element is `NaN`, and `false` otherwise. + pub fn is_nan(&self) -> bool { + let mut is_nan = false; + for x in self.0 { + is_nan |= x.is_nan(); + } + is_nan + } + /// Returns the transpose of `self`. #[must_use] #[inline(always)] @@ -56,31 +85,15 @@ impl Mat2 { Mat2::from_rows([[m[0], m[2]], [m[1], m[3]]]) } - #[must_use] - pub fn mul_mat2(self: &Mat2, rhs: Mat2) -> Mat2 { - let mut result = Mat2::IDENTITY; - { - let result = result.as_rows_mut(); - let lhs = self.as_rows(); - let rhs = rhs.as_rows(); - for i in 0..2 { - for j in 0..2 { - result[i][j] = lhs[i][0] * rhs[0][j] + lhs[i][1] * rhs[1][j]; - } - } - } - result - } - #[must_use] #[inline] - pub fn mul_point2(self: &Mat2, point: Point2) -> Point2 { - self.mul_vec2(point.as_vec2()).as_point2() + pub fn transform_point2(self: &Mat2, point: Point2) -> Point2 { + self.transform_vec2(point.as_vec2()).as_point2() } #[must_use] #[inline] - pub fn mul_vec2(self: &Mat2, vec: Vec2) -> Vec2 { + pub fn transform_vec2(self: &Mat2, vec: Vec2) -> Vec2 { let vec = Vec2::new(vec.x, vec.y); let rows = self.as_rows(); Vec2::new( @@ -95,7 +108,18 @@ impl std::ops::Mul for Mat2 { #[inline(always)] fn mul(self, rhs: Self) -> Self::Output { - self.mul_mat2(rhs) + let mut result = Mat2::IDENTITY; + { + let result = result.as_rows_mut(); + let lhs = self.as_rows(); + let rhs = rhs.as_rows(); + for i in 0..2 { + for j in 0..2 { + result[i][j] = lhs[i][0] * rhs[0][j] + lhs[i][1] * rhs[1][j]; + } + } + } + result } } @@ -111,7 +135,7 @@ impl std::ops::Mul for Mat2 { #[inline(always)] fn mul(self, rhs: Vec2) -> Self::Output { - self.mul_vec2(rhs) + self.transform_vec2(rhs) } } @@ -120,7 +144,7 @@ impl std::ops::Mul for Mat2 { #[inline(always)] fn mul(self, rhs: Point2) -> Self::Output { - self.mul_point2(rhs) + self.transform_point2(rhs) } } diff --git a/narcissus-maths/src/mat3.rs b/narcissus-maths/src/mat3.rs index 38f6ae2..c2692bd 100644 --- a/narcissus-maths/src/mat3.rs +++ b/narcissus-maths/src/mat3.rs @@ -80,6 +80,35 @@ impl Mat3 { ]) } + /// Returns `true` if all elements are finite. + /// + /// If any element is `NaN`, positive infinity, or negative infinity, returns `false`. + pub fn is_finite(&self) -> bool { + let mut is_finite = true; + for x in self.0 { + is_finite &= x.is_finite(); + } + is_finite + } + + /// Returns `true` if any element is positive infinity, or negative infinity, and `false` otherwise. + pub fn is_infinite(&self) -> bool { + let mut is_infinite = false; + for x in self.0 { + is_infinite |= x.is_infinite(); + } + is_infinite + } + + /// Returns `true` if any element is `NaN`, and `false` otherwise. + pub fn is_nan(&self) -> bool { + let mut is_nan = false; + for x in self.0 { + is_nan |= x.is_nan(); + } + is_nan + } + /// Returns the transpose of `self`. #[must_use] #[inline(always)] @@ -88,26 +117,9 @@ impl Mat3 { Mat3::from_rows([[m[0], m[3], m[6]], [m[1], m[4], m[7]], [m[2], m[5], m[8]]]) } - #[must_use] - pub fn mul_mat3(self: &Mat3, rhs: Mat3) -> Mat3 { - let mut result = Mat3::IDENTITY; - { - let result = result.as_rows_mut(); - let lhs = self.as_rows(); - let rhs = rhs.as_rows(); - for i in 0..3 { - for j in 0..3 { - result[i][j] = - lhs[i][0] * rhs[0][j] + lhs[i][1] * rhs[1][j] + lhs[i][2] * rhs[2][j]; - } - } - } - result - } - #[must_use] #[inline] - pub fn mul_point2(self: &Mat3, point: Point2) -> Point2 { + pub fn transform_point2(self: &Mat3, point: Point2) -> Point2 { let vec = Vec3::new(point.x, point.y, 1.0); let rows = self.as_rows(); Point2::new( @@ -118,7 +130,7 @@ impl Mat3 { #[must_use] #[inline] - pub fn mul_vec2(self: &Mat3, vec: Vec2) -> Vec2 { + pub fn transform_vec2(self: &Mat3, vec: Vec2) -> Vec2 { let vec = Vec3::new(vec.x, vec.y, 0.0); let rows = self.as_rows(); Vec2::new( @@ -129,13 +141,13 @@ impl Mat3 { #[must_use] #[inline] - pub fn mul_point3(self: &Mat3, point: Point3) -> Point3 { - self.mul_vec3(point.as_vec3()).as_point3() + pub fn transform_point3(self: &Mat3, point: Point3) -> Point3 { + self.transform_vec3(point.as_vec3()).as_point3() } #[must_use] #[inline] - pub fn mul_vec3(self: &Mat3, vec: Vec3) -> Vec3 { + pub fn transform_vec3(self: &Mat3, vec: Vec3) -> Vec3 { let rows = self.as_rows(); Vec3::new( Vec3::dot(rows[0].into(), vec), @@ -150,7 +162,19 @@ impl std::ops::Mul for Mat3 { #[inline(always)] fn mul(self, rhs: Self) -> Self::Output { - self.mul_mat3(rhs) + let mut result = Mat3::IDENTITY; + { + let result = result.as_rows_mut(); + let lhs = self.as_rows(); + let rhs = rhs.as_rows(); + for i in 0..3 { + for j in 0..3 { + result[i][j] = + lhs[i][0] * rhs[0][j] + lhs[i][1] * rhs[1][j] + lhs[i][2] * rhs[2][j]; + } + } + } + result } } @@ -166,7 +190,7 @@ impl std::ops::Mul for Mat3 { #[inline(always)] fn mul(self, rhs: Vec3) -> Self::Output { - self.mul_vec3(rhs) + self.transform_vec3(rhs) } } @@ -175,7 +199,7 @@ impl std::ops::Mul for Mat3 { #[inline(always)] fn mul(self, rhs: Point3) -> Self::Output { - self.mul_point3(rhs) + self.transform_point3(rhs) } } @@ -184,7 +208,7 @@ impl std::ops::Mul for Mat3 { #[inline(always)] fn mul(self, rhs: Vec2) -> Self::Output { - self.mul_vec2(rhs) + self.transform_vec2(rhs) } } @@ -193,7 +217,7 @@ impl std::ops::Mul for Mat3 { #[inline(always)] fn mul(self, rhs: Point2) -> Self::Output { - self.mul_point2(rhs) + self.transform_point2(rhs) } } diff --git a/narcissus-maths/src/mat4.rs b/narcissus-maths/src/mat4.rs index 12425c6..ce703a1 100644 --- a/narcissus-maths/src/mat4.rs +++ b/narcissus-maths/src/mat4.rs @@ -208,6 +208,35 @@ impl Mat4 { ]) } + /// Returns `true` if all elements are finite. + /// + /// If any element is `NaN`, positive infinity, or negative infinity, returns `false`. + pub fn is_finite(&self) -> bool { + let mut is_finite = true; + for x in self.0 { + is_finite &= x.is_finite(); + } + is_finite + } + + /// Returns `true` if any element is positive infinity, or negative infinity, and `false` otherwise. + pub fn is_infinite(&self) -> bool { + let mut is_infinite = false; + for x in self.0 { + is_infinite |= x.is_infinite(); + } + is_infinite + } + + /// Returns `true` if any element is `NaN`, and `false` otherwise. + pub fn is_nan(&self) -> bool { + let mut is_nan = false; + for x in self.0 { + is_nan |= x.is_nan(); + } + is_nan + } + #[allow(dead_code)] #[inline(always)] fn transpose_base(self) -> Mat4 { @@ -247,41 +276,41 @@ impl Mat4 { /// Transforms the given [`Vec2`] `vec` by `self`. #[must_use] #[inline] - pub fn mul_vec2(&self, vec: Vec2) -> Vec2 { + pub fn transform_vec2(&self, vec: Vec2) -> Vec2 { let vec = Vec4::new(vec.x, vec.y, 0.0, 0.0); - let vec = self.mul_vec4(vec); + let vec = self.transform_vec4(vec); Vec2::new(vec.x, vec.y) } /// Transforms the given [`Point2`] `point` by `self`. #[must_use] #[inline] - pub fn mul_point2(&self, point: Point2) -> Point2 { + pub fn transform_point2(&self, point: Point2) -> Point2 { let vec = Vec4::new(point.x, point.y, 0.0, 1.0); - let vec = self.mul_vec4(vec); + let vec = self.transform_vec4(vec); Point2::new(vec.x, vec.y) } /// Transforms the given [`Vec3`] `vec` by `self`. #[must_use] #[inline] - pub fn mul_vec3(&self, vec: Vec3) -> Vec3 { + pub fn transform_vec3(&self, vec: Vec3) -> Vec3 { let vec = Vec4::new(vec.x, vec.y, vec.z, 0.0); - let vec = self.mul_vec4(vec); + let vec = self.transform_vec4(vec); [vec.x, vec.y, vec.z].into() } /// Transforms the given [`Point3`] `point` by `self`. #[must_use] #[inline] - pub fn mul_point3(&self, point: Point3) -> Point3 { + pub fn transform_point3(&self, point: Point3) -> Point3 { let vec = Vec4::new(point.x, point.y, point.z, 1.0); - let vec = self.mul_vec4(vec); + let vec = self.transform_vec4(vec); Point3::new(vec.x, vec.y, vec.z) } #[inline(always)] - fn mul_vec4_base(&self, vec: Vec4) -> Vec4 { + fn transform_vec4_base(&self, vec: Vec4) -> Vec4 { let rows = self.as_rows(); Vec4::new( Vec4::dot(rows[0].into(), vec), @@ -295,7 +324,7 @@ impl Mat4 { #[allow(dead_code)] #[inline] #[target_feature(enable = "sse4.1")] - unsafe fn mul_vec4_sse41(&self, vec: Vec4) -> Vec4 { + unsafe fn transform_vec4_sse41(&self, vec: Vec4) -> Vec4 { use std::arch::x86_64::{_mm_hadd_ps, _mm_mul_ps}; let vec = vec.into(); @@ -312,10 +341,10 @@ impl Mat4 { /// Transforms the given [`Vec4`] `vec` by `self`. #[must_use] #[inline(always)] - pub fn mul_vec4(&self, vec: Vec4) -> Vec4 { + pub fn transform_vec4(&self, vec: Vec4) -> Vec4 { #[cfg(not(target_feature = "sse4.1"))] { - self.mul_vec4_base(vec) + self.transform_vec4_base(vec) } #[cfg(target_feature = "sse4.1")] @@ -323,96 +352,91 @@ impl Mat4 { self.mul_vec4_sse41(vec) } } +} - #[allow(dead_code)] - #[inline(always)] - fn mul_mat4_base(self: &Mat4, rhs: Mat4) -> Mat4 { - let mut result = Mat4::IDENTITY; - { - let result = result.as_rows_mut(); - let lhs = self.as_rows(); - let rhs = rhs.as_rows(); - for i in 0..4 { - for j in 0..4 { - result[i][j] = lhs[i][0] * rhs[0][j] - + lhs[i][1] * rhs[1][j] - + lhs[i][2] * rhs[2][j] - + lhs[i][3] * rhs[3][j]; - } +#[allow(dead_code)] +#[inline(always)] +fn mul_mat4_base(lhs: Mat4, rhs: Mat4) -> Mat4 { + let mut result = Mat4::IDENTITY; + { + let result = result.as_rows_mut(); + let lhs = lhs.as_rows(); + let rhs = rhs.as_rows(); + for i in 0..4 { + for j in 0..4 { + result[i][j] = lhs[i][0] * rhs[0][j] + + lhs[i][1] * rhs[1][j] + + lhs[i][2] * rhs[2][j] + + lhs[i][3] * rhs[3][j]; } } - result } + result +} - // Safety: Requires SSE2. - #[allow(dead_code)] - #[inline] - #[target_feature(enable = "sse2")] - unsafe fn mul_mat4_sse2(&self, rhs: Mat4) -> Mat4 { - use std::arch::x86_64::{__m128, _mm_add_ps, _mm_mul_ps, _mm_shuffle_ps}; +// Safety: Requires SSE2. +#[allow(dead_code)] +#[inline] +#[target_feature(enable = "sse2")] +unsafe fn mul_mat4_sse2(lhs: Mat4, rhs: Mat4) -> Mat4 { + use std::arch::x86_64::{__m128, _mm_add_ps, _mm_mul_ps, _mm_shuffle_ps}; - #[inline(always)] - fn linear_combine(a: __m128, mat: &[__m128; 4]) -> __m128 { - unsafe { - let r = _mm_mul_ps(_mm_shuffle_ps(a, a, 0x00), mat[0]); - let r = _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(a, a, 0x55), mat[1])); - let r = _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(a, a, 0xaa), mat[2])); - _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(a, a, 0xff), mat[3])) - } + #[inline(always)] + fn linear_combine(a: __m128, mat: &[__m128; 4]) -> __m128 { + unsafe { + let r = _mm_mul_ps(_mm_shuffle_ps(a, a, 0x00), mat[0]); + let r = _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(a, a, 0x55), mat[1])); + let r = _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(a, a, 0xaa), mat[2])); + _mm_add_ps(r, _mm_mul_ps(_mm_shuffle_ps(a, a, 0xff), mat[3])) } - - let lhs = self.as_m128_array(); - let rhs = rhs.as_m128_array(); - - let x0 = linear_combine(lhs[0], &rhs); - let x1 = linear_combine(lhs[1], &rhs); - let x2 = linear_combine(lhs[2], &rhs); - let x3 = linear_combine(lhs[3], &rhs); - - Mat4::from_m128_array([x0, x1, x2, x3]) } - // Safety: Requires AVX2. - #[allow(dead_code)] - #[inline] - #[target_feature(enable = "avx2")] - unsafe fn mul_mat4_avx2(&self, rhs: Mat4) -> Mat4 { - use std::arch::x86_64::{ - __m128, __m256, _mm256_add_ps, _mm256_broadcast_ps, _mm256_loadu_ps, _mm256_mul_ps, - _mm256_shuffle_ps, _mm256_storeu_ps, _mm256_zeroupper, - }; - - #[inline(always)] - unsafe fn two_linear_combine(a: __m256, m: &[__m128; 4]) -> __m256 { - let r = _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0x00), _mm256_broadcast_ps(&m[0])); - let r = _mm256_add_ps( - r, - _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0x55), _mm256_broadcast_ps(&m[1])), - ); - let r = _mm256_add_ps( - r, - _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0xaa), _mm256_broadcast_ps(&m[2])), - ); - _mm256_add_ps( - r, - _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0xff), _mm256_broadcast_ps(&m[3])), - ) - } + let lhs = lhs.as_m128_array(); + let rhs = rhs.as_m128_array(); - _mm256_zeroupper(); + let x0 = linear_combine(lhs[0], &rhs); + let x1 = linear_combine(lhs[1], &rhs); + let x2 = linear_combine(lhs[2], &rhs); + let x3 = linear_combine(lhs[3], &rhs); - let a0 = _mm256_loadu_ps(&self.0[0]); - let a1 = _mm256_loadu_ps(&self.0[8]); - let rhs = rhs.as_m128_array(); + Mat4::from_m128_array([x0, x1, x2, x3]) +} - let x0 = two_linear_combine(a0, &rhs); - let x1 = two_linear_combine(a1, &rhs); +// Safety: Requires AVX2. +#[allow(dead_code)] +#[inline] +#[target_feature(enable = "avx2")] +unsafe fn mul_mat4_avx2(lhs: Mat4, rhs: Mat4) -> Mat4 { + use std::arch::x86_64::{ + __m128, __m256, _mm256_add_ps, _mm256_broadcast_ps, _mm256_loadu_ps, _mm256_mul_ps, + _mm256_shuffle_ps, _mm256_storeu_ps, _mm256_zeroupper, + }; - let mut result = Mat4::IDENTITY; - _mm256_storeu_ps(&mut result.0[0], x0); - _mm256_storeu_ps(&mut result.0[8], x1); - result - } + #[inline(always)] + unsafe fn two_linear_combine(a: __m256, m: &[__m128; 4]) -> __m256 { + let m0 = _mm256_broadcast_ps(&m[0]); + let m1 = _mm256_broadcast_ps(&m[1]); + let m2 = _mm256_broadcast_ps(&m[2]); + let m3 = _mm256_broadcast_ps(&m[3]); + let r = _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0x00), m0); + let r = _mm256_add_ps(r, _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0x55), m1)); + let r = _mm256_add_ps(r, _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0xaa), m2)); + _mm256_add_ps(r, _mm256_mul_ps(_mm256_shuffle_ps(a, a, 0xff), m3)) + } + + _mm256_zeroupper(); + + let a0 = _mm256_loadu_ps(&lhs.0[0]); + let a1 = _mm256_loadu_ps(&lhs.0[8]); + let rhs = rhs.as_m128_array(); + + let x0 = two_linear_combine(a0, &rhs); + let x1 = two_linear_combine(a1, &rhs); + + let mut result = Mat4::IDENTITY; + _mm256_storeu_ps(&mut result.0[0], x0); + _mm256_storeu_ps(&mut result.0[8], x1); + result } impl std::ops::Mul for Mat4 { @@ -422,15 +446,15 @@ impl std::ops::Mul for Mat4 { fn mul(self, rhs: Self) -> Self::Output { #[cfg(not(target_feature = "sse2"))] { - self.mul_mat4_base(rhs) + mul_mat4_base(self, rhs) } #[cfg(all(target_feature = "sse2", not(target_feature = "avx2")))] unsafe { - self.mul_mat4_sse2(rhs) + mul_mat4_sse2(self, rhs) } #[cfg(target_feature = "avx2")] unsafe { - self.mul_mat4_avx2(rhs) + mul_mat4_avx2(self, rhs) } } } @@ -447,7 +471,7 @@ impl std::ops::Mul for Mat4 { #[inline(always)] fn mul(self, rhs: Vec4) -> Self::Output { - self.mul_vec4(rhs) + self.transform_vec4(rhs) } } @@ -456,7 +480,7 @@ impl std::ops::Mul for Mat4 { #[inline(always)] fn mul(self, rhs: Vec3) -> Self::Output { - self.mul_vec3(rhs) + self.transform_vec3(rhs) } } @@ -465,7 +489,7 @@ impl std::ops::Mul for Mat4 { #[inline(always)] fn mul(self, rhs: Point3) -> Self::Output { - self.mul_point3(rhs) + self.transform_point3(rhs) } } @@ -474,7 +498,7 @@ impl std::ops::Mul for Mat4 { #[inline(always)] fn mul(self, rhs: Vec2) -> Self::Output { - self.mul_vec2(rhs) + self.transform_vec2(rhs) } } @@ -483,7 +507,7 @@ impl std::ops::Mul for Mat4 { #[inline(always)] fn mul(self, rhs: Point2) -> Self::Output { - self.mul_point2(rhs) + self.transform_point2(rhs) } } @@ -550,20 +574,20 @@ mod tests { assert_eq!(SCALE * I, SCALE); assert_eq!(M * I, M); - assert_eq!(I.mul_mat4_base(I), I); - assert_eq!(SCALE.mul_mat4_base(I), SCALE); - assert_eq!(M.mul_mat4_base(I), M); + assert_eq!(mul_mat4_base(I, I), I); + assert_eq!(mul_mat4_base(SCALE, I), SCALE); + assert_eq!(mul_mat4_base(M, I), M); if std::is_x86_feature_detected!("sse2") { - assert_eq!(unsafe { I.mul_mat4_sse2(I) }, I); - assert_eq!(unsafe { SCALE.mul_mat4_sse2(I) }, SCALE); - assert_eq!(unsafe { M.mul_mat4_sse2(I) }, M); + assert_eq!(unsafe { mul_mat4_sse2(I, I) }, I); + assert_eq!(unsafe { mul_mat4_sse2(SCALE, I) }, SCALE); + assert_eq!(unsafe { mul_mat4_sse2(M, I) }, M); } if std::is_x86_feature_detected!("avx2") { - assert_eq!(unsafe { I.mul_mat4_avx2(I) }, I); - assert_eq!(unsafe { SCALE.mul_mat4_avx2(I) }, SCALE); - assert_eq!(unsafe { M.mul_mat4_avx2(I) }, M); + assert_eq!(unsafe { mul_mat4_avx2(I, I) }, I); + assert_eq!(unsafe { mul_mat4_avx2(SCALE, I) }, SCALE); + assert_eq!(unsafe { mul_mat4_avx2(M, I) }, M); } } @@ -616,15 +640,15 @@ mod tests { if std::is_x86_feature_detected!("sse4.1") { unsafe { - assert_eq!(I.mul_vec4_sse41(Vec4::ZERO), Vec4::ZERO); - assert_eq!(I.mul_vec4_sse41(V4), V4); - assert_eq!(SCALE.mul_vec4_sse41(Vec4::ZERO), Vec4::ZERO); + assert_eq!(I.transform_vec4_sse41(Vec4::ZERO), Vec4::ZERO); + assert_eq!(I.transform_vec4_sse41(V4), V4); + assert_eq!(SCALE.transform_vec4_sse41(Vec4::ZERO), Vec4::ZERO); assert_eq!( - SCALE.mul_vec4_sse41(Vec4::ONE), + SCALE.transform_vec4_sse41(Vec4::ONE), Vec4::new(2.0, 2.0, 2.0, 1.0) ); assert_eq!( - TRANSLATE.mul_vec4_sse41(Vec4::new(0.0, 0.0, 0.0, 1.0)), + TRANSLATE.transform_vec4_sse41(Vec4::new(0.0, 0.0, 0.0, 1.0)), Vec4::new(1.0, 2.0, 3.0, 1.0) ); } diff --git a/narcissus-maths/src/quat.rs b/narcissus-maths/src/quat.rs index df374dc..43ec65d 100644 --- a/narcissus-maths/src/quat.rs +++ b/narcissus-maths/src/quat.rs @@ -1,3 +1,5 @@ +use crate::{sin_cos_pi_f32, HalfTurn, Vec3}; + #[derive(Clone, Copy, PartialEq, Debug)] #[repr(C)] pub struct Quat { @@ -28,4 +30,77 @@ impl Quat { c: std::f32::NAN, d: std::f32::NAN, }; + + /// Create a new quaternion from its components. + #[inline(always)] + pub fn new(a: f32, b: f32, c: f32, d: f32) -> Self { + Self { a, b, c, d } + } + + /// Returns a quaternion representing a `rotation` in half turns around the given `axis`. + pub fn from_axis_rotation(axis: Vec3, rotation: HalfTurn) -> Self { + let (s, c) = sin_cos_pi_f32(rotation.as_f32() * 0.5); + let v = axis * s; + Self { + a: v.x, + b: v.y, + c: v.z, + d: c, + } + } + + /// Rotates `rhs` by `self`. + pub fn transform_vec3(self, rhs: Vec3) -> Vec3 { + let d = self.d; + let v = Vec3::new(self.a, self.b, self.c); + rhs * (d * d - Vec3::dot(v, v)) + + v * Vec3::dot(rhs, v) * 2.0 + + Vec3::cross(v, rhs) * d * 2.0 + } +} + +impl std::ops::Mul for Quat { + type Output = Vec3; + + #[inline(always)] + fn mul(self, rhs: Vec3) -> Self::Output { + self.transform_vec3(rhs) + } +} + +#[cfg(test)] +mod tests { + use super::{HalfTurn, Quat, Vec3}; + + #[test] + fn constructors() { + assert_eq!( + Quat::from_axis_rotation(Vec3::X, HalfTurn(1.0)), + Quat::new(1.0, 0.0, 0.0, 0.0) + ); + assert_eq!( + Quat::from_axis_rotation(Vec3::Y, HalfTurn(1.0)), + Quat::new(0.0, 1.0, 0.0, 0.0) + ); + assert_eq!( + Quat::from_axis_rotation(Vec3::Z, HalfTurn(1.0)), + Quat::new(0.0, 0.0, 1.0, 0.0) + ); + } + + #[test] + fn multiplication() { + let rot_180_x = Quat::from_axis_rotation(Vec3::X, HalfTurn::new(1.0)); + assert_eq!(rot_180_x * Vec3::X, Vec3::X); + assert_eq!(rot_180_x * Vec3::Y, -Vec3::Y); + assert_eq!(rot_180_x * Vec3::Z, -Vec3::Z); + let rot_180_y = Quat::from_axis_rotation(Vec3::Y, HalfTurn::new(1.0)); + assert_eq!(rot_180_y * Vec3::X, -Vec3::X); + assert_eq!(rot_180_y * Vec3::Y, Vec3::Y); + assert_eq!(rot_180_y * Vec3::Z, -Vec3::Z); + let rot_180_z = Quat::from_axis_rotation(Vec3::Z, HalfTurn::new(1.0)); + assert_eq!(rot_180_z * Vec3::X, -Vec3::X); + assert_eq!(rot_180_z * Vec3::Y, -Vec3::Y); + assert_eq!(rot_180_z * Vec3::Z, Vec3::Z); + } } diff --git a/narcissus-maths/src/vec2.rs b/narcissus-maths/src/vec2.rs index ad91d1c..e0b9430 100644 --- a/narcissus-maths/src/vec2.rs +++ b/narcissus-maths/src/vec2.rs @@ -52,6 +52,7 @@ impl Vec2 { /// Returns the dot product of `a` and `b`. #[inline] + #[must_use] pub fn dot(a: Self, b: Self) -> f32 { a.x * b.x + a.y * b.y } diff --git a/narcissus-maths/src/vec3.rs b/narcissus-maths/src/vec3.rs index 488b479..5bb096b 100644 --- a/narcissus-maths/src/vec3.rs +++ b/narcissus-maths/src/vec3.rs @@ -56,12 +56,14 @@ impl Vec3 { /// Returns the dot product of `a` and `b`. #[inline] + #[must_use] pub fn dot(a: Vec3, b: Vec3) -> f32 { a.x * b.x + a.y * b.y + a.z * b.z } /// Returns the cross product of `a` and `b`. #[inline] + #[must_use] pub fn cross(a: Vec3, b: Vec3) -> Vec3 { [ a.y * b.z - a.z * b.y, diff --git a/narcissus-maths/src/vec4.rs b/narcissus-maths/src/vec4.rs index a72d4e5..c3d7a03 100644 --- a/narcissus-maths/src/vec4.rs +++ b/narcissus-maths/src/vec4.rs @@ -54,6 +54,7 @@ impl Vec4 { /// Returns the dot product of `a` and `b`. #[inline] + #[must_use] pub fn dot(a: Vec4, b: Vec4) -> f32 { a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w }