Initial psxgte cleanup

17 files changed, 658 insertions, 764 deletions

diff --git a/libpsn00b/psxgte/applymatrixlv.s b/libpsn00b/psxgte/applymatrixlv.s
deleted file mode 100644
index 3180d0f..0000000
--- a/libpsn00b/psxgte/applymatrixlv.s
+++ /dev/null
@@ -1,40 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global ApplyMatrixLV
-.type ApplyMatrixLV, @function
-ApplyMatrixLV:
-
-	# Load matrix to GTE
-	lw		$t0, 0($a0)
-	lw		$t1, 4($a0)
-	ctc2	$t0, $0
-	ctc2	$t1, $1
-	lw		$t0, 8($a0)
-	lw		$t1, 12($a0)
-	lhu		$t2, 16($a0)
-	ctc2	$t0, $2
-	ctc2	$t1, $3
-	ctc2	$t2, $4
-
-	lw		$t0, 0($a1)
-	lw		$t1, 4($a1)
-	mtc2	$t0, C2_IR1
-	lw		$t0, 8($a1)
-	mtc2	$t1, C2_IR2
-	mtc2	$t0, C2_IR3
-
-	nMVMVA(1, 0, 3, 3, 0)
-
-	swc2	C2_IR1, 0($a2)
-	swc2	C2_IR2, 4($a2)
-	swc2	C2_IR3, 8($a2)
-
-	jr		$ra
-	move	$v0, $a2
-	
\ No newline at end of file
diff --git a/libpsn00b/psxgte/compmatrixlv.s b/libpsn00b/psxgte/compmatrixlv.s
deleted file mode 100644
index 2908eb9..0000000
--- a/libpsn00b/psxgte/compmatrixlv.s
+++ /dev/null
@@ -1,100 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.set MATRIX_r11r12,	0
-.set MATRIX_r13r21,	4
-.set MATRIX_r22r23,	8
-.set MATRIX_r31r32,	12
-.set MATRIX_r33,	16
-.set MATRIX_trx,	20
-.set MATRIX_try,	24
-.set MATRIX_trz,	28
-
-
-.global CompMatrixLV
-.type CompMatrixLV, @function
-CompMatrixLV:
-
-	# Load matrix v0 to GTE
-	lw		$t0, MATRIX_r11r12($a0)
-	lw		$t1, MATRIX_r13r21($a0)
-	ctc2	$t0, C2_R11R12
-	ctc2	$t1, C2_R13R21
-	lw		$t0, MATRIX_r22r23($a0)
-	lw		$t1, MATRIX_r31r32($a0)
-	lhu		$t2, MATRIX_r33($a0)
-	ctc2	$t0, C2_R22R23
-	lw		$t0, MATRIX_trx($a0)
-	ctc2	$t1, C2_R31R32
-	lw		$t1, MATRIX_try($a0)
-	ctc2	$t2, C2_R33
-	lw		$t2, MATRIX_trz($a0)
-	ctc2	$t0, C2_TRX
-	ctc2	$t1, C2_TRY
-	ctc2	$t2, C2_TRZ
-	
-	lw		$t0, MATRIX_trx($a1)
-	lw		$t1, MATRIX_try($a1)
-	mtc2	$t0, C2_IR1
-	lw		$t0, MATRIX_trz($a1)
-	mtc2	$t1, C2_IR2
-	mtc2	$t0, C2_IR3
-
-	nMVMVA(1, 0, 3, 0, 0)
-
-	swc2	C2_IR1, MATRIX_trx($a2)
-	swc2	C2_IR2, MATRIX_try($a2)
-	swc2	C2_IR3, MATRIX_trz($a2)
-	
-	lhu		$t1, 2*(0+(3*1))($a1)		# Load values for first
-	lhu		$t0, 2*(0+(3*0))($a1)		# R11 R21 R31
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(0+(3*2))($a1)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	lhu		$t1, 2*(1+(3*1))($a1)		# Load values for second
-	lhu		$t0, 2*(1+(3*0))($a1)		# R12 R22 R32
-	MVMVA(1, 0, 0, 3, 0)				# First multiply
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(1+(3*2))($a1)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	mfc2	$t0, C2_IR1					# Store results of first
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(0+(3*0))($a2)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(0+(3*1))($a2)
-	sh		$t0, 2*(0+(3*2))($a2)
-
-	lhu		$t1, 2*(2+(3*1))($a1)		# Load values for third
-	lhu		$t0, 2*(2+(3*0))($a1)		# R13 R23 R33
-	MVMVA(1, 0, 0, 3, 0)				# Second multiply
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(2+(3*2))($a1)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	mfc2	$t0, C2_IR1					# Store results of second
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(1+(3*0))($a2)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(1+(3*1))($a2)
-	sh		$t0, 2*(1+(3*2))($a2)
-	MVMVA(1, 0, 0, 3, 0)				# Third multiply
-
-	mfc2	$t0, C2_IR1					# Store results of third
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(2+(3*0))($a2)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(2+(3*1))($a2)
-	sh		$t0, 2*(2+(3*2))($a2)
-
-	jr		$ra
-	move	$v0, $a2
\ No newline at end of file
diff --git a/libpsn00b/psxgte/hirotmatrix.c b/libpsn00b/psxgte/hirotmatrix.c
deleted file mode 100644
index 56516b0..0000000
--- a/libpsn00b/psxgte/hirotmatrix.c
+++ /dev/null
@@ -1,35 +0,0 @@
-#include <psxgte.h>
-
-MATRIX *HiRotMatrix(VECTOR *r, MATRIX *m) {
-
-	short s[3],c[3];
-	MATRIX tm[3];
-
-	s[0] = hisin(r->vx);	s[1] = hisin(r->vy);	s[2] = hisin(r->vz);
-	c[0] = hicos(r->vx);	c[1] = hicos(r->vy);	c[2] = hicos(r->vz);
-
-	// mX
-	m->m[0][0] = ONE;		m->m[0][1] = 0;			m->m[0][2] = 0;
-	m->m[1][0] = 0;			m->m[1][1] = c[0];		m->m[1][2] = -s[0];
-	m->m[2][0] = 0;			m->m[2][1] = s[0];		m->m[2][2] = c[0];
-
-	// mY
-	tm[0].m[0][0] = c[1];	tm[0].m[0][1] = 0;		tm[0].m[0][2] = s[1];
-	tm[0].m[1][0] = 0;		tm[0].m[1][1] = ONE;	tm[0].m[1][2] = 0;
-	tm[0].m[2][0] = -s[1];	tm[0].m[2][1] = 0;		tm[0].m[2][2] = c[1];
-
-	// mZ
-	tm[1].m[0][0] = c[2];	tm[1].m[0][1] = -s[2];	tm[1].m[0][2] = 0;
-	tm[1].m[1][0] = s[2];	tm[1].m[1][1] = c[2];	tm[1].m[1][2] = 0;
-	tm[1].m[2][0] = 0;		tm[1].m[2][1] = 0;		tm[1].m[2][2] = ONE;
-
-	PushMatrix();
-
-	MulMatrix0( m, &tm[0], &tm[2] );
-	MulMatrix0( &tm[2], &tm[1], m );
-
-	PopMatrix();
-
-	return m;
-
-}
diff --git a/libpsn00b/psxgte/hisin.c b/libpsn00b/psxgte/hisin.c
deleted file mode 100644
index 68d5d28..0000000
--- a/libpsn00b/psxgte/hisin.c
+++ /dev/null
@@ -1,33 +0,0 @@
-/* Based on isin_S4 implementation from coranac:
- *	http://www.coranac.com/2009/07/sines/
- *
- */
-
-#define qN	15
-#define qA	12
-#define B	19900
-#define	C	3516
-
-int hisin(int x) {
-
-    int c, x2, y;
-
-    c= x<<(30-qN);              // Semi-circle info into carry.
-    x -= 1<<qN;                 // sine -> cosine calc
-
-    x= x<<(31-qN);              // Mask with PI
-    x= x>>(31-qN);              // Note: SIGNED shift! (to qN)
-    x= x*x>>(2*qN-14);          // x=x^2 To Q14
-
-    y= B - (x*C>>14);           // B - x^2*C
-    y= (1<<qA)-(x*y>>16);       // A - x^2*(B-x^2*C)
-
-    return c>=0 ? y : -y;
-
-}
-
-int hicos(int x) {
-
-    return hisin( x+32768 );
-
-}
diff --git a/libpsn00b/psxgte/initgeom.s b/libpsn00b/psxgte/initgeom.s
index d004ecc..6d1e36a 100644
--- a/libpsn00b/psxgte/initgeom.s
+++ b/libpsn00b/psxgte/initgeom.s
@@ -2,9 +2,7 @@
 
 .include "gtereg.inc"
 
