From 84ad551026450ef5e330b13f0257dfb27a7ca20d Mon Sep 17 00:00:00 2001 From: Matthew Gordon Date: Sat, 29 Mar 2025 15:36:29 -0300 Subject: [PATCH] Add sin and cos to Float --- src/math/affine3.rs | 160 ++++++++++++++++++++++++++++++++++++++++++++ src/math/number.rs | 11 +++ 2 files changed, 171 insertions(+) create mode 100644 src/math/affine3.rs diff --git a/src/math/affine3.rs b/src/math/affine3.rs new file mode 100644 index 0000000..5e4276c --- /dev/null +++ b/src/math/affine3.rs @@ -0,0 +1,160 @@ +use super::{Float, Mat3, Mat4, Vec3, Vec4}; +use std::ops::{Mul, MulAssign}; + +#[derive(PartialEq, Debug)] +pub struct Affine3 { + matrix: Mat4, +} + +impl Affine3 { + pub fn translation(delta: Vec3) -> Self { + #[rustfmt::skip] + let matrix = Mat4::new(T::one(), T::zero(), T::zero(), delta.x(), + T::zero(), T::one() , T::zero(), delta.y(), + T::zero(), T::zero(), T::one(), delta.z(), + T::zero(), T::zero(), T::zero(), T::one()); + Self { matrix } + } + + pub fn rotation(axis: Vec3, angle: T) -> Self { + let x = axis.x(); + let y = axis.y(); + let z = axis.z(); + let cos = angle.cos(); + let ncos = T::one() - cos; + let sin = angle.sin(); + #[rustfmt::skip] + let matrix = Mat4::new(x*x*ncos+cos, y*x*ncos-z*sin, z*x*ncos+y*sin, T::zero(), + x*y*ncos+z*sin, y*y*ncos+cos, z*y*ncos-x*sin, T::zero(), + x*z*ncos-y*sin, y*z*ncos+x*sin, z*z*ncos+cos, T::zero(), + T::zero(), T::zero(), T::zero(), T::one()); + Self { matrix } + } + + pub fn scale(s: T) -> Self { + #[rustfmt::skip] + let matrix = Mat4::new(s, T::zero(), T::zero(), T::zero(), + T::zero(), s , T::zero(), T::zero(), + T::zero(), T::zero(), s, T::zero(), + T::zero(), T::zero(), T::zero(), T::one()); + Self { matrix } + } + + pub fn get_element(&self, row: usize, column: usize) -> T { + self.matrix.get_element(row, column) + } + + pub fn get_row(&self, row: usize) -> Vec4 { + self.matrix.get_row(row) + } + + pub fn get_column(&self, column: usize) -> Vec4 { + self.matrix.get_column(column) + } + + pub fn linear_map(&self) -> Mat3 { + Mat3::new( + self.matrix.get_element(0, 0), + self.matrix.get_element(0, 1), + self.matrix.get_element(0, 2), + self.matrix.get_element(1, 0), + self.matrix.get_element(1, 1), + self.matrix.get_element(1, 2), + self.matrix.get_element(2, 0), + self.matrix.get_element(2, 1), + self.matrix.get_element(2, 2), + ) + } + + pub fn inverse(&self) -> Affine3 { + // linear map should always be invertable. + let inner = self.linear_map().try_inverse().unwrap(); + let translation = inner * self.matrix.get_column(3).xyz(); + #[rustfmt::skip] + let matrix = Mat4::new( + inner.get_element(0,0), inner.get_element(0,1), inner.get_element(0,2), translation.x(), + inner.get_element(1,0), inner.get_element(1,1), inner.get_element(1,2), translation.y(), + inner.get_element(2,0), inner.get_element(2,1), inner.get_element(2,2), translation.z(), + T::zero(), T::zero(), T::zero(), T::one()); + Self { matrix } + } +} + +impl Mul> for Affine3 { + type Output = Self; + + fn mul(self, rhs: Affine3) -> Affine3 { + let matrix = self.matrix * rhs.matrix; + Affine3 { matrix } + } +} + +impl Mul> for Affine3 { + type Output = Mat4; + + fn mul(self, rhs: Mat4) -> Mat4 { + self.matrix * rhs + } +} + +impl Mul> for Mat4 { + type Output = Mat4; + + fn mul(self, rhs: Affine3) -> Mat4 { + self * rhs.matrix + } +} + +impl MulAssign> for Affine3 { + fn mul_assign(&mut self, rhs: Affine3) { + self.matrix *= rhs.matrix + } +} + +impl Mul> for Affine3 { + type Output = Vec4; + + fn mul(self, rhs: Vec4) -> Vec4 { + self.matrix * rhs + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[test] + fn translate_translates_vector() { + let p = Vec4::new(1.0, 2.0, 3.0, 1.0); + let v = Vec3::new(4.0, 5.0, 6.0); + let target = Affine3::translation(v); + let diff = (target * p).xyz() - (p.xyz() + v); + assert!(diff.norm() < 0.0000000001); + } + + #[test] + fn rotate_rotates_vector() { + let x = Vec4::new(1.0, 0.0, 0.0, 1.0); + let y = Vec4::new(0.0, 1.0, 0.0, 1.0); + let z = Vec4::new(0.0, 0.0, 1.0, 1.0); + let target = Affine3::rotation(z.xyz(), std::f64::consts::PI/2.0) * y; + let diff = -x.xyz() - target.xyz(); + assert!(diff.norm() < 0.0000000001); + } + + #[test] + fn linear_map_is_inner_matrix() { + #[rustfmt::skip] + let target = Affine3{ + matrix: Mat4::new(1.0, 2.0, 3.0, 4.0, + 5.0, 6.0, 7.0, 8.0, + 9.0, 10.0, 11.0, 12.0, + 0.0, 0.0, 0.0, 1.0)}; + let linear_map = target.linear_map(); + for i in 0..2 { + for j in 0..2 { + assert!(linear_map.get_element(i, j) == target.get_element(i, j)); + } + } + } +} diff --git a/src/math/number.rs b/src/math/number.rs index ba8b707..a7637b9 100644 --- a/src/math/number.rs +++ b/src/math/number.rs @@ -37,6 +37,8 @@ pub trait Float: { fn abs(self) -> Self; fn sqrt(self) -> Self; + fn sin(self) -> Self; + fn cos(self) -> Self; } impl HasZero for f64 { @@ -55,7 +57,16 @@ impl Float for f64 { fn abs(self) -> Self { self.abs() } + fn sqrt(self) -> Self { self.sqrt() } + + fn sin(self) -> Self { + self.sin() + } + + fn cos(self) -> Self { + self.cos() + } }