From 056fe395675d6e6d3a017776b0ec63008f3f9615 Mon Sep 17 00:00:00 2001
From: Falco Girgis <gyrovorbis@gmail.com>
Date: Sun, 27 Apr 2025 14:59:40 -0500
Subject: [PATCH] Accelerated rw::RawMatrix layer.

---
 vendor/librw/src/base.cpp    | 23 +++++++++++++++++++----
 vendor/librw/src/dc/rwdc.cpp |  4 ++--
 vendor/librw/src/rwbase.h    | 32 +++++++++++++++++++++++++++++---
 3 files changed, 50 insertions(+), 9 deletions(-)

diff --git a/vendor/librw/src/base.cpp b/vendor/librw/src/base.cpp
index 9f45b459..dcc01dfd 100644
--- a/vendor/librw/src/base.cpp
+++ b/vendor/librw/src/base.cpp
@@ -151,8 +151,9 @@ slerp(const Quat &q, const Quat &p, float32 a)
 	float32 phi = acosf(c);
 	if(phi > 0.00001f){
 		float32 s = sinf(phi);
-		return add(scale(q1, sinf((1.0f-a)*phi)/s),
-		           scale(p,  sinf(a*phi)/s));
+        float invS = dc::Invert<true, false>(s);
+		return add(scale(q1, sinf((1.0f-a)*phi) * invS),
+		           scale(p,  sinf(a*phi) * invS));
 	}
 	return q1;
 }
@@ -202,6 +203,7 @@ V3d::transformVectors(V3d *out, const V3d *in, int32 n, const Matrix *m)
 void
 RawMatrix::mult(RawMatrix *dst, RawMatrix *src1, RawMatrix *src2)
 {
+#ifndef DC_SH4
 	dst->right.x = src1->right.x*src2->right.x + src1->right.y*src2->up.x + src1->right.z*src2->at.x + src1->rightw*src2->pos.x;
 	dst->right.y = src1->right.x*src2->right.y + src1->right.y*src2->up.y + src1->right.z*src2->at.y + src1->rightw*src2->pos.y;
 	dst->right.z = src1->right.x*src2->right.z + src1->right.y*src2->up.z + src1->right.z*src2->at.z + src1->rightw*src2->pos.z;
@@ -218,11 +220,15 @@ RawMatrix::mult(RawMatrix *dst, RawMatrix *src1, RawMatrix *src2)
 	dst->pos.y   = src1->pos.x*src2->right.y   + src1->pos.y*src2->up.y   + src1->pos.z*src2->at.y + src1->posw*src2->pos.y;
 	dst->pos.z   = src1->pos.x*src2->right.z   + src1->pos.y*src2->up.z   + src1->pos.z*src2->at.z + src1->posw*src2->pos.z;
 	dst->posw    = src1->pos.x*src2->rightw    + src1->pos.y*src2->upw    + src1->pos.z*src2->atw  + src1->posw*src2->posw;
+#else
+    dc::mat_mult(*dst, *src2, *src1);
+#endif
 }
 
 void
 RawMatrix::transpose(RawMatrix *dst, RawMatrix *src)
 {
+#ifndef DC_SH4
 	dst->right.x = src->right.x;
 	dst->up.x = src->right.y;
 	dst->at.x = src->right.z;
@@ -239,18 +245,27 @@ RawMatrix::transpose(RawMatrix *dst, RawMatrix *src)
 	dst->upw = src->pos.y;
 	dst->atw = src->pos.z;
 	dst->posw = src->posw;
+#else
+    dc::mat_load_transpose(*src);
+    dc::mat_store2(*dst);
+#endif
 }
 
 void
 RawMatrix::setIdentity(RawMatrix *dst)
 {
-	static RawMatrix identity = {
+#ifndef DC_SH4
+	static RawMatrix identity = {{
 		{ 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
-	};
+	}};
 	*dst = identity;
+#else
+    dc::mat_identity2();
+    dc::mat_store2(*dst);
+#endif
 }
 
 //
diff --git a/vendor/librw/src/dc/rwdc.cpp b/vendor/librw/src/dc/rwdc.cpp
index 2849461f..3793f59b 100644
--- a/vendor/librw/src/dc/rwdc.cpp
+++ b/vendor/librw/src/dc/rwdc.cpp
@@ -3530,12 +3530,12 @@ uploadSkinMatrices(Atomic *a, Matrix* skinMatrices)
 	return skinMatrices[0].identityError() < 0.01f;
 }
 
-static RawMatrix normal2texcoord = {
+static RawMatrix normal2texcoord = {{
 	{ 0.5f / 127,  0.0f, 0.0f }, 0.0f,
 	{ 0.0f, -0.5f / 127, 0.0f }, 0.0f,
 	{ 0.0f,  0.0f, 1.0f }, 0.0f,
 	{ 0.5f,  0.5f, 0.0f }, 1.0f
-};
+}};
 
 void
 uploadEnvMatrix(Frame *frame, RawMatrix *world, matrix_t* envMatrix)
diff --git a/vendor/librw/src/rwbase.h b/vendor/librw/src/rwbase.h
index 8b567709..941fb989 100644
--- a/vendor/librw/src/rwbase.h
+++ b/vendor/librw/src/rwbase.h
@@ -339,9 +339,9 @@ inline V3d rotate(const V3d &v, const Quat &q) { return mult(mult(q, makeQuat(0.
 Quat lerp(const Quat &q, const Quat &p, float32 r);
 Quat slerp(const Quat &q, const Quat &p, float32 a);
 
-struct __attribute__((aligned(8))) RawMatrix 
+struct alignas(8) RawMatrixBase
 {
-	V3d right;
+    V3d right;
 	float32 rightw;
 	V3d up;
 	float32 upw;
@@ -349,6 +349,32 @@ struct __attribute__((aligned(8))) RawMatrix
 	float32 atw;
 	V3d pos;
 	float32 posw;
+};
+
+struct RawMatrix: public RawMatrixBase
+{
+    RawMatrix() {}
+
+    RawMatrix(RawMatrixBase &&aggregate):
+	    RawMatrixBase{aggregate}
+    {}
+
+    RawMatrix(const RawMatrix &rhs) {
+        *this = rhs;
+    }
+
+    operator matrix_t *() {
+        return reinterpret_cast<matrix_t *>(this);
+    }
+
+    operator const matrix_t *() const {
+        return reinterpret_cast<const matrix_t *>(this);
+    }
+
+    RawMatrix &operator=(const RawMatrix &rhs) {
+        dc::mat_copy(*this, rhs);
+        return *this;
+    }
 
 	// NB: this is dst = src2*src1, i.e. src1 is applied first, then src2
 	static void mult(RawMatrix *dst, RawMatrix *src1, RawMatrix *src2);
@@ -356,7 +382,7 @@ struct __attribute__((aligned(8))) RawMatrix
 	static void setIdentity(RawMatrix *dst);
 };
 
-struct Matrix
+struct alignas(8) Matrix
 {
 	enum Type {
 		TYPENORMAL	= 1,