4 files changed, 157 insertions, 0 deletions
diff --git a/src/core/matrix4x4.rs b/src/core/matrix4x4.rs
new file mode 100644
index 0000000..521d7ab
--- /dev/null
+++ b/src/core/matrix4x4.rs
@@ -0,0 +1,35 @@
+use crate::{Number, Float};
+
+pub struct Matrix4x4<T: Number> {
+    pub m: [[T; 4]; 4],
+}
+
+pub type Matrix4x4f = Matrix4x4<Float>;
+
+impl<T: Number> Matrix4x4<T> {
+    pub fn new(v00: T, v01: T, v02: T, v03: T,
+               v10: T, v11: T, v12: T, v13: T,
+               v20: T, v21: T, v22: T, v23: T,
+               v30: T, v31: T, v32: T, v33: T) -> Self {
+        Self {
+            m: [
+                [v00, v01, v02, v03],
+                [v10, v11, v12, v13],
+                [v20, v21, v22, v23],
+                [v30, v31, v32, v33],
+            ],
+        }
+    }
+}
+
+impl Matrix4x4f {
+    pub fn new_ident(v: Float) -> Self {
+        let z = 0.0;
+        Self::new(
+            v, z, z, z,
+            z, v, z, z,
+            z, z, v, z,
+            z, z, z, v,
+            )
+    }
+}
diff --git a/src/core/mod.rs b/src/core/mod.rs
index 938a16e..7885b9e 100644
--- a/src/core/mod.rs
+++ b/src/core/mod.rs
@@ -2,6 +2,8 @@ pub mod vector2;
 pub mod vector3;
 pub mod bound;
 pub mod spectrum;
+pub mod transform;
+pub mod matrix4x4;
 mod ray;
 
 pub use vector2::{Vector2i, Vector2f};
diff --git a/src/core/transform.rs b/src/core/transform.rs
new file mode 100644
index 0000000..2c2c123
--- /dev/null
+++ b/src/core/transform.rs
@@ -0,0 +1,111 @@
+use super::matrix4x4::Matrix4x4f;
+use crate::Float;
+use crate::core::Vector3f;
+use std::ops;
+
+pub struct Transform {
+    m: Matrix4x4f,
+}
+
+impl Transform {
+    pub fn new() -> Transform {
+        Transform {
+            m: Matrix4x4f::new_ident(1.0),
+        }
+    }
+
+    pub fn eval_point(&self, p: &Vector3f) -> Vector3f {
+        let m = &self.m.m;
+        let x = m[0][0]*p.x + m[0][1]*p.y + m[0][2]*p.z + m[0][3];
+        let y = m[1][0]*p.x + m[1][1]*p.y + m[1][2]*p.z + m[1][3];
+        let z = m[2][0]*p.x + m[2][1]*p.y + m[2][2]*p.z + m[2][3];
+        let w = m[3][0]*p.x + m[3][1]*p.y + m[3][2]*p.z + m[3][3];
+
+        let mut out = Vector3f::new_xyz(x, y, z);
+        if w != 1.0 {
+            out /= w;
+        }
+
+        out
+    }
+
+    // Take care when transforming surface normal vector, TODO implement method for this
+    pub fn eval_vector(&self, v: &Vector3f) -> Vector3f {
+        let m = &self.m.m;
+        let x = m[0][0]*v.x + m[0][1]*v.y + m[0][2]*v.z;
+        let y = m[1][0]*v.x + m[1][1]*v.y + m[1][2]*v.z;
+        let z = m[2][0]*v.x + m[2][1]*v.y + m[2][2]*v.z;
+
+        Vector3f::new_xyz(x, y, z)
+    }
+}
+
+// Creation of different transformations
+impl Transform {
+    pub fn new_translate(delta: &Vector3f) -> Self {
+        Transform { m: Matrix4x4f::new(
+                1.0, 0.0, 0.0, delta.x,
+                0.0, 1.0, 0.0, delta.y,
+                0.0, 0.0, 1.0, delta.z,
+                0.0, 0.0, 0.0, 1.0)
+        }
+    }
+
+    pub fn new_scale(x: Float, y: Float, z: Float) -> Self {
+        Transform { m: Matrix4x4f::new(
+                x, 0.0, 0.0, 0.0,
+                0.0, y, 0.0, 0.0,
+                0.0, 0.0, z, 0.0,
+                0.0, 0.0, 0.0, 1.0)
+        }
+    }
+
+    pub fn new_rotate_x(theta: Float) -> Self {
+        let theta = theta.to_radians();
+        let cost = theta.cos();
+        let sint = theta.sin();
+        Transform { m: Matrix4x4f::new(
+                1.0, 0.0, 0.0, 0.0,
+                0.0, cost, -sint, 0.0,
+                0.0, sint, cost, 0.0,
+                0.0, 0.0, 0.0, 1.0)
+        }
+    }
+
+    pub fn new_rotate_y(theta: Float) -> Self {
+        let theta = theta.to_radians();
+        let cost = theta.cos();
+        let sint = theta.sin();
+        Transform { m: Matrix4x4f::new(
+                cost, 0.0, sint, 0.0,
+                0.0, 1.0, 0.0, 0.0,
+                -sint, 0.0, cost, 0.0,
+                0.0, 0.0, 0.0, 1.0)
+        }
+    }
+
+    pub fn new_rotate_z(theta: Float) -> Self {
+        let theta = theta.to_radians();
+        let cost = theta.cos();
+        let sint = theta.sin();
+        Transform { m: Matrix4x4f::new(
+                cost, -sint, 0.0, 0.0,
+                sint, cost, 0.0, 0.0,
+                0.0, 0.0, 1.1, 0.0,
+                0.0, 0.0, 0.0, 1.0)
+        }
+    }
+
+    pub fn new_look_at(pos: &Vector3f, look: &Vector3f, up: &Vector3f) -> Self {
+        let dir = (*look - *pos).norm();  // This is what the z axis should map to
+        let right = up.norm().cross(&dir).norm();
+        let newup = dir.cross(&right);
+
+        Transform { m: Matrix4x4f::new(
+                right.x, newup.x, dir.x, pos.x,
+                right.y, newup.y, dir.y, pos.y,
+                right.z, newup.z, dir.z, pos.z,
+                0.0    , 0.0    , 0.0  , 1.0)
+        }
+    }
+}
diff --git a/src/core/vector3.rs b/src/core/vector3.rs
index e3aa9a6..05bd977 100644
--- a/src/core/vector3.rs
+++ b/src/core/vector3.rs
@@ -81,4 +81,13 @@ impl Vector3f {
         new.norm_in();
         new
     }
+
+    pub fn cross(&self, op: &Self) -> Self {
+        Self::new_xyz(
+            self.y * op.z - self.z * op.y,
+            self.z * op.x - self.x * op.z,
+            self.x * op.y - self.y * op.x,
+            )
+
+    }
 }