-.section .text
-
-
+.section .text.InitGeom
 .global InitGeom
 .type InitGeom, @function
 InitGeom:
diff --git a/libpsn00b/psxgte/isin.c b/libpsn00b/psxgte/isin.c
index 79e2970..a0397bd 100644
--- a/libpsn00b/psxgte/isin.c
+++ b/libpsn00b/psxgte/isin.c
@@ -1,34 +1,46 @@
-/* Based on isin_S4 implementation from coranac:
- *	http://www.coranac.com/2009/07/sines/
+/*
+ * PSn00bSDK (incomplete) trigonometry library
+ * (C) 2019-2022 Lameguy64, spicyjpeg - MPL licensed
  *
+ * Based on isin_S4 implementation from coranac:
+ * https://www.coranac.com/2009/07/sines
  */
 
-#define qN	10
-#define qA	12
-#define B	19900
-#define	C	3516
+#define qN_l	10
+#define qN_h	15
+#define qA		12
+#define B		19900
+#define	C		3516
 
-int isin(int x) {
-
-    int c, x2, y;
+static inline int _isin(int qN, int x) {
+	int c, x2, y;
 
-    c= x<<(30-qN);              // Semi-circle info into carry.
-    x -= 1<<qN;                 // sine -> cosine calc
+	c  = x << (30 - qN);			// Semi-circle info into carry.
+	x -= 1 << qN;					// sine -> cosine calc
 
-    x= x<<(31-qN);              // Mask with PI
-    x= x>>(31-qN);              // Note: SIGNED shift! (to qN)
+	x <<= (31 - qN);				// Mask with PI
+	x >>= (31 - qN);				// Note: SIGNED shift! (to qN)
+	x  *= x;
+	x >>= (2 * qN - 14);			// x=x^2 To Q14
 
-    x= x*x>>(2*qN-14);          // x=x^2 To Q14
+	y = B - (x * C >> 14);			// B - x^2*C
+	y = (1 << qA) - (x * y >> 16);	// A - x^2*(B-x^2*C)
 
-    y= B - (x*C>>14);           // B - x^2*C
-    y= (1<<qA)-(x*y>>16);       // A - x^2*(B-x^2*C)
-
-    return c>=0 ? y : -y;
+	return (c >= 0) ? y : (-y);
+}
 
+int isin(int x) {
+	return _isin(qN_l, x);
 }
 
 int icos(int x) {
+	return _isin(qN_l, x + (1 << qN_l));
+}
 
-    return isin( x+1024 );
+int hisin(int x) {
+	return _isin(qN_h, x);
+}
 
+int hicos(int x) {
+	return _isin(qN_h, x + (1 << qN_h));
 }
diff --git a/libpsn00b/psxgte/matrix.c b/libpsn00b/psxgte/matrix.c
index b4dea12..805fd1b 100644
--- a/libpsn00b/psxgte/matrix.c
+++ b/libpsn00b/psxgte/matrix.c
@@ -1,7 +1,6 @@
 #include <psxgte.h>
 
 MATRIX *RotMatrix(SVECTOR *r, MATRIX *m) {
-
 	short s[3],c[3];
 	MATRIX tm[3];
 
@@ -24,22 +23,47 @@ MATRIX *RotMatrix(SVECTOR *r, MATRIX *m) {
 	tm[1].m[2][0] = 0;		tm[1].m[2][1] = 0;		tm[1].m[2][2] = ONE;
 
 	PushMatrix();
-
 	MulMatrix0( m, &tm[0], &tm[2] );
 	MulMatrix0( &tm[2], &tm[1], m );
-
 	PopMatrix();
 
 	return m;
+}
+
+MATRIX *HiRotMatrix(VECTOR *r, MATRIX *m) {
+	short s[3],c[3];
+	MATRIX tm[3];
+
+	s[0] = hisin(r->vx);	s[1] = hisin(r->vy);	s[2] = hisin(r->vz);
+	c[0] = hicos(r->vx);	c[1] = hicos(r->vy);	c[2] = hicos(r->vz);
 
+	// mX
+	m->m[0][0] = ONE;		m->m[0][1] = 0;			m->m[0][2] = 0;
+	m->m[1][0] = 0;			m->m[1][1] = c[0];		m->m[1][2] = -s[0];
+	m->m[2][0] = 0;			m->m[2][1] = s[0];		m->m[2][2] = c[0];
+
+	// mY
+	tm[0].m[0][0] = c[1];	tm[0].m[0][1] = 0;		tm[0].m[0][2] = s[1];
+	tm[0].m[1][0] = 0;		tm[0].m[1][1] = ONE;	tm[0].m[1][2] = 0;
+	tm[0].m[2][0] = -s[1];	tm[0].m[2][1] = 0;		tm[0].m[2][2] = c[1];
+
+	// mZ
+	tm[1].m[0][0] = c[2];	tm[1].m[0][1] = -s[2];	tm[1].m[0][2] = 0;
+	tm[1].m[1][0] = s[2];	tm[1].m[1][1] = c[2];	tm[1].m[1][2] = 0;
+	tm[1].m[2][0] = 0;		tm[1].m[2][1] = 0;		tm[1].m[2][2] = ONE;
+
+	PushMatrix();
+	MulMatrix0( m, &tm[0], &tm[2] );
+	MulMatrix0( &tm[2], &tm[1], m );
+	PopMatrix();
+
+	return m;
 }
 
 MATRIX *TransMatrix(MATRIX *m, VECTOR *r) {
-
 	m->t[0] = r->vx;
 	m->t[1] = r->vy;
 	m->t[2] = r->vz;
 
 	return m;
-
 }
diff --git a/libpsn00b/psxgte/matrix.s b/libpsn00b/psxgte/matrix.s
new file mode 100644
index 0000000..9de0ccd
--- /dev/null
+++ b/libpsn00b/psxgte/matrix.s
@@ -0,0 +1,439 @@
+.set noreorder
+
+.include "gtereg.inc"
+.include "inline_s.inc"
+
+.set MATRIX_r11r12,	0
+.set MATRIX_r13r21,	4
+.set MATRIX_r22r23,	8
+.set MATRIX_r31r32,	12
+.set MATRIX_r33,	16
+.set MATRIX_trx,	20
+.set MATRIX_try,	24
+.set MATRIX_trz,	28
+
+.section .text.ApplyMatrixLV
+.global ApplyMatrixLV
+.type ApplyMatrixLV, @function
+ApplyMatrixLV:
+	# Load matrix to GTE
+	lw		$t0, 0($a0)
+	lw		$t1, 4($a0)
+	ctc2	$t0, $0
+	ctc2	$t1, $1
+	lw		$t0, 8($a0)
+	lw		$t1, 12($a0)
+	lhu		$t2, 16($a0)
+	ctc2	$t0, $2
+	ctc2	$t1, $3
+	ctc2	$t2, $4
+
+	lw		$t0, 0($a1)
+	lw		$t1, 4($a1)
+	mtc2	$t0, C2_IR1
+	lw		$t0, 8($a1)
+	mtc2	$t1, C2_IR2
+	mtc2	$t0, C2_IR3
+
+	nMVMVA(1, 0, 3, 3, 0)
+
+	swc2	C2_IR1, 0($a2)
+	swc2	C2_IR2, 4($a2)
+	swc2	C2_IR3, 8($a2)
+
+	jr		$ra
+	move	$v0, $a2
+
+.section .text.CompMatrixLV
+.global CompMatrixLV
+.type CompMatrixLV, @function
+CompMatrixLV:
+	# Load matrix v0 to GTE
+	lw		$t0, MATRIX_r11r12($a0)
+	lw		$t1, MATRIX_r13r21($a0)
+	ctc2	$t0, C2_R11R12
+	ctc2	$t1, C2_R13R21
+	lw		$t0, MATRIX_r22r23($a0)
+	lw		$t1, MATRIX_r31r32($a0)
+	lhu		$t2, MATRIX_r33($a0)
+	ctc2	$t0, C2_R22R23
+	lw		$t0, MATRIX_trx($a0)
+	ctc2	$t1, C2_R31R32
+	lw		$t1, MATRIX_try($a0)
+	ctc2	$t2, C2_R33
+	lw		$t2, MATRIX_trz($a0)
+	ctc2	$t0, C2_TRX
+	ctc2	$t1, C2_TRY
+	ctc2	$t2, C2_TRZ
+	
+	lw		$t0, MATRIX_trx($a1)
+	lw		$t1, MATRIX_try($a1)
+	mtc2	$t0, C2_IR1
+	lw		$t0, MATRIX_trz($a1)
+	mtc2	$t1, C2_IR2
+	mtc2	$t0, C2_IR3
+
+	nMVMVA(1, 0, 3, 0, 0)
+
+	swc2	C2_IR1, MATRIX_trx($a2)
+	swc2	C2_IR2, MATRIX_try($a2)
+	swc2	C2_IR3, MATRIX_trz($a2)
+	
+	lhu		$t1, 2*(0+(3*1))($a1)		# Load values for first
+	lhu		$t0, 2*(0+(3*0))($a1)		# R11 R21 R31
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(0+(3*2))($a1)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	lhu		$t1, 2*(1+(3*1))($a1)		# Load values for second
+	lhu		$t0, 2*(1+(3*0))($a1)		# R12 R22 R32
+	MVMVA(1, 0, 0, 3, 0)				# First multiply
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(1+(3*2))($a1)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	mfc2	$t0, C2_IR1					# Store results of first
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(0+(3*0))($a2)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(0+(3*1))($a2)
+	sh		$t0, 2*(0+(3*2))($a2)
+
+	lhu		$t1, 2*(2+(3*1))($a1)		# Load values for third
+	lhu		$t0, 2*(2+(3*0))($a1)		# R13 R23 R33
+	MVMVA(1, 0, 0, 3, 0)				# Second multiply
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(2+(3*2))($a1)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	mfc2	$t0, C2_IR1					# Store results of second
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(1+(3*0))($a2)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(1+(3*1))($a2)
+	sh		$t0, 2*(1+(3*2))($a2)
+	MVMVA(1, 0, 0, 3, 0)				# Third multiply
+
+	mfc2	$t0, C2_IR1					# Store results of third
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(2+(3*0))($a2)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(2+(3*1))($a2)
+	sh		$t0, 2*(2+(3*2))($a2)
+
+	jr		$ra
+	move	$v0, $a2
+
+.section .text.MulMatrix
+.global MulMatrix
+.type MulMatrix, @function
+MulMatrix:
+	# Load m1 to GTE
+	lw		$t0, 0($a1)
+	lw		$t1, 4($a1)
+	ctc2	$t0, $0
+	ctc2	$t1, $1
+	lw		$t0, 8($a1)
+	lw		$t1, 12($a1)
+	lhu		$t2, 16($a1)
+	ctc2	$t0, $2
+	ctc2	$t1, $3
+	ctc2	$t2, $4
+
+	lhu		$t1, 2*(0+(3*1))($a0)		# Load values for first
+	lhu		$t0, 2*(0+(3*0))($a0)		# R11 R21 R31
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(0+(3*2))($a0)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	lhu		$t1, 2*(1+(3*1))($a0)		# Load values for second
+	lhu		$t0, 2*(1+(3*0))($a0)		# R12 R22 R32
+	MVMVA(1, 0, 0, 3, 0)				# First multiply
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(1+(3*2))($a0)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	mfc2	$t0, C2_IR1					# Store results of first
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(0+(3*0))($a0)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(0+(3*1))($a0)
+	sh		$t0, 2*(0+(3*2))($a0)
+
+	lhu		$t1, 2*(2+(3*1))($a0)		# Load values for third
+	lhu		$t0, 2*(2+(3*0))($a0)		# R13 R23 R33
+	MVMVA(1, 0, 0, 3, 0)				# Second multiply
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(2+(3*2))($a0)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	mfc2	$t0, C2_IR1					# Store results of second
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(1+(3*0))($a0)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(1+(3*1))($a0)
+	sh		$t0, 2*(1+(3*2))($a0)
+	MVMVA(1, 0, 0, 3, 0)				# Third multiply
+
+	mfc2	$t0, C2_IR1					# Store results of third
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(2+(3*0))($a0)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(2+(3*1))($a0)
+	sh		$t0, 2*(2+(3*2))($a0)
+
+	jr		$ra
+	move	$v0, $a0
+
+.section .text.MulMatrix0
+.global MulMatrix0
+.type MulMatrix0, @function
+MulMatrix0:
+	# Load m1 to GTE
+	lw		$t0, 0($a0)
+	lw		$t1, 4($a0)
+	ctc2	$t0, $0
+	ctc2	$t1, $1
+	lw		$t0, 8($a0)
+	lw		$t1, 12($a0)
+	lhu		$t2, 16($a0)
+	ctc2	$t0, $2
+	ctc2	$t1, $3
+	ctc2	$t2, $4
+
+	lhu		$t1, 2*(0+(3*1))($a1)		# Load values for first
+	lhu		$t0, 2*(0+(3*0))($a1)		# R11 R21 R31
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(0+(3*2))($a1)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	lhu		$t1, 2*(1+(3*1))($a1)		# Load values for second
+	lhu		$t0, 2*(1+(3*0))($a1)		# R12 R22 R32
+	MVMVA(1, 0, 0, 3, 0)				# First multiply
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(1+(3*2))($a1)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	mfc2	$t0, C2_IR1					# Store results of first
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(0+(3*0))($a2)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(0+(3*1))($a2)
+	sh		$t0, 2*(0+(3*2))($a2)
+
+	lhu		$t1, 2*(2+(3*1))($a1)		# Load values for third
+	lhu		$t0, 2*(2+(3*0))($a1)		# R13 R23 R33
+	MVMVA(1, 0, 0, 3, 0)				# Second multiply
+	sll		$t1, 16
+	or		$t0, $t1
+	lhu		$t1, 2*(2+(3*2))($a1)
+	mtc2	$t0, C2_VXY0
+	mtc2	$t1, C2_VZ0
+
+	mfc2	$t0, C2_IR1					# Store results of second
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(1+(3*0))($a2)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(1+(3*1))($a2)
+	sh		$t0, 2*(1+(3*2))($a2)
+	MVMVA(1, 0, 0, 3, 0)				# Third multiply
+
+	mfc2	$t0, C2_IR1					# Store results of third
+	mfc2	$t1, C2_IR2
+	sh		$t0, 2*(2+(3*0))($a2)
+	mfc2	$t0, C2_IR3
+	sh		$t1, 2*(2+(3*1))($a2)
+	sh		$t0, 2*(2+(3*2))($a2)
+
+	jr		$ra
+	move	$v0, $a2
+
+.section .text.ScaleMatrix
+.global ScaleMatrix
+.type ScaleMatrix, @function
+ScaleMatrix:
+	lwc2	C2_IR0,	0($a1)			# X
+
+	lh		$v0, 2*(0+(3*0))($a0)
+	lh		$v1, 2*(0+(3*1))($a0)
+	mtc2	$v0, C2_IR1
+	lh		$v0, 2*(0+(3*2))($a0)
+	mtc2	$v1, C2_IR2
+	mtc2	$v0, C2_IR3
+
+	nGPF(1)
+
+	mfc2	$v0, C2_IR1
+	mfc2	$v1, C2_IR2
+	sh		$v0, 2*(0+(3*0))($a0)
+	mfc2	$v0, C2_IR3
+	sh		$v1, 2*(0+(3*1))($a0)
+	sh		$v0, 2*(0+(3*2))($a0)
+
+	lwc2	C2_IR0,	4($a1)			# Y
+
+	lh		$v0, 2*(1+(3*0))($a0)
+	lh		$v1, 2*(1+(3*1))($a0)
+	mtc2	$v0, C2_IR1
+	lh		$v0, 2*(1+(3*2))($a0)
+	mtc2	$v1, C2_IR2
+	mtc2	$v0, C2_IR3
+
+	nGPF(1)
+
+	mfc2	$v0, C2_IR1
+	mfc2	$v1, C2_IR2
+	sh		$v0, 2*(1+(3*0))($a0)
+	mfc2	$v0, C2_IR3
+	sh		$v1, 2*(1+(3*1))($a0)
+	sh		$v0, 2*(1+(3*2))($a0)
+
+	lwc2	C2_IR0,	8($a1)			# Z
+
+	lh		$v0, 2*(2+(3*0))($a0)
+	lh		$v1, 2*(2+(3*1))($a0)
+	mtc2	$v0, C2_IR1
+	lh		$v0, 2*(2+(3*2))($a0)
+	mtc2	$v1, C2_IR2
+	mtc2	$v0, C2_IR3
+
+	nGPF(1)
+
+	mfc2	$v0, C2_IR1
+	mfc2	$v1, C2_IR2
+	sh		$v0, 2*(2+(3*0))($a0)
+	mfc2	$v0, C2_IR3
+	sh		$v1, 2*(2+(3*1))($a0)
+	sh		$v0, 2*(2+(3*2))($a0)
+
+	jr		$ra
+	move	$v0, $a0
+
+.section .text.ScaleMatrixL
+.global ScaleMatrixL
+.type ScaleMatrixL, @function
+ScaleMatrixL:
+	lwc2	C2_IR0,	0($a1)			# X
+
+	lh		$v0, 2*(0+(3*0))($a0)
+	lh		$v1, 2*(1+(3*0))($a0)
+	mtc2	$v0, C2_IR1
+	lh		$v0, 2*(2+(3*0))($a0)
+	mtc2	$v1, C2_IR2
+	mtc2	$v0, C2_IR3
+
+	nGPF(1)
+
+	mfc2	$v0, C2_IR1
+	mfc2	$v1, C2_IR2
+	sh		$v0, 2*(0+(3*0))($a0)
+	mfc2	$v0, C2_IR3
+	sh		$v1, 2*(1+(3*0))($a0)
+	sh		$v0, 2*(2+(3*0))($a0)
+
+	lwc2	C2_IR0,	4($a1)			# Y
+
+	lh		$v0, 2*(0+(3*1))($a0)
+	lh		$v1, 2*(1+(3*1))($a0)
+	mtc2	$v0, C2_IR1
+	lh		$v0, 2*(2+(3*1))($a0)
+	mtc2	$v1, C2_IR2
+	mtc2	$v0, C2_IR3
+
+	nGPF(1)
+
+	mfc2	$v0, C2_IR1
+	mfc2	$v1, C2_IR2
+	sh		$v0, 2*(0+(3*1))($a0)
+	mfc2	$v0, C2_IR3
+	sh		$v1, 2*(1+(3*1))($a0)
+	sh		$v0, 2*(2+(3*1))($a0)
+
+	lwc2	C2_IR0,	8($a1)			# Z
+
+	lh		$v0, 2*(0+(3*2))($a0)
+	lh		$v1, 2*(1+(3*2))($a0)
+	mtc2	$v0, C2_IR1
+	lh		$v0, 2*(2+(3*2))($a0)
+	mtc2	$v1, C2_IR2
+	mtc2	$v0, C2_IR3
+
+	nGPF(1)
+
+	mfc2	$v0, C2_IR1
+	mfc2	$v1, C2_IR2
+	sh		$v0, 2*(0+(3*2))($a0)
+	mfc2	$v0, C2_IR3
+	sh		$v1, 2*(1+(3*2))($a0)
+	sh		$v0, 2*(2+(3*2))($a0)
+
+	jr		$ra
+	move	$v0, $a0
+
+.section .text.PushMatrix
+.global PushMatrix
+.type PushMatrix, @function
+PushMatrix:
+	la		$a0, _matrix_stack
+	cfc2	$v0, C2_R11R12
+	cfc2	$v1, C2_R13R21
+	sw		$v0, 0($a0)
+	cfc2	$v0, C2_R22R23
+	sw		$v1, 4($a0)
+	sw		$v0, 8($a0)
+	cfc2	$v0, C2_R31R32
+	cfc2	$v1, C2_R33
+	sw		$v0, 12($a0)
+	sw		$v1, 16($a0)
+	cfc2	$v0, C2_TRX
+	cfc2	$v1, C2_TRY
+	sw		$v0, 20($a0)
+	cfc2	$v0, C2_TRZ
+	sw		$v1, 24($a0)
+	jr		$ra
+	sw		$v0, 28($a0)
+
+.section .text.PopMatrix
+.global PopMatrix
+.type PopMatrix, @function
+PopMatrix:
+	la		$a0, _matrix_stack
+	lw		$v0, 0($a0)
+	lw		$v1, 4($a0)
+	ctc2	$v0, C2_R11R12
+	ctc2	$v1, C2_R13R21
+	lw		$v0, 8($a0)
+	lw		$v1, 12($a0)
+	ctc2	$v0, C2_R22R23
+	lw		$v0, 16($a0)
+	ctc2	$v1, C2_R31R32
+	ctc2	$v0, C2_R33
+	lw		$v0, 20($a0)
+	lw		$v1, 24($a0)
+	ctc2	$v0, C2_TRX
+	lw		$v0, 28($a0)
+	ctc2	$v1, C2_TRY
+	ctc2	$v0, C2_TRZ
+	jr		$ra
+	nop
+
+.section .data._matrix_stack
+.type _matrix_stack, @object
+_matrix_stack:
+	.word 0, 0, 0, 0, 0, 0, 0, 0
diff --git a/libpsn00b/psxgte/mulmatrix.s b/libpsn00b/psxgte/mulmatrix.s
deleted file mode 100644
index 08c79c2..0000000
--- a/libpsn00b/psxgte/mulmatrix.s
+++ /dev/null
@@ -1,74 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global MulMatrix
-.type MulMatrix, @function
-MulMatrix:
-
-	# Load m1 to GTE
-	lw		$t0, 0($a1)
-	lw		$t1, 4($a1)
-	ctc2	$t0, $0
-	ctc2	$t1, $1
-	lw		$t0, 8($a1)
-	lw		$t1, 12($a1)
-	lhu		$t2, 16($a1)
-	ctc2	$t0, $2
-	ctc2	$t1, $3
-	ctc2	$t2, $4
-
-	lhu		$t1, 2*(0+(3*1))($a0)		# Load values for first
-	lhu		$t0, 2*(0+(3*0))($a0)		# R11 R21 R31
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(0+(3*2))($a0)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	lhu		$t1, 2*(1+(3*1))($a0)		# Load values for second
-	lhu		$t0, 2*(1+(3*0))($a0)		# R12 R22 R32
-	MVMVA(1, 0, 0, 3, 0)				# First multiply
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(1+(3*2))($a0)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	mfc2	$t0, C2_IR1					# Store results of first
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(0+(3*0))($a0)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(0+(3*1))($a0)
-	sh		$t0, 2*(0+(3*2))($a0)
-
-	lhu		$t1, 2*(2+(3*1))($a0)		# Load values for third
-	lhu		$t0, 2*(2+(3*0))($a0)		# R13 R23 R33
-	MVMVA(1, 0, 0, 3, 0)				# Second multiply
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(2+(3*2))($a0)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	mfc2	$t0, C2_IR1					# Store results of second
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(1+(3*0))($a0)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(1+(3*1))($a0)
-	sh		$t0, 2*(1+(3*2))($a0)
-	MVMVA(1, 0, 0, 3, 0)				# Third multiply
-
-	mfc2	$t0, C2_IR1					# Store results of third
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(2+(3*0))($a0)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(2+(3*1))($a0)
-	sh		$t0, 2*(2+(3*2))($a0)
-
-	jr		$ra
-	move	$v0, $a0
diff --git a/libpsn00b/psxgte/mulmatrix0.s b/libpsn00b/psxgte/mulmatrix0.s
deleted file mode 100644
index c2fd859..0000000
--- a/libpsn00b/psxgte/mulmatrix0.s
+++ /dev/null
@@ -1,74 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global MulMatrix0
-.type MulMatrix0, @function
-MulMatrix0:
-
-	# Load m1 to GTE
-	lw		$t0, 0($a0)
-	lw		$t1, 4($a0)
-	ctc2	$t0, $0
-	ctc2	$t1, $1
-	lw		$t0, 8($a0)
-	lw		$t1, 12($a0)
-	lhu		$t2, 16($a0)
-	ctc2	$t0, $2
-	ctc2	$t1, $3
-	ctc2	$t2, $4
-
-	lhu		$t1, 2*(0+(3*1))($a1)		# Load values for first
-	lhu		$t0, 2*(0+(3*0))($a1)		# R11 R21 R31
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(0+(3*2))($a1)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	lhu		$t1, 2*(1+(3*1))($a1)		# Load values for second
-	lhu		$t0, 2*(1+(3*0))($a1)		# R12 R22 R32
-	MVMVA(1, 0, 0, 3, 0)				# First multiply
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(1+(3*2))($a1)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	mfc2	$t0, C2_IR1					# Store results of first
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(0+(3*0))($a2)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(0+(3*1))($a2)
-	sh		$t0, 2*(0+(3*2))($a2)
-
-	lhu		$t1, 2*(2+(3*1))($a1)		# Load values for third
-	lhu		$t0, 2*(2+(3*0))($a1)		# R13 R23 R33
-	MVMVA(1, 0, 0, 3, 0)				# Second multiply
-	sll		$t1, 16
-	or		$t0, $t1
-	lhu		$t1, 2*(2+(3*2))($a1)
-	mtc2	$t0, C2_VXY0
-	mtc2	$t1, C2_VZ0
-
-	mfc2	$t0, C2_IR1					# Store results of second
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(1+(3*0))($a2)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(1+(3*1))($a2)
-	sh		$t0, 2*(1+(3*2))($a2)
-	MVMVA(1, 0, 0, 3, 0)				# Third multiply
-
-	mfc2	$t0, C2_IR1					# Store results of third
-	mfc2	$t1, C2_IR2
-	sh		$t0, 2*(2+(3*0))($a2)
-	mfc2	$t0, C2_IR3
-	sh		$t1, 2*(2+(3*1))($a2)
-	sh		$t0, 2*(2+(3*2))($a2)
-
-	jr		$ra
-	move	$v0, $a2
diff --git a/libpsn00b/psxgte/pushpopmatrix.s b/libpsn00b/psxgte/pushpopmatrix.s
deleted file mode 100644
index ca6b992..0000000
--- a/libpsn00b/psxgte/pushpopmatrix.s
+++ /dev/null
@@ -1,68 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global PushMatrix
-.type PushMatrix, @function
-PushMatrix:
-	la		$a0, _matrix_stack
-	cfc2	$v0, C2_R11R12
-	cfc2	$v1, C2_R13R21
-	sw		$v0, 0($a0)
-	cfc2	$v0, C2_R22R23
-	sw		$v1, 4($a0)
-	sw		$v0, 8($a0)
-	cfc2	$v0, C2_R31R32
-	cfc2	$v1, C2_R33
-	sw		$v0, 12($a0)
-	sw		$v1, 16($a0)
-	cfc2	$v0, C2_TRX
-	cfc2	$v1, C2_TRY
-	sw		$v0, 20($a0)
-	cfc2	$v0, C2_TRZ
-	sw		$v1, 24($a0)
-	jr		$ra
-	sw		$v0, 28($a0)
-
-.global PopMatrix
-.type PopMatrix, @function
-PopMatrix:
-	la		$a0, _matrix_stack
-	lw		$v0, 0($a0)
-	lw		$v1, 4($a0)
-	ctc2	$v0, C2_R11R12
-	ctc2	$v1, C2_R13R21
-	lw		$v0, 8($a0)
-	lw		$v1, 12($a0)
-	ctc2	$v0, C2_R22R23
-	lw		$v0, 16($a0)
-	ctc2	$v1, C2_R31R32
-	ctc2	$v0, C2_R33
-	lw		$v0, 20($a0)
-	lw		$v1, 24($a0)
-	ctc2	$v0, C2_TRX
-	lw		$v0, 28($a0)
-	ctc2	$v1, C2_TRY
-	ctc2	$v0, C2_TRZ
-	jr		$ra
-	nop
-
-
-.section .data
-
-
-.type matrix_stack, @object
-_matrix_stack:
-	.word 0
-	.word 0
-	.word 0
-	.word 0
-	.word 0
-	.word 0
-	.word 0
-	.word 0
-
diff --git a/libpsn00b/psxgte/scalematrix.s b/libpsn00b/psxgte/scalematrix.s
deleted file mode 100644
index 1b2b6dd..0000000
--- a/libpsn00b/psxgte/scalematrix.s
+++ /dev/null
@@ -1,68 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global ScaleMatrix
-.type ScaleMatrix, @function
-ScaleMatrix:
-
-	lwc2	C2_IR0,	0($a1)			# X
-
-	lh		$v0, 2*(0+(3*0))($a0)
-	lh		$v1, 2*(0+(3*1))($a0)
-	mtc2	$v0, C2_IR1
-	lh		$v0, 2*(0+(3*2))($a0)
-	mtc2	$v1, C2_IR2
-	mtc2	$v0, C2_IR3
-
-	nGPF(1)
-
-	mfc2	$v0, C2_IR1
-	mfc2	$v1, C2_IR2
-	sh		$v0, 2*(0+(3*0))($a0)
-	mfc2	$v0, C2_IR3
-	sh		$v1, 2*(0+(3*1))($a0)
-	sh		$v0, 2*(0+(3*2))($a0)
-
-	lwc2	C2_IR0,	4($a1)			# Y
-
-	lh		$v0, 2*(1+(3*0))($a0)
-	lh		$v1, 2*(1+(3*1))($a0)
-	mtc2	$v0, C2_IR1
-	lh		$v0, 2*(1+(3*2))($a0)
-	mtc2	$v1, C2_IR2
-	mtc2	$v0, C2_IR3
-
-	nGPF(1)
-
-	mfc2	$v0, C2_IR1
-	mfc2	$v1, C2_IR2
-	sh		$v0, 2*(1+(3*0))($a0)
-	mfc2	$v0, C2_IR3
-	sh		$v1, 2*(1+(3*1))($a0)
-	sh		$v0, 2*(1+(3*2))($a0)
-
-	lwc2	C2_IR0,	8($a1)			# Z
-
-	lh		$v0, 2*(2+(3*0))($a0)
-	lh		$v1, 2*(2+(3*1))($a0)
-	mtc2	$v0, C2_IR1
-	lh		$v0, 2*(2+(3*2))($a0)
-	mtc2	$v1, C2_IR2
-	mtc2	$v0, C2_IR3
-
-	nGPF(1)
-
-	mfc2	$v0, C2_IR1
-	mfc2	$v1, C2_IR2
-	sh		$v0, 2*(2+(3*0))($a0)
-	mfc2	$v0, C2_IR3
-	sh		$v1, 2*(2+(3*1))($a0)
-	sh		$v0, 2*(2+(3*2))($a0)
-
-	jr		$ra
-	move	$v0, $a0
diff --git a/libpsn00b/psxgte/scalematrixl.s b/libpsn00b/psxgte/scalematrixl.s
deleted file mode 100644
index 53c2d14..0000000
--- a/libpsn00b/psxgte/scalematrixl.s
+++ /dev/null
@@ -1,68 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global ScaleMatrixL
-.type ScaleMatrixL, @function
-ScaleMatrixL:
-
-	lwc2	C2_IR0,	0($a1)			# X
-
-	lh		$v0, 2*(0+(3*0))($a0)
-	lh		$v1, 2*(1+(3*0))($a0)
-	mtc2	$v0, C2_IR1
-	lh		$v0, 2*(2+(3*0))($a0)
-	mtc2	$v1, C2_IR2
-	mtc2	$v0, C2_IR3
-
-	nGPF(1)
-
-	mfc2	$v0, C2_IR1
-	mfc2	$v1, C2_IR2
-	sh		$v0, 2*(0+(3*0))($a0)
-	mfc2	$v0, C2_IR3
-	sh		$v1, 2*(1+(3*0))($a0)
-	sh		$v0, 2*(2+(3*0))($a0)
-
-	lwc2	C2_IR0,	4($a1)			# Y
-
-	lh		$v0, 2*(0+(3*1))($a0)
-	lh		$v1, 2*(1+(3*1))($a0)
-	mtc2	$v0, C2_IR1
-	lh		$v0, 2*(2+(3*1))($a0)
-	mtc2	$v1, C2_IR2
-	mtc2	$v0, C2_IR3
-
-	nGPF(1)
-
-	mfc2	$v0, C2_IR1
-	mfc2	$v1, C2_IR2
-	sh		$v0, 2*(0+(3*1))($a0)
-	mfc2	$v0, C2_IR3
-	sh		$v1, 2*(1+(3*1))($a0)
-	sh		$v0, 2*(2+(3*1))($a0)
-
-	lwc2	C2_IR0,	8($a1)			# Z
-
-	lh		$v0, 2*(0+(3*2))($a0)
-	lh		$v1, 2*(1+(3*2))($a0)
-	mtc2	$v0, C2_IR1
-	lh		$v0, 2*(2+(3*2))($a0)
-	mtc2	$v1, C2_IR2
-	mtc2	$v0, C2_IR3
-
-	nGPF(1)
-
-	mfc2	$v0, C2_IR1
-	mfc2	$v1, C2_IR2
-	sh		$v0, 2*(0+(3*2))($a0)
-	mfc2	$v0, C2_IR3
-	sh		$v1, 2*(1+(3*2))($a0)
-	sh		$v0, 2*(2+(3*2))($a0)
-
-	jr		$ra
-	move	$v0, $a0
diff --git a/libpsn00b/psxgte/square0.s b/libpsn00b/psxgte/square0.s
deleted file mode 100644
index a8ca107..0000000
--- a/libpsn00b/psxgte/square0.s
+++ /dev/null
@@ -1,27 +0,0 @@
-.set noreorder
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-
-.global Square0
-.type Square0, @function
-Square0:
-
-	# a0 - Pointer to input vector (v0)
-	# a1 - Pointer to output vector (v1)
-
-	lwc2	C2_IR1, 0($a0)
-	lwc2	C2_IR2, 4($a0)
-	lwc2	C2_IR3, 8($a0)
-
-	nSQR(0)
-
-	swc2	C2_IR1, 0($a1)
-	swc2	C2_IR2, 4($a1)
-	swc2	C2_IR3, 8($a1)
-
-	jr		$ra
-	nop
diff --git a/libpsn00b/psxgte/squareroot.s b/libpsn00b/psxgte/squareroot.s
index 72198b4..4377ee4 100644
--- a/libpsn00b/psxgte/squareroot.s
+++ b/libpsn00b/psxgte/squareroot.s
@@ -3,10 +3,7 @@
 .include "gtereg.inc"
 .include "inline_s.inc"
 
-.section .text
-
-# Implementation based from Sony libs
-
+.section .text.SquareRoot12
 .global SquareRoot12
 .type SquareRoot12, @function
 SquareRoot12:
@@ -34,27 +31,26 @@ SquareRoot12:
 .Lvalue_greater12:
 	addi	$t4, -64
 	sll		$t4, 1
-	la		$t5, sqrt_table
+	la		$t5, _sqrt_table
 	addu	$t5, $t4
 	lh		$t5, 0($t5)
 	nop
-	
+
 	bltz	$t1, .L1594c
 	nop
 	jr		$ra
 	sllv	$v0, $t5, $t1
-	
-.L1594c:
 
+.L1594c:
 	sub		$t1, $0 , $t1
 	jr		$ra
 	srl		$v0, $t5, $t1
-	
+
 .Lbad_sqr12:
 	jr		$ra
 	move	$v0, $0
-	
-	
+
+.section .text.SquareRoot0
 .global SquareRoot0
 .type SquareRoot0, @function
 SquareRoot0:
@@ -82,7 +78,7 @@ SquareRoot0:
 .Lvalue_greater:
 	addi	$t4, -64
 	sll		$t4, 1
-	la		$t5, sqrt_table
+	la		$t5, _sqrt_table
 	addu	$t5, $t4
 	lh		$t5, 0($t5)
 	nop
@@ -92,32 +88,31 @@ SquareRoot0:
 .Lbad_sqr:
 	jr		$ra
 	move	$v0, $0
-	
 
-.section .data
-	
-sqrt_table:
-	.hword	0x1000,0x101f,0x103f,0x105e,0x107e,0x109c,0x10bb,0x10da
-	.hword	0x10f8,0x1116,0x1134,0x1152,0x116f,0x118c,0x11a9,0x11c6
-	.hword	0x11e3,0x1200,0x121c,0x1238,0x1254,0x1270,0x128c,0x12a7
-	.hword	0x12c2,0x12de,0x12f9,0x1314,0x132e,0x1349,0x1364,0x137e
-	.hword	0x1398,0x13b2,0x13cc,0x13e6,0x1400,0x1419,0x1432,0x144c
-	.hword	0x1465,0x147e,0x1497,0x14b0,0x14c8,0x14e1,0x14f9,0x1512
-	.hword	0x152a,0x1542,0x155a,0x1572,0x158a,0x15a2,0x15b9,0x15d1
-	.hword	0x15e8,0x1600,0x1617,0x162e,0x1645,0x165c,0x1673,0x1689
-	.hword	0x16a0,0x16b7,0x16cd,0x16e4,0x16fa,0x1710,0x1726,0x173c
-	.hword	0x1752,0x1768,0x177e,0x1794,0x17aa,0x17bf,0x17d5,0x17ea
-	.hword	0x1800,0x1815,0x182a,0x183f,0x1854,0x1869,0x187e,0x1893
-	.hword	0x18a8,0x18bd,0x18d1,0x18e6,0x18fa,0x190f,0x1923,0x1938
-	.hword	0x194c,0x1960,0x1974,0x1988,0x199c,0x19b0,0x19c4,0x19d8
-	.hword	0x19ec,0x1a00,0x1a13,0x1a27,0x1a3a,0x1a4e,0x1a61,0x1a75
-	.hword	0x1a88,0x1a9b,0x1aae,0x1ac2,0x1ad5,0x1ae8,0x1afb,0x1b0e
-	.hword	0x1b21,0x1b33,0x1b46,0x1b59,0x1b6c,0x1b7e,0x1b91,0x1ba3
-	.hword	0x1bb6,0x1bc8,0x1bdb,0x1bed,0x1c00,0x1c12,0x1c24,0x1c36
-	.hword	0x1c48,0x1c5a,0x1c6c,0x1c7e,0x1c90,0x1ca2,0x1cb4,0x1cc6
-	.hword	0x1cd8,0x1ce9,0x1cfb,0x1d0d,0x1d1e,0x1d30,0x1d41,0x1d53
-	.hword	0x1d64,0x1d76,0x1d87,0x1d98,0x1daa,0x1dbb,0x1dcc,0x1ddd
-	.hword	0x1dee,0x1e00,0x1e11,0x1e22,0x1e33,0x1e43,0x1e54,0x1e65
-	.hword	0x1e76,0x1e87,0x1e98,0x1ea8,0x1eb9,0x1eca,0x1eda,0x1eeb
-	.hword	0x1efb,0x1f0c,0x1f1c,0x1f2d,0x1f3d,0x1f4e,0x1f5e,0x1f6e
-	.hword	0x1f7e,0x1f8f,0x1f9f,0x1faf,0x1fbf,0x1fcf,0x1fdf,0x1fef
\ No newline at end of file
+.section .data._sqrt_table
+.type _sqrt_table, @object
+_sqrt_table:
+	.hword 0x1000, 0x101f, 0x103f, 0x105e, 0x107e, 0x109c, 0x10bb, 0x10da
+	.hword 0x10f8, 0x1116, 0x1134, 0x1152, 0x116f, 0x118c, 0x11a9, 0x11c6
+	.hword 0x11e3, 0x1200, 0x121c, 0x1238, 0x1254, 0x1270, 0x128c, 0x12a7
+	.hword 0x12c2, 0x12de, 0x12f9, 0x1314, 0x132e, 0x1349, 0x1364, 0x137e
+	.hword 0x1398, 0x13b2, 0x13cc, 0x13e6, 0x1400, 0x1419, 0x1432, 0x144c
+	.hword 0x1465, 0x147e, 0x1497, 0x14b0, 0x14c8, 0x14e1, 0x14f9, 0x1512
+	.hword 0x152a, 0x1542, 0x155a, 0x1572, 0x158a, 0x15a2, 0x15b9, 0x15d1
+	.hword 0x15e8, 0x1600, 0x1617, 0x162e, 0x1645, 0x165c, 0x1673, 0x1689
+	.hword 0x16a0, 0x16b7, 0x16cd, 0x16e4, 0x16fa, 0x1710, 0x1726, 0x173c
+	.hword 0x1752, 0x1768, 0x177e, 0x1794, 0x17aa, 0x17bf, 0x17d5, 0x17ea
+	.hword 0x1800, 0x1815, 0x182a, 0x183f, 0x1854, 0x1869, 0x187e, 0x1893
+	.hword 0x18a8, 0x18bd, 0x18d1, 0x18e6, 0x18fa, 0x190f, 0x1923, 0x1938
+	.hword 0x194c, 0x1960, 0x1974, 0x1988, 0x199c, 0x19b0, 0x19c4, 0x19d8
+	.hword 0x19ec, 0x1a00, 0x1a13, 0x1a27, 0x1a3a, 0x1a4e, 0x1a61, 0x1a75
+	.hword 0x1a88, 0x1a9b, 0x1aae, 0x1ac2, 0x1ad5, 0x1ae8, 0x1afb, 0x1b0e
+	.hword 0x1b21, 0x1b33, 0x1b46, 0x1b59, 0x1b6c, 0x1b7e, 0x1b91, 0x1ba3
+	.hword 0x1bb6, 0x1bc8, 0x1bdb, 0x1bed, 0x1c00, 0x1c12, 0x1c24, 0x1c36
+	.hword 0x1c48, 0x1c5a, 0x1c6c, 0x1c7e, 0x1c90, 0x1ca2, 0x1cb4, 0x1cc6
+	.hword 0x1cd8, 0x1ce9, 0x1cfb, 0x1d0d, 0x1d1e, 0x1d30, 0x1d41, 0x1d53
+	.hword 0x1d64, 0x1d76, 0x1d87, 0x1d98, 0x1daa, 0x1dbb, 0x1dcc, 0x1ddd
+	.hword 0x1dee, 0x1e00, 0x1e11, 0x1e22, 0x1e33, 0x1e43, 0x1e54, 0x1e65
+	.hword 0x1e76, 0x1e87, 0x1e98, 0x1ea8, 0x1eb9, 0x1eca, 0x1eda, 0x1eeb
+	.hword 0x1efb, 0x1f0c, 0x1f1c, 0x1f2d, 0x1f3d, 0x1f4e, 0x1f5e, 0x1f6e
+	.hword 0x1f7e, 0x1f8f, 0x1f9f, 0x1faf, 0x1fbf, 0x1fcf, 0x1fdf, 0x1fef
diff --git a/libpsn00b/psxgte/vector.s b/libpsn00b/psxgte/vector.s
new file mode 100644
index 0000000..2f2f8d6
--- /dev/null
+++ b/libpsn00b/psxgte/vector.s
@@ -0,0 +1,123 @@
+.set noreorder
+.set noat
+
+.include "gtereg.inc"
+.include "inline_s.inc"
+
+.section .text.Square0
+.global Square0
+.type Square0, @function
+Square0:
+	# a0 - Pointer to input vector (v0)
+	# a1 - Pointer to output vector (v1)
+
+	lwc2	C2_IR1, 0($a0)
+	lwc2	C2_IR2, 4($a0)
+	lwc2	C2_IR3, 8($a0)
+
+	nSQR(0)
+
+	swc2	C2_IR1, 0($a1)
+	swc2	C2_IR2, 4($a1)
+	swc2	C2_IR3, 8($a1)
+
+	jr		$ra
+	nop
+
+.section .text.VectorNormalS
+.global VectorNormalS
+.type VectorNormalS, @function
+VectorNormalS:
+	
+	lw		$t0, 0($a0)
+	lw		$t1, 4($a0)
+	lw		$t2, 8($a0)
+	
+	mtc2	$t0, C2_IR1
+	mtc2	$t1, C2_IR2
+	mtc2	$t2, C2_IR3
+	
+	nSQR(0)
+	
+	mfc2	$t3, C2_MAC1
+	mfc2	$t4, C2_MAC2
+	mfc2	$t5, C2_MAC3
+	
+	add		$t3, $t4
+	add		$v0, $t3, $t5
+	mtc2	$v0, C2_LZCS
+	nop
+	nop
+	mfc2	$v1, C2_LZCR
+	
+	addiu	$at, $0 , -2
+	and		$v1, $at
+	
+	addiu	$t6, $0 , 0x1f
+	sub		$t6, $v1
+	sra		$t6, 1
+	addiu	$t3, $v1, -24
+	
+	bltz	$t3, .Lvalue_neg
+	nop
+	b		.Lvalue_pos
+	sllv	$t4, $v0, $t3
+.Lvalue_neg:
+	addiu	$t3, $0 , 24
+	sub		$t3, $v1
+	srav	$t4, $v0, $t3
+.Lvalue_pos:
+	addi	$t4, -64
+	sll		$t4, 1
+	
+	la		$t5, _norm_table
+	addu	$t5, $t4
+	lh		$t5, 0($t5)
+	
+	mtc2	$t0, C2_IR1
+	mtc2	$t1, C2_IR2
+	mtc2	$t2, C2_IR3
+	mtc2	$t5, C2_IR0
+	
+	nGPF(0)
+	
+	mfc2	$t0, C2_MAC1
+	mfc2	$t1, C2_MAC2
+	mfc2	$t2, C2_MAC3
+	
+	sra		$t0, $t6
+	sra		$t1, $t6
+	sra		$t2, $t6
+	
+	sh		$t0, 0($a1)
+	sh		$t1, 2($a1)
+	jr		$ra
+	sh		$t2, 4($a1)
+
+.section .data._norm_table
+.type _norm_table, @object
+_norm_table:
+	.hword 0x1000, 0x0fe0, 0x0fc1, 0x0fa3, 0x0f85, 0x0f68, 0x0f4c, 0x0f30
+	.hword 0x0f15, 0x0efb, 0x0ee1, 0x0ec7, 0x0eae, 0x0e96, 0x0e7e, 0x0e66
+	.hword 0x0e4f, 0x0e38, 0x0e22, 0x0e0c, 0x0df7, 0x0de2, 0x0dcd, 0x0db9
+	.hword 0x0da5, 0x0d91, 0x0d7e, 0x0d6b, 0x0d58, 0x0d45, 0x0d33, 0x0d21
+	.hword 0x0d10, 0x0cff, 0x0cee, 0x0cdd, 0x0ccc, 0x0cbc, 0x0cac, 0x0c9c
+	.hword 0x0c8d, 0x0c7d, 0x0c6e, 0x0c5f, 0x0c51, 0x0c42, 0x0c34, 0x0c26
+	.hword 0x0c18, 0x0c0a, 0x0bfd, 0x0bef, 0x0be2, 0x0bd5, 0x0bc8, 0x0bbb
+	.hword 0x0baf, 0x0ba2, 0x0b96, 0x0b8a, 0x0b7e, 0x0b72, 0x0b67, 0x0b5b
+	.hword 0x0b50, 0x0b45, 0x0b39, 0x0b2e, 0x0b24, 0x0b19, 0x0b0e, 0x0b04
+	.hword 0x0af9, 0x0aef, 0x0ae5, 0x0adb, 0x0ad1, 0x0ac7, 0x0abd, 0x0ab4
+	.hword 0x0aaa, 0x0aa1, 0x0a97, 0x0a8e, 0x0a85, 0x0a7c, 0x0a73, 0x0a6a
+	.hword 0x0a61, 0x0a59, 0x0a50, 0x0a47, 0x0a3f, 0x0a37, 0x0a2e, 0x0a26
+	.hword 0x0a1e, 0x0a16, 0x0a0e, 0x0a06, 0x09fe, 0x09f6, 0x09ef, 0x09e7
+	.hword 0x09e0, 0x09d8, 0x09d1, 0x09c9, 0x09c2, 0x09bb, 0x09b4, 0x09ad
+	.hword 0x09a5, 0x099e, 0x0998, 0x0991, 0x098a, 0x0983, 0x097c, 0x0976
+	.hword 0x096f, 0x0969, 0x0962, 0x095c, 0x0955, 0x094f, 0x0949, 0x0943
+	.hword 0x093c, 0x0936, 0x0930, 0x092a, 0x0924, 0x091e, 0x0918, 0x0912
+	.hword 0x090d, 0x0907, 0x0901, 0x08fb, 0x08f6, 0x08f0, 0x08eb, 0x08e5
+	.hword 0x08e0, 0x08da, 0x08d5, 0x08cf, 0x08ca, 0x08c5, 0x08bf, 0x08ba
+	.hword 0x08b5, 0x08b0, 0x08ab, 0x08a6, 0x08a1, 0x089c, 0x0897, 0x0892
+	.hword 0x088d, 0x0888, 0x0883, 0x087e, 0x087a, 0x0875, 0x0870, 0x086b
+	.hword 0x0867, 0x0862, 0x085e, 0x0859, 0x0855, 0x0850, 0x084c, 0x0847
+	.hword 0x0843, 0x083e, 0x083a, 0x0836, 0x0831, 0x082d, 0x0829, 0x0824
+	.hword 0x0820, 0x081c, 0x0818, 0x0814, 0x0810, 0x080c, 0x0808, 0x0804
diff --git a/libpsn00b/psxgte/vectornormals.s b/libpsn00b/psxgte/vectornormals.s
deleted file mode 100644
index 85e94e6..0000000
--- a/libpsn00b/psxgte/vectornormals.s
+++ /dev/null
@@ -1,110 +0,0 @@
-.set noreorder
-.set noat
-
-.include "gtereg.inc"
-.include "inline_s.inc"
-
-.section .text
-
-# Implementation based from Sony libs
-
-.global VectorNormalS
-.type VectorNormalS, @function
-VectorNormalS:
-	
-	lw		$t0, 0($a0)
-	lw		$t1, 4($a0)
-	lw		$t2, 8($a0)
-	
-	mtc2	$t0, C2_IR1
-	mtc2	$t1, C2_IR2
-	mtc2	$t2, C2_IR3
-	
-	nSQR(0)
-	
-	mfc2	$t3, C2_MAC1
-	mfc2	$t4, C2_MAC2
-	mfc2	$t5, C2_MAC3
-	
-	add		$t3, $t4
-	add		$v0, $t3, $t5
-	mtc2	$v0, C2_LZCS
-	nop
-	nop
-	mfc2	$v1, C2_LZCR
-	
-	addiu	$at, $0 , -2
-	and		$v1, $at
-	
-	addiu	$t6, $0 , 0x1f
-	sub		$t6, $v1
-	sra		$t6, 1
-	addiu	$t3, $v1, -24
-	
-	bltz	$t3, .Lvalue_neg
-	nop
-	b		.Lvalue_pos
-	sllv	$t4, $v0, $t3
-.Lvalue_neg:
-	addiu	$t3, $0 , 24
-	sub		$t3, $v1
-	srav	$t4, $v0, $t3
-.Lvalue_pos:
-	addi	$t4, -64
-	sll		$t4, 1
-	
-	la		$t5, _norm_table
-	addu	$t5, $t4
-	lh		$t5, 0($t5)
-	
-	mtc2	$t0, C2_IR1
-	mtc2	$t1, C2_IR2
-	mtc2	$t2, C2_IR3
-	mtc2	$t5, C2_IR0
-	
-	nGPF(0)
-	
-	mfc2	$t0, C2_MAC1
-	mfc2	$t1, C2_MAC2
-	mfc2	$t2, C2_MAC3
-	
-	sra		$t0, $t6
-	sra		$t1, $t6
-	sra		$t2, $t6
-	
-	sh		$t0, 0($a1)
-	sh		$t1, 2($a1)
-	jr		$ra
-	sh		$t2, 4($a1)
-
-
-.section .data
-
-.global _norm_table
-.type _norm_table, @object
-_norm_table:
-	.hword	0x1000, 0x0FE0, 0x0FC1, 0x0FA3, 0x0F85, 0x0F68, 0x0F4C, 0x0F30
-	.hword	0x0F15, 0x0EFB, 0x0EE1, 0x0EC7, 0x0EAE, 0x0E96, 0x0E7E, 0x0E66
-	.hword	0x0E4F, 0x0E38, 0x0E22, 0x0E0C, 0x0DF7, 0x0DE2, 0x0DCD, 0x0DB9
-	.hword	0x0DA5, 0x0D91, 0x0D7E, 0x0D6B, 0x0D58, 0x0D45, 0x0D33, 0x0D21
-	.hword	0x0D10, 0x0CFF, 0x0CEE, 0x0CDD, 0x0CCC, 0x0CBC, 0x0CAC, 0x0C9C
-	.hword	0x0C8D, 0x0C7D, 0x0C6E, 0x0C5F, 0x0C51, 0x0C42, 0x0C34, 0x0C26
-	.hword	0x0C18, 0x0C0A, 0x0BFD, 0x0BEF, 0x0BE2, 0x0BD5, 0x0BC8, 0x0BBB
-	.hword	0x0BAF, 0x0BA2, 0x0B96, 0x0B8A, 0x0B7E, 0x0B72, 0x0B67, 0x0B5B
-	.hword	0x0B50, 0x0B45, 0x0B39, 0x0B2E, 0x0B24, 0x0B19, 0x0B0E, 0x0B04
-	.hword	0x0AF9, 0x0AEF, 0x0AE5, 0x0ADB, 0x0AD1, 0x0AC7, 0x0ABD, 0x0AB4
-	.hword	0x0AAA, 0x0AA1, 0x0A97, 0x0A8E, 0x0A85, 0x0A7C, 0x0A73, 0x0A6A
-	.hword	0x0A61, 0x0A59, 0x0A50, 0x0A47, 0x0A3F, 0x0A37, 0x0A2E, 0x0A26
-	.hword	0x0A1E, 0x0A16, 0x0A0E, 0x0A06, 0x09FE, 0x09F6, 0x09EF, 0x09E7
-	.hword	0x09E0, 0x09D8, 0x09D1, 0x09C9, 0x09C2, 0x09BB, 0x09B4, 0x09AD
-	.hword	0x09A5, 0x099E, 0x0998, 0x0991, 0x098A, 0x0983, 0x097C, 0x0976
-	.hword	0x096F, 0x0969, 0x0962, 0x095C, 0x0955, 0x094F, 0x0949, 0x0943
-	.hword	0x093C, 0x0936, 0x0930, 0x092A, 0x0924, 0x091E, 0x0918, 0x0912
-	.hword	0x090D, 0x0907, 0x0901, 0x08FB, 0x08F6, 0x08F0, 0x08EB, 0x08E5
-	.hword	0x08E0, 0x08DA, 0x08D5, 0x08CF, 0x08CA, 0x08C5, 0x08BF, 0x08BA
-	.hword	0x08B5, 0x08B0, 0x08AB, 0x08A6, 0x08A1, 0x089C, 0x0897, 0x0892
-	.hword	0x088D, 0x0888, 0x0883, 0x087E, 0x087A, 0x0875, 0x0870, 0x086B
-	.hword	0x0867, 0x0862, 0x085E, 0x0859, 0x0855, 0x0850, 0x084C, 0x0847
-	.hword	0x0843, 0x083E, 0x083A, 0x0836, 0x0831, 0x082D, 0x0829, 0x0824
-	.hword	0x0820, 0x081C, 0x0818, 0x0814, 0x0810, 0x080C, 0x0808, 0x0804
-	
\ No newline at end of file