Support scaling matricies

include/PSX/GTE/gte.hpp

@@ -22,10 +22,6 @@ namespace JabyEngine {
         /*
             RotTrans
 
-            TODO: Can we use gte_stsv instead of gte_stlvnl for writing to a SVECTOR?
-                  Do we have to use gte_stflg??
-                  Look at: RotTransSV???
-
             Perform coordinate transformation using a rotation matrix
             input:  Input vector
             output: Output vector
@@ -38,6 +34,29 @@ namespace JabyEngine {
             stflg(flag);
         }
 
+        /*
+            ScaleMatrix
+
+            m: Pointer to matrix (input/output)
+            v: Pointer to scale vector (input)
+
+            result: m
+            Scales m by v. The components of v are fixed point decimals in which 1.0 represents 4096
+        */
+        static ROTMATRIX& scale_matrix(ROTMATRIX& m, const VECTOR& v) {
+            static const auto multiply_matrix_row = [](int32_t value, ROTMATRIX& matrix, size_t row) {
+                ldir0(value);           // lwc2	r8,	v.x
+                ldclmv(matrix, row);    // load matrix row to r9 - r11 (mtc2)
+                gpf12();                // gte_gpf12
+                stclmv(matrix, row);    // store matrix row
+            };
+
+            multiply_matrix_row(v.x, m, 0);
+            multiply_matrix_row(v.y, m, 1);
+            multiply_matrix_row(v.z, m, 2);
+            return m;
+        }
+
         /*
             SetRotMatrix
 
@@ -214,7 +233,7 @@ namespace JabyEngine {
 
         inline GPU::Vertex MATRIX :: apply_to(const GPU::Vertex& vertex) const {
             GPU::Vertex result;
-            
+
             apply_matrix(*this, vertex, result);
             return result;
         }

include/PSX/GTE/gte_instruction.hpp

@@ -21,6 +21,11 @@ namespace JabyEngine {
             __asm__ volatile("lwc2 $5, 4(%0)" :: "r"(&vector));
         }
 
+        // Load int32_t to ir0 register (for multiplying usually)
+        static __always_inline void ldir0(const int32_t& value) {
+            __asm__ volatile("lwc2 $8, 0(%0)" :: "r"(&value));
+        }
+
         // Load LS 16 bits of VECTOR to 16 bit universal vector.
         static __always_inline void ldlv0(const VECTOR& vector) {
             __asm__ volatile("lhu  $13, 4(%0)"    :: "r"(&vector) : "$12", "$13");
@@ -119,5 +124,14 @@ namespace JabyEngine {
             __asm__ volatile("nop");
             __asm__ volatile("cop2 0x049E012");
        }
+
+        /*
+            Last half of LoadAverage12.
+        */
+        static __always_inline void gpf12(){
+            __asm__ volatile("nop");
+            __asm__ volatile("nop");
+            __asm__ volatile("cop2 0x0198003D");
+        }
     }
 }

src/Library/src/BootLoader/start_boot.cpp

@@ -2,6 +2,7 @@
 #include "../../internal-include/GPU/gpu_internal.hpp"
 #include <stdio.h>
 
+#include <PSX/GTE/gte.hpp>
 #include <PSX/System/syscalls.hpp>
 
 extern "C" uint32_t __heap_start;
@@ -12,6 +13,15 @@ extern "C" uint32_t __planschi_start;
 extern "C" uint32_t __planschi_end;
 
 namespace JabyEngine {
+    static void test_gte_scale() {
+        auto matrix = GTE::ROTMATRIX::identity();
+
+        GTE::scale_matrix(matrix, GTE::VECTOR::create(4096*2, 4096*2, 4096*2));
+        printf("|%i|%i|%i|\n", matrix.matrix[0][0], matrix.matrix[0][1], matrix.matrix[0][2]);
+        printf("|%i|%i|%i|\n", matrix.matrix[1][0], matrix.matrix[1][1], matrix.matrix[1][2]);
+        printf("|%i|%i|%i|\n", matrix.matrix[2][0], matrix.matrix[2][1], matrix.matrix[2][2]);
+    }
+
     static void test_bios_font() {
         static constexpr uint16_t SJIS = 0x83B5;
 
@@ -57,6 +67,7 @@ namespace JabyEngine {
                 GPU::display_logo();
                 GTE::setup();
                 test_bios_font();
+                test_gte_scale();
 
                 //Pause??
                 SPU::setup();