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");
+        }
     }
 }