diff --git a/engine/inc/math/m4x4.hpp b/engine/inc/math/m4x4.hpp
index a009c14..a003eb0 100644
--- a/engine/inc/math/m4x4.hpp
+++ b/engine/inc/math/m4x4.hpp
@@ -20,39 +20,28 @@ class M4x4 {
  public:
   MATRIX data alignas(sizeof(float) * 16);
 
-  static M4x4 Identity;
+  static const M4x4 Identity;
+
+  /** Create matrix with random values */
+  M4x4() {}
 
-  /**
-   * Create matrix with random values
-   */
-  explicit M4x4();
   M4x4(const M4x4& v) { copy(this, v); }
+
   explicit M4x4(const float* v) { copy(this, v); }
-  inline M4x4(const float& m11, const float& m12, const float& m13,
-              const float& m14, const float& m21, const float& m22,
-              const float& m23, const float& m24, const float& m31,
-              const float& m32, const float& m33, const float& m34,
-              const float& m41, const float& m42, const float& m43,
-              const float& m44) {
-    set(m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42,
-        m43, m44);
-  }
+
+  M4x4(const float& m11, const float& m12, const float& m13, const float& m14,
+       const float& m21, const float& m22, const float& m23, const float& m24,
+       const float& m31, const float& m32, const float& m33, const float& m34,
+       const float& m41, const float& m42, const float& m43, const float& m44);
 
   static void copy(M4x4* out, const float* in);
-  static void copy(M4x4* out, const M4x4& in) { copy(out, in.data); }
+  static void copy(M4x4* out, const M4x4& in);
 
   void operator=(const M4x4& v);
   Vec4 operator*(const Vec4& v) const;
-
-  M4x4 operator*(const M4x4& v) const {
-    M4x4 result;
-    cross(result.data, this->data, v.data);
-    return result;
-  }
-
-  void operator*=(const M4x4& v) { cross(this->data, this->data, v.data); }
-
-  float& operator[](const u8& index) { return data[index]; }
+  M4x4 operator*(const M4x4& v) const;
+  void operator*=(const M4x4& v);
+  float& operator[](const u8& index);
 
   /**
    * 1 0 0 0
@@ -62,13 +51,7 @@ class M4x4 {
    */
   void identity();
 
-  /**
-   * 1 0 0 0
-   * 0 1 0 0
-   * 0 0 1 0
-   * 0 0 0 1
-   */
-  void unit() { identity(); }
+  inline void unit() { identity(); }
 
   void set(const float& m11, const float& m12, const float& m13,
            const float& m14, const float& m21, const float& m22,
@@ -77,86 +60,22 @@ class M4x4 {
            const float& m41, const float& m42, const float& m43,
            const float& m44);
 
-  void translate(const Vec4& v) {
-    M4x4 temp = M4x4::Identity;
-    temp.translationX(v.x);
-    temp.translationY(v.y);
-    temp.translationZ(v.z);
-    cross(this->data, temp.data, this->data);
-  }
+  void translate(const Vec4& v);
+  void translateX(const float& v);
+  void translateY(const float& v);
+  void translateZ(const float& v);
 
-  void translateX(const float& v) {
-    M4x4 temp = M4x4::Identity;
-    temp.translationX(v);
-    cross(this->data, temp.data, this->data);
-  }
+  void rotateByAngle(const float& angle, const Vec4& axis);
+  void rotate(const Vec4& v);
+  void rotateX(const float& radians);
+  void rotateY(const float& radians);
+  void rotateZ(const float& radians);
 
-  void translateY(const float& v) {
-    M4x4 temp = M4x4::Identity;
-    temp.translationY(v);
-    cross(this->data, temp.data, this->data);
-  }
-
-  void translateZ(const float& v) {
-    M4x4 temp = M4x4::Identity;
-    temp.translationZ(v);
-    cross(this->data, temp.data, this->data);
-  }
-
-  /** Rotate M4x4. */
-  void rotate(const Vec4& v) {
-    M4x4 temp = M4x4::Identity;
-
-    temp.rotationZ(v.z);
-    cross(this->data, temp.data, this->data);
-
-    temp.identity();
-    temp.rotationY(v.y);
-    cross(this->data, temp.data, this->data);
-
-    temp.identity();
-    temp.rotationX(v.x);
-    cross(this->data, temp.data, this->data);
-  }
-
-  /** Rotate M4x4 by X. */
-  void rotateX(const float& radians) {
-    M4x4 temp = M4x4::Identity;
-    temp.rotationX(radians);
-    cross(this->data, temp.data, this->data);
-  }
-
-  /** Rotate M4x4 by Y. */
-  void rotateY(const float& radians) {
-    M4x4 temp = M4x4::Identity;
-    temp.rotationY(radians);
-    cross(this->data, temp.data, this->data);
-  }
-
-  /** Rotate M4x4 by Z. */
-  void rotateZ(const float& radians) {
-    M4x4 temp = M4x4::Identity;
-    temp.rotationZ(radians);
-    cross(this->data, temp.data, this->data);
-  }
-
-  /** Rotate M4x4 by angle. */
-  void rotateByAngle(const float& angle, const Vec4& axis) {
-    M4x4 temp;
-    temp.rotationByAngle(angle, axis);
-    cross(this->data, temp.data, this->data);
-  }
-
-  void scale(const float& v) { scale(Vec4(v, v, v, 1.0F)); }
-  void scaleX(const float& v) { scale(Vec4(v, 1.0F, 1.0F, 1.0F)); }
-  void scaleY(const float& v) { scale(Vec4(1.0F, v, 1.0F, 1.0F)); }
-  void scaleZ(const float& v) { scale(Vec4(1.0F, 1.0F, v, 1.0F)); }
-
-  void scale(const Vec4& v) {
-    M4x4 temp = M4x4::Identity;
-    temp.setScale(v);
-    cross(this->data, temp.data, this->data);
-  }
+  void scale(const float& v);
+  void scale(const Vec4& v);
+  void scaleX(const float& v);
+  void scaleY(const float& v);
+  void scaleZ(const float& v);
 
   /** Create perspective projection M4x4 (gluPerspective) */
   static M4x4 perspective(const float& fov, const float& width,
diff --git a/engine/inc/math/math.hpp b/engine/inc/math/math.hpp
index dc0ade6..dbf07d6 100644
--- a/engine/inc/math/math.hpp
+++ b/engine/inc/math/math.hpp
@@ -26,9 +26,11 @@ class Math {
   constexpr static float HALF_PI = 1.5707963267948966192313216916398F;
   static float cos(float x);
   static float asin(float x);
+  static float atan2(float y, float x);
   static float mod(float x, float y);
-  static inline float sin(float x) { return cos(x - HALF_PI); }
-  static inline float tan(float x) { return sin(x) / cos(x); }
+  static float acos(float x);
+  static float sin(float x);
+  static float tan(float x);
   static float invSqrt(float x);
   static bool equalf(const float& a, const float& b,
                      const float& epsilon = 0.00001F);
diff --git a/engine/inc/math/plane.hpp b/engine/inc/math/plane.hpp
index 68a57c3..3657ded 100644
--- a/engine/inc/math/plane.hpp
+++ b/engine/inc/math/plane.hpp
@@ -25,9 +25,8 @@ class Plane {
   ~Plane();
 
   void update(const Vec4& a, const Vec4& b, const Vec4& c);
-  inline float distanceTo(const Vec4& t_vec) const {
-    return this->distance + this->normal.innerProduct(t_vec);
-  }
+
+  float distanceTo(const Vec4& t_vec) const;
 
   void print() const;
   void print(const char* name) const;
diff --git a/engine/inc/math/vec4.hpp b/engine/inc/math/vec4.hpp
index 673d280..9cc643d 100644
--- a/engine/inc/math/vec4.hpp
+++ b/engine/inc/math/vec4.hpp
@@ -63,6 +63,9 @@ class Vec4 {
   void operator*=(const Vec4& v);
   void operator/=(const float& v);
 
+  /** (0,0,0,1) */
+  void unit();
+
   inline void set(const Vec4& v) { copy(this, v.xyzw); }
   inline void set(const float* v) { copy(this, v); }
   void set(const float& x, const float& y = 0.0F, const float& z = 0.0F,
@@ -94,13 +97,7 @@ class Vec4 {
    * @param max Opposite max vertex (ex. far, right, up vertex of bounding
    * box)
    */
-  u8 collidesBox(const Vec4& min, const Vec4& max) const {
-    return ((this->x <= max.x && this->x >= min.x) &&
-            (this->y < max.y && this->y >= min.y) &&
-            (this->z <= max.z && this->z >= min.z))
-               ? 1
-               : 0;
-  }
+  bool collidesBox(const Vec4& min, const Vec4& max) const;
 
   /**
    * Checks if this vector is on given box (this->y >= box.y)
@@ -109,12 +106,7 @@ class Vec4 {
    * @param max Opposite max vertex (ex. far, right, up vertex of bounding
    * box)
    */
-  u8 isOnBox(const Vec4& min, const Vec4& max) const {
-    return ((this->x <= max.x && this->x >= min.x) && (this->y >= max.y) &&
-            (this->z <= max.z && this->z >= min.z))
-               ? 1
-               : 0;
-  }
+  bool isOnBox(const Vec4& min, const Vec4& max) const;
 
   /** Check if given triangle should be backface culled */
   static u8 shouldBeBackfaceCulled(const Vec4* cameraPos, const Vec4* v0,
diff --git a/engine/src/math/m4x4.cpp b/engine/src/math/m4x4.cpp
index 4b8fb6a..32dccca 100644
--- a/engine/src/math/m4x4.cpp
+++ b/engine/src/math/m4x4.cpp
@@ -17,10 +17,20 @@ namespace Tyra {
 
 VECTOR M4x4::upVec = {0.0F, 1.0F, 0.0F, 1.0F};
 VECTOR M4x4::viewVec = {0.0F, 0.0F, 0.0F, 1.0F};
-M4x4 M4x4::Identity = M4x4(1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F,
-                           0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F);
 
-M4x4::M4x4() {}
+const M4x4 M4x4::Identity =
+    M4x4(1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F, 0.0F, 0.0F, 0.0F, 1.0F, 0.0F,
+         0.0F, 0.0F, 0.0F, 1.0F);
+
+M4x4::M4x4(const float& m11, const float& m12, const float& m13,
+           const float& m14, const float& m21, const float& m22,
+           const float& m23, const float& m24, const float& m31,
+           const float& m32, const float& m33, const float& m34,
+           const float& m41, const float& m42, const float& m43,
+           const float& m44) {
+  set(m11, m12, m13, m14, m21, m22, m23, m24, m31, m32, m33, m34, m41, m42, m43,
+      m44);
+}
 
 void M4x4::copy(M4x4* out, const float* in) {
   asm volatile(
@@ -36,6 +46,8 @@ void M4x4::copy(M4x4* out, const float* in) {
       : "r"(out->data), "r"(in));
 }
 
+void M4x4::copy(M4x4* out, const M4x4& in) { copy(out, in.data); }
+
 void M4x4::operator=(const M4x4& v) { copy(this, v); }
 
 Vec4 M4x4::operator*(const Vec4& v) const {
@@ -56,6 +68,16 @@ Vec4 M4x4::operator*(const Vec4& v) const {
   return result;
 }
 
+M4x4 M4x4::operator*(const M4x4& v) const {
+  M4x4 result;
+  cross(result.data, this->data, v.data);
+  return result;
+}
+
+void M4x4::operator*=(const M4x4& v) { cross(this->data, this->data, v.data); }
+
+float& M4x4::operator[](const u8& index) { return data[index]; }
+
 void M4x4::set(const float& m11, const float& m12, const float& m13,
                const float& m14, const float& m21, const float& m22,
                const float& m23, const float& m24, const float& m31,
@@ -98,6 +120,85 @@ void M4x4::identity() {
       : "r"(this->data));
 }
 
+void M4x4::translate(const Vec4& v) {
+  M4x4 temp = M4x4::Identity;
+  temp.translationX(v.x);
+  temp.translationY(v.y);
+  temp.translationZ(v.z);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::translateX(const float& v) {
+  M4x4 temp = M4x4::Identity;
+  temp.translationX(v);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::translateY(const float& v) {
+  M4x4 temp = M4x4::Identity;
+  temp.translationY(v);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::translateZ(const float& v) {
+  M4x4 temp = M4x4::Identity;
+  temp.translationZ(v);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::rotate(const Vec4& v) {
+  M4x4 temp = M4x4::Identity;
+
+  temp.rotationZ(v.z);
+  cross(this->data, temp.data, this->data);
+
+  temp.identity();
+  temp.rotationY(v.y);
+  cross(this->data, temp.data, this->data);
+
+  temp.identity();
+  temp.rotationX(v.x);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::rotateX(const float& radians) {
+  M4x4 temp = M4x4::Identity;
+  temp.rotationX(radians);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::rotateY(const float& radians) {
+  M4x4 temp = M4x4::Identity;
+  temp.rotationY(radians);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::rotateZ(const float& radians) {
+  M4x4 temp = M4x4::Identity;
+  temp.rotationZ(radians);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::rotateByAngle(const float& angle, const Vec4& axis) {
+  M4x4 temp;
+  temp.rotationByAngle(angle, axis);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::scale(const float& v) { scale(Vec4(v, v, v, 1.0F)); }
+
+void M4x4::scale(const Vec4& v) {
+  M4x4 temp = M4x4::Identity;
+  temp.setScale(v);
+  cross(this->data, temp.data, this->data);
+}
+
+void M4x4::scaleX(const float& v) { scale(Vec4(v, 1.0F, 1.0F, 1.0F)); }
+
+void M4x4::scaleY(const float& v) { scale(Vec4(1.0F, v, 1.0F, 1.0F)); }
+
+void M4x4::scaleZ(const float& v) { scale(Vec4(1.0F, 1.0F, v, 1.0F)); }
+
 M4x4 M4x4::perspective(const float& fov, const float& width,
                        const float& height, const float& projectionScale,
                        const float& aspectRatio, const float& near,
diff --git a/engine/src/math/math.cpp b/engine/src/math/math.cpp
index ad7fc61..24e704b 100644
--- a/engine/src/math/math.cpp
+++ b/engine/src/math/math.cpp
@@ -14,6 +14,22 @@
 
 #include "math/math.hpp"
 
+extern volatile const u32 TYRA_MATH_ATAN_TABLE[9] alignas(sizeof(float)) = {
+    0x3f7ffff5, 0xbeaaa61c, 0x3e4c40a6, 0xbe0e6c63, 0x3dc577df,
+    0xbd6501c4, 0x3cb31652, 0xbb84d7e7, 0x3f490fdb,
+};
+
+extern volatile const float TYRA_MATH_ATAN_TABLE2[8] = {
+    0.0f,
+    (Tyra::Math::PI / 2.0f),
+    (Tyra::Math::PI / 2.0f),
+    (Tyra::Math::PI),
+    (-Tyra::Math::PI),
+    (-Tyra::Math::PI / 2.0f),
+    (-Tyra::Math::PI / 2.0f),
+    0.0f,
+};
+
 namespace Tyra {
 
 float Math::cos(float x) {
@@ -77,6 +93,98 @@ float Math::cos(float x) {
 
 float Math::invSqrt(float x) { return 1.0F / sqrt(x); }
 
+float Math::atan2(float y, float x) {
+  float r;
+  asm volatile(
+      "mtc1		$0, %0      \n\t"
+      "abs.s		$f1, %1   \n\t"
+      "abs.s		$f2, %2   \n\t"
+      "c.lt.s		%2, %0    \n\t"
+      "move		$9, $0      \n\t"
+      "bc1f		_atan_00    \n\t"
+      "addiu		$9, $9, 4   \n\t"
+      "_atan_00:            \n\t"
+      "c.eq.s		%2, %0      \n\t"
+      "bc1f		_atan_00_1    \n\t"
+      "c.eq.s		%1, %2      \n\t"
+      "bc1t		_atan_06      \n\t"
+      "c.lt.s		%1, %0      \n\t"
+      "bc1f		_atan_00_1    \n\t"
+      "addiu		$9, $0, 3	    \n\t"
+      "_atan_00_1:		    \n\t"
+      "mul.s		%1, %1, %2    \n\t"
+      "c.lt.s		%1, %0	    \n\t"
+      "bc1f		_atan_01    \n\t"
+      "addiu		$9, $9, 2			    \n\t"
+      "c.lt.s		$f2, $f1	    \n\t"
+      "bc1f		_atan_02    \n\t"
+      "addiu		$9, $9, 1	    \n\t"
+      "b			_atan_02	    \n\t"
+      "_atan_01:			    \n\t"
+      "c.lt.s		$f1, $f2    \n\t"
+      "bc1f		_atan_02    \n\t"
+      "addiu		$9, $9, 1	    \n\t"
+      "_atan_02:			    \n\t"
+      "c.lt.s		$f1, $f2    \n\t"
+      "bc1f		_atan_03    \n\t"
+      "mov.s		%1, $f2    \n\t"
+      "mov.s		%2, $f1    \n\t"
+      "b			_atan_04    \n\t"
+      "_atan_03:    \n\t"
+      "mov.s		%1, $f1    \n\t"
+      "mov.s		%2, $f2    \n\t"
+      "_atan_04:    \n\t"
+      "mfc1		$6, %1    \n\t"
+      "mfc1		$7, %2    \n\t"
+      "la			$8, TYRA_MATH_ATAN_TABLE    \n\t"
+      "lqc2		$vf4, 0x0($8)    \n\t"
+      "lqc2		$vf5, 0x10($8)    \n\t"
+      "lqc2		$vf6, 0x20($8)    \n\t"
+      "qmtc2		$6, $vf21    \n\t"
+      "qmtc2		$7, $vf22    \n\t"
+      "vadd.x		$vf23, $vf21, $vf22    \n\t"
+      "vsub.x		$vf22, $vf22, $vf21    \n\t"
+      "vdiv     $Q, $vf22x, $vf23x    \n\t"
+      "vwaitq    \n\t"
+      "vaddq.x  $vf21, $vf0, $Q		    \n\t"
+      "vmul.x		$vf22, $vf21, $vf21    \n\t"
+      "vmulax.x	$ACC, $vf21, $vf4	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmadday.x	$ACC, $vf21, $vf4	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmaddaz.x	$ACC, $vf21, $vf4	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmaddaw.x	$ACC, $vf21, $vf4	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmaddax.x	$ACC, $vf21, $vf5	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmadday.x	$ACC, $vf21, $vf5	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmaddaz.x	$ACC, $vf21, $vf5	    \n\t"
+      "vmul.x		$vf21, $vf21, $vf22    \n\t"
+      "vmaddaw.x	$ACC, $vf21, $vf5	    \n\t"
+      "vmaddw.x	$vf21, $vf6, $vf0    \n\t"
+      "qmfc2		$6, $vf21    \n\t"
+      "mtc1		$6, %0    \n\t"
+      "andi		$8, $9, 1    \n\t"
+      "sll			$9, $9, 2    \n\t"
+      "la			$7, TYRA_MATH_ATAN_TABLE2    \n\t"
+      "add			$9, $9, $7    \n\t"
+      "lw			$7, 0x0($9)    \n\t"
+      "mtc1		$7, %1    \n\t"
+      "beq			$8, $0, _atan_05    \n\t"
+      "sub.s		%0, %1, %0    \n\t"
+      "b			_atan_06    \n\t"
+      "_atan_05:    \n\t"
+      "add.s		%0, %1, %0    \n\t"
+      "_atan_06:    \n\t"
+      : "=&f"(r)
+      : "f"(x), "f"(y)
+      : "$6", "$7", "$8", "$9", "$f0", "$f1", "$f2");
+
+  return r;
+}
+
 float Math::asin(float x) {
   float r;
   asm volatile(
@@ -162,4 +270,14 @@ float Math::mod(float x, float y) {
   return f;
 }
 
+float Math::acos(float x) {
+  float y = sqrt(1.0f - x * x);
+  float t = atan2(y, x);
+  return t;
+}
+
+float Math::sin(float x) { return cos(x - HALF_PI); }
+
+float Math::tan(float x) { return sin(x) / cos(x); }
+
 }  // namespace Tyra
diff --git a/engine/src/math/plane.cpp b/engine/src/math/plane.cpp
index d63ac20..feb386d 100644
--- a/engine/src/math/plane.cpp
+++ b/engine/src/math/plane.cpp
@@ -14,27 +14,19 @@
 #include "math/plane.hpp"
 
 namespace Tyra {
+
 Plane::Plane() { this->distance = 0; }
 
-/** Create by specyfying 3 points.
- * This function assumes that the points
- * are given in counter clockwise order
- */
 Plane::Plane(const Vec4& a, const Vec4& b, const Vec4& c) {
   this->update(a, b, c);
 }
 
 Plane::~Plane() {}
 
-// ----
-// Methods
-// ----
+float Plane::distanceTo(const Vec4& t_vec) const {
+  return this->distance + this->normal.innerProduct(t_vec);
+}
 
-/**
- * Set plane by specyfying 3 points.
- * This function assumes that the points
- * are given in counter clockwise order
- */
 void Plane::update(const Vec4& a, const Vec4& b, const Vec4& c) {
   Vec4 aux1 = a - b;
   Vec4 aux2 = c - b;
diff --git a/engine/src/math/vec4.cpp b/engine/src/math/vec4.cpp
index 8d7d9ef..aeacd0e 100644
--- a/engine/src/math/vec4.cpp
+++ b/engine/src/math/vec4.cpp
@@ -69,7 +69,7 @@ Vec4 Vec4::operator*(const float& v) const {
       "sqc2       $vf6,   0x0(%0)         \n\t"
       :
       : "r"(res.xyzw), "r"(this->xyzw), "f"(v));
-  res.w = w;  // Comment below. TODO: fix it in asm
+  res.w = w;
   return res;
 }
 
@@ -124,6 +124,10 @@ void Vec4::operator=(const Vec4& v) { copy(this, v); }
 
 Vec4 Vec4::operator-(void) const { return Vec4(-x, -y, -z); }
 
+void Vec4::unit() {
+  asm volatile("sqc2		$vf0, 0x00(%0)	\n" : : "r"(this->xyzw));
+}
+
 void Vec4::copy(Vec4* out, const float* in) {
   asm volatile(
       "lqc2		$vf1, 0x00(%1)	\n"
@@ -160,6 +164,21 @@ int Vec4::getRelativeAngleBetween(const Vec4& v) const {
   return acos(getRelativeCosBetween(v));
 }
 
+bool Vec4::collidesBox(const Vec4& min, const Vec4& max) const {
+  return ((this->x <= max.x && this->x >= min.x) &&
+          (this->y < max.y && this->y >= min.y) &&
+          (this->z <= max.z && this->z >= min.z))
+             ? 1
+             : 0;
+}
+
+bool Vec4::isOnBox(const Vec4& min, const Vec4& max) const {
+  return ((this->x <= max.x && this->x >= min.x) && (this->y >= max.y) &&
+          (this->z <= max.z && this->z >= min.z))
+             ? 1
+             : 0;
+}
+
 float Vec4::innerProduct(const Vec4& v) const {
   float result;
   asm volatile(