From b935d4da135e195b06b57051407f9e5706c3e03b Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Wed, 23 Dec 2020 20:24:08 +0100
Subject: [PATCH 1/9] fixed newline warning

---
 src/engine/include/models/bounding_box.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/engine/include/models/bounding_box.hpp b/src/engine/include/models/bounding_box.hpp
index cab4a9b..a2f6c44 100644
--- a/src/engine/include/models/bounding_box.hpp
+++ b/src/engine/include/models/bounding_box.hpp
@@ -91,4 +91,4 @@ private:
     BoundingBoxFace _bottomFace;
 };
 
-#endif
\ No newline at end of file
+#endif

From f95e805d338cd5a50b32f228777b2f9b018d43a4 Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Wed, 23 Dec 2020 20:28:28 +0100
Subject: [PATCH 2/9] added data alignment

---
 src/engine/include/models/math/matrix.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/engine/include/models/math/matrix.hpp b/src/engine/include/models/math/matrix.hpp
index 5e13bd7..03f229e 100644
--- a/src/engine/include/models/math/matrix.hpp
+++ b/src/engine/include/models/math/matrix.hpp
@@ -19,7 +19,7 @@ class Matrix
 {
 
 public:
-    float data[16];
+    float data[16] __attribute__((aligned(64)));
 
     Matrix(float m11, float m12, float m13, float m14,
            float m21, float m22, float m23, float m24,

From e0ad58f72f9f3fc3a116a877c7c4d34b3cef8d1a Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Wed, 23 Dec 2020 20:28:41 +0100
Subject: [PATCH 3/9] fixed bug with matrix multiply

---
 src/engine/models/math/matrix.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/engine/models/math/matrix.cpp b/src/engine/models/math/matrix.cpp
index ec927a1..b0e9e2f 100644
--- a/src/engine/models/math/matrix.cpp
+++ b/src/engine/models/math/matrix.cpp
@@ -160,7 +160,7 @@ Matrix Matrix::operator*(const Matrix &t)
         "sqc2         vf3, 0x20(%0) \n\t"
         "sqc2         vf4, 0x30(%0) \n\t"
         :
-        : "r"(result.data), "r"(this->data), "r"(t.data)
+        : "r"(result.data), "r"(t.data), "r"(this->data)
         : "memory");
     return result;
 }
@@ -197,7 +197,7 @@ void Matrix::operator*=(const Matrix &t)
         "sqc2         vf3, 0x20(%0) \n\t"
         "sqc2         vf4, 0x30(%0) \n\t"
         :
-        : "r"(this->data), "r"(this->data), "r"(t.data)
+        : "r"(this->data), "r"(t.data), "r"(this->data)
         : "memory");
 }
 

From 4b2deb8b3999aa82c9dfec1d740378919b539de7 Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Wed, 23 Dec 2020 22:10:12 +0100
Subject: [PATCH 4/9] matrix refactor

---
 src/engine/include/models/math/matrix.hpp | 128 +++++--
 src/engine/models/math/matrix.cpp         | 413 ++++++++++++----------
 2 files changed, 318 insertions(+), 223 deletions(-)

diff --git a/src/engine/include/models/math/matrix.hpp b/src/engine/include/models/math/matrix.hpp
index 03f229e..6d97b7d 100644
--- a/src/engine/include/models/math/matrix.hpp
+++ b/src/engine/include/models/math/matrix.hpp
@@ -13,79 +13,139 @@
 
 #include "vector3.hpp"
 #include "../screen_settings.hpp"
+#include <string.h>
 
-/** https://en.wikipedia.org/wiki/Matrix_(mathematics) */
+/** 4x4 Matrix class. */
 class Matrix
 {
 
 public:
     float data[16] __attribute__((aligned(64)));
 
-    Matrix(float m11, float m12, float m13, float m14,
-           float m21, float m22, float m23, float m24,
-           float m31, float m32, float m33, float m34,
-           float m41, float m42, float m43, float m44);
-    Matrix(const Matrix &v);
-    Matrix operator*(const Matrix &v);
-    void operator*=(const Matrix &v);
-    Matrix();
-    ~Matrix();
+    // ----
+    // Constructors/Destructors
+    // ----
 
-    void lookAt(Vector3 &t_up, Vector3 &t_position, Vector3 &t_target);
+    /** Create empty matrix. */
+    Matrix();
+
+    /** 
+     * Create identity/empty matrix.
+     * @param t_do_identity Identity matrix if true, random matrix values otherwise
+     */
+    Matrix(const u8 &t_do_identity)
+    {
+        if (t_do_identity)
+            identity();
+    }
+
+    /** Init matrix with given values. */
+    Matrix(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);
+
+    /** Init matrix with other matrix values. */
+    Matrix(const Matrix &v) { memcpy(data, v.data, 16 * sizeof(float)); }
+
+    // ----
+    // Operators
+    // ----
+
+    Matrix operator*(const Matrix &v)
+    {
+        Matrix result;
+        cross(result.data, this->data, v.data);
+        return result;
+    }
+
+    inline void operator*=(const Matrix &v) { cross(this->data, this->data, v.data); }
+
+    float &operator[](const u8 &t_index) { return data[t_index]; }
+
+    // ----
+    // Functions
+    // ----
+
+    /** 
+     * 1 0 0 0
+     * 0 1 0 0
+     * 0 0 1 0
+     * 0 0 0 1
+     */
     void identity();
+
+    /** 
+     * 1 0 0 0
+     * 0 1 0 0
+     * 0 0 1 0
+     * 0 0 0 1
+     */
     inline void unit() { identity(); };
 
-    // Translation
-
-    void translate(const Vector3 &t_val);
-
+    /** Create translation matrix. */
     inline void translation(const Vector3 &t_val)
     {
         identity();
         translate(t_val);
     }
 
-    // Rotation
-
-    void rotateX(const float &t_radians);
-    void rotateY(const float &t_radians);
-    void rotateZ(const float &t_radians);
-
+    /** Create X rotation matrix. */
     inline void rotationX(const float &t_radians)
     {
         identity();
         rotateX(t_radians);
     }
 
+    /** Create Y rotation matrix. */
     inline void rotationY(const float &t_radians)
     {
         identity();
         rotateY(t_radians);
     }
 
+    /** Create Z rotation matrix. */
     inline void rotationZ(const float &t_radians)
     {
         identity();
         rotateZ(t_radians);
     }
 
-    inline void rotate(const Vector3 &t_val)
-    {
-        rotateX(t_val.x);
-        rotateY(t_val.y);
-        rotateX(t_val.z);
-    }
+    /** Create angle rotation matrix. */
+    void rotationByAngle(const float &t_angle, const Vector3 &t_axis);
 
-    inline void rotation(const Vector3 &t_val)
-    {
-        identity();
-        rotate(t_val);
-    }
+    /** Create scale matrix. */
+    void setScale(const Vector3 &t_val);
 
-    //
+    /** 
+     * Create perspective projection matrix. 
+     * 
+     * Complies with OpenGL standard (gluPerspective). 
+     * @see https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluPerspective.xml
+     * @param screen Screen settings
+     */
+    void setPerspective(const ScreenSettings &screen);
 
-    void setPerspective(ScreenSettings &screen);
+    /** 
+     * Create a view matrix that transforms coordinates in 
+     * such a way that the user looks at a target vector 
+     * direction from a position vector. 
+     * 
+     * Complies with OpenGL standard (glLookAt).  
+     * @see https://learnopengl.com/Getting-started/Camera
+     */
+    void lookAt(const Vector3 &t_position, const Vector3 &t_target);
+
+    /** Print matrix (printf). */
     const void print() const;
+
+private:
+    void rotateX(const float &t_radians);
+    void rotateY(const float &t_radians);
+    void rotateZ(const float &t_radians);
+    void translate(const Vector3 &t_val);
+    void cross(float res[16], const float a[16], const float b[16]);
+    void setCamera(const float t_pos[4], const float t_vz[4], const float t_vy[4]);
 };
 
 #endif
