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;
         }