Add sin and cos to Float

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<T: Float> {
+    matrix: Mat4<T>,
+}
+
+impl<T: Float> Affine3<T> {
+    pub fn translation(delta: Vec3<T>) -> 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<T>, 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<T> {
+        self.matrix.get_row(row)
+    }
+
+    pub fn get_column(&self, column: usize) -> Vec4<T> {
+        self.matrix.get_column(column)
+    }
+
+    pub fn linear_map(&self) -> Mat3<T> {
+        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<T> {
+        // 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<T: Float> Mul<Affine3<T>> for Affine3<T> {
+    type Output = Self;
+
+    fn mul(self, rhs: Affine3<T>) -> Affine3<T> {
+        let matrix = self.matrix * rhs.matrix;
+        Affine3 { matrix }
+    }
+}
+
+impl<T: Float> Mul<Mat4<T>> for Affine3<T> {
+    type Output = Mat4<T>;
+
+    fn mul(self, rhs: Mat4<T>) -> Mat4<T> {
+        self.matrix * rhs
+    }
+}
+
+impl<T: Float> Mul<Affine3<T>> for Mat4<T> {
+    type Output = Mat4<T>;
+
+    fn mul(self, rhs: Affine3<T>) -> Mat4<T> {
+        self * rhs.matrix
+    }
+}
+
+impl<T: Float> MulAssign<Affine3<T>> for Affine3<T> {
+    fn mul_assign(&mut self, rhs: Affine3<T>) {
+        self.matrix *= rhs.matrix
+    }
+}
+
+impl<T: Float> Mul<Vec4<T>> for Affine3<T> {
+    type Output = Vec4<T>;
+
+    fn mul(self, rhs: Vec4<T>) -> Vec4<T> {
+        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));
+            }
+        }
+    }
+}

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()
+    }
 }