diff --git a/src/engine/models/math/matrix.cpp b/src/engine/models/math/matrix.cpp
index b0e9e2f..71032b7 100644
--- a/src/engine/models/math/matrix.cpp
+++ b/src/engine/models/math/matrix.cpp
@@ -17,11 +17,16 @@
 // Constructors/Destructors
 // ----
 
-/** Create by specifying all points */
-Matrix::Matrix(float m11, float m12, float m13, float m14,
-               float m21, float m22, float m23, float m24,
-               float m31, float m32, float m33, float m34,
-               float m41, float m42, float m43, float m44)
+Matrix::Matrix()
+{
+    for (u8 i = 0; i < 16; i++)
+        data[i] = 0.0F;
+}
+
+Matrix::Matrix(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)
 {
     data[0] = m11;
     data[1] = m12;
@@ -44,29 +49,13 @@ Matrix::Matrix(float m11, float m12, float m13, float m14,
     data[15] = m44;
 }
 
-/** Create with another matrix values */
-Matrix::Matrix(const Matrix &v)
-{
-    data[0] = v.data[0];
-    data[1] = v.data[1];
-    data[2] = v.data[2];
-    data[3] = v.data[3];
+// ----
+// Operators
+// ----
 
-    data[4] = v.data[4];
-    data[5] = v.data[5];
-    data[6] = v.data[6];
-    data[7] = v.data[7];
-
-    data[8] = v.data[8];
-    data[9] = v.data[9];
-    data[10] = v.data[10];
-    data[11] = v.data[11];
-
-    data[12] = v.data[12];
-    data[13] = v.data[13];
-    data[14] = v.data[14];
-    data[15] = v.data[15];
-}
+// ----
+// Functions
+// ----
 
 void Matrix::identity()
 {
@@ -84,13 +73,145 @@ void Matrix::identity()
         : "r"(this->data));
 }
 
-void Matrix::translate(const Vector3 &t_val)
+void Matrix::rotationByAngle(const float &t_angle, const Vector3 &t_axis)
 {
-    this->data[12] += t_val.x; // 3,0
-    this->data[13] += t_val.y; // 3,1
-    this->data[14] += t_val.z; // 3,2
+    Vector3 localAxis = Vector3(t_axis);
+    localAxis.normalize();
+    float x = localAxis.x;
+    float y = localAxis.y;
+    float z = localAxis.z;
+
+    float c = Math::cos(t_angle);
+    float s = Math::sin(t_angle);
+
+    this->data[0] = x * x * (1 - c) + c;
+    this->data[1] = y * x * (1 - c) + z * s;
+    this->data[2] = x * z * (1 - c) - y * s;
+    this->data[3] = 0.0F;
+
+    this->data[4] = x * y * (1 - c) - z * s;
+    this->data[5] = y * y * (1 - c) + c;
+    this->data[6] = y * z * (1 - c) + x * s;
+    this->data[7] = 0.0F;
+
+    this->data[8] = x * z * (1 - c) + y * s;
+    this->data[9] = y * z * (1 - c) - x * s;
+    this->data[10] = z * z * (1 - c) + c;
+    this->data[11] = 0.0F;
+
+    this->data[12] = 0.0F;
+    this->data[13] = 0.0F;
+    this->data[14] = 0.0F;
+    this->data[15] = 1.0F;
 }
 
+void Matrix::setScale(const Vector3 &t_val)
+{
+    this->identity();
+    this->data[0] = t_val.x;
+    this->data[5] = t_val.y;
+    this->data[10] = t_val.z;
+    this->data[15] = 1.0F;
+}
+
+void Matrix::setPerspective(const ScreenSettings &t_screen)
+{
+    float fovYdiv2 = Math::HALF_ANG2RAD * t_screen.fov;
+    float cotFOV = 1.0F / (Math::sin(fovYdiv2) / Math::cos(fovYdiv2));
+    float w = cotFOV * (t_screen.width / t_screen.projectionScale) / t_screen.aspectRatio;
+    float h = cotFOV * (t_screen.height / t_screen.projectionScale);
+
+    this->data[0] = w;
+    this->data[1] = 0.0F;
+    this->data[2] = 0.0F;
+    this->data[3] = 0.0F;
+
+    this->data[4] = 0.0F;
+    this->data[5] = -h;
+    this->data[6] = 0.0F;
+    this->data[7] = 0.0F;
+
+    this->data[8] = 0.0F;
+    this->data[9] = 0.0F;
+    this->data[10] =
+        (t_screen.farPlaneDist + t_screen.nearPlaneDist) /
+        (t_screen.farPlaneDist - t_screen.nearPlaneDist);
+    this->data[11] = -1.0F;
+
+    this->data[12] = 0.0F;
+    this->data[13] = 0.0F;
+    this->data[14] =
+        (2.0F * t_screen.farPlaneDist * t_screen.nearPlaneDist) /
+        (t_screen.farPlaneDist - t_screen.nearPlaneDist);
+    this->data[15] = 0.0F;
+}
+
+void Matrix::lookAt(const Vector3 &t_position, const Vector3 &t_target)
+{
+    VECTOR up_vec, view_vec;
+    VECTOR eye = {t_position.x, t_position.y, t_position.z, 1.0F};
+    VECTOR obj = {t_target.x, t_target.y, t_target.z, 1.0F};
+    asm volatile(
+        "lqc2           vf4, 0x00(%2)	# eye                               \n\t"
+        "lqc2		    vf5, 0x00(%3)	# obj                               \n\t"
+        "vsub.xyz	    vf7, vf4, vf5	# view_vec = vf7                    \n\t"
+        "vmove.xyzw	    vf6, vf0                                            \n\t"
+        "vaddw.y		vf6, vf0, vf0	# vf6 = { 0.0f, 1.0f, 0.0f, 1.0f }  \n\t"
+        "vopmula.xyz	ACC, vf6, vf7                                       \n\t"
+        "vopmsub.xyz	vf9, vf7, vf6	# vec = vf9                         \n\t"
+        "vopmula.xyz	ACC, vf7, vf9                                       \n\t"
+        "vopmsub.xyz	vf8, vf9, vf7	# up_vec = vf8                      \n\t"
+        "sqc2		    vf7, 0x00(%0)	# view_vec                          \n\t"
+        "sqc2		    vf6, 0x00(%1)	# up_vec                            \n\t"
+        :
+        : "r"(view_vec), "r"(up_vec), "r"(eye), "r"(obj));
+    Matrix temp;
+    temp.setCamera(eye, view_vec, up_vec);
+    identity();
+    cross(this->data, this->data, temp.data);
+
+    // Vector3 camForward, camUp, camRight;
+    // Vector3 t_up = Vector3(0.0F, 1.0F, 0.0F);
+    // camForward = t_position - t_target;
+    // camForward.normalize();
+    // camRight = t_up * camForward;
+    // camRight.normalize();
+    // camUp = camForward * camRight;
+
+    // data[0] = camRight.x;
+    // data[4] = camRight.y;
+    // data[8] = camRight.z;
+    // data[12] = -camRight.innerProduct(t_position);
+
+    // data[1] = camUp.x;
+    // data[5] = camUp.y;
+    // data[9] = camUp.z;
+    // data[13] = -camUp.innerProduct(t_position);
+
+    // data[2] = camForward.x;
+    // data[6] = camForward.y;
+    // data[10] = camForward.z;
+    // data[14] = -camForward.innerProduct(t_position);
+
+    // data[3] = 0;
+    // data[7] = 0;
+    // data[11] = 0;
+    // data[15] = 1;
+}
+
+const void Matrix::print() const
+{
+    printf("MATRIX(\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n)\n",
+           data[0], data[1], data[2], data[3],
+           data[4], data[5], data[6], data[7],
+           data[8], data[9], data[10], data[11],
+           data[12], data[13], data[14], data[15]);
+}
+
+// ----
+// Private
+// ----
+
 void Matrix::rotateX(const float &t_radians)
 {
     Matrix temp = Matrix();
@@ -127,45 +248,80 @@ void Matrix::rotateZ(const float &t_radians)
     this->data[5] = c;  // 1,1
 }
 
