`sys_matrix` OK, documented, rename some other mathematics functions (#787)

* Import data and bss, add some name options

* Decomp rest of stack functions,
remove RSPMatrix,
split header

* sys_matrix OK (thanks Tharo)

Co-authored-by: Tharo <17233964+Thar0@users.noreply.github.com>

* pragma weaks in sinf and cosf, change documentation
change mcros to use 0x8000

* Rename sinf and cosf

* Correct initialisation of sMtxFClear

* More minor cleanup

* Rename stack functions

* Matrix_InsertMatrix -> Matrix_Mult

* The big transpose rename

* Fix MirRay_DrawReflectedBeam

* Matrix_InsertTranslation -> Matrix_Translate

* Matrix_Insert.Rotation_s -> Matrix_Rotate$1S
+ RotateY -> RotateYS

* Matrix_Insert(.)Rotation_f -> Matrix_Rotate$1F

* Matrix_RotateStateAroundXAxis -> Matrix_RotateXFApply
Matrix_SetStateXRotation -> Matrix_RotateXFNew

* Matrix_MultiplyVector3fByState -> Matrix_MultVec3f

* Matrix_MultiplyVector3fXZByCurrentState -> Matrix_MultVec3fXZ

* Matrix_GetStateTranslation -> Matrix_MultZero

* Matrix_GetStateTranslationAndScaled(.) -> Matrix_MultVec$1

* Matrix_FromRSPMatrix -> Matrix_MtxToMtxF

* Matrix_MultiplyVector3fByMatrix -> Matrix_MultVec3fExt

* Matrix_TransposeXYZ -> Matrix_Transpose

* Matrix_ToRSPMatrix -> Matrix_MtxFToMtx

* Matrix_AppendToPolyOpaDisp -> Matrix_MtxFToNewMtx
and document the conversion functions

* Matrix_NormalizeXYZ -> Matrix_ReplaceRotation

* Matrix_InsertRotationAroundUnitVector_f -> Matrix_RotateAxisF and S

* Matrix_InsertRotation -> Matrix_RotateZYX

* Document the last functions

* Small cleanup

* Matrix_JointPosition -> Matrix_TranslateRotateZYX

* Matrix_SetStateRotationAndTranslation -> Matrix_SetTranslateRotateYXZ

* func_8018219C -> Matrix_MtxFToYXZRot

* func_801822C4 -> Matrix_MtxFToZYXRot

* Fix files

* Format

* Review 1

* Renames

* Fix warning in EnDragon

* Format

* Convert `mode` to an actual enum

* Add enums, typedefs, externs to sys_matrix header

* Review

* One more

* More review

* Fix function names

* Format

* Fix names

* Format

* Review

* engineer's review

* Fix build

* Format

* Fix again

Co-authored-by: Tharo <17233964+Thar0@users.noreply.github.com>

7 files changed, 422 insertions, 132 deletions

diff --git a/include/PR/gbi.h b/include/PR/gbi.h
index ace2cd2e8..8e5aefe8e 100644
--- a/include/PR/gbi.h
+++ b/include/PR/gbi.h
@@ -1347,60 +1347,329 @@ typedef union {
     long int	force_structure_alignment[4];
 } Hilite;
 
-#define gdSPDefLights0(ar,ag,ab)					\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ { 0, 0, 0},0,{ 0, 0, 0},0,{ 0, 0, 0},0}}} }
-#define gdSPDefLights1(ar,ag,ab,r1,g1,b1,x1,y1,z1)			\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}}} }
-#define gdSPDefLights2(ar,ag,ab,r1,g1,b1,x1,y1,z1,r2,g2,b2,x2,y2,z2)	\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}},	\
-			{{ {r2,g2,b2},0,{r2,g2,b2},0,{x2,y2,z2},0}}} }
-#define gdSPDefLights3(ar,ag,ab,r1,g1,b1,x1,y1,z1,r2,g2,b2,x2,y2,z2,r3,g3,b3,x3,y3,z3)									\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}},	\
-			{{ {r2,g2,b2},0,{r2,g2,b2},0,{x2,y2,z2},0}},	\
-			{{ {r3,g3,b3},0,{r3,g3,b3},0,{x3,y3,z3},0}}} }
-#define gdSPDefLights4(ar,ag,ab,r1,g1,b1,x1,y1,z1,r2,g2,b2,x2,y2,z2,r3,g3,b3,x3,y3,z3,r4,g4,b4,x4,y4,z4)						\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}},	\
-			{{ {r2,g2,b2},0,{r2,g2,b2},0,{x2,y2,z2},0}},	\
-			{{ {r3,g3,b3},0,{r3,g3,b3},0,{x3,y3,z3},0}},	\
-			{{ {r4,g4,b4},0,{r4,g4,b4},0,{x4,y4,z4},0}}} }
-#define gdSPDefLights5(ar,ag,ab,r1,g1,b1,x1,y1,z1,r2,g2,b2,x2,y2,z2,r3,g3,b3,x3,y3,z3,r4,g4,b4,x4,y4,z4,r5,g5,b5,x5,y5,z5)				\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}},	\
-			{{ {r2,g2,b2},0,{r2,g2,b2},0,{x2,y2,z2},0}},	\
-			{{ {r3,g3,b3},0,{r3,g3,b3},0,{x3,y3,z3},0}},	\
-			{{ {r4,g4,b4},0,{r4,g4,b4},0,{x4,y4,z4},0}},	\
-			{{ {r5,g5,b5},0,{r5,g5,b5},0,{x5,y5,z5},0}}} }
-
-
-#define gdSPDefLights6(ar,ag,ab,r1,g1,b1,x1,y1,z1,r2,g2,b2,x2,y2,z2,r3,g3,b3,x3,y3,z3,r4,g4,b4,x4,y4,z4,r5,g5,b5,x5,y5,z5,r6,g6,b6,x6,y6,z6)		\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}},	\
-			{{ {r2,g2,b2},0,{r2,g2,b2},0,{x2,y2,z2},0}},	\
-			{{ {r3,g3,b3},0,{r3,g3,b3},0,{x3,y3,z3},0}},	\
-			{{ {r4,g4,b4},0,{r4,g4,b4},0,{x4,y4,z4},0}},	\
-			{{ {r5,g5,b5},0,{r5,g5,b5},0,{x5,y5,z5},0}},	\
-			{{ {r6,g6,b6},0,{r6,g6,b6},0,{x6,y6,z6},0}}} }
-
-
-#define gdSPDefLights7(ar,ag,ab,r1,g1,b1,x1,y1,z1,r2,g2,b2,x2,y2,z2,r3,g3,b3,x3,y3,z3,r4,g4,b4,x4,y4,z4,r5,g5,b5,x5,y5,z5,r6,g6,b6,x6,y6,z6,r7,g7,b7,x7,y7,z7)	\
-		{ 	{{ {ar,ag,ab},0,{ar,ag,ab},0}},			\
-		       {{{ {r1,g1,b1},0,{r1,g1,b1},0,{x1,y1,z1},0}},	\
-			{{ {r2,g2,b2},0,{r2,g2,b2},0,{x2,y2,z2},0}},	\
-			{{ {r3,g3,b3},0,{r3,g3,b3},0,{x3,y3,z3},0}},	\
-			{{ {r4,g4,b4},0,{r4,g4,b4},0,{x4,y4,z4},0}},	\
-			{{ {r5,g5,b5},0,{r5,g5,b5},0,{x5,y5,z5},0}},	\
-			{{ {r6,g6,b6},0,{r6,g6,b6},0,{x6,y6,z6},0}},	\
-			{{ {r7,g7,b7},0,{r7,g7,b7},0,{x7,y7,z7},0}}} }
-
-
-#define gdSPDefLookAt(rightx,righty,rightz,upx,upy,upz)			    \
-		{	{{ {{0,0,0},0,{0,0,0},0,{rightx,righty,rightz},0}}, \
-			{ {{0,0x80,0},0,{0,0x80,0},0,{upx,upy,upz},0}}}   }
+#define gdSPDefLights0(ar,ag,ab)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { 0, 0, 0 }, 0,     \
+                    { 0, 0, 0 }, 0,     \
+                    { 0, 0, 0 }, 0      \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights1(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights2(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1,        \
+                       r2,g2,b2,        \
+                       x2,y2,z2)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }},                     \
+                {{                      \
+                    { r2, g2, b2 }, 0,  \
+                    { r2, g2, b2 }, 0,  \
+                    { x2, y2, z2 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights3(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1,        \
+                       r2,g2,b2,        \
+                       x2,y2,z2,        \
+                       r3,g3,b3,        \
+                       x3,y3,z3)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }},                     \
+                {{                      \
+                    { r2, g2, b2 }, 0,  \
+                    { r2, g2, b2 }, 0,  \
+                    { x2, y2, z2 }, 0   \
+                }},                     \
+                {{                      \
+                    { r3, g3, b3 }, 0,  \
+                    { r3, g3, b3 }, 0,  \
+                    { x3, y3, z3 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights4(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1,        \
+                       r2,g2,b2,        \
+                       x2,y2,z2,        \
+                       r3,g3,b3,        \
+                       x3,y3,z3,        \
+                       r4,g4,b4,        \
+                       x4,y4,z4)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }},                     \
+                {{                      \
+                    { r2, g2, b2 }, 0,  \
+                    { r2, g2, b2 }, 0,  \
+                    { x2, y2, z2 }, 0   \
+                }},                     \
+                {{                      \
+                    { r3, g3, b3 }, 0,  \
+                    { r3, g3, b3 }, 0,  \
+                    { x3, y3, z3 }, 0   \
+                }},                     \
+                {{                      \
+                    { r4, g4, b4 }, 0,  \
+                    { r4, g4, b4 }, 0,  \
+                    { x4, y4, z4 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights5(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1,        \
+                       r2,g2,b2,        \
+                       x2,y2,z2,        \
+                       r3,g3,b3,        \
+                       x3,y3,z3,        \
+                       r4,g4,b4,        \
+                       x4,y4,z4,        \
+                       r5,g5,b5,        \
+                       x5,y5,z5)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }},                     \
+                {{                      \
+                    { r2, g2, b2 }, 0,  \
+                    { r2, g2, b2 }, 0,  \
+                    { x2, y2, z2 }, 0   \
+                }},                     \
+                {{                      \
+                    { r3, g3, b3 }, 0,  \
+                    { r3, g3, b3 }, 0,  \
+                    { x3, y3, z3 }, 0   \
+                }},                     \
+                {{                      \
+                    { r4, g4, b4 }, 0,  \
+                    { r4, g4, b4 }, 0,  \
+                    { x4, y4, z4 }, 0   \
+                }},                     \
+                {{                      \
+                    { r5, g5, b5 }, 0,  \
+                    { r5, g5, b5 }, 0,  \
+                    { x5, y5, z5 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights6(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1,        \
+                       r2,g2,b2,        \
+                       x2,y2,z2,        \
+                       r3,g3,b3,        \
+                       x3,y3,z3,        \
+                       r4,g4,b4,        \
+                       x4,y4,z4,        \
+                       r5,g5,b5,        \
+                       x5,y5,z5,        \
+                       r6,g6,b6,        \
+                       x6,y6,z6)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }},                     \
+                {{                      \
+                    { r2, g2, b2 }, 0,  \
+                    { r2, g2, b2 }, 0,  \
+                    { x2, y2, z2 }, 0   \
+                }},                     \
+                {{                      \
+                    { r3, g3, b3 }, 0,  \
+                    { r3, g3, b3 }, 0,  \
+                    { x3, y3, z3 }, 0   \
+                }},                     \
+                {{                      \
+                    { r4, g4, b4 }, 0,  \
+                    { r4, g4, b4 }, 0,  \
+                    { x4, y4, z4 }, 0   \
+                }},                     \
+                {{                      \
+                    { r5, g5, b5 }, 0,  \
+                    { r5, g5, b5 }, 0,  \
+                    { x5, y5, z5 }, 0   \
+                }},                     \
+                {{                      \
+                    { r6, g6, b6 }, 0,  \
+                    { r6, g6, b6 }, 0,  \
+                    { x6, y6, z6 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLights7(ar,ag,ab,        \
+                       r1,g1,b1,        \
+                       x1,y1,z1,        \
+                       r2,g2,b2,        \
+                       x2,y2,z2,        \
+                       r3,g3,b3,        \
+                       x3,y3,z3,        \
+                       r4,g4,b4,        \
+                       x4,y4,z4,        \
+                       r5,g5,b5,        \
+                       x5,y5,z5,        \
+                       r6,g6,b6,        \
+                       x6,y6,z6,        \
+                       r7,g7,b7,        \
+                       x7,y7,z7)        \
+        {                               \
+            {{                          \
+                { ar, ag, ab }, 0,      \
+                { ar, ag, ab }, 0       \
+            }},                         \
+            {                           \
+                {{                      \
+                    { r1, g1, b1 }, 0,  \
+                    { r1, g1, b1 }, 0,  \
+                    { x1, y1, z1 }, 0   \
+                }},                     \
+                {{                      \
+                    { r2, g2, b2 }, 0,  \
+                    { r2, g2, b2 }, 0,  \
+                    { x2, y2, z2 }, 0   \
+                }},                     \
+                {{                      \
+                    { r3, g3, b3 }, 0,  \
+                    { r3, g3, b3 }, 0,  \
+                    { x3, y3, z3 }, 0   \
+                }},                     \
+                {{                      \
+                    { r4, g4, b4 }, 0,  \
+                    { r4, g4, b4 }, 0,  \
+                    { x4, y4, z4 }, 0   \
+                }},                     \
+                {{                      \
+                    { r5, g5, b5 }, 0,  \
+                    { r5, g5, b5 }, 0,  \
+                    { x5, y5, z5 }, 0   \
+                }},                     \
+                {{                      \
+                    { r6, g6, b6 }, 0,  \
+                    { r6, g6, b6 }, 0,  \
+                    { x6, y6, z6 }, 0   \
+                }},                     \
+                {{                      \
+                    { r7, g7, b7 }, 0,  \
+                    { r7, g7, b7 }, 0,  \
+                    { x7, y7, z7 }, 0   \
+                }}                      \
+            }                           \
+        }
+
+#define gdSPDefLookAt(rightx,righty,rightz,upx,upy,upz) \
+        {{                                              \
+                {{                                      \
+                    { 0, 0, 0 }, 0,                     \
+                    { 0, 0, 0 }, 0,                     \
+                    { rightx, righty, rightz }, 0       \
+                }},                                     \
+                {{                                      \
+                    { 0, 0x80, 0 }, 0,                  \
+                    { 0, 0x80, 0 }, 0,                  \
+                    { upx, upy, upz }, 0                \
+                }}                                      \
+        }}
+
+#define qs1616(e) ((s32)((e) * 0x00010000))
+
+#define IPART(x) ((qs1616(x) >> 16) & 0xFFFF)
+#define FPART(x) (qs1616(x) & 0xFFFF)
+
+#define gdSPDefMtx(     \
+        xx, yx, zx, wx, \
+        xy, yy, zy, wy, \
+        xz, yz, zz, wz, \
+        xw, yw, zw, ww) \
+    {{                                  \
+        (IPART(xx) << 0x10) | IPART(xy),  \
+        (IPART(xz) << 0x10) | IPART(xw),  \
+        (IPART(yx) << 0x10) | IPART(yy),  \
+        (IPART(yz) << 0x10) | IPART(yw),  \
+        (IPART(zx) << 0x10) | IPART(zy),  \
+        (IPART(zz) << 0x10) | IPART(zw),  \
+        (IPART(wx) << 0x10) | IPART(wy),  \
+        (IPART(wz) << 0x10) | IPART(ww),  \
+        (FPART(xx) << 0x10) | FPART(xy),  \
+        (FPART(xz) << 0x10) | FPART(xw),  \
+        (FPART(yx) << 0x10) | FPART(yy),  \
+        (FPART(yz) << 0x10) | FPART(yw),  \
+        (FPART(zx) << 0x10) | FPART(zy),  \
+        (FPART(zz) << 0x10) | FPART(zw),  \
+        (FPART(wx) << 0x10) | FPART(wy),  \
+        (FPART(wz) << 0x10) | FPART(ww),  \
+    }}
 
 /*
  *  Graphics DMA Packet
diff --git a/include/PR/ultratypes.h b/include/PR/ultratypes.h
index 0e6f4cf39..8b4e1a82c 100644
--- a/include/PR/ultratypes.h
+++ b/include/PR/ultratypes.h
@@ -27,6 +27,10 @@ typedef void* TexturePtr;
 typedef long int Mtx_t[4][4];
 typedef union {
     Mtx_t m;
+    struct {
+        u16 intPart[4][4];
+        u16 fracPart[4][4];
+    };
     long long int forc_structure_alignment;
 } Mtx; // size = 0x40
 
@@ -34,10 +38,10 @@ typedef float MtxF_t[4][4];
 typedef union {
     MtxF_t mf;
     struct {
-        float xx, xy, xz, xw,
-              yx, yy, yz, yw,
-              zx, zy, zz, zw,
-              wx, wy, wz, ww;
+        float xx, yx, zx, wx,
+              xy, yy, zy, wy,
+              xz, yz, zz, wz,
+              xw, yw, zw, ww;
     };
 } MtxF; // size = 0x40
 
diff --git a/include/functions.h b/include/functions.h
index 8c04ba259..b4ff2ad8d 100644
--- a/include/functions.h
+++ b/include/functions.h
@@ -148,8 +148,8 @@ void GfxPrint_Open(GfxPrint* printer, Gfx* dList);
 Gfx* GfxPrint_Close(GfxPrint* printer);
 s32 GfxPrint_VPrintf(GfxPrint* printer, const char* fmt, va_list args);
 s32 GfxPrint_Printf(GfxPrint* printer, const char* fmt, ...);
-void MtxConv_F2L(MatrixInternal* m1, MtxF* m2);
-void MtxConv_L2F(MtxF* m1, MatrixInternal* m2);
+void MtxConv_F2L(Mtx* m1, MtxF* m2);
+void MtxConv_L2F(MtxF* m1, Mtx* m2);
 void __assert(const char* file, u32 lineNum);
 // void func_800862B4(void);
 void* SystemArena_MallocMin1(u32 size);
@@ -220,7 +220,7 @@ OSIntMask osSetIntMask(OSIntMask im);
 // OSIntMask osGetIntMask(void);
 // void osVoiceSetWord(void);
 // void guScale(void);
-f32 __sinf(f32 __x);
+f32 sinf(f32 __x);
 s16 sins(u16 x);
 OSTask* _VirtualToPhysicalTask(OSTask* intp);
 void osSpTaskLoad(OSTask* intp);
@@ -377,7 +377,7 @@ void __osRestoreInt(OSIntMask im);
 void __osViInit(void);
 void __osViSwapContext(void);
 OSMesgQueue* osPiGetCmdQueue(void);
-f32 __cosf(f32 __x);
+f32 cosf(f32 __x);
 s32 osEPiReadIo(OSPiHandle* pihandle, u32 devAddr, u32* data);
 void osViSetSpecialFeatures(u32 func);
 s16 coss(u16 x);
@@ -3072,53 +3072,12 @@ s32 Math3D_XYInSphere(Sphere16* sphere, f32 x, f32 y);
 s32 Math3D_YZInSphere(Sphere16* sphere, f32 y, f32 z);
 // void func_8017FB1C(UNK_TYPE1 param_1, UNK_TYPE1 param_2, UNK_TYPE1 param_3, UNK_TYPE1 param_4, UNK_TYPE4 param_5, UNK_TYPE4 param_6, UNK_TYPE4 param_7, UNK_TYPE4 param_8, UNK_TYPE4 param_9, UNK_TYPE4 param_10, UNK_TYPE4 param_11);
 // void func_8017FD44(void);
+
 u16 Math_GetAtan2Tbl(f32 opposite, f32 adjacent);
 s16 Math_Atan2S(f32 opposite, f32 adjacent);
 f32 Math_Atan2F(f32 opposite, f32 adjacent);
 s16 Math_FAtan2F(f32 adjacent, f32 opposite);
 f32 Math_Acot2F(f32 adjacent, f32 opposite);
-void Matrix_StateAlloc(GameState* gameState);
-void Matrix_StatePush(void);
-void Matrix_StatePop(void);
-void Matrix_CopyCurrentState(MtxF* matrix);
-void Matrix_SetCurrentState(MtxF* matrix);
-MtxF* Matrix_GetCurrentState(void);
-void Matrix_InsertMatrix(MtxF* matrix, s32 appendToState);
-void Matrix_InsertTranslation(f32 x, f32 y, f32 z, s32 appendToState);
-void Matrix_Scale(f32 xScale, f32 yScale, f32 zScale, s32 appendToState);
-void Matrix_InsertXRotation_s(s16 rotation, s32 appendToState);
-void Matrix_InsertXRotation_f(f32 rotation, s32 appendToState);
-void Matrix_RotateStateAroundXAxis(f32 rotation);
-void Matrix_SetStateXRotation(f32 rotation);
-void Matrix_RotateY(s16 rotation, s32 appendToState);
-void Matrix_InsertYRotation_f(f32 rotation, s32 appendToState);
-void Matrix_InsertZRotation_s(s16 rotation, s32 appendToState);
-void Matrix_InsertZRotation_f(f32 rotation, s32 appendToState);
-void Matrix_InsertRotation(s16 xRotation, s16 yRotation, s16 zRotation, s32 appendToState);
-void Matrix_JointPosition(Vec3f* position, Vec3s* rotation);
-void Matrix_RotateAndTranslateState(Vec3f* translation, Vec3s* rotation);
-void Matrix_SetStateRotationAndTranslation(f32 x, f32 y, f32 z, Vec3s* rotation);
-RSPMatrix* Matrix_ToRSPMatrix(MtxF* src, RSPMatrix* dst);
-Mtx* Matrix_ToMtx(Mtx* dest);
-RSPMatrix* Matrix_GetStateAsRSPMatrix(RSPMatrix* matrix);
-Mtx* Matrix_NewMtx(GraphicsContext* gfxCtx);
-RSPMatrix* Matrix_AppendStateToPolyOpaDisp(GraphicsContext* gfxCtx);
-void Matrix_AppendToPolyOpaDisp(MtxF* mtx, GraphicsContext* gfxCtx);
-void Matrix_MultiplyVector3fByState(Vec3f* src, Vec3f* dst);
-void Matrix_GetStateTranslation(Vec3f* dst);
-void Matrix_GetStateTranslationAndScaledX(f32 scale, Vec3f* dst);
-void Matrix_GetStateTranslationAndScaledY(f32 scale, Vec3f* dst);
-void Matrix_GetStateTranslationAndScaledZ(f32 scale, Vec3f* dst);
-void Matrix_MultiplyVector3fXZByCurrentState(Vec3f* src, Vec3f* dst);
-void Matrix_MtxFCopy(MtxF* dst, MtxF* src);
-void Matrix_FromRSPMatrix(Mtx* src, MtxF* dst);
-void Matrix_MultiplyVector3fByMatrix(Vec3f* src, Vec3f* dst, MtxF* matrix);
-void Matrix_TransposeXYZ(MtxF* matrix);
-void Matrix_NormalizeXYZ(MtxF* matrix);
-void func_8018219C(MtxF* pfParm1, Vec3s* psParm2, s32 iParm3);
-// void func_801822C4(void);
-void Matrix_InsertRotationAroundUnitVector_f(f32 rotation, Vec3f* vector, s32 appendToState);
-void Matrix_InsertRotationAroundUnitVector_s(s16 rotation, Vec3f* vector, s32 appendToState);
 
 u64* SysUcode_GetUCodeBoot(void);
 size_t SysUcode_GetUCodeBootSize(void);
diff --git a/include/sys_matrix.h b/include/sys_matrix.h
new file mode 100644
index 000000000..536322624
--- /dev/null
+++ b/include/sys_matrix.h
@@ -0,0 +1,81 @@
+#ifndef SYS_MATRIX_H
+#define SYS_MATRIX_H
+
+#include "z64math.h"
+
+
+struct GraphicsContext;
+struct GameState;
+
+
+typedef enum {
+    /* 0 */ MTXMODE_NEW,  // generates a new matrix
+    /* 1 */ MTXMODE_APPLY // applies transformation to the current matrix
+} MatrixMode;
+
+
+extern Mtx gIdentityMtx;
+extern MtxF gIdentityMtxF;
+
+
+/* Stack operations */
+
+void Matrix_Init(struct GameState* gameState);
+void Matrix_Push(void);
+void Matrix_Pop(void);
+void Matrix_Get(MtxF* dest);
+void Matrix_Put(MtxF* src);
+MtxF* Matrix_GetCurrent(void);
+
+/* Basic operations */
+
+void Matrix_Mult(MtxF* matrix, MatrixMode mode);
+void Matrix_Translate(f32 x, f32 y, f32 z, MatrixMode mode);
+void Matrix_Scale(f32 x, f32 y, f32 z, MatrixMode mode);
+void Matrix_RotateXS(s16 x, MatrixMode mode);
+void Matrix_RotateXF(f32 x, MatrixMode mode);
+void Matrix_RotateXFApply(f32 x);
+void Matrix_RotateXFNew(f32 x);
+void Matrix_RotateYS(s16 y, MatrixMode mode);
+void Matrix_RotateYF(f32 y, MatrixMode mode);
+void Matrix_RotateZS(s16 z, MatrixMode mode);
+void Matrix_RotateZF(f32 z, MatrixMode mode);
+
+/* Compound operations */
+
+void Matrix_RotateZYX(s16 x, s16 y, s16 z, MatrixMode mode);
+void Matrix_TranslateRotateZYX(Vec3f* translation, Vec3s* rot);
+void Matrix_SetTranslateRotateYXZ(f32 x, f32 y, f32 z, Vec3s* rot);
+
+/* Conversion and allocation operations */
+
+Mtx* Matrix_MtxFToMtx(MtxF* src, Mtx* dest);
+Mtx* Matrix_ToMtx(Mtx* dest);
+Mtx* Matrix_NewMtx(struct GraphicsContext* gfxCtx);
+Mtx* Matrix_MtxFToNewMtx(MtxF* src, struct GraphicsContext* gfxCtx);
+
+/* Vector operations */
+
+void Matrix_MultVec3f(Vec3f* src, Vec3f* dest);
+void Matrix_MultZero(Vec3f* dest);
+void Matrix_MultVecX(f32 x, Vec3f* dest);
+void Matrix_MultVecY(f32 y, Vec3f* dest);
+void Matrix_MultVecZ(f32 z, Vec3f* dest);
+void Matrix_MultVec3fXZ(Vec3f* src, Vec3f* dest);
+
+/* Copy and another conversion */
+
+void Matrix_MtxFCopy(MtxF* dest, MtxF* src);
+void Matrix_MtxToMtxF(Mtx* src, MtxF* dest);
+
+/* Miscellaneous */
+
+void Matrix_MultVec3fExt(Vec3f* src, Vec3f* dest, MtxF* mf);
+void Matrix_Transpose(MtxF* mf);
+void Matrix_ReplaceRotation(MtxF* mf);
+void Matrix_MtxFToYXZRot(MtxF* src, Vec3s* dest, s32 nonUniformScale);
+void Matrix_MtxFToZYXRot(MtxF* src, Vec3s* dest, s32 nonUniformScale);
+void Matrix_RotateAxisF(f32 angle, Vec3f* axis, MatrixMode mode);
+void Matrix_RotateAxisS(s16 angle, Vec3f* axis, MatrixMode mode);
+
+#endif
diff --git a/include/variables.h b/include/variables.h
index 4acb019a4..c37938555 100644
--- a/include/variables.h
+++ b/include/variables.h
@@ -1574,8 +1574,6 @@ extern UNK_PTR D_801D1540;
 // extern f32 sFactorialTbl[13];
 extern Vec3f gZeroVec3f;
 extern Vec3s gZeroVec3s;
-extern Mtx gIdentityMtx;
-extern MtxF gIdentityMtxF;
 // extern UNK_TYPE1 D_801D1E70;
 extern s16 gLowPassFilterData[];
 extern s16 gHighPassFilterData[];
diff --git a/include/z64.h b/include/z64.h
index e5533234a..7f1dfd701 100644
--- a/include/z64.h
+++ b/include/z64.h
@@ -23,6 +23,7 @@
 #include "sfx.h"
 #include "message_data_static.h"
 
+#include "sys_matrix.h"
 #include "z64actor.h"
 #include "z64animation.h"
 #include "z64audio.h"
@@ -305,11 +306,6 @@ typedef struct {
 } NmiBuff; // size >= 0x18
 
 typedef struct {
-    /* 0x00 */ s16 intPart[16];
-    /* 0x20 */ u16 fracPart[16];
-} RSPMatrix; // size = 0x40
-
-typedef struct {
     /* 0x0 */ s8 letterboxTarget;
     /* 0x1 */ s8 letterboxMagnitude;
     /* 0x2 */ s8 pillarboxTarget;
@@ -383,13 +379,6 @@ typedef struct {
     /* 0x1A */ s16 unk1A;
 } s80874650; // size = 0x1C
 
-typedef struct {
-    /* 0x00 */ f32 x[4];
-    /* 0x10 */ f32 y[4];
-    /* 0x20 */ f32 z[4];
-    /* 0x30 */ f32 w[4];
-} z_Matrix; // size = 0x40
-
 typedef union {
     F3DVertexColor color;
     F3DVertexNormal normal;
@@ -1421,16 +1410,6 @@ typedef struct {
     /* 0x14 */ Gfx* monoDl;
 } VisMono; // size = 0x18
 
-typedef enum {
-    MTXMODE_NEW,  // generates a new matrix
-    MTXMODE_APPLY // applies transformation to the current matrix
-} MatrixMode;
-
-typedef struct {
-    /* 0x00 */ u16 intPart[4][4];
-    /* 0x20 */ u16 fracPart[4][4];
-} MatrixInternal; // size = 0x40
-
 typedef struct DebugDispObject {
     /* 0x00 */ Vec3f pos;
     /* 0x0C */ Vec3s rot;
diff --git a/include/z64math.h b/include/z64math.h
index 78e1743ec..33f05bb0a 100644
--- a/include/z64math.h
+++ b/include/z64math.h
@@ -1,5 +1,5 @@
-#ifndef _Z64MATH_H_
-#define _Z64MATH_H_
+#ifndef Z64MATH_H
+#define Z64MATH_H
 
 #include "ultra64.h"
 
@@ -111,7 +111,7 @@ typedef struct {
 
 // Trig macros
 #define DEGF_TO_BINANG(degreesf) (s16)(degreesf * 182.04167f + .5f)
-#define RADF_TO_BINANG(radf) (s16)(radf * (32768.0f / M_PI))
+#define RADF_TO_BINANG(radf) (s16)(radf * (0x8000 / M_PI))
 #define RADF_TO_DEGF(radf) (radf * (180.0f / M_PI))
 #define DEGF_TO_RADF(degf) (degf * (M_PI / 180.0f))
 #define BINANG_ROT180(angle) ((s16)(angle + 0x8000))
@@ -119,7 +119,7 @@ typedef struct {
 #define BINANG_ADD(a, b) ((s16)(a + b))
 #define DEG_TO_RAD(degrees) ((degrees) * (M_PI / 180.0f))
 #define BINANG_TO_DEGF(binang) ((f32)binang * (360.0001525f / 65535.0f))
-#define BINANG_TO_RAD(binang) (((f32)binang / 32768.0f) * M_PI)
+#define BINANG_TO_RAD(binang) (((f32)binang / 0x8000) * M_PI)
 
 // Vector macros
 #define SQXZ(vec) ((vec.x) * (vec.x) + (vec.z) * (vec.z))