From 95d5264300862ebcfd0aab20bc3bef2d197a8425 Mon Sep 17 00:00:00 2001
From: gizmo98 <atz.SE@gmx.de>
Date: Sat, 9 Jun 2018 12:29:01 +0200
Subject: [PATCH] Add better matrix load, insert and store fixed to float logic

---
 src/GBI.h                |  5 +++++
 src/RSP_LoadMatrix.cpp   |  8 +++++++-
 src/gSP.cpp              | 44 ++++++++++++++--------------------------
 src/uCodes/ZSortBOSS.cpp |  8 +++++---
 4 files changed, 32 insertions(+), 33 deletions(-)

diff --git a/src/GBI.h b/src/GBI.h
index 541fc4c6..fcc9214e 100644
--- a/src/GBI.h
+++ b/src/GBI.h
@@ -69,6 +69,11 @@
 #define _SHIFTR( v, s, w )	\
 	(((u32)v >> s) & ((0x01 << w) - 1))
 
+union s32u32 {
+	s32 s;
+	u32 u;
+};
+
 // BG flags
 #define	G_BGLT_LOADBLOCK	0x0033
 #define	G_BGLT_LOADTILE		0xfff4
diff --git a/src/RSP_LoadMatrix.cpp b/src/RSP_LoadMatrix.cpp
index 1dc2086e..81a340df 100644
--- a/src/RSP_LoadMatrix.cpp
+++ b/src/RSP_LoadMatrix.cpp
@@ -3,6 +3,8 @@
 
 void RSP_LoadMatrix( f32 mtx[4][4], u32 address )
 {
+    s32u32 value;
+
     struct _N64Matrix
     {
         s16 integer[4][4];
@@ -13,5 +15,9 @@ void RSP_LoadMatrix( f32 mtx[4][4], u32 address )
 
     for (i = 0; i < 4; i++)
         for (j = 0; j < 4; j++)
-            mtx[i][j] = (f32)(n64Mat->integer[i][j^1]) + _FIXED2FLOAT(n64Mat->fraction[i][j^1],16);
+        {
+            value.s = (s32)n64Mat->integer[i][j^1] << 16;
+            value.u += n64Mat->fraction[i][j^1];
+            mtx[i][j] = _FIXED2FLOAT(value.s,16);
+        }
 }
diff --git a/src/gSP.cpp b/src/gSP.cpp
index db29e3b7..506d4fac 100644
--- a/src/gSP.cpp
+++ b/src/gSP.cpp
@@ -1572,40 +1572,26 @@ void gSPInsertMatrix( u32 where, u32 num )
 {
 	DebugMsg(DEBUG_NORMAL, "gSPInsertMatrix(%u, %u);\n", where, num);
 
+	s32u32 value;
 	f32 fraction, integer;
 
 	if ((where & 0x3) || (where > 0x3C))
 		return;
 
-	// integer elements of the matrix to be changed
-	if (where < 0x20) {
-		fraction = modff( gSP.matrix.combined[0][where >> 1], &integer );
-		gSP.matrix.combined[0][where >> 1] = (f32)((s16)_SHIFTR( num, 16, 16 ) + abs( fraction ));
-
-		fraction = modff( gSP.matrix.combined[0][(where >> 1) + 1], &integer );
-		gSP.matrix.combined[0][(where >> 1) + 1] = (f32)((s16)_SHIFTR( num, 0, 16 ) + abs( fraction ));
-	} 
-	// fractional elements of the matrix to be changed
-	else {
-		f32 newValue;
-
-		fraction = modff( gSP.matrix.combined[0][(where - 0x20) >> 1], &integer );
-		newValue = integer + _FIXED2FLOAT( _SHIFTR( num, 16, 16 ), 16);
-
-		// Make sure the sign isn't lost
-		if ((integer == 0.0f) && (fraction != 0.0f))
-			newValue = copysignf( newValue, fraction );
-
-		gSP.matrix.combined[0][(where - 0x20) >> 1] = newValue;
-
-		fraction = modff( gSP.matrix.combined[0][((where - 0x20) >> 1) + 1], &integer );
-		newValue = integer + _FIXED2FLOAT( _SHIFTR( num, 0, 16 ), 16 );
-
-		// Make sure the sign isn't lost
-		if ((integer == 0.0f) && (fraction != 0.0f))
-			newValue = copysignf( newValue, fraction );
-
-		gSP.matrix.combined[0][((where - 0x20) >> 1) + 1] = newValue;
+	for(int i = 0; i < 2;i++)
+	{
+		// integer elements of the matrix to be changed
+		if (where < 0x20) {
+			value.s = (s32)(gSP.matrix.combined[0][(where >> 1) + i] * 65536.0f);
+			value.u = value.u & 0x0000FFFF + (s32)(_SHIFTR( num, 16 * (1 - i), 16 ) << 16);
+			gSP.matrix.combined[0][(where >> 1) + i] = _FIXED2FLOAT( value.s, 16 );
+		}
+		// fractional elements of the matrix to be changed
+		else {
+			value.s = (s32)(gSP.matrix.combined[0][((where - 0x20) >> 1) + i] * 65535.0f) ;
+			value.u = value.u & 0xFFFF0000 + _SHIFTR( num, 16 * (1 - i), 16 );
+			gSP.matrix.combined[0][((where - 0x20) >> 1) + i] = _FIXED2FLOAT( value.s, 16 );
+		}
 	}
 }
 
diff --git a/src/uCodes/ZSortBOSS.cpp b/src/uCodes/ZSortBOSS.cpp
index 9033391b..6b7a32db 100644
--- a/src/uCodes/ZSortBOSS.cpp
+++ b/src/uCodes/ZSortBOSS.cpp
@@ -122,6 +122,8 @@ void ZSortBOSS_ClearBuffer( u32, u32 )
 static
 void StoreMatrix( f32 mtx[4][4], u32 address )
 {
+	s32u32 value;
+
 	struct _N64Matrix
 	{
 		s16 integer[4][4];
@@ -133,9 +135,9 @@ void StoreMatrix( f32 mtx[4][4], u32 address )
 	
 	for(i = 0; i < 4; i++) {
 		for(j = 0; j < 4; j++) {
-			fractpart = modff(mtx[i][j], &intpart);
-			n64Mat->fraction[i][j^1] = (u16)(fractpart * 65536.f);
-			n64Mat->integer[i][j^1] = ((mtx[i][j] < 0.0) && (n64Mat->fraction[i][j^1] != 0)) ? (s16)(intpart-1) : (s16)intpart;
+			value.s = (s32)(mtx[i][j] * 65536.0f);
+			n64Mat->fraction[i][j^1] = (u16)(value.u & 0x0000FFFF);
+			n64Mat->integer[i][j^1] = (s16)(value.s >> 16);
 		}
 	}
 }