-Matrix Matrix::operator*(const Matrix &t)
+void Matrix::translate(const Vector3 &t_val)
 {
-    Matrix result;
-    asm volatile(
-        "lqc2         vf1, 0x00(%1) \n\t"
-        "lqc2         vf2, 0x10(%1) \n\t"
-        "lqc2         vf3, 0x20(%1) \n\t"
-        "lqc2         vf4, 0x30(%1) \n\t"
-        "lqc2         vf5, 0x00(%2) \n\t"
-        "lqc2         vf6, 0x10(%2) \n\t"
-        "lqc2         vf7, 0x20(%2) \n\t"
-        "lqc2         vf8, 0x30(%2) \n\t"
-        "vmulax.xyzw  ACC, vf5, vf1 \n\t"
-        "vmadday.xyzw ACC, vf6, vf1 \n\t"
-        "vmaddaz.xyzw ACC, vf7, vf1 \n\t"
-        "vmaddw.xyzw  vf1, vf8, vf1 \n\t"
-        "vmulax.xyzw  ACC, vf5, vf2 \n\t"
-        "vmadday.xyzw ACC, vf6, vf2 \n\t"
-        "vmaddaz.xyzw ACC, vf7, vf2 \n\t"
-        "vmaddw.xyzw  vf2, vf8, vf2 \n\t"
-        "vmulax.xyzw  ACC, vf5, vf3 \n\t"
-        "vmadday.xyzw ACC, vf6, vf3 \n\t"
-        "vmaddaz.xyzw ACC, vf7, vf3 \n\t"
-        "vmaddw.xyzw  vf3, vf8, vf3 \n\t"
-        "vmulax.xyzw  ACC, vf5, vf4 \n\t"
-        "vmadday.xyzw ACC, vf6, vf4 \n\t"
-        "vmaddaz.xyzw ACC, vf7, vf4 \n\t"
-        "vmaddw.xyzw  vf4, vf8, vf4 \n\t"
-        "sqc2         vf1, 0x00(%0) \n\t"
-        "sqc2         vf2, 0x10(%0) \n\t"
-        "sqc2         vf3, 0x20(%0) \n\t"
-        "sqc2         vf4, 0x30(%0) \n\t"
-        :
-        : "r"(result.data), "r"(t.data), "r"(this->data)
-        : "memory");
-    return result;
+    this->data[12] += t_val.x; // 3,0
+    this->data[13] += t_val.y; // 3,1
+    this->data[14] += t_val.z; // 3,2
 }
 
-void Matrix::operator*=(const Matrix &t)
+void Matrix::setCamera(const float t_pos[4], const float t_vz[4], const float t_vy[4])
+{
+    //	Matrix  vf4, vf5, vf6, vf7
+    //	t_pos   vf8
+    //	t_vz    vf9
+    //	t_vy    vf10
+    //	vtmp    vf11
+    asm volatile(
+        "lqc2		    vf9, 0x00(%2)       \n\t"
+        "lqc2		    vf10, 0x00(%3)      \n\t"
+        // mtmp.unit()
+        "vsub.w		    vf5, vf0, vf0   # mtmp[1][PW] = 0.0F \n\t"
+        // vtmp.outerProduct(vy, vz);
+        "vopmula.xyz	ACC, vf10, vf9      \n\t"
+        "vopmsub.xyz	vf11, vf9, vf10     \n\t"
+        // mtmp[0] = vtmp.normalize();
+        "vmul.xyz	    vf12, vf11, vf11    \n\t"
+        "vaddy.x		vf12, vf12, vf12    \n\t"
+        "vaddz.x		vf12, vf12, vf12    \n\t"
+        "vrsqrt		    Q, vf0w, vf12x      \n\t"
+        "vsub.xyzw	    vf4, vf0, vf0       \n\t"
+        "vwaitq                             \n\t"
+        "vmulq.xyz      vf4, vf11, Q        \n\t"
+        // mtmp[2] = vz.normalize();
+        "vmul.xyz       vf12, vf9, vf9      \n\t"
+        "vaddy.x		vf12, vf12, vf12    \n\t"
+        "vaddz.x		vf12, vf12, vf12    \n\t"
+        "vrsqrt	        Q, vf0w, vf12x      \n\t"
+        "vsub.xyzw      vf6, vf0, vf0       \n\t"
+        "vwaitq                             \n\t"
+        "vmulq.xyz      vf6, vf9, Q         \n\t"
+        // mtmp[1].outerProduct(mtmp[2], mtmp[0]);
+        "vopmula.xyz	ACC, vf6, vf4       \n\t"
+        "vopmsub.xyz	vf5, vf4, vf6       \n\t"
+        // mtmp.transpose(pos);
+        "lqc2		    vf7, 0x00(%1)       \n\t"
+        // m = mtmp.inverse();
+        "qmfc2.ni		$11, vf0            \n\t"
+        "qmfc2.ni		$8, vf4             \n\t"
+        "qmfc2.ni		$9, vf5             \n\t"
+        "qmfc2.ni		$10, vf6            \n\t"
+
+        "pextlw			$12, $9, $8 \n\t"
+        "pextuw			$13, $9, $8 \n\t"
+        "pextlw			$14, $11, $10 \n\t"
+        "pextuw			$15, $11, $10 \n\t"
+        "pcpyld			$8, $14, $12 \n\t"
+        "pcpyud			$9, $12, $14 \n\t"
+        "pcpyld			$10, $15, $13 \n\t"
+
+        "qmtc2.ni		$8, vf16 \n\t"
+        "qmtc2.ni		$9, vf17 \n\t"
+        "qmtc2.ni		$10, vf18 \n\t"
+        "vmulax.xyz		ACC, vf16, vf7 \n\t"
+        "vmadday.xyz	ACC, vf17, vf7 \n\t"
+        "vmaddz.xyz		vf5, vf18, vf7 \n\t"
+        "vsub.xyzw		vf5, vf0, vf5 \n\t"
+
+        "sq				$8, 0x00(%0) \n\t"
+        "sq				$9, 0x10(%0) \n\t"
+        "sq				$10, 0x20(%0) \n\t"
+        "sqc2			vf5, 0x30(%0) \n\t"
+        :
+        : "r"(this->data), "r"(t_pos), "r"(t_vz), "r"(t_vy));
+}
+
+void Matrix::cross(float res[16], const float a[16], const float b[16])
 {
     asm volatile(
         "lqc2         vf1, 0x00(%1) \n\t"
@@ -197,127 +353,6 @@ void Matrix::operator*=(const Matrix &t)
         "sqc2         vf3, 0x20(%0) \n\t"
         "sqc2         vf4, 0x30(%0) \n\t"
         :
-        : "r"(this->data), "r"(t.data), "r"(this->data)
+        : "r"(res), "r"(b), "r"(a)
         : "memory");
 }
-
-/** Create empty matrix */
-Matrix::Matrix()
-{
-    data[0] = 0.0F;
-    data[1] = 0.0F;
-    data[2] = 0.0F;
-    data[3] = 0.0F;
-
-    data[4] = 0.0F;
-    data[5] = 0.0F;
-    data[6] = 0.0F;
-    data[7] = 0.0F;
-
-    data[8] = 0.0F;
-    data[9] = 0.0F;
-    data[10] = 0.0F;
-    data[11] = 0.0F;
-
-    data[12] = 0.0F;
-    data[13] = 0.0F;
-    data[14] = 0.0F;
-    data[15] = 0.0F;
-}
-
-Matrix::~Matrix() {}
-
-// ----
-// Methods
-// ----
-
-/** Set up a perspective projection matrix
- * 
- * Clone of gluPerspective()
- * https://www.khronos.org/registry/OpenGL-Refpages/gl2.1/xhtml/gluPerspective.xml
- * @param fov (FOV in radians)/2
- * @param aspect Aspect ratio
- * @param scrW Half of screen width
- * @param scrH Half of screen height
- * @param zNear Distance to near plane
- * @param zFar Distance to far plane
- * @param projScale Projection scale
- */
-void Matrix::setPerspective(ScreenSettings &t_screen)
-{
-    float fovYdiv2 = Math::HALF_ANG2RAD * t_screen.fov;
-    float cotFOV = 1.0F / (Math::sin(fovYdiv2) / Math::cos(fovYdiv2));
-    float w = cotFOV * (t_screen.width / t_screen.projectionScale) / t_screen.aspectRatio;
-    float h = cotFOV * (t_screen.height / t_screen.projectionScale);
-
-    this->data[0] = w;
-    this->data[1] = 0.0F;
-    this->data[2] = 0.0F;
-    this->data[3] = 0.0F;
-
-    this->data[4] = 0.0F;
-    this->data[5] = -h;
-    this->data[6] = 0.0F;
-    this->data[7] = 0.0F;
-
-    this->data[8] = 0.0F;
-    this->data[9] = 0.0F;
-    this->data[10] =
-        (t_screen.farPlaneDist + t_screen.nearPlaneDist) /
-        (t_screen.farPlaneDist - t_screen.nearPlaneDist);
-    this->data[11] = -1.0F;
-
-    this->data[12] = 0.0F;
-    this->data[13] = 0.0F;
-    this->data[14] =
-        (2.0F * t_screen.farPlaneDist * t_screen.nearPlaneDist) /
-        (t_screen.farPlaneDist - t_screen.nearPlaneDist);
-    this->data[15] = 0.0F;
-}
-
-/** Create a view matrix that transforms coordinates in
- * such a way that the user looks at a target vector
- * direction from a position vector.
- * 
- * Clone of OpenGL lookAt function
- * https://learnopengl.com/Getting-started/Camera
-*/
-void Matrix::lookAt(Vector3 &t_up, Vector3 &t_position, Vector3 &t_target)
-{
-    Vector3 camForward, camUp, camRight;
-
-    camForward = t_position - t_target;
-    camForward.normalize();
-    camRight = t_up * camForward;
-    camRight.normalize();
-    camUp = camForward * camRight;
-
-    data[0] = camRight.x;
-    data[4] = camRight.y;
-    data[8] = camRight.z;
-    data[12] = -camRight.innerProduct(t_position);
-
-    data[1] = camUp.x;
-    data[5] = camUp.y;
-    data[9] = camUp.z;
-    data[13] = -camUp.innerProduct(t_position);
-
-    data[2] = camForward.x;
-    data[6] = camForward.y;
-    data[10] = camForward.z;
-    data[14] = -camForward.innerProduct(t_position);
-
-    data[3] = 0;
-    data[7] = 0;
-    data[11] = 0;
-    data[15] = 1;
-}
-
-const void Matrix::print() const
-{
-    printf("MATRIX(\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n%f, %f, %f, %f\n)\n",
-           data[0], data[1], data[2], data[3],
-           data[4], data[5], data[6], data[7],
-           data[8], data[9], data[10], data[11],
-           data[12], data[13], data[14], data[15]);
-}

From 7dd89476126431948953c981b84ddf778398bc50 Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Wed, 23 Dec 2020 22:38:57 +0100
Subject: [PATCH 5/9] added vec3 multiply

---
 src/engine/include/models/math/matrix.hpp | 13 +++++++++++--
 src/engine/models/math/matrix.cpp         | 20 +++++++++++++++++++-
 2 files changed, 30 insertions(+), 3 deletions(-)

diff --git a/src/engine/include/models/math/matrix.hpp b/src/engine/include/models/math/matrix.hpp
index 6d97b7d..b30f6d1 100644
--- a/src/engine/include/models/math/matrix.hpp
+++ b/src/engine/include/models/math/matrix.hpp
@@ -52,7 +52,16 @@ public:
     // Operators
     // ----
 
-    Matrix operator*(const Matrix &v)
+    Matrix operator*(const Matrix &v) const
+    {
+        Matrix result;
+        cross(result.data, this->data, v.data);
+        return result;
+    }
+
+    Vector3 operator*(const Vector3 &v) const;
+
+    Matrix operator*(const Matrix &v) const
     {
         Matrix result;
         cross(result.data, this->data, v.data);
@@ -144,7 +153,7 @@ private:
     void rotateY(const float &t_radians);
     void rotateZ(const float &t_radians);
     void translate(const Vector3 &t_val);
-    void cross(float res[16], const float a[16], const float b[16]);
+    void cross(float res[16], const float a[16], const float b[16]) const;
     void setCamera(const float t_pos[4], const float t_vz[4], const float t_vy[4]);
 };
 
diff --git a/src/engine/models/math/matrix.cpp b/src/engine/models/math/matrix.cpp
index 71032b7..1f4ff64 100644
--- a/src/engine/models/math/matrix.cpp
+++ b/src/engine/models/math/matrix.cpp
@@ -53,6 +53,24 @@ Matrix::Matrix(const float &m11, const float &m12, const float &m13, const float
 // Operators
 // ----
 
+Vector3 Matrix::operator*(const Vector3 &v) const
+{
+    Vector3 result;
+    asm volatile(
+        "lqc2         vf4, 0x00(%1)  \n\t"
+        "lqc2         vf5, 0x10(%1) \n\t"
+        "lqc2         vf6, 0x20(%1) \n\t"
+        "lqc2         vf7, 0x30(%1) \n\t"
+        "lqc2         vf8, 0x00(%2)  \n\t"
+        "vmulax.xyzw  ACC, vf4, vf8 \n\t"
+        "vmadday.xyzw ACC, vf5, vf8 \n\t"
+        "vmaddaz.xyzw ACC, vf6, vf8 \n\t"
+        "vmaddw.xyzw  vf9, vf7, vf8 \n\t"
+        "sqc2         vf9, 0x00(%0)  \n\t"
+        :
+        : "r"(result.xyz), "r"(this->data), "r"(v.xyz));
+}
+
 // ----
 // Functions
 // ----
@@ -321,7 +339,7 @@ void Matrix::setCamera(const float t_pos[4], const float t_vz[4], const float t_
         : "r"(this->data), "r"(t_pos), "r"(t_vz), "r"(t_vy));
 }
 
-void Matrix::cross(float res[16], const float a[16], const float b[16])
+void Matrix::cross(float res[16], const float a[16], const float b[16]) const
 {
     asm volatile(
         "lqc2         vf1, 0x00(%1) \n\t"

From 5a932b35ca8d904edf26609acfd15dbb25a56b84 Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Thu, 24 Dec 2020 10:32:51 +0100
Subject: [PATCH 6/9] fixed bug, added full rotation

---
 src/engine/include/models/math/matrix.hpp | 26 +++++++++++++++++------
 src/engine/models/math/matrix.cpp         |  6 ++++--
 2 files changed, 23 insertions(+), 9 deletions(-)

diff --git a/src/engine/include/models/math/matrix.hpp b/src/engine/include/models/math/matrix.hpp
index b30f6d1..d14855b 100644
--- a/src/engine/include/models/math/matrix.hpp
+++ b/src/engine/include/models/math/matrix.hpp
@@ -52,13 +52,6 @@ public:
     // Operators
     // ----
 
-    Matrix operator*(const Matrix &v) const
-    {
-        Matrix result;
-        cross(result.data, this->data, v.data);
-        return result;
-    }
-
     Vector3 operator*(const Vector3 &v) const;
 
     Matrix operator*(const Matrix &v) const
@@ -99,6 +92,25 @@ public:
         translate(t_val);
     }
 
+    /** Create rotation matrix. */
+    inline void rotation(const Vector3 &t_val)
+    {
+        identity();
+        Matrix temp = Matrix();
+
+        temp.identity();
+        temp.rotateX(t_val.x);
+        operator*=(temp);
+
+        temp.identity();
+        temp.rotateY(t_val.y);
+        operator*=(temp);
+
+        temp.identity();
+        temp.rotateZ(t_val.z);
+        operator*=(temp);
+    }
+
     /** Create X rotation matrix. */
     inline void rotationX(const float &t_radians)
     {
diff --git a/src/engine/models/math/matrix.cpp b/src/engine/models/math/matrix.cpp
index 1f4ff64..805c433 100644
--- a/src/engine/models/math/matrix.cpp
+++ b/src/engine/models/math/matrix.cpp
@@ -55,7 +55,8 @@ Matrix::Matrix(const float &m11, const float &m12, const float &m13, const float
 
 Vector3 Matrix::operator*(const Vector3 &v) const
 {
-    Vector3 result;
+    float a[4] = {v.x, v.y, v.z, 1.0F};
+    float res[4];
     asm volatile(
         "lqc2         vf4, 0x00(%1)  \n\t"
         "lqc2         vf5, 0x10(%1) \n\t"
@@ -68,7 +69,8 @@ Vector3 Matrix::operator*(const Vector3 &v) const
         "vmaddw.xyzw  vf9, vf7, vf8 \n\t"
         "sqc2         vf9, 0x00(%0)  \n\t"
         :
-        : "r"(result.xyz), "r"(this->data), "r"(v.xyz));
+        : "r"(res), "r"(this->data), "r"(a));
+    return Vector3(res[0], res[1], res[2]);
 }
 
 // ----

From 8589a298da3a7effebf57d590d6d681bc70a88d1 Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Thu, 24 Dec 2020 10:33:30 +0100
Subject: [PATCH 7/9] added comments, increased performance

---
 src/engine/include/models/math/vector3.hpp |  99 ++++++--
 src/engine/models/math/vector3.cpp         | 281 ++++++++-------------
 2 files changed, 180 insertions(+), 200 deletions(-)

diff --git a/src/engine/include/models/math/vector3.hpp b/src/engine/include/models/math/vector3.hpp
index 641f6b1..53939af 100644
--- a/src/engine/include/models/math/vector3.hpp
+++ b/src/engine/include/models/math/vector3.hpp
@@ -12,6 +12,7 @@
 #define _TYRA_VECTOR3_
 
 #include <tamtypes.h>
+#include <stdio.h>
 
 class Math; // Forward definition
 
@@ -20,6 +21,7 @@ class Vector3
 {
 
 public:
+    /** This trick will allow use to use vec.x and vex[0] */
     union
     {
         struct
@@ -31,32 +33,91 @@ public:
         float xyz[3] __attribute__((__aligned__(16)));
     };
 
-    Vector3(float t_x, float t_y, float t_z);
-    Vector3(const Vector3 &v);
+    // ----
+    // Constructors/Destructors
+    // ----
+
+    /** Create empty vector. */
     Vector3();
-    ~Vector3();
 
-    Vector3 operator+(Vector3 v);
-    Vector3 operator-(const Vector3 &v);
-    Vector3 operator*(Vector3 &v);
-    Vector3 operator*(const float &t);
+    /** Create vector with given vector values. */
+    Vector3(const Vector3 &t_v) { set(t_v); }
+
+    /** Create vector with given values. */
+    Vector3(const float &t_x, const float &t_y, const float &t_z) { set(t_x, t_y, t_z); }
+
+    // ----
+    // Operators
+    // ----
+
+    Vector3 operator+(const Vector3 &v) const;
+    Vector3 operator-(const Vector3 &v) const;
+    /** Also called "cross product" */
+    Vector3 operator*(const Vector3 &v) const;
+    Vector3 operator*(const float &t) const;
+    Vector3 operator/(const float &t) const;
+    Vector3 operator-(void) const { return Vector3(-x, -y, -z); }
+    void operator+=(const Vector3 &t);
     void operator*=(const float &t);
-    Vector3 operator/(float t);
-    Vector3 operator-(void);
 
-    static u8 shouldBeBackfaceCulled(const Vector3 *t_cameraPos, const Vector3 *v0, const Vector3 *v1, const Vector3 *v2);
-    u8 collidesSquare(const Vector3 &t_min, const Vector3 &t_max) const;
-    u8 isOnSquare(const Vector3 &t_min, const Vector3 &t_max) const;
-    float length();
-    void normalize();
-    void setByLerp(const Vector3 &v1, const Vector3 &v2, const float &t_interp, const float &t_scale);
-    float innerProduct(Vector3 &v);
+    // ----
+    // Functions
+    // ----
+
+    /** Set vector values via VU0. */
     void set(const Vector3 &v);
-    void rotate(const Vector3 &v, u8 inversed = false);
+
+    /** Set vector values. */
     void set(const float &t_x, const float &t_y, const float &t_z);
-    void copy(Vector3 &v);
+
+    /** Also called dot3. */
+    float innerProduct(const Vector3 &v) const;
+
+    /** Get vector length */
+    float length() const;
+
+    /** Normalize vector. */
+    void normalize();
+
+    /** Returns distance between two vectors */
     float distanceTo(const Vector3 &v) const;
-    const void print() const;
+
+    /** 
+     * Checks intersection with given box
+     * @param t_min Opposite min vertex (ex. near, left, down vertex of bounding box)
+     * @param t_max Opposite max vertex (ex. far, right, up vertex of bounding box)
+     */
+    u8 collidesBox(const Vector3 &t_min, const Vector3 &t_max) const
+    {
+        return ((this->x <= t_max.x && this->x >= t_min.x) && (this->y < t_max.y && this->y >= t_min.y) && (this->z <= t_max.z && this->z >= t_min.z)) ? 1 : 0;
+    }
+
+    /** 
+     * Checks if this vector is on given box (this->y >= box.y)
+     * @param t_min Opposite min vertex (ex. near, left, down vertex of bounding box)
+     * @param t_max Opposite max vertex (ex. far, right, up vertex of bounding box)
+     */
+    u8 isOnBox(const Vector3 &t_min, const Vector3 &t_max) const
+    {
+        return ((this->x <= t_max.x && this->x >= t_min.x) && (this->y >= t_max.y) && (this->z <= t_max.z && this->z >= t_min.z)) ? 1 : 0;
+    }
+
+    /** Check if given triangle should be backface culled. */
+    static u8 shouldBeBackfaceCulled(const Vector3 *t_cameraPos, const Vector3 *t_v0, const Vector3 *t_v1, const Vector3 *t_v2);
+
+    /** 
+     * Set by linear interpolation between two vertices (animation). 
+     * @param t_v1 Before vertex 
+     * @param t_v2 After vertex 
+     * @param t_interp State of interpolation 
+     * @param t_scale Scale 
+     */
+    void setByLerp(const Vector3 &t_v1, const Vector3 &t_v2, const float &t_interp, const float &t_scale);
+
+    const void print() const
+    {
+        printf("Vector3(%f, %f, %f)\n", x, y, z);
+    }
 };
 
 #endif
diff --git a/src/engine/models/math/vector3.cpp b/src/engine/models/math/vector3.cpp
index 7adafd3..57f4303 100644
--- a/src/engine/models/math/vector3.cpp
+++ b/src/engine/models/math/vector3.cpp
@@ -9,63 +9,24 @@
 */
 
 #include "../../include/models/math/vector3.hpp"
-
 #include "../../include/utils/math.hpp"
-#include <stdio.h>
 
 // ----
 // Constructors/Destructors
 // ----
 
-/** Create by specifying 3 points */
-Vector3::Vector3(float t_x, float t_y, float t_z)
-{
-    x = t_x;
-    y = t_y;
-    z = t_z;
-}
-
-/** Create with another vector values */
-Vector3::Vector3(const Vector3 &another)
-{
-    x = another.x;
-    y = another.y;
-    z = another.z;
-}
-
-void Vector3::setByLerp(const Vector3 &v1, const Vector3 &v2, const float &t_interp, const float &t_scale)
-{
-    asm volatile(
-        "lqc2      vf4, 0x0(%1)  \n\t" // vf4 = v1
-        "lqc2      vf5, 0x0(%2)  \n\t" // vf5 = v2
-        "mfc1      $8,  %3       \n\t" // vf6 = t
-        "qmtc2     $8,  vf6      \n\t" // lerp:
-        "vsub.xyz  vf7, vf5, vf4 \n\t" // vf7 = v2 - v1
-        "vmulx.xyz vf8, vf7, vf6 \n\t" // vf8 = vf7 * t
-        "vadd.xyz  vf9, vf8, vf4 \n\t" // vf9 = vf8 + vf4
-        "sqc2      vf9, 0x0(%0)  \n\t" // v0  = vf9
-        :
-        : "r"(&this->xyz), "r"(&v1.xyz), "r"(&v2.xyz), "f"(t_interp));
-    x *= t_scale;
-    y *= t_scale;
-    z *= t_scale;
-}
-
-/** Create empty vector */
 Vector3::Vector3()
 {
-    x = 0;
-    y = 0;
-    z = 0;
+    x = 0.0F;
+    y = 0.0F;
+    z = 0.0F;
 }
 
-Vector3::~Vector3() {}
-
 // ----
-// Methods
+// Operators
 // ----
 
-Vector3 Vector3::operator+(Vector3 v)
+Vector3 Vector3::operator+(const Vector3 &v) const
 {
     Vector3 result;
     asm volatile( // VU0 Macro program
@@ -78,7 +39,7 @@ Vector3 Vector3::operator+(Vector3 v)
     return result;
 }
 
-Vector3 Vector3::operator-(const Vector3 &v)
+Vector3 Vector3::operator-(const Vector3 &v) const
 {
     Vector3 result;
     asm volatile( // VU0 Macro program
@@ -91,17 +52,7 @@ Vector3 Vector3::operator-(const Vector3 &v)
     return result;
 }
 
-Vector3 Vector3::operator-(void)
-{
-    Vector3 result;
-    result.x = -x;
-    result.y = -y;
-    result.z = -z;
-    return result;
-}
-
-/** Also called "cross product" */
-Vector3 Vector3::operator*(Vector3 &v)
+Vector3 Vector3::operator*(const Vector3 &v) const
 {
     Vector3 res;
     asm volatile(                                     // VU0 Macro program
@@ -119,7 +70,7 @@ Vector3 Vector3::operator*(Vector3 &v)
     return res;
 }
 
-Vector3 Vector3::operator*(const float &t)
+Vector3 Vector3::operator*(const float &t) const
 {
     Vector3 result;
     asm volatile(
@@ -133,6 +84,26 @@ Vector3 Vector3::operator*(const float &t)
     return result;
 }
 
+Vector3 Vector3::operator/(const float &t) const
+{
+    Vector3 result;
+    result.x = x / t;
+    result.y = y / t;
+    result.z = z / t;
+    return result;
+}
+
+void Vector3::operator+=(const Vector3 &t)
+{
+    asm volatile( // VU0 Macro program
+        "lqc2      vf4, 0x0(%0)  \n\t"
+        "lqc2      vf5, 0x0(%1)  \n\t"
+        "vadd.xyz  vf4, vf4, vf5 \n\t"
+        "sqc2      vf4, 0x0(%0)  \n\t"
+        :
+        : "r"(this->xyz), "r"(t.xyz));
+}
+
 void Vector3::operator*=(const float &t)
 {
     asm volatile(
@@ -145,54 +116,46 @@ void Vector3::operator*=(const float &t)
         : "r"(this->xyz), "f"(t));
 }
 
-Vector3 Vector3::operator/(float t)
+// ----
+// Functions
+// ----
+
+void Vector3::set(const Vector3 &v)
 {
-    Vector3 result;
-    result.x = x / t;
-    result.y = y / t;
-    result.z = z / t;
+    asm volatile( // VU0 Macro program
+        "lq $6, 0x0(%1) \n\t"
+        "sq $6, 0x0(%0) \n\t"
+        :
+        : "r"(this->xyz), "r"(v.xyz));
+}
+
+void Vector3::set(const float &t_x, const float &t_y, const float &t_z)
+{
+    x = t_x;
+    y = t_y;
+    z = t_z;
+}
+
+float Vector3::innerProduct(const Vector3 &v) const
+{
+    float result;
+    asm volatile( // VU0 Macro program
+        "lqc2     vf4, 0x0(%1)  \n\t"
+        "lqc2     vf5, 0x0(%2)  \n\t"
+        "vmul.xyz vf6, vf4, vf5 \n\t"
+        "vaddy.x  vf6, vf6, vf6 \n\t"
+        "vaddz.x  vf6, vf6, vf6 \n\t"
+        "qmfc2    $2,  vf6      \n\t"
+        "mtc1     $2,  %0       \n\t"
+        : "=f"(result)
+        : "r"(this->xyz), "r"(v.xyz));
     return result;
+    // return (x * v.x + y * v.y + z * v.z);
 }
 
-u8 Vector3::shouldBeBackfaceCulled(const Vector3 *t_cameraPos, const Vector3 *v0, const Vector3 *v1, const Vector3 *v2)
+float Vector3::length() const
 {
-    register float dot;
-    asm volatile(
-        "lqc2        vf4, 0x0(%1)  \n\t" // vf4 = cameraPos
-        "lqc2        vf5, 0x0(%2)  \n\t" // vf5 = v0
-        "lqc2        vf6, 0x0(%3)  \n\t" // vf6 = v1
-        "lqc2        vf7, 0x0(%4)  \n\t" // vf7 = v2
-        "vsub.xyz    vf8, vf7, vf5 \n\t" // vf8 = vf7(v2) - vf5(v0)
-        "vsub.xyz    vf9, vf6, vf5 \n\t" // vf9 = vf6(v1) - vf5(v0)
-        "vopmula.xyz ACC, vf8, vf9 \n\t" // vf6 = cross(vf8, vf9)
-        "vopmsub.xyz vf6, vf9, vf8 \n\t"
-        "vsub.w      vf6, vf6, vf6 \n\t"
-        "vsub.xyz    vf7, vf5, vf4 \n\t" // vf7 = vf5(v0) - vf4(cameraPos)
-        "vmul.xyz    vf5, vf7, vf6 \n\t" // vf5 = dot(vf7, vf6)
-        "vaddy.x     vf5, vf5, vf5 \n\t"
-        "vaddz.x     vf5, vf5, vf5 \n\t"
-        "qmfc2       $2,  vf5      \n\t" // store result on `dot` variable
-        "mtc1        $2,  %0       \n\t"
-        : "=f"(dot)
-        : "r"(t_cameraPos->xyz), "r"(v0->xyz), "r"(v1->xyz), "r"(v2->xyz));
-    return dot <= 0.0F;
-}
-
-/** Checks intersection with given square */
-u8 Vector3::collidesSquare(const Vector3 &t_min, const Vector3 &t_max) const
-{
-    return ((this->x <= t_max.x && this->x >= t_min.x) && (this->y < t_max.y && this->y >= t_min.y) && (this->z <= t_max.z && this->z >= t_min.z)) ? 1 : 0;
-}
-
-/** Checks is this vector is on given square */
-u8 Vector3::isOnSquare(const Vector3 &t_min, const Vector3 &t_max) const
-{
-    return ((this->x <= t_max.x && this->x >= t_min.x) && (this->y >= t_max.y) && (this->z <= t_max.z && this->z >= t_min.z)) ? 1 : 0;
-}
-
-float Vector3::length()
-{
-    register float result;
+    float result;
     asm volatile( // VU0 Macro program
         "lqc2     vf4, 0x0(%1)  \n\t"
         "vmul.xyz vf5, vf4, vf4 \n\t"
@@ -227,85 +190,6 @@ void Vector3::normalize()
         : "r"(this->xyz));
 }
 
-/** Also called dot3 */
-float Vector3::innerProduct(Vector3 &v)
-{
-    register float result;
-    asm volatile( // VU0 Macro program
-        "lqc2     vf4, 0x0(%1)  \n\t"
-        "lqc2     vf5, 0x0(%2)  \n\t"
-        "vmul.xyz vf6, vf4, vf5 \n\t"
-        "vaddy.x  vf6, vf6, vf6 \n\t"
-        "vaddz.x  vf6, vf6, vf6 \n\t"
-        "qmfc2    $2,  vf6      \n\t"
-        "mtc1     $2,  %0       \n\t"
-        : "=f"(result)
-        : "r"(this->xyz), "r"(v.xyz));
-    return result;
-    // return (x * v.x + y * v.y + z * v.z);
-}
-
-void Vector3::set(const float &t_x, const float &t_y, const float &t_z)
-{
-    x = t_x;
-    y = t_y;
-    z = t_z;
-}
-
-void Vector3::rotate(const Vector3 &v, u8 inversed)
-{
-    VECTOR cameraPos = {x, y, z, 0.0F};
-    VECTOR rotation;
-    if (inversed)
-    {
-        rotation[0] = -v.x;
-        rotation[1] = -v.y;
-        rotation[2] = -v.z;
-    }
-    else
-    {
-        rotation[0] = v.x;
-        rotation[1] = v.y;
-        rotation[2] = v.z;
-    }
-    rotation[3] = 0.0F;
-
-    MATRIX rotationMatrix;
-    matrix_unit(rotationMatrix);
-    matrix_rotate(rotationMatrix, rotationMatrix, rotation);
-    VECTOR result;
-    asm volatile(
-        "lqc2         vf4, 0x0(%1)  \n\t"
-        "lqc2         vf5, 0x10(%1) \n\t"
-        "lqc2         vf6, 0x20(%1) \n\t"
-        "lqc2         vf7, 0x30(%1) \n\t"
-        "lqc2         vf8, 0x0(%2)  \n\t"
-        "vmulax.xyzw  ACC, vf4, vf8 \n\t"
-        "vmadday.xyzw ACC, vf5, vf8 \n\t"
-        "vmaddaz.xyzw ACC, vf6, vf8 \n\t"
-        "vmaddw.xyzw  vf9, vf7, vf8 \n\t"
-        "sqc2         vf9, 0x0(%0)  \n\t"
-        :
-        : "r"(&result), "r"(&rotationMatrix), "r"(&cameraPos));
-    set(Vector3(result[0], result[1], result[2]));
-}
-
-void Vector3::set(const Vector3 &v)
-{
-    this->x = v.x;
-    this->y = v.y;
-    this->z = v.z;
-}
-
-void Vector3::copy(Vector3 &v)
-{
-    asm volatile( // VU0 Macro program
-        "lq $6, 0x0(%1) \n\t"
-        "sq $6, 0x0(%0) \n\t"
-        :
-        : "r"(v.xyz), "r"(this->xyz));
-}
-
 float Vector3::distanceTo(const Vector3 &v) const
 {
     register float result;
@@ -329,7 +213,42 @@ float Vector3::distanceTo(const Vector3 &v) const
     //                   (this->z - v.z) * (this->z - v.z));
 }
 
-const void Vector3::print() const
+u8 Vector3::shouldBeBackfaceCulled(const Vector3 *t_cameraPos, const Vector3 *t_v0, const Vector3 *t_v1, const Vector3 *t_v2)
 {
-    printf("Vector3(%f, %f, %f)\n", x, y, z);
+    register float dot;
+    asm volatile(
+        "lqc2        vf4, 0x0(%1)  \n\t" // vf4 = cameraPos
+        "lqc2        vf5, 0x0(%2)  \n\t" // vf5 = v0
+        "lqc2        vf6, 0x0(%3)  \n\t" // vf6 = v1
+        "lqc2        vf7, 0x0(%4)  \n\t" // vf7 = v2
+        "vsub.xyz    vf8, vf7, vf5 \n\t" // vf8 = vf7(v2) - vf5(v0)
+        "vsub.xyz    vf9, vf6, vf5 \n\t" // vf9 = vf6(v1) - vf5(v0)
+        "vopmula.xyz ACC, vf8, vf9 \n\t" // vf6 = cross(vf8, vf9)
+        "vopmsub.xyz vf6, vf9, vf8 \n\t"
+        "vsub.w      vf6, vf6, vf6 \n\t"
+        "vsub.xyz    vf7, vf5, vf4 \n\t" // vf7 = vf5(v0) - vf4(cameraPos)
+        "vmul.xyz    vf5, vf7, vf6 \n\t" // vf5 = dot(vf7, vf6)
+        "vaddy.x     vf5, vf5, vf5 \n\t"
+        "vaddz.x     vf5, vf5, vf5 \n\t"
+        "qmfc2       $2,  vf5      \n\t" // store result on `dot` variable
+        "mtc1        $2,  %0       \n\t"
+        : "=f"(dot)
+        : "r"(t_cameraPos->xyz), "r"(t_v0->xyz), "r"(t_v1->xyz), "r"(t_v2->xyz));
+    return dot <= 0.0F;
+}
+
+void Vector3::setByLerp(const Vector3 &t_v1, const Vector3 &t_v2, const float &t_interp, const float &t_scale)
+{
+    asm volatile(
+        "lqc2      vf4, 0x0(%1)  \n\t" // vf4 = v1
+        "lqc2      vf5, 0x0(%2)  \n\t" // vf5 = v2
+        "mfc1      $8,  %3       \n\t" // vf6 = t
+        "qmtc2     $8,  vf6      \n\t" // lerp:
+        "vsub.xyz  vf7, vf5, vf4 \n\t" // vf7 = v2 - v1
+        "vmulx.xyz vf8, vf7, vf6 \n\t" // vf8 = vf7 * t
+        "vadd.xyz  vf9, vf8, vf4 \n\t" // vf9 = vf8 + vf4
+        "sqc2      vf9, 0x0(%0)  \n\t" // v0  = vf9
+        :
+        : "r"(&this->xyz), "r"(&t_v1.xyz), "r"(&t_v2.xyz), "f"(t_interp));
+    operator*=(t_scale);
 }

From e39c8c5de88feaa255729e2b2515e5be75f9a26d Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Thu, 24 Dec 2020 10:34:56 +0100
Subject: [PATCH 8/9] changes to match latest matrix/vector3 api

---
 src/engine/include/modules/camera_base.hpp | 5 +++--
 src/engine/include/modules/renderer.hpp    | 2 +-
 src/engine/modules/camera_base.cpp         | 8 ++++----
 src/engine/modules/renderer.cpp            | 4 ++--
 src/samples/dolphin/camera.cpp             | 5 +----
 src/samples/dolphin/camera.hpp             | 3 +--
 src/samples/floors/camera.cpp              | 5 +----
 src/samples/floors/camera.hpp              | 1 -
 src/samples/floors/objects/player.cpp      | 4 ++--
 9 files changed, 15 insertions(+), 22 deletions(-)

diff --git a/src/engine/include/modules/camera_base.hpp b/src/engine/include/modules/camera_base.hpp
index 342acc7..3cefcad 100644
--- a/src/engine/include/modules/camera_base.hpp
+++ b/src/engine/include/modules/camera_base.hpp
@@ -21,7 +21,7 @@ class CameraBase
 {
 
 public:
-    CameraBase(ScreenSettings *t_screen, Vector3 *t_position, Vector3 *t_up);
+    CameraBase(ScreenSettings *t_screen, Vector3 *t_position);
     virtual ~CameraBase(){};
 
     /**
@@ -56,9 +56,10 @@ protected:
     void updatePlanes(Vector3 t_target);
 
 private:
-    Vector3 *p_position, *p_up;
+    Vector3 *p_position;
     float farPlaneDist, nearPlaneDist, nearHeight, nearWidth, farHeight, farWidth;
     Vector3 ftl, ftr, fbl, fbr, ntl, ntr, nbl, nbr;
+    Vector3 up; // always 0.0F, 1.0F, 0.0F
 };
 
 #endif
diff --git a/src/engine/include/modules/renderer.hpp b/src/engine/include/modules/renderer.hpp
index d2cdb76..1ad7acb 100644
--- a/src/engine/include/modules/renderer.hpp
+++ b/src/engine/include/modules/renderer.hpp
@@ -125,7 +125,7 @@ private:
     void flipBuffers();
     void beginFrameIfNeeded();
     u8 isFrameEmpty;
-    Matrix perspective;
+    Matrix perspective, camRotation;
     Light light;
     RenderData renderData;
     TextureRepository textureRepo;
diff --git a/src/engine/modules/camera_base.cpp b/src/engine/modules/camera_base.cpp
index 124ee77..9576306 100644
--- a/src/engine/modules/camera_base.cpp
+++ b/src/engine/modules/camera_base.cpp
@@ -18,7 +18,7 @@
 // Constructors/Destructors
 // ----
 
-CameraBase::CameraBase(ScreenSettings *t_screen, Vector3 *t_position, Vector3 *t_up)
+CameraBase::CameraBase(ScreenSettings *t_screen, Vector3 *t_position)
     : screen(t_screen)
 {
     PRINT_LOG("Initializing frustum");
@@ -30,7 +30,7 @@ CameraBase::CameraBase(ScreenSettings *t_screen, Vector3 *t_position, Vector3 *t
     farHeight = tang * farPlaneDist;
     farWidth = farHeight * screen->aspectRatio;
     p_position = t_position;
-    p_up = t_up;
+    up.set(0.0F, 1.0F, 0.0F);
     PRINT_LOG("CameraBase initialized!");
 }
 
@@ -41,7 +41,7 @@ CameraBase::CameraBase(ScreenSettings *t_screen, Vector3 *t_position, Vector3 *t
 void CameraBase::lookAt(Vector3 &t_target)
 {
     updatePlanes(t_target);
-    worldView.lookAt(*p_up, *p_position, t_target);
+    worldView.lookAt(*p_position, t_target);
 }
 
 void CameraBase::updatePlanes(Vector3 t_target)
@@ -51,7 +51,7 @@ void CameraBase::updatePlanes(Vector3 t_target)
     Z = *p_position - t_target;
     Z.normalize();
     // X axis of camera of given "up" vector and Z axis
-    X = *p_up * Z;
+    X = up * Z;
     X.normalize();
     // the real "up" vector is the cross product of Z and X
     Y = Z * X;
diff --git a/src/engine/modules/renderer.cpp b/src/engine/modules/renderer.cpp
index 02cadff..1a7cc2d 100644
--- a/src/engine/modules/renderer.cpp
+++ b/src/engine/modules/renderer.cpp
@@ -277,8 +277,8 @@ void Renderer::draw(Mesh &t_mesh, LightBulb *t_bulbs, u16 t_bulbsCount)
     if (!t_mesh.isDataLoaded())
         PRINT_ERR("Can't draw, because no mesh data was loaded!");
 
-    Vector3 rotatedCamera = Vector3(*renderData.cameraPosition);
-    rotatedCamera.rotate(t_mesh.rotation, true);
+    camRotation.rotation(-t_mesh.rotation);
+    Vector3 rotatedCamera = Vector3(camRotation * *renderData.cameraPosition);
 
     if (t_mesh.getCurrentAnimationFrame() != t_mesh.getNextAnimationFrame())
         t_mesh.animate();
diff --git a/src/samples/dolphin/camera.cpp b/src/samples/dolphin/camera.cpp
index 5c122f7..855428f 100755
--- a/src/samples/dolphin/camera.cpp
+++ b/src/samples/dolphin/camera.cpp
@@ -14,12 +14,9 @@
 
 const float CAMERA_Y = 15.0F;
 
-Camera::Camera(ScreenSettings *t_screen) : CameraBase(t_screen, &position, &up)
+Camera::Camera(ScreenSettings *t_screen) : CameraBase(t_screen, &position)
 {
     verticalLevel = 30.0F;
-    up.x = 0.0F;
-    up.y = 1.0F;
-    up.z = 0.0F;
 }
 
 Camera::~Camera() {}
diff --git a/src/samples/dolphin/camera.hpp b/src/samples/dolphin/camera.hpp
index 7bcbde3..d13d38c 100755
--- a/src/samples/dolphin/camera.hpp
+++ b/src/samples/dolphin/camera.hpp
@@ -22,7 +22,7 @@ class Camera : public CameraBase
 {
 
 public:
-    Vector3 position,up, unitCirclePosition;
+    Vector3 position, up, unitCirclePosition;
     float horizontalLevel, verticalLevel;
 
     Camera(ScreenSettings *t_screen);
@@ -34,7 +34,6 @@ public:
 
 protected:
     Vector3 *getPosition() { return &position; };
-    Vector3 *getUp() { return &up; };
     ScreenSettings screen;
 };
 
diff --git a/src/samples/floors/camera.cpp b/src/samples/floors/camera.cpp
index 56c4aa1..e797c0f 100644
--- a/src/samples/floors/camera.cpp
+++ b/src/samples/floors/camera.cpp
@@ -19,13 +19,10 @@ const float CAMERA_Y = 30.0F;
 // Constructors/Destructors
 // ----
 
-Camera::Camera(ScreenSettings *t_screen) : CameraBase(t_screen, &position, &up)
+Camera::Camera(ScreenSettings *t_screen) : CameraBase(t_screen, &position)
 {
     PRINT_LOG("Initializing camera");
     verticalLevel = 80.0F;
-    up.x = 0.0F;
-    up.y = 1.0F;
-    up.z = 0.0F;
     PRINT_LOG("Camera initialized!");
 }
 
diff --git a/src/samples/floors/camera.hpp b/src/samples/floors/camera.hpp
index 4d6adf8..49735d2 100644
--- a/src/samples/floors/camera.hpp
+++ b/src/samples/floors/camera.hpp
@@ -35,7 +35,6 @@ public:
 
 protected:
     Vector3 *getPosition() { return &position; };
-    Vector3 *getUp() { return &up; };
     ScreenSettings screen;
 };
 
diff --git a/src/samples/floors/objects/player.cpp b/src/samples/floors/objects/player.cpp
index 6d6ae79..738d03a 100644
--- a/src/samples/floors/objects/player.cpp
+++ b/src/samples/floors/objects/player.cpp
@@ -211,13 +211,13 @@ FloorsCheck *Player::checkFloors(const FloorManager &t_floorManager, const Vecto
     for (u32 i = 0; i < t_floorManager.floorAmount; i++)
     {
         Utils::getMinMax(t_floorManager.floors[i].mesh, min, max);
-        if (result->currentFloor == NULL && this->mesh.position.isOnSquare(min, max))
+        if (result->currentFloor == NULL && this->mesh.position.isOnBox(min, max))
         {
             result->currentFloor = &t_floorManager.floors[i];
             result->currFloorMin.set(min);
             result->currFloorMax.set(max);
         }
-        if (result->willCollideFloor == NULL && t_nextPos.collidesSquare(min, max))
+        if (result->willCollideFloor == NULL && t_nextPos.collidesBox(min, max))
         {
             result->willCollideFloor = &t_floorManager.floors[i];
             result->willCollideFloorMin.set(min);

From 91934df5e884dbbfaf68ad85f902f443152f6cc1 Mon Sep 17 00:00:00 2001
From: h4570 <hastoklai@gmail.com>
Date: Thu, 24 Dec 2020 10:35:04 +0100
Subject: [PATCH 9/9] fixed warning

---
 src/engine/models/bounding_box.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/engine/models/bounding_box.cpp b/src/engine/models/bounding_box.cpp
index dc90364..2b22fc9 100644
--- a/src/engine/models/bounding_box.cpp
+++ b/src/engine/models/bounding_box.cpp
@@ -46,4 +46,4 @@ BoundingBox::BoundingBox(Vector3 *t_vertices)
 
 BoundingBox::~BoundingBox()
 {
-}
\ No newline at end of file
+}