15 files changed, 2298 insertions, 445 deletions

diff --git a/src/code/sys_matrix.c b/src/code/sys_matrix.c
index a09d95200..f2ed19c38 100644
--- a/src/code/sys_matrix.c
+++ b/src/code/sys_matrix.c
@@ -1,89 +1,1944 @@
+/**
+ * @file sys_matrix.c
+ * @brief: Matrix system that mostly uses a matrix stack, and concerns affine transformations.
+ *
+ * @note The RSP matrix format (and hence the `MtxF` format) is column-major: vectors are presumed to be row vectors,
+ * and matrices as a column of row vectors. This means that, for example, a translation matrix
+ * \f[
+ *  \begin{pmatrix}
+ *      1 & 0 & 0 & x \\
+ *      0 & 1 & 0 & y \\
+ *      0 & 0 & 1 & z \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ * will be stored as
+ *
+ *     { { 1, 0, 0, 0 },
+ *       { 0, 1, 0, 0 },
+ *       { 0, 0, 1, 0 },
+ *       { x, y, z, 1 }, }
+ *
+ * @note As such, we label the elements in column-major order so we can follow the same conventions for multiplying
+ * matrices as the rest of the world, i.e. that \f$ [AB]_{ij} = \sum_k A_{ik} B_{kj} \f$.
+ *
+ * This file is primarily concerned with matrices representing affine transformations, implemented using an augmented
+ * matrix formalism,
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      A & b \\
+ *      0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ A \f$ is a \f$ 3 \times 3 \f$ matrix (the *linear part*) and \f$ b \f$ a \f$ 3 \times 1 \f$ matrix, i.e. a
+ * 3D vector (the *translation part*), and most of the functions assume that the matrices have this form.
+ *
+ * Throughout this file, `mode` indicates whether to multiply the matrix on top of the stack by the new construction
+ * (APPLY), or to just overwrite it (NEW).
+ */
+
 #include "global.h"
 
-void Matrix_StateAlloc(GameState* gameState) {
-    sMatrixStack = (MtxF*)THA_AllocEndAlign16(&gameState->heap, 0x500);
+/* data */
+
+// clang-format off
+Mtx gIdentityMtx = gdSPDefMtx(
+    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
+);
+// clang-format on
+
+MtxF gIdentityMtxF = { {
+    { 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 },
+} };
+
+/* bss */
+
+MtxF* sMatrixStack;   //!< original name: "Matrix_stack"
+MtxF* sCurrentMatrix; //!< original name: "Matrix_now"
+
+#define MATRIX_STACK_SIZE 20
+
+/* Stack operations */
+
+/**
+ * @brief Create the matrix stack and set the pointer to the top of it.
+ *
+ * @remark original name: "new_Matrix"
+ */
+void Matrix_Init(GameState* gameState) {
+    sMatrixStack = THA_AllocEndAlign16(&gameState->heap, MATRIX_STACK_SIZE * sizeof(MtxF));
     sCurrentMatrix = sMatrixStack;
 }
 
-void Matrix_StatePush(void) {
+/**
+ * @brief Place a new matrix on the top of the stack and move the stack pointer up.
+ *
+ * @remark original name: "Matrix_push"
+ */
+void Matrix_Push(void) {
     MtxF* prev = sCurrentMatrix;
 
     sCurrentMatrix++;
     Matrix_MtxFCopy(sCurrentMatrix, prev);
 }
 
-void Matrix_StatePop(void) {
+/**
+ * @brief Discard the top matrix on the stack and move stack pointer to the next one down.
+ *
+ * @remark original name: "Matrix_pull"
+ */
+void Matrix_Pop(void) {
     sCurrentMatrix--;
 }
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_CopyCurrentState.s")
+/**
+ * @brief Copy the top matrix from the stack.
+ *
+ * @param[out] dest Matrix into which to copy.
+ *
+ * @remark original name: "Matrix_get"
+ */
+void Matrix_Get(MtxF* dest) {
+    Matrix_MtxFCopy(dest, sCurrentMatrix);
+}
+
+/**
+ * @brief Overwrite the top matrix on the stack.
+ *
+ * @param[in] src Matrix from which to copy.
+ *
+ * @remark original name: "Matrix_put"
+ */
+void Matrix_Put(MtxF* src) {
+    Matrix_MtxFCopy(sCurrentMatrix, src);
+}
+
+/**
+ * @brief Return pointer to the top of the matrix stack.
+ *
+ * @return pointer to top matrix on the stack.
+ *
+ * @remark original name: get_Matrix_now
+ */
+MtxF* Matrix_GetCurrent(void) {
+    return sCurrentMatrix;
+}
+
+/* General transformation matrix functions */
+
+/**
+ * @brief General multiplication of current by a matrix.
+ *      - APPLY: current * mf -> current
+ *      - NEW: mf -> current
+ *
+ * @param mf Matrix to multiply by.
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_mult"
+ */
+void Matrix_Mult(MtxF* mf, MatrixMode mode) {
+    MtxF* cmf = Matrix_GetCurrent();
+
+    if (mode == MTXMODE_APPLY) {
+        SkinMatrix_MtxFMtxFMult(cmf, mf, cmf);
+    } else {
+        Matrix_MtxFCopy(sCurrentMatrix, mf);
+    }
+}
+
+/**
+ * @brief Right-multiply current by a translation matrix T.
+ *      - APPLY: current * T -> current
+ *      - NEW: T -> current
+ *
+ * T is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      1 & 0 & 0 & x \\
+ *      0 & 1 & 0 & y \\
+ *      0 & 0 & 1 & z \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix} .
+ * \f]
+ *
+ * @param x translation distance in the x direction.
+ * @param y translation distance in the y direction.
+ * @param z translation distance in the z direction.
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_translate"
+ */
+void Matrix_Translate(f32 x, f32 y, f32 z, MatrixMode mode) {
+    MtxF* cmf = sCurrentMatrix;
+    f32 tempX;
+    f32 tempY;
+
+    if (mode == MTXMODE_APPLY) {
+        tempX = cmf->xx;
+        tempY = cmf->xy;
+        cmf->xw += tempX * x + tempY * y + cmf->xz * z;
+        tempX = cmf->yx;
+        tempY = cmf->yy;
+        cmf->yw += tempX * x + tempY * y + cmf->yz * z;
+        tempX = cmf->zx;
+        tempY = cmf->zy;
+        cmf->zw += tempX * x + tempY * y + cmf->zz * z;
+        tempX = cmf->wx;
+        tempY = cmf->wy;
+        cmf->ww += tempX * x + tempY * y + cmf->wz * z;
+    } else {
+        SkinMatrix_SetTranslate(cmf, x, y, z);
+    }
+}
+
+/**
+ * @brief Right-multiply by the diagonal scale matrix S = diag(x,y,z,1).
+ *      - APPLY: current * S -> current
+ *      - NEW: S -> current
+ *
+ * S is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      x & 0 & 0 & 0 \\
+ *      0 & y & 0 & 0 \\
+ *      0 & 0 & z & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix} .
+ * \f]
+ *
+ * @param x scale in x direction.
+ * @param y scale in y direction.
+ * @param z scale in z direction.
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_scale"
+ */
+void Matrix_Scale(f32 x, f32 y, f32 z, MatrixMode mode) {
+    MtxF* cmf = sCurrentMatrix;
+
+    if (mode == MTXMODE_APPLY) {
+        cmf->xx *= x;
+        cmf->yx *= x;
+        cmf->zx *= x;
+        cmf->xy *= y;
+        cmf->yy *= y;
+        cmf->zy *= y;
+        cmf->xz *= z;
+        cmf->yz *= z;
+        cmf->zz *= z;
+        cmf->wx *= x;
+        cmf->wy *= y;
+        cmf->wz *= z;
+    } else {
+        SkinMatrix_SetScale(cmf, x, y, z);
+    }
+}
+
+/**
+ * @brief Right-multiply by a rotation about the x axis
+ *      - APPLY: current * R -> current
+ *      - NEW: R -> current
+ *
+ * R is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      1 & 0 & 0 & 0 \\
+ *      0 & c & -s & 0 \\
+ *      0 & s & c & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ c = \cos x, s = \sin x \f$.
+ *
+ * @note The same as Matrix_RotateXF(), but uses a binary angle.
+ *
+ * @param x rotation angle (binary).
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_RotateX"
+ */
+void Matrix_RotateXS(s16 x, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempY;
+    f32 tempZ;
+
+    if (mode == MTXMODE_APPLY) {
+        if (x != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = Math_SinS(x);
+            cos = Math_CosS(x);
+
+            tempY = cmf->xy;
+            tempZ = cmf->xz;
+            cmf->xy = tempY * cos + tempZ * sin;
+            cmf->xz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->yy;
+            tempZ = cmf->yz;
+            cmf->yy = tempY * cos + tempZ * sin;
+            cmf->yz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->zy;
+            tempZ = cmf->zz;
+            cmf->zy = tempY * cos + tempZ * sin;
+            cmf->zz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->wy;
+            tempZ = cmf->wz;
+            cmf->wy = tempY * cos + tempZ * sin;
+            cmf->wz = tempZ * cos - tempY * sin;
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (x != 0) {
+            sin = Math_SinS(x);
+            cos = Math_CosS(x);
+        } else {
+            sin = 0.0f;
+            cos = 1.0f;
+        }
+
+        cmf->yx = 0.0f;
+        cmf->zx = 0.0f;
+        cmf->wx = 0.0f;
+        cmf->xy = 0.0f;
+        cmf->wy = 0.0f;
+        cmf->xz = 0.0f;
+        cmf->wz = 0.0f;
+        cmf->xw = 0.0f;
+        cmf->yw = 0.0f;
+        cmf->zw = 0.0f;
+        cmf->xx = 1.0f;
+        cmf->ww = 1.0f;
+        cmf->yy = cos;
+        cmf->zz = cos;
+        cmf->zy = sin;
+        cmf->yz = -sin;
+    }
+}
+
+// Unused
+/**
+ * @brief Right-multiply by a rotation about the x axis.
+ *      - APPLY: current * R -> current
+ *      - NEW: R -> current
+ *
+ * R is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      1 & 0 & 0 & 0 \\
+ *      0 & c & -s & 0 \\
+ *      0 & s & c & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ c = \cos x, s = \sin x \f$.
+ *
+ * @note The same as Matrix_RotateXS(), but uses a float angle in radians.
+ *
+ * @param x rotation angle (radians).
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name may have been "Matrix_RotateX", but clashed with the previous function.
+ */
+void Matrix_RotateXF(f32 x, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempY;
+    f32 tempZ;
+    f32 zero = 0.0;
+    f32 one = 1.0;
+
+    if (mode == MTXMODE_APPLY) {
+        if (x != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = sinf(x);
+            cos = cosf(x);
+
+            tempY = cmf->xy;
+            tempZ = cmf->xz;
+            cmf->xy = tempY * cos + tempZ * sin;
+            cmf->xz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->yy;
+            tempZ = cmf->yz;
+            cmf->yy = tempY * cos + tempZ * sin;
+            cmf->yz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->zy;
+            tempZ = cmf->zz;
+            cmf->zy = tempY * cos + tempZ * sin;
+            cmf->zz = tempZ * cos - tempY * sin;
+
+            tempY = cmf->wy;
+            tempZ = cmf->wz;
+            cmf->wy = tempY * cos + tempZ * sin;
+            cmf->wz = tempZ * cos - tempY * sin;
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (x != 0) {
+            sin = sinf(x);
+            cos = cosf(x);
+        } else {
+            sin = zero;
+            cos = one;
+        }
+
+        cmf->xx = one;
+        cmf->yx = zero;
+        cmf->zx = zero;
+        cmf->wx = zero;
+        cmf->xy = zero;
+        cmf->yy = cos;
+        cmf->zy = sin;
+        cmf->wy = zero;
+        cmf->xz = zero;
+        cmf->yz = -sin;
+        cmf->zz = cos;
+        cmf->wz = zero;
+        cmf->xw = zero;
+        cmf->yw = zero;
+        cmf->zw = zero;
+        cmf->ww = one;
+    }
+}
+
+/**
+ * @brief Right-multiply by a rotation about the x axis.
+ *      current * R -> current
+ *
+ * @note Matrix_RotateXF() with mode APPLY.
+ *
+ * @param x rotation angle (radians).
+ */
+void Matrix_RotateXFApply(f32 x) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempY;
+    f32 tempZ;
+    s32 pad;
+
+    if (x != 0.0f) {
+        cmf = sCurrentMatrix;
+
+        sin = sins(RADF_TO_BINANG(x)) * SHT_MINV;
+        cos = coss(RADF_TO_BINANG(x)) * SHT_MINV;
+
+        tempY = cmf->xy;
+        tempZ = cmf->xz;
+        cmf->xy = (tempY * cos) + (tempZ * sin);
+        cmf->xz = (tempZ * cos) - (tempY * sin);
+
+        tempY = cmf->yy;
+        tempZ = cmf->yz;
+        cmf->yy = (tempY * cos) + (tempZ * sin);
+        cmf->yz = (tempZ * cos) - (tempY * sin);
+
+        tempY = cmf->zy;
+        tempZ = cmf->zz;
+        cmf->zy = (tempY * cos) + (tempZ * sin);
+        cmf->zz = (tempZ * cos) - (tempY * sin);
+
+        tempY = cmf->wy;
+        tempZ = cmf->wz;
+        cmf->wy = (tempY * cos) + (tempZ * sin);
+        cmf->wz = (tempZ * cos) - (tempY * sin);
+    }
+}
+
+/**
+ * @brief Replace current by a rotation about the x axis.
+ *      R -> current
+ *
+ * @note Matrix_RotateXF() with mode NEW.
+ *
+ * @param x rotation angle (radians).
+ */
+void Matrix_RotateXFNew(f32 x) {
+    MtxF* cmf = sCurrentMatrix;
+    s32 pad[2];
+    f32 sin;
+    f32 cos;
+
+    cmf->xx = 1.0f;
+    cmf->yx = 0.0f;
+    cmf->zx = 0.0f;
+    cmf->wx = 0.0f;
+    cmf->xy = 0.0f;
+    cmf->wy = 0.0f;
+    cmf->xz = 0.0f;
+    cmf->wz = 0.0f;
+    cmf->xw = 0.0f;
+    cmf->yw = 0.0f;
+    cmf->zw = 0.0f;
+    cmf->ww = 1.0f;
+
+    if (x != 0.0f) {
+        sin = sinf(x);
+        cos = cosf(x);
+
+        cmf->yy = cos;
+        cmf->zz = cos;
+        cmf->yz = -sin;
+        cmf->zy = sin;
+    } else {
+        cmf->yy = 1.0f;
+        cmf->zy = 0.0f;
+        cmf->yz = 0.0f;
+        cmf->zz = 1.0f;
+    }
+}
+
+/**
+ * @brief Right-multiply by a rotation about the y axis
+ *      - APPLY: current * R -> current
+ *      - NEW: R -> current
+ *
+ * R is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      c & 0 & s & 0 \\
+ *      0 & 1 & 0 & 0 \\
+ *      -s & 0 & c & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ c = \cos y, s = \sin y \f$.
+ *
+ * @note The same as Matrix_RotateYF(), but uses a binary angle.
+ *
+ * @param y rotation angle (binary).
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_RotateY"
+ */
+void Matrix_RotateYS(s16 y, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempX;
+    f32 tempZ;
+
+    if (mode == MTXMODE_APPLY) {
+        if (y != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = Math_SinS(y);
+            cos = Math_CosS(y);
+
+            tempX = cmf->xx;
+            tempZ = cmf->xz;
+            cmf->xx = tempX * cos - tempZ * sin;
+            cmf->xz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->yx;
+            tempZ = cmf->yz;
+            cmf->yx = tempX * cos - tempZ * sin;
+            cmf->yz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->zx;
+            tempZ = cmf->zz;
+            cmf->zx = tempX * cos - tempZ * sin;
+            cmf->zz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->wx;
+            tempZ = cmf->wz;
+            cmf->wx = tempX * cos - tempZ * sin;
+            cmf->wz = tempX * sin + tempZ * cos;
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (y != 0) {
+            sin = Math_SinS(y);
+            cos = Math_CosS(y);
+        } else {
+            sin = 0.0f;
+            cos = 1.0f;
+        }
+
+        cmf->yx = 0.0f;
+        cmf->wx = 0.0f;
+        cmf->xy = 0.0f;
+        cmf->zy = 0.0f;
+        cmf->wy = 0.0f;
+        cmf->yz = 0.0f;
+        cmf->wz = 0.0f;
+        cmf->xw = 0.0f;
+        cmf->yw = 0.0f;
+        cmf->zw = 0.0f;
+        cmf->yy = 1.0f;
+        cmf->ww = 1.0f;
+        cmf->xx = cos;
+        cmf->zz = cos;
+        cmf->zx = -sin;
+        cmf->xz = sin;
+    }
+}
+
+/**
+ * @brief Right-multiply by a rotation about the y axis.
+ *      - APPLY: current * R -> current
+ *      - NEW: R -> current
+ *
+ * R is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      c & 0 & s & 0 \\
+ *      0 & 1 & 0 & 0 \\
+ *      -s & 0 & c & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ c = \cos y, s = \sin y \f$.
+ *
+ * @note The same as Matrix_RotateYS(), but uses a float angle in radians.
+ *
+ * @param y rotation angle (radians).
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name may have been "Matrix_RotateY", but clashed with the previous function.
+ */
+void Matrix_RotateYF(f32 y, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempX;
+    f32 tempZ;
+    f32 zero = 0.0;
+    f32 one = 1.0;
+
+    if (mode == MTXMODE_APPLY) {
+        if (y != 0.0f) {
+            cmf = sCurrentMatrix;
+
+            sin = sinf(y);
+            cos = cosf(y);
+
+            tempX = cmf->xx;
+            tempZ = cmf->xz;
+            cmf->xx = tempX * cos - tempZ * sin;
+            cmf->xz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->yx;
+            tempZ = cmf->yz;
+            cmf->yx = tempX * cos - tempZ * sin;
+            cmf->yz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->zx;
+            tempZ = cmf->zz;
+            cmf->zx = tempX * cos - tempZ * sin;
+            cmf->zz = tempX * sin + tempZ * cos;
+
+            tempX = cmf->wx;
+            tempZ = cmf->wz;
+            cmf->wx = tempX * cos - tempZ * sin;
+            cmf->wz = tempX * sin + tempZ * cos;
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (y != 0.0f) {
+            sin = sinf(y);
+            cos = cosf(y);
+        } else {
+            cos = one;
+            sin = zero;
+        }
+
+        cmf->yx = zero;
+        cmf->wx = zero;
+        cmf->xy = zero;
+        cmf->zy = zero;
+        cmf->wy = zero;
+        cmf->yz = zero;
+        cmf->wz = zero;
+        cmf->xw = zero;
+        cmf->yw = zero;
+        cmf->zw = zero;
+        cmf->yy = one;
+        cmf->ww = one;
+        cmf->xx = cos;
+        cmf->zz = cos;
+        cmf->zx = -sin;
+        cmf->xz = sin;
+    }
+}
+
+/**
+ * @brief Right-multiply by a rotation about the z axis.
+ *      - APPLY: current * R -> current
+ *      - NEW: R -> current
+ *
+ * R is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      c & -s & 0 & 0 \\
+ *      s & c & 0 & 0 \\
+ *      0 & 0 & 1 & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ c = \cos z, s = \sin z \f$.
+ *
+ * @note The same as Matrix_RotateZF, but uses a binary angle.
+ *
+ * @param z rotation angle (binary).
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_RotateZ"
+ */
+void Matrix_RotateZS(s16 z, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempX;
+    f32 tempY;
+    f32 zero = 0.0;
+    f32 one = 1.0;
+
+    if (mode == MTXMODE_APPLY) {
+        if (z != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = Math_SinS(z);
+            cos = Math_CosS(z);
+
+            tempX = cmf->xx;
+            tempY = cmf->xy;
+            cmf->xx = tempX * cos + tempY * sin;
+            cmf->xy = tempY * cos - tempX * sin;
+
+            tempX = cmf->yx;
+            tempY = cmf->yy;
+            cmf->yx = tempX * cos + tempY * sin;
+            cmf->yy = tempY * cos - tempX * sin;
+
+            tempX = cmf->zx;
+            tempY = cmf->zy;
+            cmf->zx = tempX * cos + tempY * sin;
+            cmf->zy = tempY * cos - tempX * sin;
+
+            tempX = cmf->wx;
+            tempY = cmf->wy;
+            cmf->wx = tempX * cos + tempY * sin;
+            cmf->wy = tempY * cos - tempX * sin;
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (z != 0) {
+            sin = Math_SinS(z);
+            cos = Math_CosS(z);
+        } else {
+            sin = zero;
+            cos = one;
+        }
+
+        cmf->zx = zero;
+        cmf->wx = zero;
+        cmf->zy = zero;
+        cmf->wy = zero;
+        cmf->xz = zero;
+        cmf->yz = zero;
+        cmf->wz = zero;
+        cmf->xw = zero;
+        cmf->yw = zero;
+        cmf->zw = zero;
+        cmf->zz = one;
+        cmf->ww = one;
+        cmf->xx = cos;
+        cmf->yy = cos;
+        cmf->yx = sin;
+        cmf->xy = -sin;
+    }
+}
+
+/**
+ * @brief Right-multiply by a rotation about the z axis.
+ *      - APPLY: current * R -> current
+ *      - NEW: R -> current
+ *
+ * R is given by
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      c & -s & 0 & 0 \\
+ *      s & c & 0 & 0 \\
+ *      0 & 0 & 1 & 0 \\
+ *      0 & 0 & 0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where \f$ c = \cos z, s = \sin z \f$.
+ *
+ * @note The same as Matrix_RotateYS(), but uses a float angle in radians.
+ *
+ * @param z rotation angle (radians).
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name may have been "Matrix_RotateZ", but clashed with the previous function.
+ */
+void Matrix_RotateZF(f32 z, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 tempX;
+    f32 tempY;
+
+    if (mode == MTXMODE_APPLY) {
+        if (z != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = sinf(z);
+            cos = cosf(z);
+
+            tempX = cmf->xx;
+            tempY = cmf->xy;
+            cmf->xx = tempX * cos + tempY * sin;
+            cmf->xy = tempY * cos - tempX * sin;
+
+            tempX = cmf->yx;
+            tempY = cmf->yy;
+            cmf->yx = tempX * cos + tempY * sin;
+            cmf->yy = tempY * cos - tempX * sin;
+
+            tempX = cmf->zx;
+            tempY = cmf->zy;
+            cmf->zx = tempX * cos + tempY * sin;
+            cmf->zy = tempY * cos - tempX * sin;
+
+            tempX = cmf->wx;
+            tempY = cmf->wy;
+            cmf->wx = tempX * cos + tempY * sin;
+            cmf->wy = tempY * cos - tempX * sin;
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (z != 0) {
+            sin = sinf(z);
+            cos = cosf(z);
+        } else {
+            sin = 0.0f;
+            cos = 1.0f;
+        }
+
+        cmf->zx = 0.0f;
+        cmf->wx = 0.0f;
+        cmf->zy = 0.0f;
+        cmf->wy = 0.0f;
+        cmf->xz = 0.0f;
+        cmf->yz = 0.0f;
+        cmf->wz = 0.0f;
+        cmf->xw = 0.0f;
+        cmf->yw = 0.0f;
+        cmf->zw = 0.0f;
+        cmf->zz = 1.0f;
+        cmf->ww = 1.0f;
+        cmf->xx = cos;
+        cmf->yy = cos;
+        cmf->yx = sin;
+        cmf->xy = -sin;
+    }
+}
+
+/**
+ * @brief Rotate using ZYX Tait-Bryan angles.
+ *      - APPLY: current Rz Ry Rx -> current
+ *      - NEW: Rz Ry Rx -> current
+ *
+ * This means a (column) vector is first rotated around X, then around Y, then around Z, then (if `mode` is APPLY) gets
+ * transformed by what the matrix was before adding the ZYX rotation.
+ *
+ * See previous functions for the forms of Rz, Ry, Rx
+ *
+ * @param x binary angle to rotate about x axis
+ * @param y binary angle to rotate about y axis
+ * @param z binary angle to rotate about z axis
+ * @param mode APPLY or NEW
+ *
+ * @remark original name: "Matrix_RotateXYZ", changed to reflect rotation order.
+ */
+void Matrix_RotateZYX(s16 x, s16 y, s16 z, MatrixMode mode) {
+    MtxF* cmf = sCurrentMatrix;
+    f32 temp1;
+    f32 temp2;
+    f32 sin;
+    f32 cos;
+
+    if (mode == MTXMODE_APPLY) {
+        if (z != 0) { // Added in MM, OoT always follows the nonzero path
+            sin = Math_SinS(z);
+            cos = Math_CosS(z);
+
+            temp1 = cmf->xx;
+            temp2 = cmf->xy;
+            cmf->xx = temp1 * cos + temp2 * sin;
+            cmf->xy = temp2 * cos - temp1 * sin;
+
+            temp1 = cmf->yx;
+            temp2 = cmf->yy;
+            cmf->yx = temp1 * cos + temp2 * sin;
+            cmf->yy = temp2 * cos - temp1 * sin;
+
+            temp1 = cmf->zx;
+            temp2 = cmf->zy;
+            cmf->zx = temp1 * cos + temp2 * sin;
+            cmf->zy = temp2 * cos - temp1 * sin;
+
+            temp1 = cmf->wx;
+            temp2 = cmf->wy;
+            cmf->wx = temp1 * cos + temp2 * sin;
+            cmf->wy = temp2 * cos - temp1 * sin;
+        }
+
+        if (y != 0) {
+            sin = Math_SinS(y);
+            cos = Math_CosS(y);
+
+            temp1 = cmf->xx;
+            temp2 = cmf->xz;
+            cmf->xx = temp1 * cos - temp2 * sin;
+            cmf->xz = temp1 * sin + temp2 * cos;
+
+            temp1 = cmf->yx;
+            temp2 = cmf->yz;
+            cmf->yx = temp1 * cos - temp2 * sin;
+            cmf->yz = temp1 * sin + temp2 * cos;
+
+            temp1 = cmf->zx;
+            temp2 = cmf->zz;
+            cmf->zx = temp1 * cos - temp2 * sin;
+            cmf->zz = temp1 * sin + temp2 * cos;
+
+            temp1 = cmf->wx;
+            temp2 = cmf->wz;
+            cmf->wx = temp1 * cos - temp2 * sin;
+            cmf->wz = temp1 * sin + temp2 * cos;
+        }
+
+        if (x != 0) {
+            sin = Math_SinS(x);
+            cos = Math_CosS(x);
+
+            temp1 = cmf->xy;
+            temp2 = cmf->xz;
+            cmf->xy = temp1 * cos + temp2 * sin;
+            cmf->xz = temp2 * cos - temp1 * sin;
+
+            temp1 = cmf->yy;
+            temp2 = cmf->yz;
+            cmf->yy = temp1 * cos + temp2 * sin;
+            cmf->yz = temp2 * cos - temp1 * sin;
+
+            temp1 = cmf->zy;
+            temp2 = cmf->zz;
+            cmf->zy = temp1 * cos + temp2 * sin;
+            cmf->zz = temp2 * cos - temp1 * sin;
+
+            temp1 = cmf->wy;
+            temp2 = cmf->wz;
+            cmf->wy = temp1 * cos + temp2 * sin;
+            cmf->wz = temp2 * cos - temp1 * sin;
+        }
+    } else {
+        SkinMatrix_SetRotateRPY(cmf, x, y, z);
+    }
+}
+
+/**
+ * @brief Translate and rotate using ZYX Tait-Bryan angles.
+ *      current T Rz Ry Rx -> current
+ *
+ * This means a (column) vector is first rotated around X, then around Y, then around Z, then translated, then gets
+ * transformed by whatever the matrix was previously.
+ *
+ * @param translation vector by which to translate.
+ * @param rot vector of rotation angles.
+ *
+ * @remark original name appears to be "Matrix_softcv3_mult"
+ */
+void Matrix_TranslateRotateZYX(Vec3f* translation, Vec3s* rot) {
+    MtxF* cmf = sCurrentMatrix;
+    f32 sin = Math_SinS(rot->z);
+    f32 cos = Math_CosS(rot->z);
+    f32 temp1;
+    f32 temp2;
+
+    // No check for z != 0, presumably since translation is interleaved.
+    temp1 = cmf->xx;
+    temp2 = cmf->xy;
+    cmf->xw += temp1 * translation->x + temp2 * translation->y + cmf->xz * translation->z;
+    cmf->xx = temp1 * cos + temp2 * sin;
+    cmf->xy = temp2 * cos - temp1 * sin;
+
+    temp1 = cmf->yx;
+    temp2 = cmf->yy;
+    cmf->yw += temp1 * translation->x + temp2 * translation->y + cmf->yz * translation->z;
+    cmf->yx = temp1 * cos + temp2 * sin;
+    cmf->yy = temp2 * cos - temp1 * sin;
+
+    temp1 = cmf->zx;
+    temp2 = cmf->zy;
+    cmf->zw += temp1 * translation->x + temp2 * translation->y + cmf->zz * translation->z;
+    cmf->zx = temp1 * cos + temp2 * sin;
+    cmf->zy = temp2 * cos - temp1 * sin;
+
+    temp1 = cmf->wx;
+    temp2 = cmf->wy;
+    cmf->ww += temp1 * translation->x + temp2 * translation->y + cmf->wz * translation->z;
+    cmf->wx = temp1 * cos + temp2 * sin;
+    cmf->wy = temp2 * cos - temp1 * sin;
+
+    if (rot->y != 0) {
+        sin = Math_SinS(rot->y);
+        cos = Math_CosS(rot->y);
+
+        temp1 = cmf->xx;
+        temp2 = cmf->xz;
+        cmf->xx = temp1 * cos - temp2 * sin;
+        cmf->xz = temp1 * sin + temp2 * cos;
+
+        temp1 = cmf->yx;
+        temp2 = cmf->yz;
+        cmf->yx = temp1 * cos - temp2 * sin;
+        cmf->yz = temp1 * sin + temp2 * cos;
+
+        temp1 = cmf->zx;
+        temp2 = cmf->zz;
+        cmf->zx = temp1 * cos - temp2 * sin;
+        cmf->zz = temp1 * sin + temp2 * cos;
+
+        temp1 = cmf->wx;
+        temp2 = cmf->wz;
+        cmf->wx = temp1 * cos - temp2 * sin;
+        cmf->wz = temp1 * sin + temp2 * cos;
+    }
+
+    if (rot->x != 0) {
+        sin = Math_SinS(rot->x);
+        cos = Math_CosS(rot->x);
+
+        temp1 = cmf->xy;
+        temp2 = cmf->xz;
+        cmf->xy = temp1 * cos + temp2 * sin;
+        cmf->xz = temp2 * cos - temp1 * sin;
+
+        temp1 = cmf->yy;
+        temp2 = cmf->yz;
+        cmf->yy = temp1 * cos + temp2 * sin;
+        cmf->yz = temp2 * cos - temp1 * sin;
+
+        temp1 = cmf->zy;
+        temp2 = cmf->zz;
+        cmf->zy = temp1 * cos + temp2 * sin;
+        cmf->zz = temp2 * cos - temp1 * sin;
+
+        temp1 = cmf->wy;
+        temp2 = cmf->wz;
+        cmf->wy = temp1 * cos + temp2 * sin;
+        cmf->wz = temp2 * cos - temp1 * sin;
+    }
+}
+
+/**
+ * @brief Set current to a general translation and rotation using YXZ Tait-Bryan angles: T Ry Rx Rz -> current
+ *
+ * This means a (column) vector is first rotated around Y, then around X, then around Z, then translated, then gets
+ * transformed by whatever the matrix was previously.
+ *
+ * @param x amount to translate in X direction.
+ * @param y amount to translate in Y direction.
+ * @param z amount to translate in Z direction.
+ * @param rot vector of rotation angles.
+ *
+ * @remark original name appears to be "Matrix_softcv3_load"
+ */
+void Matrix_SetTranslateRotateYXZ(f32 x, f32 y, f32 z, Vec3s* rot) {
+    MtxF* cmf = sCurrentMatrix;
+    f32 sinY = Math_SinS(rot->y);
+    f32 cosY = Math_CosS(rot->y);
+    f32 cosTemp;
+    f32 sinTemp;
+
+    cmf->xx = cosY;
+    cmf->zx = -sinY;
+    cmf->xw = x;
+    cmf->yw = y;
+    cmf->zw = z;
+    cmf->wx = 0.0f;
+    cmf->wy = 0.0f;
+    cmf->wz = 0.0f;
+    cmf->ww = 1.0f;
+
+    if (rot->x != 0) {
+        sinTemp = Math_SinS(rot->x);
+        cosTemp = Math_CosS(rot->x);
+
+        cmf->zz = cosY * cosTemp;
+        cmf->zy = cosY * sinTemp;
+        cmf->xz = sinY * cosTemp;
+        cmf->xy = sinY * sinTemp;
+        cmf->yz = -sinTemp;
+        cmf->yy = cosTemp;
+    } else {
+        cmf->zz = cosY;
+        cmf->xz = sinY;
+        cmf->yz = 0.0f;
+        cmf->zy = 0.0f;
+        cmf->xy = 0.0f;
+        cmf->yy = 1.0f;
+    }
+
+    if (rot->z != 0) {
+        sinTemp = Math_SinS(rot->z);
+        cosTemp = Math_CosS(rot->z);
+
+        sinY = cmf->xx;
+        cosY = cmf->xy;
+        cmf->xx = sinY * cosTemp + cosY * sinTemp;
+        cmf->xy = cosY * cosTemp - sinY * sinTemp;
+
+        sinY = cmf->zx;
+        cosY = cmf->zy;
+        cmf->zx = sinY * cosTemp + cosY * sinTemp;
+        cmf->zy = cosY * cosTemp - sinY * sinTemp;
+
+        cosY = cmf->yy;
+        cmf->yx = cosY * sinTemp;
+        cmf->yy = cosY * cosTemp;
+    } else {
+        cmf->yx = 0.0f;
+    }
+}
+
+/**
+ * @brief Converts a floating-point MtxF to a fixed-point RSP-compatible matrix.
+ *
+ * @param[in] src MtxF to convert.
+ * @param[out] dest mtx to output to.
+ *
+ * @return dest
+ *
+ * @remark original name: "_MtxF_to_Mtx"
+ */
+Mtx* Matrix_MtxFToMtx(MtxF* src, Mtx* dest) {
+    s32 temp;
+    u16* intPart = (u16*)&dest->m[0][0];
+    u16* fracPart = (u16*)&dest->m[2][0];
+
+    // For some reason the first 9 elements use the intPart temp for the fractional part.
+    temp = src->xx * 0x10000;
+    intPart[0] = (temp >> 0x10);
+    intPart[16 + 0] = temp;
+
+    temp = src->yx * 0x10000;
+    intPart[1] = (temp >> 0x10);
+    intPart[16 + 1] = temp;
+
+    temp = src->zx * 0x10000;
+    intPart[2] = (temp >> 0x10);
+    intPart[16 + 2] = temp;
+
+    temp = src->wx * 0x10000;
+    intPart[3] = (temp >> 0x10);
+    intPart[16 + 3] = temp;
+
+    temp = src->xy * 0x10000;
+    intPart[4] = (temp >> 0x10);
+    intPart[16 + 4] = temp;
+
+    temp = src->yy * 0x10000;
+    intPart[5] = (temp >> 0x10);
+    intPart[16 + 5] = temp;
+
+    temp = src->zy * 0x10000;
+    intPart[6] = (temp >> 0x10);
+    intPart[16 + 6] = temp;
+
+    temp = src->wy * 0x10000;
+    intPart[7] = (temp >> 0x10);
+    intPart[16 + 7] = temp;
+
+    temp = src->xz * 0x10000;
+    intPart[8] = (temp >> 0x10);
+    intPart[16 + 8] = temp;
+
+    temp = src->yz * 0x10000;
+    intPart[9] = (temp >> 0x10);
+    fracPart[9] = temp;
+
+    temp = src->zz * 0x10000;
+    intPart[10] = (temp >> 0x10);
+    fracPart[10] = temp;
+
+    temp = src->wz * 0x10000;
+    intPart[11] = (temp >> 0x10);
+    fracPart[11] = temp;
+
+    temp = src->xw * 0x10000;
+    intPart[12] = (temp >> 0x10);
+    fracPart[12] = temp;
+
+    temp = src->yw * 0x10000;
+    intPart[13] = (temp >> 0x10);
+    fracPart[13] = temp;
+
+    temp = src->zw * 0x10000;
+    intPart[14] = (temp >> 0x10);
+    fracPart[14] = temp;
+
+    temp = src->ww * 0x10000;
+    intPart[15] = (temp >> 0x10);
+    fracPart[15] = temp;
+
+    return dest;
+}
+
+/**
+ * @brief Converts current to a fixed-point RSP-compatible matrix.
+ *
+ * @note Debug uses Matrix_CheckFloats to test current first.
+ *
+ * @param[out] dest mtx to output to.
+ *
+ * @return dest
+ *
+ * @remark original name: "_Matrix_to_Mtx"
+ */
+Mtx* Matrix_ToMtx(Mtx* dest) {
+    return Matrix_MtxFToMtx(sCurrentMatrix, dest);
+}
+
+/**
+ * @brief Converts current to a RSP-compatible matrix and saves it to allocated space in the OPA buffer.
+ *
+ * @param[in,out] gfxCtx Graphics context.
+ *
+ * @return allocated mtx.
+ *
+ * @remark original name: "_Matrix_to_Mtx_new"
+ */
+Mtx* Matrix_NewMtx(GraphicsContext* gfxCtx) {
+    return Matrix_ToMtx(GRAPH_ALLOC(gfxCtx, sizeof(Mtx)));
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_SetCurrentState.s")
+// Unused
+/**
+ * @brief Converts src to a RSP-compatible matrix and saves it to allocated space in the OPA buffer.
+ *
+ * @param[in] src MtxF to convert.
+ * @param[in,out] gfxCtx Graphics context.
+ *
+ * @return allocated mtx.
+ *
+ * @remark original name unknown, likely close to "_Matrix_MtxF_to_Mtx_new"
+ */
+Mtx* Matrix_MtxFToNewMtx(MtxF* src, GraphicsContext* gfxCtx) {
+    return Matrix_MtxFToMtx(src, GRAPH_ALLOC(gfxCtx, sizeof(Mtx)));
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_GetCurrentState.s")
+/**
+ * @brief Calculates current * (src,1) and writes its components to dest.
+ *
+ * This assumes that current has the form
+ *
+ * \f[
+ *  M =
+ *  \begin{pmatrix}
+ *      A & b \\
+ *      0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * where A is \f$ 3 \times 3 \f$ and b \f$ 3 \times 1 \f$, and so calculates
+ *
+ * \f[
+ *  MX =
+ *  \begin{pmatrix}
+ *      A & b \\
+ *      0 & 1
+ *  \end{pmatrix}
+ *  \begin{pmatrix}
+ *      x \\
+ *      1
+ *  \end{pmatrix}
+ *  =
+ *  \begin{pmatrix}
+ *      Ax + b \\
+ *      1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * and discards the extra w component (1).
+ *
+ * @param[in] src input vector
+ * @param[out] dest output vector
+ *
+ * @remark original name: "Matrix_Position"
+ */
+void Matrix_MultVec3f(Vec3f* src, Vec3f* dest) {
+    MtxF* cmf = sCurrentMatrix;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertMatrix.s")
+    dest->x = cmf->xw + (cmf->xx * src->x + cmf->xy * src->y + cmf->xz * src->z);
+    dest->y = cmf->yw + (cmf->yx * src->x + cmf->yy * src->y + cmf->yz * src->z);
+    dest->z = cmf->zw + (cmf->zx * src->x + cmf->zy * src->y + cmf->zz * src->z);
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertTranslation.s")
+/**
+ * @brief Multiply the vector `(0, 0, 0, 1)` by current.
+ *
+ * Can also see it as obtaining the translation vector part of current, but the former interpretation is consistent with
+ * the other functions nearby.
+ *
+ * @note Special case of Matrix_MultVec3f() with `src = { 0, 0, 0 }`; the same assumptions apply.
+ *
+ * @param[out] dest output vector.
+ *
+ * @remark original name: "Matrix_Position_Zero"
+ */
+void Matrix_MultZero(Vec3f* dest) {
+    MtxF* cmf = sCurrentMatrix;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_Scale.s")
+    dest->x = cmf->xw;
+    dest->y = cmf->yw;
+    dest->z = cmf->zw;
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertXRotation_s.s")
+/**
+ * @brief Multiply the vector `(x, 0, 0, 1)` by current.
+ *
+ * I.e. calculate \f$ A(x, 0, 0) + b \f$.
+ *
+ * @note Special case of Matrix_MultVec3f() with `src = { x, 0, 0 }`; the same assumptions apply.
+ *
+ * @param[in] x multiplier of unit vector in x direction.
+ * @param[out] dest output vector.
+ *
+ * @remark original name: "Matrix_Position_VecX"
+ */
+void Matrix_MultVecX(f32 x, Vec3f* dest) {
+    MtxF* cmf = sCurrentMatrix;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertXRotation_f.s")
+    dest->x = cmf->xw + cmf->xx * x;
+    dest->y = cmf->yw + cmf->yx * x;
+    dest->z = cmf->zw + cmf->zx * x;
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_RotateStateAroundXAxis.s")
+/**
+ * @brief Multiply the vector `(0, y, 0, 1)` by current.
+ *
+ * I.e. calculate \f$ A(0, y, 0) + b \f$.
+ *
+ * @note Special case of Matrix_MultVec3f() with `src = { 0, y, 0 }`; the same assumptions apply.
+ *
+ * @param[in] y multiplier of unit vector in y direction.
+ * @param[out] dest output vector.
+ *
+ * @remark original name is most likely "Matrix_Position_VecY" by analogy with the other two.
+ */
+void Matrix_MultVecY(f32 y, Vec3f* dest) {
+    MtxF* cmf = sCurrentMatrix;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_SetStateXRotation.s")
+    dest->x = cmf->xw + cmf->xy * y;
+    dest->y = cmf->yw + cmf->yy * y;
+    dest->z = cmf->zw + cmf->zy * y;
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_RotateY.s")
+/**
+ * @brief Multiply the vector `(0, 0, z, 1)` by current.
+ *
+ * I.e. calculate \f$ A(0, 0, z) + b \f$.
+ *
+ * @note Special case of Matrix_MultVec3f() with `src = { 0, 0, z }`; the same assumptions apply.
+ *
+ * @param[in] z multiplier of unit vector in z direction.
+ * @param[out] dest output vector.
+ *
+ * @remark original name: "Matrix_Position_VecZ"
+ */
+void Matrix_MultVecZ(f32 z, Vec3f* dest) {
+    MtxF* cmf = sCurrentMatrix;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertYRotation_f.s")
+    dest->x = cmf->xw + cmf->xz * z;
+    dest->y = cmf->yw + cmf->yz * z;
+    dest->z = cmf->zw + cmf->zz * z;
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertZRotation_s.s")
+/**
+ * @brief Calculates current * (src,1) and writes its x and z components to dest.
+ *
+ * The same as Matrix_MultVec3f(), but only applies to the x and z components; the same assumptions apply.
+ *
+ * @note Unlike the previous functions, does *not* just multiply (x, 0, z, 1) and save the x,y,z components.
+ *
+ * @param[in] src input vector.
+ * @param[out] dest output vector.
+ */
+void Matrix_MultVec3fXZ(Vec3f* src, Vec3f* dest) {
+    MtxF* cmf = sCurrentMatrix;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertZRotation_f.s")
+    dest->x = cmf->xw + (cmf->xx * src->x + cmf->xy * src->y + cmf->xz * src->z);
+    dest->z = cmf->zw + (cmf->zx * src->x + cmf->zy * src->y + cmf->zz * src->z);
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertRotation.s")
+/**
+ * @brief Copies the matrix src into dest.
+ *
+ * @param[out] dest matrix to copy to.
+ * @param[in] src matrix to copy from.
+ *
+ * @remark original name: "Matrix_copy_MtxF"
+ */
+void Matrix_MtxFCopy(MtxF* dest, MtxF* src) {
+    f32 fv0;
+    f32 fv1;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_JointPosition.s")
+    // This ought to be a loop, but all attempts to match it as one have so far failed.
+    if (1) {
+        fv0 = src->mf[0][0];
+        fv1 = src->mf[0][1];
+        dest->mf[0][0] = fv0;
+        dest->mf[0][1] = fv1;
+        fv0 = src->mf[0][2];
+        fv1 = src->mf[0][3];
+        dest->mf[0][2] = fv0;
+        dest->mf[0][3] = fv1;
+    }
+    if (1) {
+        fv0 = src->mf[1][0];
+        fv1 = src->mf[1][1];
+        dest->mf[1][0] = fv0;
+        dest->mf[1][1] = fv1;
+        fv0 = src->mf[1][2];
+        fv1 = src->mf[1][3];
+        dest->mf[1][2] = fv0;
+        dest->mf[1][3] = fv1;
+    }
+    if (1) {
+        fv0 = src->mf[2][0];
+        fv1 = src->mf[2][1];
+        dest->mf[2][0] = fv0;
+        dest->mf[2][1] = fv1;
+        fv0 = src->mf[2][2];
+        fv1 = src->mf[2][3];
+        dest->mf[2][2] = fv0;
+        dest->mf[2][3] = fv1;
+    }
+    if (1) {
+        fv0 = src->mf[3][0];
+        fv1 = src->mf[3][1];
+        dest->mf[3][0] = fv0;
+        dest->mf[3][1] = fv1;
+        fv0 = src->mf[3][2];
+        fv1 = src->mf[3][3];
+        dest->mf[3][2] = fv0;
+        dest->mf[3][3] = fv1;
+    }
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_SetStateRotationAndTranslation.s")
+/**
+ * @brief Converts fixed-point RSP-compatible matrix to an MtxF.
+ *
+ * @param[in] src mtx to convert
+ * @param[out] dest MtxF to output to
+ *
+ * @remark original name: "Matrix_MtxtoMtxF"
+ */
+void Matrix_MtxToMtxF(Mtx* src, MtxF* dest) {
+    u16* intPart = (u16*)&src->m[0][0];
+    u16* fracPart = (u16*)&src->m[2][0];
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_ToRSPMatrix.s")
+    dest->xx = ((intPart[0] << 0x10) | fracPart[0]) * (1 / (f32)0x10000);
+    dest->yx = ((intPart[1] << 0x10) | fracPart[1]) * (1 / (f32)0x10000);
+    dest->zx = ((intPart[2] << 0x10) | fracPart[2]) * (1 / (f32)0x10000);
+    dest->wx = ((intPart[3] << 0x10) | fracPart[3]) * (1 / (f32)0x10000);
+    dest->xy = ((intPart[4] << 0x10) | fracPart[4]) * (1 / (f32)0x10000);
+    dest->yy = ((intPart[5] << 0x10) | fracPart[5]) * (1 / (f32)0x10000);
+    dest->zy = ((intPart[6] << 0x10) | fracPart[6]) * (1 / (f32)0x10000);
+    dest->wy = ((intPart[7] << 0x10) | fracPart[7]) * (1 / (f32)0x10000);
+    dest->xz = ((intPart[8] << 0x10) | fracPart[8]) * (1 / (f32)0x10000);
+    dest->yz = ((intPart[9] << 0x10) | fracPart[9]) * (1 / (f32)0x10000);
+    dest->zz = ((intPart[10] << 0x10) | fracPart[10]) * (1 / (f32)0x10000);
+    dest->wz = ((intPart[11] << 0x10) | fracPart[11]) * (1 / (f32)0x10000);
+    dest->xw = ((intPart[12] << 0x10) | fracPart[12]) * (1 / (f32)0x10000);
+    dest->yw = ((intPart[13] << 0x10) | fracPart[13]) * (1 / (f32)0x10000);
+    dest->zw = ((intPart[14] << 0x10) | fracPart[14]) * (1 / (f32)0x10000);
+    dest->ww = ((intPart[15] << 0x10) | fracPart[15]) * (1 / (f32)0x10000);
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_ToMtx.s")
+// Unused
+/**
+ * @brief Calculates mf * (src,1) and writes its components to dest.
+ *
+ * This is the same as Matrix_MultVec3f() but using a specified matrix rather than the current one; the same
+ * assumptions apply.
+ *
+ * @param[in] src input vector
+ * @param[out] dest output vector
+ * @param[in] mf matrix to multiply by
+ */
+void Matrix_MultVec3fExt(Vec3f* src, Vec3f* dest, MtxF* mf) {
+    dest->x = mf->xw + (mf->xx * src->x + mf->xy * src->y + mf->xz * src->z);
+    dest->y = mf->yw + (mf->yx * src->x + mf->yy * src->y + mf->yz * src->z);
+    dest->z = mf->zw + (mf->zx * src->x + mf->zy * src->y + mf->zz * src->z);
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_NewMtx.s")
+/**
+ * @brief Overwrite the linear part of mf with its transpose (ignores the translational part).
+ *
+ * Viz.,
+ *
+ * \f[
+ *  \begin{pmatrix}
+ *      A & b \\
+ *      0 & 1
+ *  \end{pmatrix}
+ *  \longrightarrow
+ *  \begin{pmatrix}
+ *      A^T & b \\
+ *      0 & 1
+ *  \end{pmatrix}
+ * \f]
+ *
+ * @param[in,out] mf matrix to transpose
+ *
+ * @remark original name: "Matrix_reverse"
+ */
+void Matrix_Transpose(MtxF* mf) {
+    f32 temp;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_AppendToPolyOpaDisp.s")
+    temp = mf->yx;
+    mf->yx = mf->xy;
+    mf->xy = temp;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_MultiplyVector3fByState.s")
+    temp = mf->zx;
+    mf->zx = mf->xz;
+    mf->xz = temp;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_GetStateTranslation.s")
+    temp = mf->zy;
+    mf->zy = mf->yz;
+    mf->yz = temp;
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_GetStateTranslationAndScaledX.s")
+/**
+ * @brief Decompose the linear part A of current into B * S, where B has normalised columns and S is diagonal, and
+ * replace B by `mf`.
+ *
+ * Since B is typically a rotation matrix, and the linear part R * S to `mf` * S, this operation can be
+ * seen as replacing the B rotation with `mf`, hence the function name.
+ *
+ * @param[in] mf matrix whose linear part will replace the normalised part of A.
+ */
+void Matrix_ReplaceRotation(MtxF* mf) {
+    MtxF* cmf = sCurrentMatrix;
+    f32 acc;
+    f32 component;
+    f32 curColNorm;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_GetStateTranslationAndScaledY.s")
+    // compute the Euclidean norm of the first column of the current matrix
+    acc = cmf->xx;
+    acc *= acc;
+    component = cmf->yx;
+    acc += SQ(component);
+    component = cmf->zx;
+    acc += SQ(component);
+    curColNorm = sqrtf(acc);
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_GetStateTranslationAndScaledZ.s")
+    cmf->xx = mf->xx * curColNorm;
+    cmf->yx = mf->yx * curColNorm;
+    cmf->zx = mf->zx * curColNorm;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_MultiplyVector3fXZByCurrentState.s")
+    // second column
+    acc = cmf->xy;
+    acc *= acc;
+    component = cmf->yy;
+    acc += SQ(component);
+    component = cmf->zy;
+    acc += SQ(component);
+    curColNorm = sqrtf(acc);
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_MtxFCopy.s")
+    cmf->xy = mf->xy * curColNorm;
+    cmf->yy = mf->yy * curColNorm;
+    cmf->zy = mf->zy * curColNorm;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_FromRSPMatrix.s")
+    // third column
+    acc = cmf->xz;
+    acc *= acc;
+    component = cmf->yz;
+    acc += SQ(component);
+    component = cmf->zz;
+    acc += SQ(component);
+    curColNorm = sqrtf(acc);
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_MultiplyVector3fByMatrix.s")
+    cmf->xz = mf->xz * curColNorm;
+    cmf->yz = mf->yz * curColNorm;
+    cmf->zz = mf->zz * curColNorm;
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_TransposeXYZ.s")
+/**
+ * @brief Extract the YXZ Tait-Bryan rotation angles from the linear part \f$ A \f$ of a matrix.
+ *
+ * \f$ A \f$ should have orthogonal columns; the most general matrix of this form can be written as \f$ RS \f$
+ * with \f$ S \f$ a scale matrix.
+ *
+ * If A has columns with the same norm (such as if it is just a rotation matrix), it is sufficient (and faster) to use
+ * `nonUniformScale` off: `nonUniformScale` being set enables extraction of the angles from a matrix with columns that
+ * are orthogonal but have different scales, at the cost of requiring extra calculation.
+ *
+ * @param[in] src Matrix to extract angles from.
+ * @param[out] dest vector to write angles to.
+ * @param[in] nonUniformScale boolean: true enables handling matrices with differently-scaled columns.
+ *
+ * @remark original name: "Matrix_to_rotate_new"?
+ */
+void Matrix_MtxFToYXZRot(MtxF* src, Vec3s* dest, s32 nonUniformScale) {
+    f32 temp;
+    f32 temp2;
+    f32 temp3;
+    f32 temp4;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_NormalizeXYZ.s")
+    temp = src->xz;
+    temp *= temp;
+    temp += SQ(src->zz);
+    dest->x = Math_Atan2S(-src->yz, sqrtf(temp));
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/func_8018219C.s")
+    if ((dest->x == 0x4000) || (dest->x == -0x4000)) {
+        // cos(x) = 0 if either of these is true, and we get gimbal locking
+        // (https://en.wikipedia.org/wiki/Gimbal_lock#Loss_of_a_degree_of_freedom_with_Euler_angles); fix z to make y
+        // well-defined.
+        dest->z = 0;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/func_801822C4.s")
+        dest->y = Math_Atan2S(-src->zx, src->xx);
+    } else {
+        dest->y = Math_Atan2S(src->xz, src->zz);
+
+        if (!nonUniformScale) {
+            // assume the columns have the same normalisation
+            dest->z = Math_Atan2S(src->yx, src->yy);
+        } else {
+            temp = src->xx;
+            temp2 = src->zx;
+            temp3 = src->zy;
+
+            // find norm of the first column
+            temp *= temp;
+            temp += SQ(temp2);
+            temp2 = src->yx;
+            temp += SQ(temp2);
+            // temp = xx^2+zx^2+yx^2 == 1 for a rotation matrix
+            temp = sqrtf(temp);
+            temp = temp2 / temp; // yx in normalised column
+
+            // find norm of the second column
+            temp2 = src->xy;
+            temp2 *= temp2;
+            temp2 += SQ(temp3);
+            temp3 = src->yy;
+            temp2 += SQ(temp3);
+            // temp2 = xy^2+zy^2+yy^2 == 1 for a rotation matrix
+            temp2 = sqrtf(temp2);
+            temp2 = temp3 / temp2; // yy in normalised column
+
+            // for a rotation matrix, temp == yx and temp2 == yy which is the same as in the !nonUniformScale branch
+            dest->z = Math_Atan2S(temp, temp2);
+        }
+    }
+}
+
+/**
+ * @brief Extract the ZYX Tait-Bryan rotation angles from the linear part \f$ A \f$ of a matrix.
+ *
+ * \f$ A \f$ should have orthogonal columns; the most general matrix of this form can be written as \f$ RS \f$
+ * with \f$ S \f$ a scale matrix.
+ *
+ * If A has columns with the same norm (such as if it is just a rotation matrix), it is sufficient (and faster) to use
+ * `nonUniformScale` off: `nonUniformScale` being set enables extraction of the angles from a matrix with columns that
+ * are orthogonal but have different scales, at the cost of requiring extra calculation.
+ *
+ * @param[in] src Matrix to extract angles from.
+ * @param[out] dest vector to write angles to.
+ * @param[in] nonUniformScale boolean: true enables handling matrices with unnormalised columns.
+ *
+ * @remark original name: "Matrix_to_rotate2_new"?
+ *
+ * See Matrix_MtxFToYXZRot() for full inline documentation.
+ */
+void Matrix_MtxFToZYXRot(MtxF* src, Vec3s* dest, s32 nonUniformScale) {
+    f32 temp;
+    f32 temp2;
+    f32 temp3;
+    f32 temp4;
+
+    temp = src->xx;
+    temp *= temp;
+    temp += SQ(src->yx);
+    dest->y = Math_Atan2S(-src->zx, sqrtf(temp));
+
+    if ((dest->y == 0x4000) || (dest->y == -0x4000)) {
+        dest->x = 0;
+        dest->z = Math_Atan2S(-src->xy, src->yy);
+    } else {
+        dest->z = Math_Atan2S(src->yx, src->xx);
+
+        if (!nonUniformScale) {
+            dest->x = Math_Atan2S(src->zy, src->zz);
+        } else {
+            temp = src->xy;
+            temp2 = src->yy;
+            temp3 = src->yz;
+
+            temp *= temp;
+            temp += SQ(temp2);
+            temp2 = src->zy;
+            temp += SQ(temp2);
+            temp = sqrtf(temp);
+            temp = temp2 / temp;
+
+            temp2 = src->xz;
+            temp2 *= temp2;
+            temp2 += SQ(temp3);
+            temp3 = src->zz;
+            temp2 += SQ(temp3);
+            temp2 = sqrtf(temp2);
+            temp2 = temp3 / temp2;
+
+            dest->x = Math_Atan2S(temp, temp2);
+        }
+    }
+}
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertRotationAroundUnitVector_f.s")
+/**
+ * @brief Rotate by `angle` radians about `axis`, which is assumed to be a unit vector.
+ *
+ * @param angle rotation angle (radians).
+ * @param axis axis about which to rotate, must be a unit vector.
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name may have been "Matrix_RotateVector", but clashed with the next function.
+ */
+void Matrix_RotateAxisF(f32 angle, Vec3f* axis, MatrixMode mode) {
+    MtxF* cmf;
+    f32 sin;
+    f32 cos;
+    f32 versin;
+    f32 temp1;
+    f32 temp2;
+    f32 temp3;
+    f32 temp4;
+    f32 temp5;
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/sys_matrix/Matrix_InsertRotationAroundUnitVector_s.s")
+    if (mode == MTXMODE_APPLY) {
+        if (angle != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = sinf(angle);
+            cos = cosf(angle);
+
+            temp1 = cmf->xx;
+            temp2 = cmf->xy;
+            temp3 = cmf->xz;
+            temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
+            cmf->xx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
+            cmf->xy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
+            cmf->xz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
+
+            temp1 = cmf->yx;
+            temp2 = cmf->yy;
+            temp3 = cmf->yz;
+            temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
+            cmf->yx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
+            cmf->yy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
+            cmf->yz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
+
+            temp1 = cmf->zx;
+            temp2 = cmf->zy;
+            temp3 = cmf->zz;
+            temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
+            cmf->zx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
+            cmf->zy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
+            cmf->zz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (angle != 0) {
+            sin = sinf(angle);
+            cos = cosf(angle);
+            versin = 1.0f - cos;
+
+            cmf->xx = axis->x * axis->x * versin + cos;
+            cmf->yy = axis->y * axis->y * versin + cos;
+            cmf->zz = axis->z * axis->z * versin + cos;
+
+            if (0) {}
+
+            temp2 = axis->x * versin * axis->y;
+            temp3 = axis->z * sin;
+            cmf->yx = temp2 + temp3;
+            cmf->xy = temp2 - temp3;
+
+            temp2 = axis->x * versin * axis->z;
+            temp3 = axis->y * sin;
+            cmf->zx = temp2 - temp3;
+            cmf->xz = temp2 + temp3;
+
+            temp2 = axis->y * versin * axis->z;
+            temp3 = axis->x * sin;
+            cmf->zy = temp2 + temp3;
+            cmf->yz = temp2 - temp3;
+
+            cmf->wx = cmf->wy = cmf->wz = cmf->xw = cmf->yw = cmf->zw = 0.0f;
+            cmf->ww = 1.0f;
+        } else {
+            cmf->xx = 1.0f;
+            cmf->yx = 0.0f;
+            cmf->zx = 0.0f;
+            cmf->wx = 0.0f;
+            cmf->xy = 0.0f;
+            cmf->yy = 1.0f;
+            cmf->zy = 0.0f;
+            cmf->wy = 0.0f;
+            cmf->xz = 0.0f;
+            cmf->yz = 0.0f;
+            cmf->zz = 1.0f;
+            cmf->wz = 0.0f;
+            cmf->xw = 0.0f;
+            cmf->yw = 0.0f;
+            cmf->zw = 0.0f;
+            cmf->ww = 1.0f;
+        }
+    }
+}
+
+/**
+ * @brief Rotate by binary angle `angle` about `axis`, which is assumed to be a unit vector.
+ *
+ * @param angle rotation angle (binary).
+ * @param axis axis about which to rotate, must be a unit vector.
+ * @param mode APPLY or NEW.
+ *
+ * @remark original name: "Matrix_RotateVector"
+ */
+void Matrix_RotateAxisS(s16 angle, Vec3f* axis, MatrixMode mode) {
+    MtxF* cmf;
+    f32 cos;
+    f32 sin;
+    f32 versin;
+    f32 temp1;
+    f32 temp2;
+    f32 temp3;
+    f32 temp4;
+
+    if (mode == MTXMODE_APPLY) {
+        if (angle != 0) {
+            cmf = sCurrentMatrix;
+
+            sin = Math_SinS(angle);
+            cos = Math_CosS(angle);
+
+            temp1 = cmf->xx;
+            temp2 = cmf->xy;
+            temp3 = cmf->xz;
+            temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
+            cmf->xx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
+            cmf->xy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
+            cmf->xz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
+
+            temp1 = cmf->yx;
+            temp2 = cmf->yy;
+            temp3 = cmf->yz;
+            temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
+            cmf->yx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
+            cmf->yy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
+            cmf->yz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
+
+            temp1 = cmf->zx;
+            temp2 = cmf->zy;
+            temp3 = cmf->zz;
+            temp4 = (axis->x * temp1 + axis->y * temp2 + axis->z * temp3) * (1.0f - cos);
+            cmf->zx = temp1 * cos + axis->x * temp4 + sin * (temp2 * axis->z - temp3 * axis->y);
+            cmf->zy = temp2 * cos + axis->y * temp4 + sin * (temp3 * axis->x - temp1 * axis->z);
+            cmf->zz = temp3 * cos + axis->z * temp4 + sin * (temp1 * axis->y - temp2 * axis->x);
+        }
+    } else {
+        cmf = sCurrentMatrix;
+
+        if (angle != 0) {
+            sin = Math_SinS(angle);
+            cos = Math_CosS(angle);
+            versin = 1.0f - cos;
+
+            cmf->xx = axis->x * axis->x * versin + cos;
+            cmf->yy = axis->y * axis->y * versin + cos;
+            cmf->zz = axis->z * axis->z * versin + cos;
+
+            if (0) {}
+
+            temp2 = axis->x * versin * axis->y;
+            temp3 = axis->z * sin;
+            cmf->yx = temp2 + temp3;
+            cmf->xy = temp2 - temp3;
+
+            temp2 = axis->x * versin * axis->z;
+            temp3 = axis->y * sin;
+            cmf->zx = temp2 - temp3;
+            cmf->xz = temp2 + temp3;
+
+            temp2 = axis->y * versin * axis->z;
+            temp3 = axis->x * sin;
+            cmf->zy = temp2 + temp3;
+            cmf->yz = temp2 - temp3;
+
+            cmf->wx = cmf->wy = cmf->wz = cmf->xw = cmf->yw = cmf->zw = 0.0f;
+            cmf->ww = 1.0f;
+        } else {
+            cmf->xx = 1.0f;
+            cmf->yx = 0.0f;
+            cmf->zx = 0.0f;
+            cmf->wx = 0.0f;
+            cmf->xy = 0.0f;
+            cmf->yy = 1.0f;
+            cmf->zy = 0.0f;
+            cmf->wy = 0.0f;
+            cmf->xz = 0.0f;
+            cmf->yz = 0.0f;
+            cmf->zz = 1.0f;
+            cmf->wz = 0.0f;
+            cmf->xw = 0.0f;
+            cmf->yw = 0.0f;
+            cmf->zw = 0.0f;
+            cmf->ww = 1.0f;
+        }
+    }
+}
diff --git a/src/code/z_actor.c b/src/code/z_actor.c
index 2d75505d0..6d0b9573d 100644
--- a/src/code/z_actor.c
+++ b/src/code/z_actor.c
@@ -92,10 +92,10 @@ void ActorShadow_Draw(Actor* actor, Lights* lights, GlobalContext* globalCtx, Gf
             }
 
             func_800C0094(actor->floorPoly, actor->world.pos.x, actor->floorHeight, actor->world.pos.z, &mtx);
-            Matrix_SetCurrentState(&mtx);
+            Matrix_Put(&mtx);
 
             if ((dlist != gCircleShadowDL) || (actor->scale.x != actor->scale.z)) {
-                Matrix_RotateY(actor->shape.rot.y, MTXMODE_APPLY);
+                Matrix_RotateYS(actor->shape.rot.y, MTXMODE_APPLY);
             }
 
             shadowScale *= actor->shape.shadowScale;
@@ -151,8 +151,8 @@ void ActorShadow_DrawFoot(GlobalContext* globalCtx, Light* light, MtxF* arg2, s3
     sp58 = Math_FAtan2F(dir2, dir0);
     shadowScaleZ *= (4.5f - (light->l.dir[1] * 0.035f));
     shadowScaleZ = CLAMP_MIN(shadowScaleZ, 1.0f);
-    Matrix_SetCurrentState(arg2);
-    Matrix_RotateY(sp58, MTXMODE_APPLY);
+    Matrix_Put(arg2);
+    Matrix_RotateYS(sp58, MTXMODE_APPLY);
     Matrix_Scale(shadowScaleX, 1.0f, shadowScaleX * shadowScaleZ, MTXMODE_APPLY);
 
     gSPMatrix(POLY_OPA_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_MODELVIEW | G_MTX_LOAD);
@@ -301,9 +301,9 @@ void ActorShadow_DrawFeet(Actor* actor, Lights* mapper, GlobalContext* globalCtx
 void Actor_SetFeetPos(Actor* actor, s32 limbIndex, s32 leftFootIndex, Vec3f* leftFootPos, s32 rightFootIndex,
                       Vec3f* rightFootPos) {
     if (limbIndex == leftFootIndex) {
-        Matrix_MultiplyVector3fByState(leftFootPos, &actor->shape.feetPos[FOOT_LEFT]);
+        Matrix_MultVec3f(leftFootPos, &actor->shape.feetPos[FOOT_LEFT]);
     } else if (limbIndex == rightFootIndex) {
-        Matrix_MultiplyVector3fByState(rightFootPos, &actor->shape.feetPos[FOOT_RIGHT]);
+        Matrix_MultVec3f(rightFootPos, &actor->shape.feetPos[FOOT_RIGHT]);
     }
 }
 
@@ -533,22 +533,22 @@ void Actor_DrawZTarget(TargetContext* targetCtx, GlobalContext* globalCtx) {
                             var2 = ((entry->unkC - 120.0f) * 0.001f) + 0.15f;
                         }
 
-                        Matrix_InsertTranslation(entry->pos.x, entry->pos.y, 0.0f, MTXMODE_NEW);
+                        Matrix_Translate(entry->pos.x, entry->pos.y, 0.0f, MTXMODE_NEW);
                         Matrix_Scale(var2, 0.15f, 1.0f, MTXMODE_APPLY);
 
                         gDPSetPrimColor(OVERLAY_DISP++, 0, 0, entry->color.r, entry->color.g, entry->color.b,
                                         (u8)alpha);
 
-                        Matrix_InsertZRotation_s((targetCtx->unk4B * 512), MTXMODE_APPLY);
+                        Matrix_RotateZS((targetCtx->unk4B * 512), MTXMODE_APPLY);
 
                         for (i = 0; i < 4; i++) {
-                            Matrix_InsertZRotation_s(0x4000, MTXMODE_APPLY);
-                            Matrix_StatePush();
-                            Matrix_InsertTranslation(entry->unkC, entry->unkC, 0.0f, MTXMODE_APPLY);
+                            Matrix_RotateZS(0x4000, MTXMODE_APPLY);
+                            Matrix_Push();
+                            Matrix_Translate(entry->unkC, entry->unkC, 0.0f, MTXMODE_APPLY);
                             gSPMatrix(OVERLAY_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx),
                                       G_MTX_MODELVIEW | G_MTX_LOAD);
                             gSPDisplayList(OVERLAY_DISP++, gZTargetLockOnTriangleDL);
-                            Matrix_StatePop();
+                            Matrix_Pop();
                         }
                     }
 
@@ -566,10 +566,10 @@ void Actor_DrawZTarget(TargetContext* targetCtx, GlobalContext* globalCtx) {
 
             POLY_XLU_DISP = Gfx_CallSetupDL(POLY_XLU_DISP, 0x07);
 
-            Matrix_InsertTranslation(actor->focus.pos.x,
-                                     actor->focus.pos.y + (actor->targetArrowOffset * actor->scale.y) + 17.0f,
-                                     actor->focus.pos.z, MTXMODE_NEW);
-            Matrix_RotateY((globalCtx->gameplayFrames * 3000), MTXMODE_APPLY);
+            Matrix_Translate(actor->focus.pos.x,
+                             actor->focus.pos.y + (actor->targetArrowOffset * actor->scale.y) + 17.0f,
+                             actor->focus.pos.z, MTXMODE_NEW);
+            Matrix_RotateYS((globalCtx->gameplayFrames * 3000), MTXMODE_APPLY);
             Matrix_Scale((iREG(27) + 35) / 1000.0f, (iREG(28) + 60) / 1000.0f, (iREG(29) + 50) / 1000.0f,
                          MTXMODE_APPLY);
 
@@ -2511,14 +2511,13 @@ void Actor_Draw(GlobalContext* globalCtx, Actor* actor) {
     Lights_Draw(light, globalCtx->state.gfxCtx);
 
     if (actor->flags & ACTOR_FLAG_1000) {
-        Matrix_SetStateRotationAndTranslation(
+        Matrix_SetTranslateRotateYXZ(
             actor->world.pos.x + globalCtx->mainCamera.skyboxOffset.x,
             actor->world.pos.y + ((actor->shape.yOffset * actor->scale.y) + globalCtx->mainCamera.skyboxOffset.y),
             actor->world.pos.z + globalCtx->mainCamera.skyboxOffset.z, &actor->shape.rot);
     } else {
-        Matrix_SetStateRotationAndTranslation(actor->world.pos.x,
-                                              actor->world.pos.y + (actor->shape.yOffset * actor->scale.y),
-                                              actor->world.pos.z, &actor->shape.rot);
+        Matrix_SetTranslateRotateYXZ(actor->world.pos.x, actor->world.pos.y + (actor->shape.yOffset * actor->scale.y),
+                                     actor->world.pos.z, &actor->shape.rot);
     }
 
     Matrix_Scale(actor->scale.x, actor->scale.y, actor->scale.z, MTXMODE_APPLY);
@@ -3543,14 +3542,14 @@ void Actor_SpawnBodyParts(Actor* actor, GlobalContext* globalCtx, s32 arg2, Gfx*
     MtxF* currentMatrix;
 
     if (*dList != NULL) {
-        currentMatrix = Matrix_GetCurrentState();
+        currentMatrix = Matrix_GetCurrent();
         spawnedPart =
             Actor_SpawnAsChild(&globalCtx->actorCtx, actor, globalCtx, ACTOR_EN_PART, currentMatrix->mf[3][0],
                                currentMatrix->mf[3][1], currentMatrix->mf[3][2], 0, 0, actor->objBankIndex, arg2);
         if (spawnedPart != NULL) {
             part = (EnPart*)spawnedPart;
 
-            func_8018219C(currentMatrix, &part->actor.shape.rot, 0);
+            Matrix_MtxFToYXZRot(currentMatrix, &part->actor.shape.rot, false);
             part->unk_150 = *dList;
             Math_Vec3f_Copy(&part->actor.scale, &actor->scale);
         }
@@ -3772,9 +3771,9 @@ void func_800BC620(Vec3f* arg0, Vec3f* arg1, u8 alpha, GlobalContext* globalCtx)
     sp54 = BgCheck_EntityRaycastFloor2(globalCtx, &globalCtx->colCtx, &sp44, &sp48);
     if (sp44 != NULL) {
         func_800C0094(sp44, arg0->x, sp54, arg0->z, &sp58);
-        Matrix_SetCurrentState(&sp58);
+        Matrix_Put(&sp58);
     } else {
-        Matrix_InsertTranslation(arg0->x, arg0->y, arg0->z, MTXMODE_NEW);
+        Matrix_Translate(arg0->x, arg0->y, arg0->z, MTXMODE_NEW);
     }
     Matrix_Scale(arg1->x, 1.0f, arg1->z, MTXMODE_APPLY);
 
@@ -3841,16 +3840,16 @@ void Actor_DrawDoorLock(GlobalContext* globalCtx, s32 frame, s32 type) {
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_InsertTranslation(0.0f, entry->yShift, 500.0f, MTXMODE_APPLY);
-    Matrix_CopyCurrentState(&baseMtxF);
+    Matrix_Translate(0.0f, entry->yShift, 500.0f, MTXMODE_APPLY);
+    Matrix_Get(&baseMtxF);
 
-    chainsTranslateX = __sinf(entry->chainAngle - chainRotZ) * -(10 - frame) * 0.1f * entry->chainLength;
-    chainsTranslateY = __cosf(entry->chainAngle - chainRotZ) * (10 - frame) * 0.1f * entry->chainLength;
+    chainsTranslateX = sinf(entry->chainAngle - chainRotZ) * -(10 - frame) * 0.1f * entry->chainLength;
+    chainsTranslateY = cosf(entry->chainAngle - chainRotZ) * (10 - frame) * 0.1f * entry->chainLength;
 
     for (i = 0; i < 4; i++) {
-        Matrix_SetCurrentState(&baseMtxF);
-        Matrix_InsertZRotation_f(chainRotZ, MTXMODE_APPLY);
-        Matrix_InsertTranslation(chainsTranslateX, chainsTranslateY, 0.0f, MTXMODE_APPLY);
+        Matrix_Put(&baseMtxF);
+        Matrix_RotateZF(chainRotZ, MTXMODE_APPLY);
+        Matrix_Translate(chainsTranslateX, chainsTranslateY, 0.0f, MTXMODE_APPLY);
         if (entry->chainsScale != 1.0f) {
             Matrix_Scale(entry->chainsScale, entry->chainsScale, entry->chainsScale, MTXMODE_APPLY);
         }
@@ -3867,7 +3866,7 @@ void Actor_DrawDoorLock(GlobalContext* globalCtx, s32 frame, s32 type) {
         chainRotZ += rotZStep;
     }
 
-    Matrix_SetCurrentState(&baseMtxF);
+    Matrix_Put(&baseMtxF);
     Matrix_Scale(frame * 0.1f, frame * 0.1f, frame * 0.1f, MTXMODE_APPLY);
 
     gSPMatrix(POLY_OPA_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
@@ -4516,7 +4515,7 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
         u32 gameplayFrames = globalCtx->gameplayFrames;
         f32 effectAlphaScaled;
 
-        currentMatrix = Matrix_GetCurrentState();
+        currentMatrix = Matrix_GetCurrent();
 
         // Apply sfx along with damage effect
         if ((actor != NULL) && (effectAlpha > 0.05f) && (globalCtx->gameOverCtx.state == 0)) {
@@ -4565,15 +4564,15 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
 
                     gDPSetEnvColor(POLY_XLU_DISP++, KREG(20) + 200, KREG(21) + 200, KREG(22) + 255, (u8)alpha);
 
-                    Matrix_InsertTranslation(limbPos->x, limbPos->y, limbPos->z, MTXMODE_NEW);
+                    Matrix_Translate(limbPos->x, limbPos->y, limbPos->z, MTXMODE_NEW);
                     Matrix_Scale(frozenScale, frozenScale, frozenScale, MTXMODE_APPLY);
 
                     if (limbIndex & 1) {
-                        Matrix_InsertYRotation_f(M_PI, MTXMODE_APPLY);
+                        Matrix_RotateYF(M_PI, MTXMODE_APPLY);
                     }
 
                     if (limbIndex & 2) {
-                        Matrix_InsertZRotation_f(M_PI, MTXMODE_APPLY);
+                        Matrix_RotateZF(M_PI, MTXMODE_APPLY);
                     }
 
                     gSPMatrix(POLY_XLU_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx),
@@ -4604,8 +4603,8 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
                                Gfx_TwoTexScroll(globalCtx->state.gfxCtx, 0, twoTexScrollParam * 3,
                                                 twoTexScrollParam * -12, 32, 64, 1, 0, 0, 32, 32));
 
-                    Matrix_InsertTranslation(limbPos->x, limbPos->y, limbPos->z, MTXMODE_NEW);
-                    Matrix_NormalizeXYZ(&globalCtx->billboardMtxF);
+                    Matrix_Translate(limbPos->x, limbPos->y, limbPos->z, MTXMODE_NEW);
+                    Matrix_ReplaceRotation(&globalCtx->billboardMtxF);
                     Matrix_Scale(steamScale, steamScale, 1.0f, MTXMODE_APPLY);
 
                     gSPMatrix(POLY_XLU_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx),
@@ -4625,7 +4624,7 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
                     type = 255;
                 }
 
-                Matrix_SetCurrentState(&globalCtx->billboardMtxF);
+                Matrix_Put(&globalCtx->billboardMtxF);
                 Matrix_Scale((effectScale * 0.005f) * 1.35f, (effectScale * 0.005f), (effectScale * 0.005f) * 1.35f,
                              MTXMODE_APPLY);
 
@@ -4651,7 +4650,7 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
                                Gfx_TwoTexScroll(globalCtx->state.gfxCtx, 0, 0, 0, 32, 64, 1, 0,
                                                 ((limbIndex * 10 + gameplayFrames) * -20) & 0x1FF, 32, 128));
 
-                    Matrix_InsertYRotation_f(M_PI, MTXMODE_APPLY);
+                    Matrix_RotateYF(M_PI, MTXMODE_APPLY);
                     currentMatrix->mf[3][0] = limbPos->x;
                     currentMatrix->mf[3][1] = limbPos->y;
                     currentMatrix->mf[3][2] = limbPos->z;
@@ -4688,12 +4687,12 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
                     gDPSetEnvColor(POLY_XLU_DISP++, 255, 255, 100, 128);
                 }
 
-                Matrix_SetCurrentState(&globalCtx->billboardMtxF);
+                Matrix_Put(&globalCtx->billboardMtxF);
                 Matrix_Scale(lightOrbsScale, lightOrbsScale, 1.0f, MTXMODE_APPLY);
 
                 // Apply and draw a light orb over each limb of frozen actor
                 for (limbIndex = 0; limbIndex < limbPosCount; limbIndex++, limbPos++) {
-                    Matrix_InsertZRotation_f(randPlusMinusPoint5Scaled(2 * M_PI), MTXMODE_APPLY);
+                    Matrix_RotateZF(randPlusMinusPoint5Scaled(2 * M_PI), MTXMODE_APPLY);
                     currentMatrix->mf[3][0] = limbPos->x;
                     currentMatrix->mf[3][1] = limbPos->y;
                     currentMatrix->mf[3][2] = limbPos->z;
@@ -4726,14 +4725,14 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
 
                 gDPSetEnvColor(POLY_XLU_DISP++, (u8)(sREG(20) + 255), (u8)(sREG(21) + 255), (u8)sREG(22), (u8)sREG(23));
 
-                Matrix_SetCurrentState(&globalCtx->billboardMtxF);
+                Matrix_Put(&globalCtx->billboardMtxF);
                 Matrix_Scale(electricSparksScale, electricSparksScale, electricSparksScale, MTXMODE_APPLY);
 
                 // Every limb draws two electric sparks at random orientations
                 for (limbIndex = 0; limbIndex < limbPosCount; limbIndex++, limbPos++) {
                     // first electric spark
-                    Matrix_RotateStateAroundXAxis(Rand_ZeroFloat(2 * M_PI));
-                    Matrix_InsertZRotation_f(Rand_ZeroFloat(2 * M_PI), MTXMODE_APPLY);
+                    Matrix_RotateXFApply(Rand_ZeroFloat(2 * M_PI));
+                    Matrix_RotateZF(Rand_ZeroFloat(2 * M_PI), MTXMODE_APPLY);
                     currentMatrix->mf[3][0] = randPlusMinusPoint5Scaled((f32)sREG(24) + 30.0f) + limbPos->x;
                     currentMatrix->mf[3][1] = randPlusMinusPoint5Scaled((f32)sREG(24) + 30.0f) + limbPos->y;
                     currentMatrix->mf[3][2] = randPlusMinusPoint5Scaled((f32)sREG(24) + 30.0f) + limbPos->z;
@@ -4744,8 +4743,8 @@ void Actor_DrawDamageEffects(GlobalContext* globalCtx, Actor* actor, Vec3f limbP
                     gSPDisplayList(POLY_XLU_DISP++, gElectricSparkVtxDL);
 
                     // second electric spark
-                    Matrix_RotateStateAroundXAxis(Rand_ZeroFloat(2 * M_PI));
-                    Matrix_InsertZRotation_f(Rand_ZeroFloat(2 * M_PI), MTXMODE_APPLY);
+                    Matrix_RotateXFApply(Rand_ZeroFloat(2 * M_PI));
+                    Matrix_RotateZF(Rand_ZeroFloat(2 * M_PI), MTXMODE_APPLY);
                     currentMatrix->mf[3][0] = randPlusMinusPoint5Scaled((f32)sREG(24) + 30.0f) + limbPos->x;
                     currentMatrix->mf[3][1] = randPlusMinusPoint5Scaled((f32)sREG(24) + 30.0f) + limbPos->y;
                     currentMatrix->mf[3][2] = randPlusMinusPoint5Scaled((f32)sREG(24) + 30.0f) + limbPos->z;
diff --git a/src/code/z_bgcheck.c b/src/code/z_bgcheck.c
index 58c71c33e..0667bd84b 100644
--- a/src/code/z_bgcheck.c
+++ b/src/code/z_bgcheck.c
@@ -248,20 +248,20 @@ void func_800C0094(CollisionPoly* poly, f32 tx, f32 ty, f32 tz, MtxF* dest) {
         phi_f12 = 0.0f;
     }
     dest->xx = z_f14;
-    dest->xy = (-nx) * phi_f14;
-    dest->xz = (-nx) * phi_f12;
-    dest->yx = nx;
+    dest->yx = (-nx) * phi_f14;
+    dest->zx = (-nx) * phi_f12;
+    dest->xy = nx;
     dest->yy = ny;
-    dest->yz = nz;
-    dest->zx = 0.0f;
-    dest->zy = -phi_f12;
+    dest->zy = nz;
+    dest->xz = 0.0f;
+    dest->yz = -phi_f12;
     dest->zz = phi_f14;
-    dest->wx = tx;
-    dest->wy = ty;
-    dest->wz = tz;
-    dest->xw = 0.0f;
-    dest->yw = 0.0f;
-    dest->zw = 0.0f;
+    dest->xw = tx;
+    dest->yw = ty;
+    dest->zw = tz;
+    dest->wx = 0.0f;
+    dest->wy = 0.0f;
+    dest->wz = 0.0f;
     dest->ww = 1.0f;
 }
 
diff --git a/src/code/z_collision_check.c b/src/code/z_collision_check.c
index a08e3f822..7ac956e5d 100644
--- a/src/code/z_collision_check.c
+++ b/src/code/z_collision_check.c
@@ -3691,7 +3691,7 @@ void Collider_UpdateSpheres(s32 limb, ColliderJntSph* collider) {
             D_801EE1C0.x = collider->elements[i].dim.modelSphere.center.x;
             D_801EE1C0.y = collider->elements[i].dim.modelSphere.center.y;
             D_801EE1C0.z = collider->elements[i].dim.modelSphere.center.z;
-            Matrix_MultiplyVector3fByState(&D_801EE1C0, &D_801EE1D0);
+            Matrix_MultVec3f(&D_801EE1C0, &D_801EE1D0);
             collider->elements[i].dim.worldSphere.center.x = D_801EE1D0.x;
             collider->elements[i].dim.worldSphere.center.y = D_801EE1D0.y;
             collider->elements[i].dim.worldSphere.center.z = D_801EE1D0.z;
@@ -3733,7 +3733,7 @@ void Collider_UpdateSphere(s32 limb, ColliderSphere* collider) {
         D_801EE1E0.x = collider->dim.modelSphere.center.x;
         D_801EE1E0.y = collider->dim.modelSphere.center.y;
         D_801EE1E0.z = collider->dim.modelSphere.center.z;
-        Matrix_MultiplyVector3fByState(&D_801EE1E0, &D_801EE1F0);
+        Matrix_MultVec3f(&D_801EE1E0, &D_801EE1F0);
         collider->dim.worldSphere.center.x = D_801EE1F0.x;
         collider->dim.worldSphere.center.y = D_801EE1F0.y;
         collider->dim.worldSphere.center.z = D_801EE1F0.z;
diff --git a/src/code/z_debug_display.c b/src/code/z_debug_display.c
index ab3478fa1..fb7378751 100644
--- a/src/code/z_debug_display.c
+++ b/src/code/z_debug_display.c
@@ -68,10 +68,10 @@ void DebugDisplay_DrawSpriteI8(DebugDispObject* dispObj, void* texture, GlobalCo
     func_8012C6FC(globalCtx->state.gfxCtx);
 
     gDPSetPrimColor(POLY_XLU_DISP++, 0, 0, dispObj->color.r, dispObj->color.g, dispObj->color.b, dispObj->color.a);
-    Matrix_InsertTranslation(dispObj->pos.x, dispObj->pos.y, dispObj->pos.z, MTXMODE_NEW);
+    Matrix_Translate(dispObj->pos.x, dispObj->pos.y, dispObj->pos.z, MTXMODE_NEW);
     Matrix_Scale(dispObj->scale.x, dispObj->scale.y, dispObj->scale.z, MTXMODE_APPLY);
-    Matrix_InsertMatrix(&globalCtx->billboardMtxF, MTXMODE_APPLY);
-    Matrix_InsertRotation(dispObj->rot.x, dispObj->rot.y, dispObj->rot.z, MTXMODE_APPLY);
+    Matrix_Mult(&globalCtx->billboardMtxF, MTXMODE_APPLY);
+    Matrix_RotateZYX(dispObj->rot.x, dispObj->rot.y, dispObj->rot.z, MTXMODE_APPLY);
 
     gDPLoadTextureBlock(POLY_XLU_DISP++, texture, G_IM_FMT_I, G_IM_SIZ_8b, 16, 16, 0, G_TX_NOMIRROR | G_TX_WRAP,
                         G_TX_NOMIRROR | G_TX_WRAP, G_TX_NOMASK, G_TX_NOMASK, G_TX_NOLOD, G_TX_NOLOD);
@@ -94,7 +94,7 @@ void DebugDisplay_DrawPolygon(DebugDispObject* dispObj, void* arg1, GlobalContex
 
     gSPSetLights1(POLY_XLU_DISP++, sDebugDisplayLight1);
 
-    Matrix_SetStateRotationAndTranslation(dispObj->pos.x, dispObj->pos.y, dispObj->pos.z, &dispObj->rot);
+    Matrix_SetTranslateRotateYXZ(dispObj->pos.x, dispObj->pos.y, dispObj->pos.z, &dispObj->rot);
     Matrix_Scale(dispObj->scale.x, dispObj->scale.y, dispObj->scale.z, MTXMODE_APPLY);
     gSPMatrix(POLY_XLU_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
 
diff --git a/src/code/z_eff_footmark.c b/src/code/z_eff_footmark.c
index d246aaffa..f959867fe 100644
--- a/src/code/z_eff_footmark.c
+++ b/src/code/z_eff_footmark.c
@@ -106,7 +106,7 @@ void EffFootmark_Draw(GlobalContext* globalCtx) {
 
     for (footmark = globalCtx->footprintInfo, i = 0; i < 100; i++, footmark++) {
         if (footmark->actor != NULL) {
-            Matrix_SetCurrentState(&footmark->displayMatrix);
+            Matrix_Put(&footmark->displayMatrix);
             Matrix_Scale(footmark->size * (1.0f / 0x100) * 0.7f, 1, footmark->size * (1.0f / 0x100), MTXMODE_APPLY);
 
             gSPMatrix(gfxCtx->polyXlu.p++, Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_NOPUSH | G_MTX_LOAD);
diff --git a/src/code/z_fcurve_data_skelanime.c b/src/code/z_fcurve_data_skelanime.c
index e22b5f351..c8eb927fa 100644
--- a/src/code/z_fcurve_data_skelanime.c
+++ b/src/code/z_fcurve_data_skelanime.c
@@ -104,7 +104,7 @@ void SkelCurve_DrawLimb(GlobalContext* globalCtx, s32 limbIndex, SkelAnimeCurve*
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     if (overrideLimbDraw == NULL ||
         (overrideLimbDraw != NULL && overrideLimbDraw(globalCtx, skelCurve, limbIndex, thisx))) {
@@ -126,7 +126,7 @@ void SkelCurve_DrawLimb(GlobalContext* globalCtx, s32 limbIndex, SkelAnimeCurve*
         pos.y = transform->y;
         pos.z = transform->z;
 
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         Matrix_Scale(scale.x, scale.y, scale.z, MTXMODE_APPLY);
 
         if (lod == 0) {
@@ -164,7 +164,7 @@ void SkelCurve_DrawLimb(GlobalContext* globalCtx, s32 limbIndex, SkelAnimeCurve*
         SkelCurve_DrawLimb(globalCtx, limb->firstChildIdx, skelCurve, overrideLimbDraw, postLimbDraw, lod, thisx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->nextLimbIdx != LIMB_DONE) {
         SkelCurve_DrawLimb(globalCtx, limb->nextLimbIdx, skelCurve, overrideLimbDraw, postLimbDraw, lod, thisx);
diff --git a/src/code/z_fireobj.c b/src/code/z_fireobj.c
index 379ebda1d..f627c6394 100644
--- a/src/code/z_fireobj.c
+++ b/src/code/z_fireobj.c
@@ -159,7 +159,7 @@ void FireObj_Draw(GlobalContext* globalCtx, FireObj* fire) {
         vec.x = 0;
         vec.y = Camera_GetCamDirYaw(GET_ACTIVE_CAM(globalCtx)) + 0x8000;
         vec.z = 0;
-        Matrix_SetStateRotationAndTranslation(fire->position.x, fire->position.y, fire->position.z, &vec);
+        Matrix_SetTranslateRotateYXZ(fire->position.x, fire->position.y, fire->position.z, &vec);
         Matrix_Scale(fire->xScale, fire->yScale, 1.0f, MTXMODE_APPLY);
 
         gSPMatrix(POLY_XLU_DISP++, Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
diff --git a/src/code/z_lights.c b/src/code/z_lights.c
index 5d47b4153..e9fae530a 100644
--- a/src/code/z_lights.c
+++ b/src/code/z_lights.c
@@ -431,7 +431,7 @@ void Lights_DrawGlow(GlobalContext* globalCtx) {
 
                     gDPSetPrimColor(dl++, 0, 0, params->color[0], params->color[1], params->color[2], 50);
 
-                    Matrix_InsertTranslation(params->x, params->y, params->z, MTXMODE_NEW);
+                    Matrix_Translate(params->x, params->y, params->z, MTXMODE_NEW);
                     Matrix_Scale(scale, scale, scale, MTXMODE_APPLY);
 
                     gSPMatrix(dl++, Matrix_NewMtx(globalCtx->state.gfxCtx),
diff --git a/src/code/z_skelanime.c b/src/code/z_skelanime.c
index 6563eecfc..0a3334bc8 100644
--- a/src/code/z_skelanime.c
+++ b/src/code/z_skelanime.c
@@ -33,7 +33,7 @@ void SkelAnime_DrawLimbLod(GlobalContext* globalCtx, s32 limbIndex, void** skele
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
     rot = jointTable[limbIndex];
@@ -44,7 +44,7 @@ void SkelAnime_DrawLimbLod(GlobalContext* globalCtx, s32 limbIndex, void** skele
 
     dList = limb->dLists[lod];
     if ((overrideLimbDraw == NULL) || (overrideLimbDraw(globalCtx, limbIndex, &dList, &pos, &rot, actor) == 0)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (dList != NULL) {
             Gfx* polyTemp = POLY_OPA_DISP;
 
@@ -63,7 +63,7 @@ void SkelAnime_DrawLimbLod(GlobalContext* globalCtx, s32 limbIndex, void** skele
         SkelAnime_DrawLimbLod(globalCtx, limb->child, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor, lod);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         SkelAnime_DrawLimbLod(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor,
@@ -91,7 +91,7 @@ void SkelAnime_DrawLod(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTa
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
     pos.x = jointTable[0].x;
@@ -102,7 +102,7 @@ void SkelAnime_DrawLod(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTa
     dList = rootLimb->dLists[lod];
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &dList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (dList != NULL) {
             Gfx* polyTemp = POLY_OPA_DISP;
 
@@ -123,7 +123,7 @@ void SkelAnime_DrawLod(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTa
                               lod);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     CLOSE_DISPS(globalCtx->state.gfxCtx);
 }
@@ -143,7 +143,7 @@ void SkelAnime_DrawFlexLimbLod(GlobalContext* globalCtx, s32 limbIndex, void** s
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
@@ -157,7 +157,7 @@ void SkelAnime_DrawFlexLimbLod(GlobalContext* globalCtx, s32 limbIndex, void** s
     newDList = limbDList = limb->dLists[lod];
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &newDList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (newDList != NULL) {
             Matrix_ToMtx(*mtx);
             gSPMatrix(POLY_OPA_DISP++, *mtx, G_MTX_LOAD);
@@ -178,7 +178,7 @@ void SkelAnime_DrawFlexLimbLod(GlobalContext* globalCtx, s32 limbIndex, void** s
                                   lod, mtx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         SkelAnime_DrawFlexLimbLod(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor,
@@ -211,7 +211,7 @@ void SkelAnime_DrawFlexLod(GlobalContext* globalCtx, void** skeleton, Vec3s* joi
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
     gSPSegment(POLY_OPA_DISP++, 0x0D, mtx);
-    Matrix_StatePush();
+    Matrix_Push();
 
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
     pos.x = jointTable[0].x;
@@ -223,7 +223,7 @@ void SkelAnime_DrawFlexLod(GlobalContext* globalCtx, void** skeleton, Vec3s* joi
     newDList = limbDList = rootLimb->dLists[lod];
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &newDList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (newDList != NULL) {
             Gfx* polyTemp = POLY_OPA_DISP;
 
@@ -246,7 +246,7 @@ void SkelAnime_DrawFlexLod(GlobalContext* globalCtx, void** skeleton, Vec3s* joi
                                   actor, lod, &mtx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     CLOSE_DISPS(globalCtx->state.gfxCtx);
 }
@@ -263,7 +263,7 @@ void SkelAnime_DrawLimbOpa(GlobalContext* globalCtx, s32 limbIndex, void** skele
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
@@ -274,7 +274,7 @@ void SkelAnime_DrawLimbOpa(GlobalContext* globalCtx, s32 limbIndex, void** skele
     dList = limb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &dList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (dList != NULL) {
             Gfx* polyTemp = POLY_OPA_DISP;
 
@@ -292,7 +292,7 @@ void SkelAnime_DrawLimbOpa(GlobalContext* globalCtx, s32 limbIndex, void** skele
         SkelAnime_DrawLimbOpa(globalCtx, limb->child, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         SkelAnime_DrawLimbOpa(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor);
@@ -318,7 +318,7 @@ void SkelAnime_DrawOpa(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTa
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
 
     pos.x = jointTable[0].x;
@@ -329,7 +329,7 @@ void SkelAnime_DrawOpa(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTa
     dList = rootLimb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &dList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (dList != NULL) {
             Gfx* polyTemp = POLY_OPA_DISP;
 
@@ -347,7 +347,7 @@ void SkelAnime_DrawOpa(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTa
         SkelAnime_DrawLimbOpa(globalCtx, rootLimb->child, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     CLOSE_DISPS(globalCtx->state.gfxCtx);
 }
@@ -363,7 +363,7 @@ void SkelAnime_DrawFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex, void** s
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
@@ -376,7 +376,7 @@ void SkelAnime_DrawFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex, void** s
     newDList = limbDList = limb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &newDList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (newDList != NULL) {
             Matrix_ToMtx(*limbMatricies);
             gSPMatrix(POLY_OPA_DISP++, *limbMatricies, G_MTX_LOAD);
@@ -397,7 +397,7 @@ void SkelAnime_DrawFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex, void** s
                                   limbMatricies);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         SkelAnime_DrawFlexLimbOpa(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor,
@@ -430,7 +430,7 @@ void SkelAnime_DrawFlexOpa(GlobalContext* globalCtx, void** skeleton, Vec3s* joi
 
     gSPSegment(POLY_OPA_DISP++, 0x0D, mtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
 
@@ -442,7 +442,7 @@ void SkelAnime_DrawFlexOpa(GlobalContext* globalCtx, void** skeleton, Vec3s* joi
     newDList = limbDList = rootLimb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &newDList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (newDList != NULL) {
             Gfx* polyTemp = POLY_OPA_DISP;
 
@@ -467,7 +467,7 @@ void SkelAnime_DrawFlexOpa(GlobalContext* globalCtx, void** skeleton, Vec3s* joi
                                   actor, &mtx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     CLOSE_DISPS(globalCtx->state.gfxCtx);
 }
@@ -483,7 +483,7 @@ void SkelAnime_DrawTransformFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex,
 
     OPEN_DISPS(globalCtx->state.gfxCtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
@@ -496,8 +496,8 @@ void SkelAnime_DrawTransformFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex,
     newDList = limbDList = limb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &newDList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
-        Matrix_StatePush();
+        Matrix_TranslateRotateZYX(&pos, &rot);
+        Matrix_Push();
 
         transformLimbDraw(globalCtx, limbIndex, actor);
 
@@ -514,7 +514,7 @@ void SkelAnime_DrawTransformFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex,
                 (*mtx)++;
             }
         }
-        Matrix_StatePop();
+        Matrix_Pop();
     }
 
     if (postLimbDraw != NULL) {
@@ -526,7 +526,7 @@ void SkelAnime_DrawTransformFlexLimbOpa(GlobalContext* globalCtx, s32 limbIndex,
                                            transformLimbDraw, actor, mtx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         SkelAnime_DrawTransformFlexLimbOpa(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw,
@@ -566,7 +566,7 @@ void SkelAnime_DrawTransformFlexOpa(GlobalContext* globalCtx, void** skeleton, V
 
     gSPSegment(POLY_OPA_DISP++, 0x0D, mtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
 
@@ -578,8 +578,8 @@ void SkelAnime_DrawTransformFlexOpa(GlobalContext* globalCtx, void** skeleton, V
     newDList = limbDList = rootLimb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &newDList, &pos, &rot, actor)) {
-        Matrix_JointPosition(&pos, &rot);
-        Matrix_StatePush();
+        Matrix_TranslateRotateZYX(&pos, &rot);
+        Matrix_Push();
 
         transformLimbDraw(globalCtx, 1, actor);
 
@@ -595,7 +595,7 @@ void SkelAnime_DrawTransformFlexOpa(GlobalContext* globalCtx, void** skeleton, V
                 Matrix_ToMtx(mtx++);
             }
         }
-        Matrix_StatePop();
+        Matrix_Pop();
     }
 
     if (postLimbDraw != NULL) {
@@ -607,7 +607,7 @@ void SkelAnime_DrawTransformFlexOpa(GlobalContext* globalCtx, void** skeleton, V
                                            postLimbDraw, transformLimbDraw, actor, &mtx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     CLOSE_DISPS(globalCtx->state.gfxCtx);
 }
@@ -660,7 +660,7 @@ Gfx* SkelAnime_DrawLimb(GlobalContext* globalCtx, s32 limbIndex, void** skeleton
     Vec3f pos;
     Vec3s rot;
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
@@ -673,7 +673,7 @@ Gfx* SkelAnime_DrawLimb(GlobalContext* globalCtx, s32 limbIndex, void** skeleton
     dList = limb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &dList, &pos, &rot, actor, &gfx)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (dList != NULL) {
             gSPMatrix(&gfx[0], Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_LOAD);
             gSPDisplayList(&gfx[1], dList);
@@ -690,7 +690,7 @@ Gfx* SkelAnime_DrawLimb(GlobalContext* globalCtx, s32 limbIndex, void** skeleton
                                  gfx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         gfx = SkelAnime_DrawLimb(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw, actor,
@@ -716,7 +716,7 @@ Gfx* SkelAnime_Draw(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTable
         return NULL;
     }
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
 
@@ -729,7 +729,7 @@ Gfx* SkelAnime_Draw(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTable
     dList = rootLimb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &dList, &pos, &rot, actor, &gfx)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (dList != NULL) {
             gSPMatrix(&gfx[0], Matrix_NewMtx(globalCtx->state.gfxCtx), G_MTX_LOAD);
             gSPDisplayList(&gfx[1], dList);
@@ -746,7 +746,7 @@ Gfx* SkelAnime_Draw(GlobalContext* globalCtx, void** skeleton, Vec3s* jointTable
                                  actor, gfx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     return gfx;
 }
@@ -763,7 +763,7 @@ Gfx* SkelAnime_DrawFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void** skel
     Vec3f pos;
     Vec3s rot;
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
@@ -776,7 +776,7 @@ Gfx* SkelAnime_DrawFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void** skel
     newDList = limbDList = limb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &newDList, &pos, &rot, actor, &gfx)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (newDList != NULL) {
             gSPMatrix(&gfx[0], Matrix_ToMtx(*mtx), G_MTX_LOAD);
             gSPDisplayList(&gfx[1], newDList);
@@ -799,7 +799,7 @@ Gfx* SkelAnime_DrawFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void** skel
                                      actor, mtx, gfx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     if (limb->sibling != LIMB_DONE) {
         gfx = SkelAnime_DrawFlexLimb(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw,
@@ -832,7 +832,7 @@ Gfx* SkelAnime_DrawFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jointT
 
     gSPSegment(gfx++, 0x0D, mtx);
 
-    Matrix_StatePush();
+    Matrix_Push();
 
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
 
@@ -845,7 +845,7 @@ Gfx* SkelAnime_DrawFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jointT
     newDList = limbDList = rootLimb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, 1, &newDList, &pos, &rot, actor, &gfx)) {
-        Matrix_JointPosition(&pos, &rot);
+        Matrix_TranslateRotateZYX(&pos, &rot);
         if (newDList != NULL) {
             gSPMatrix(&gfx[0], Matrix_ToMtx(mtx), G_MTX_LOAD);
             gSPDisplayList(&gfx[1], newDList);
@@ -868,7 +868,7 @@ Gfx* SkelAnime_DrawFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jointT
                                      actor, &mtx, gfx);
     }
 
-    Matrix_StatePop();
+    Matrix_Pop();
 
     return gfx;
 }
diff --git a/src/code/z_skin.c b/src/code/z_skin.c
index 808a00da0..de449c02d 100644
--- a/src/code/z_skin.c
+++ b/src/code/z_skin.c
@@ -13,13 +13,13 @@ void Skin_UpdateVertices(MtxF* mtx, SkinVertex* skinVertices, SkinLimbModif* mod
     Vec3f normal;
     Vec3f sp44;
 
-    wTemp.x = mtx->wx;
-    wTemp.y = mtx->wy;
-    wTemp.z = mtx->wz;
+    wTemp.x = mtx->xw;
+    wTemp.y = mtx->yw;
+    wTemp.z = mtx->zw;
 
-    mtx->wx = 0.0f;
-    mtx->wy = 0.0f;
-    mtx->wz = 0.0f;
+    mtx->xw = 0.0f;
+    mtx->yw = 0.0f;
+    mtx->zw = 0.0f;
 
     for (vertexEntry = skinVertices; vertexEntry < &skinVertices[modifEntry->vtxCount]; vertexEntry++) {
         vtx = &vtxBuf[vertexEntry->index];
@@ -39,9 +39,9 @@ void Skin_UpdateVertices(MtxF* mtx, SkinVertex* skinVertices, SkinLimbModif* mod
         vtx->n.n[2] = normal.z;
     }
 
-    mtx->wx = wTemp.x;
-    mtx->wy = wTemp.y;
-    mtx->wz = wTemp.z;
+    mtx->xw = wTemp.x;
+    mtx->yw = wTemp.y;
+    mtx->zw = wTemp.z;
 }
 
 void Skin_ApplyLimbModifications(GraphicsContext* gfxCtx, Skin* skin, s32 limbIndex, s32 arg3) {
diff --git a/src/code/z_skin_matrix.c b/src/code/z_skin_matrix.c
index f355aec9b..9b30a3c0b 100644
--- a/src/code/z_skin_matrix.c
+++ b/src/code/z_skin_matrix.c
@@ -1,13 +1,11 @@
 #include "global.h"
 
-// clang-format off
-MtxF sMtxFClear = {
-    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,
-};
-// clang-format on
+MtxF sMtxFClear = { {
+    { 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 },
+} };
 
 /**
  * Multiplies a 4 component row vector [ src , 1 ] by the matrix mf and writes the resulting 4 components to xyzDest
@@ -16,10 +14,10 @@ MtxF sMtxFClear = {
  * \f[ [\texttt{xyzDest}, \texttt{wDest}] = [\texttt{src}, 1] \cdot [mf] \f]
  */
 void SkinMatrix_Vec3fMtxFMultXYZW(MtxF* mf, Vec3f* src, Vec3f* xyzDest, f32* wDest) {
-    xyzDest->x = mf->wx + ((src->x * mf->xx) + (src->y * mf->yx) + (src->z * mf->zx));
-    xyzDest->y = mf->wy + ((src->x * mf->xy) + (src->y * mf->yy) + (src->z * mf->zy));
-    xyzDest->z = mf->wz + ((src->x * mf->xz) + (src->y * mf->yz) + (src->z * mf->zz));
-    *wDest = mf->ww + ((src->x * mf->xw) + (src->y * mf->yw) + (src->z * mf->zw));
+    xyzDest->x = mf->xw + ((src->x * mf->xx) + (src->y * mf->xy) + (src->z * mf->xz));
+    xyzDest->y = mf->yw + ((src->x * mf->yx) + (src->y * mf->yy) + (src->z * mf->yz));
+    xyzDest->z = mf->zw + ((src->x * mf->zx) + (src->y * mf->zy) + (src->z * mf->zz));
+    *wDest = mf->ww + ((src->x * mf->wx) + (src->y * mf->wy) + (src->z * mf->wz));
 }
 
 /**
@@ -29,21 +27,22 @@ void SkinMatrix_Vec3fMtxFMultXYZW(MtxF* mf, Vec3f* src, Vec3f* xyzDest, f32* wDe
  */
 void SkinMatrix_Vec3fMtxFMultXYZ(MtxF* mf, Vec3f* src, Vec3f* dest) {
     f32 mx = mf->xx;
-    f32 my = mf->yx;
-    f32 mz = mf->zx;
-    f32 mw = mf->wx;
+    f32 my = mf->xy;
+    f32 mz = mf->xz;
+    f32 mw = mf->xw;
+
     dest->x = mw + ((src->x * mx) + (src->y * my) + (src->z * mz));
 
-    mx = mf->xy;
+    mx = mf->yx;
     my = mf->yy;
-    mz = mf->zy;
-    mw = mf->wy;
+    mz = mf->yz;
+    mw = mf->yw;
     dest->y = mw + ((src->x * mx) + (src->y * my) + (src->z * mz));
 
-    mx = mf->xz;
-    my = mf->yz;
+    mx = mf->zx;
+    my = mf->zy;
     mz = mf->zz;
-    mw = mf->wz;
+    mw = mf->zw;
     dest->z = mw + ((src->x * mx) + (src->y * my) + (src->z * mz));
 }
 
@@ -59,122 +58,122 @@ void SkinMatrix_MtxFMtxFMult(MtxF* mfB, MtxF* mfA, MtxF* dest) {
 
     //---COL1---
     f32 cx = mfB->xx;
-    f32 cy = mfB->yx;
-    f32 cz = mfB->zx;
-    f32 cw = mfB->wx;
+    f32 cy = mfB->xy;
+    f32 cz = mfB->xz;
+    f32 cw = mfB->xw;
     //--------
 
     rx = mfA->xx;
-    ry = mfA->xy;
-    rz = mfA->xz;
-    rw = mfA->xw;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
     dest->xx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->yx;
+    rx = mfA->xy;
     ry = mfA->yy;
-    rz = mfA->yz;
-    rw = mfA->yw;
-    dest->yx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->xy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->zx;
-    ry = mfA->zy;
+    rx = mfA->xz;
+    ry = mfA->yz;
     rz = mfA->zz;
-    rw = mfA->zw;
-    dest->zx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    rw = mfA->wz;
+    dest->xz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->wx;
-    ry = mfA->wy;
-    rz = mfA->wz;
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
     rw = mfA->ww;
-    dest->wx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    dest->xw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
     //---2Col---
-    cx = mfB->xy;
+    cx = mfB->yx;
     cy = mfB->yy;
-    cz = mfB->zy;
-    cw = mfB->wy;
+    cz = mfB->yz;
+    cw = mfB->yw;
     //--------
     rx = mfA->xx;
-    ry = mfA->xy;
-    rz = mfA->xz;
-    rw = mfA->xw;
-    dest->xy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->yx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->yx;
+    rx = mfA->xy;
     ry = mfA->yy;
-    rz = mfA->yz;
-    rw = mfA->yw;
+    rz = mfA->zy;
+    rw = mfA->wy;
     dest->yy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->zx;
-    ry = mfA->zy;
+    rx = mfA->xz;
+    ry = mfA->yz;
     rz = mfA->zz;
-    rw = mfA->zw;
-    dest->zy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    rw = mfA->wz;
+    dest->yz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->wx;
-    ry = mfA->wy;
-    rz = mfA->wz;
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
     rw = mfA->ww;
-    dest->wy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    dest->yw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
     //---3Col---
-    cx = mfB->xz;
-    cy = mfB->yz;
+    cx = mfB->zx;
+    cy = mfB->zy;
     cz = mfB->zz;
-    cw = mfB->wz;
+    cw = mfB->zw;
     //--------
     rx = mfA->xx;
-    ry = mfA->xy;
-    rz = mfA->xz;
-    rw = mfA->xw;
-    dest->xz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->zx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->yx;
+    rx = mfA->xy;
     ry = mfA->yy;
-    rz = mfA->yz;
-    rw = mfA->yw;
-    dest->yz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->zy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->zx;
-    ry = mfA->zy;
+    rx = mfA->xz;
+    ry = mfA->yz;
     rz = mfA->zz;
-    rw = mfA->zw;
+    rw = mfA->wz;
     dest->zz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->wx;
-    ry = mfA->wy;
-    rz = mfA->wz;
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
     rw = mfA->ww;
-    dest->wz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    dest->zw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
     //---4Col---
-    cx = mfB->xw;
-    cy = mfB->yw;
-    cz = mfB->zw;
+    cx = mfB->wx;
+    cy = mfB->wy;
+    cz = mfB->wz;
     cw = mfB->ww;
     //--------
     rx = mfA->xx;
-    ry = mfA->xy;
-    rz = mfA->xz;
-    rw = mfA->xw;
-    dest->xw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->wx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->yx;
+    rx = mfA->xy;
     ry = mfA->yy;
-    rz = mfA->yz;
-    rw = mfA->yw;
-    dest->yw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->wy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->zx;
-    ry = mfA->zy;
+    rx = mfA->xz;
+    ry = mfA->yz;
     rz = mfA->zz;
-    rw = mfA->zw;
-    dest->zw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+    rw = mfA->wz;
+    dest->wz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 
-    rx = mfA->wx;
-    ry = mfA->wy;
-    rz = mfA->wz;
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
     rw = mfA->ww;
     dest->ww = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
 }
@@ -188,39 +187,39 @@ void SkinMatrix_GetClear(MtxF** mfp) {
 
 void SkinMatrix_Clear(MtxF* mf) {
     mf->xx = 1.0f;
-    mf->xy = 0.0f;
-    mf->xz = 0.0f;
-    mf->xw = 0.0f;
     mf->yx = 0.0f;
-    mf->yy = 1.0f;
-    mf->yz = 0.0f;
-    mf->yw = 0.0f;
     mf->zx = 0.0f;
-    mf->zy = 0.0f;
-    mf->zz = 1.0f;
-    mf->zw = 0.0f;
     mf->wx = 0.0f;
+    mf->xy = 0.0f;
+    mf->yy = 1.0f;
+    mf->zy = 0.0f;
     mf->wy = 0.0f;
+    mf->xz = 0.0f;
+    mf->yz = 0.0f;
+    mf->zz = 1.0f;
     mf->wz = 0.0f;
+    mf->xw = 0.0f;
+    mf->yw = 0.0f;
+    mf->zw = 0.0f;
     mf->ww = 1.0f;
 }
 
 void SkinMatrix_MtxFCopy(MtxF* src, MtxF* dest) {
     dest->xx = src->xx;
-    dest->xy = src->xy;
-    dest->xz = src->xz;
-    dest->xw = src->xw;
     dest->yx = src->yx;
-    dest->yy = src->yy;
-    dest->yz = src->yz;
-    dest->yw = src->yw;
     dest->zx = src->zx;
-    dest->zy = src->zy;
-    dest->zz = src->zz;
-    dest->zw = src->zw;
     dest->wx = src->wx;
+    dest->xy = src->xy;
+    dest->yy = src->yy;
+    dest->zy = src->zy;
     dest->wy = src->wy;
+    dest->xz = src->xz;
+    dest->yz = src->yz;
+    dest->zz = src->zz;
     dest->wz = src->wz;
+    dest->xw = src->xw;
+    dest->yw = src->yw;
+    dest->zw = src->zw;
     dest->ww = src->ww;
 }
 
@@ -282,18 +281,18 @@ s32 SkinMatrix_Invert(MtxF* src, MtxF* dest) {
  * Produces a matrix which scales x,y,z components of vectors or x,y,z rows of matrices (when applied on LHS)
  */
 void SkinMatrix_SetScale(MtxF* mf, f32 x, f32 y, f32 z) {
-    mf->xy = 0.0f;
-    mf->xz = 0.0f;
-    mf->xw = 0.0f;
     mf->yx = 0.0f;
-    mf->yz = 0.0f;
-    mf->yw = 0.0f;
     mf->zx = 0.0f;
-    mf->zy = 0.0f;
-    mf->zw = 0.0f;
     mf->wx = 0.0f;
+    mf->xy = 0.0f;
+    mf->zy = 0.0f;
     mf->wy = 0.0f;
+    mf->xz = 0.0f;
+    mf->yz = 0.0f;
     mf->wz = 0.0f;
+    mf->xw = 0.0f;
+    mf->yw = 0.0f;
+    mf->zw = 0.0f;
     mf->ww = 1.0f;
     mf->xx = x;
     mf->yy = y;
@@ -307,16 +306,16 @@ void SkinMatrix_SetRotateRPY(MtxF* mf, s16 roll, s16 pitch, s16 yaw) {
     f32 cos2;
     f32 sin = Math_SinS(yaw);
     f32 cos = Math_CosS(yaw);
-    f32 yx;
+    f32 xy;
     f32 sin2;
-    f32 zx;
+    f32 xz;
     f32 yy;
-    f32 zy;
+    f32 yz;
 
     mf->yy = cos;
-    mf->yx = -sin;
-    mf->xw = mf->yw = mf->zw = 0;
+    mf->xy = -sin;
     mf->wx = mf->wy = mf->wz = 0;
+    mf->xw = mf->yw = mf->zw = 0;
     mf->ww = 1;
 
     if (pitch != 0) {
@@ -324,19 +323,19 @@ void SkinMatrix_SetRotateRPY(MtxF* mf, s16 roll, s16 pitch, s16 yaw) {
         cos2 = Math_CosS(pitch);
 
         mf->xx = cos * cos2;
-        mf->zx = cos * sin2;
+        mf->xz = cos * sin2;
 
-        mf->xy = sin * cos2;
-        mf->zy = sin * sin2;
-        mf->xz = -sin2;
+        mf->yx = sin * cos2;
+        mf->yz = sin * sin2;
+        mf->zx = -sin2;
         mf->zz = cos2;
     } else {
         mf->xx = cos;
         if (1) {}
         if (1) {}
-        zx = sin; // required to match
-        mf->xy = sin;
-        mf->xz = mf->zx = mf->zy = 0;
+        xz = sin; // required to match
+        mf->yx = sin;
+        mf->zx = mf->xz = mf->yz = 0;
         mf->zz = 1;
     }
 
@@ -344,22 +343,22 @@ void SkinMatrix_SetRotateRPY(MtxF* mf, s16 roll, s16 pitch, s16 yaw) {
         sin2 = Math_SinS(roll);
         cos2 = Math_CosS(roll);
 
-        yx = mf->yx;
-        zx = mf->zx;
-        mf->yx = (yx * cos2) + (zx * sin2);
-        mf->zx = (zx * cos2) - (yx * sin2);
+        xy = mf->xy;
+        xz = mf->xz;
+        mf->xy = (xy * cos2) + (xz * sin2);
+        mf->xz = (xz * cos2) - (xy * sin2);
 
         if (1) {}
-        zy = mf->zy;
+        yz = mf->yz;
         yy = mf->yy;
-        mf->yy = (yy * cos2) + (zy * sin2);
-        mf->zy = (zy * cos2) - (yy * sin2);
+        mf->yy = (yy * cos2) + (yz * sin2);
+        mf->yz = (yz * cos2) - (yy * sin2);
 
         if (cos2) {}
-        mf->yz = mf->zz * sin2;
+        mf->zy = mf->zz * sin2;
         mf->zz = mf->zz * cos2;
     } else {
-        mf->yz = 0;
+        mf->zy = 0;
     }
 }
 
@@ -370,23 +369,23 @@ void SkinMatrix_SetRotateYRP(MtxF* mf, s16 yaw, s16 roll, s16 pitch) {
     f32 cos2;
     f32 sin;
     f32 cos;
-    f32 xz;
+    f32 zx;
     f32 sin2;
-    f32 yz;
+    f32 zy;
     f32 xx;
-    f32 yx;
+    f32 xy;
 
     sin = Math_SinS(roll);
     cos = Math_CosS(roll);
 
     mf->xx = cos;
-    mf->xz = -sin;
-    mf->zw = 0;
-    mf->yw = 0;
-    mf->xw = 0;
+    mf->zx = -sin;
     mf->wz = 0;
     mf->wy = 0;
     mf->wx = 0;
+    mf->zw = 0;
+    mf->yw = 0;
+    mf->xw = 0;
     mf->ww = 1;
 
     if (yaw != 0) {
@@ -394,19 +393,19 @@ void SkinMatrix_SetRotateYRP(MtxF* mf, s16 yaw, s16 roll, s16 pitch) {
         cos2 = Math_CosS(yaw);
 
         mf->zz = cos * cos2;
-        mf->yz = cos * sin2;
+        mf->zy = cos * sin2;
 
-        mf->zx = sin * cos2;
-        mf->yx = sin * sin2;
-        mf->zy = -sin2;
+        mf->xz = sin * cos2;
+        mf->xy = sin * sin2;
+        mf->yz = -sin2;
         mf->yy = cos2;
     } else {
         mf->zz = cos;
         if (1) {}
         if (1) {}
-        yx = sin; // required to match
-        mf->zx = sin;
-        mf->yx = mf->yz = mf->zy = 0;
+        xy = sin; // required to match
+        mf->xz = sin;
+        mf->xy = mf->zy = mf->yz = 0;
         mf->yy = 1;
     }
 
@@ -414,19 +413,19 @@ void SkinMatrix_SetRotateYRP(MtxF* mf, s16 yaw, s16 roll, s16 pitch) {
         sin2 = Math_SinS(pitch);
         cos2 = Math_CosS(pitch);
         xx = mf->xx;
-        yx = mf->yx;
-        mf->xx = (xx * cos2) + (yx * sin2);
-        mf->yx = yx * cos2 - (xx * sin2);
+        xy = mf->xy;
+        mf->xx = (xx * cos2) + (xy * sin2);
+        mf->xy = xy * cos2 - (xx * sin2);
         if (1) {}
-        yz = mf->yz;
-        xz = mf->xz;
-        mf->xz = (xz * cos2) + (yz * sin2);
-        mf->yz = (yz * cos2) - (xz * sin2);
+        zy = mf->zy;
+        zx = mf->zx;
+        mf->zx = (zx * cos2) + (zy * sin2);
+        mf->zy = (zy * cos2) - (zx * sin2);
         if (cos2) {}
-        mf->xy = mf->yy * sin2;
+        mf->yx = mf->yy * sin2;
         mf->yy = mf->yy * cos2;
     } else {
-        mf->xy = 0;
+        mf->yx = 0;
     }
 }
 
@@ -434,22 +433,22 @@ void SkinMatrix_SetRotateYRP(MtxF* mf, s16 yaw, s16 roll, s16 pitch) {
  * Produces a matrix which translates a vector by amounts in the x, y and z directions
  */
 void SkinMatrix_SetTranslate(MtxF* mf, f32 x, f32 y, f32 z) {
-    mf->xy = 0.0f;
-    mf->xz = 0.0f;
-    mf->xw = 0.0f;
     mf->yx = 0.0f;
-    mf->yz = 0.0f;
-    mf->yw = 0.0f;
     mf->zx = 0.0f;
+    mf->wx = 0.0f;
+    mf->xy = 0.0f;
     mf->zy = 0.0f;
-    mf->zw = 0.0f;
+    mf->wy = 0.0f;
+    mf->xz = 0.0f;
+    mf->yz = 0.0f;
+    mf->wz = 0.0f;
     mf->xx = 1.0f;
     mf->yy = 1.0f;
     mf->zz = 1.0f;
     mf->ww = 1.0f;
-    mf->wx = x;
-    mf->wy = y;
-    mf->wz = z;
+    mf->xw = x;
+    mf->yw = y;
+    mf->zw = z;
 }
 
 /**
@@ -515,19 +514,19 @@ void SkinMatrix_MtxFToMtx(MtxF* src, Mtx* dest) {
     m1[0] = (temp >> 0x10);
     m1[16 + 0] = temp & 0xFFFF;
 
-    temp = src->xy * 0x10000;
+    temp = src->yx * 0x10000;
     m1[1] = (temp >> 0x10);
     m1[16 + 1] = temp & 0xFFFF;
 
-    temp = src->xz * 0x10000;
+    temp = src->zx * 0x10000;
     m1[2] = (temp >> 0x10);
     m1[16 + 2] = temp & 0xFFFF;
 
-    temp = src->xw * 0x10000;
+    temp = src->wx * 0x10000;
     m1[3] = (temp >> 0x10);
     m1[16 + 3] = temp & 0xFFFF;
 
-    temp = src->yx * 0x10000;
+    temp = src->xy * 0x10000;
     m1[4] = (temp >> 0x10);
     m1[16 + 4] = temp & 0xFFFF;
 
@@ -535,19 +534,19 @@ void SkinMatrix_MtxFToMtx(MtxF* src, Mtx* dest) {
     m1[5] = (temp >> 0x10);
     m1[16 + 5] = temp & 0xFFFF;
 
-    temp = src->yz * 0x10000;
+    temp = src->zy * 0x10000;
     m1[6] = (temp >> 0x10);
     m1[16 + 6] = temp & 0xFFFF;
 
-    temp = src->yw * 0x10000;
+    temp = src->wy * 0x10000;
     m1[7] = (temp >> 0x10);
     m1[16 + 7] = temp & 0xFFFF;
 
-    temp = src->zx * 0x10000;
+    temp = src->xz * 0x10000;
     m1[8] = (temp >> 0x10);
     m1[16 + 8] = temp & 0xFFFF;
 
-    temp = src->zy * 0x10000;
+    temp = src->yz * 0x10000;
     m1[9] = (temp >> 0x10);
     m2[9] = temp & 0xFFFF;
 
@@ -555,19 +554,19 @@ void SkinMatrix_MtxFToMtx(MtxF* src, Mtx* dest) {
     m1[10] = (temp >> 0x10);
     m2[10] = temp & 0xFFFF;
 
-    temp = src->zw * 0x10000;
+    temp = src->wz * 0x10000;
     m1[11] = (temp >> 0x10);
     m2[11] = temp & 0xFFFF;
 
-    temp = src->wx * 0x10000;
+    temp = src->xw * 0x10000;
     m1[12] = (temp >> 0x10);
     m2[12] = temp & 0xFFFF;
 
-    temp = src->wy * 0x10000;
+    temp = src->yw * 0x10000;
     m1[13] = (temp >> 0x10);
     m2[13] = temp & 0xFFFF;
 
-    temp = src->wz * 0x10000;
+    temp = src->zw * 0x10000;
     m1[14] = (temp >> 0x10);
     m2[14] = temp & 0xFFFF;
 
@@ -612,21 +611,21 @@ void SkinMatrix_SetRotateAroundVec(MtxF* mf, s16 a, f32 x, f32 y, f32 z) {
     xz = x * z;
 
     mf->xx = (1.0f - xx) * cosA + xx;
-    mf->xy = (1.0f - cosA) * xy + z * sinA;
-    mf->xz = (1.0f - cosA) * xz - y * sinA;
-    mf->xw = 0.0f;
+    mf->yx = (1.0f - cosA) * xy + z * sinA;
+    mf->zx = (1.0f - cosA) * xz - y * sinA;
+    mf->wx = 0.0f;
 
-    mf->yx = (1.0f - cosA) * xy - z * sinA;
+    mf->xy = (1.0f - cosA) * xy - z * sinA;
     mf->yy = (1.0f - yy) * cosA + yy;
-    mf->yz = (1.0f - cosA) * yz + x * sinA;
-    mf->yw = 0.0f;
+    mf->zy = (1.0f - cosA) * yz + x * sinA;
+    mf->wy = 0.0f;
 
-    mf->zx = (1.0f - cosA) * xz + y * sinA;
-    mf->zy = (1.0f - cosA) * yz - x * sinA;
+    mf->xz = (1.0f - cosA) * xz + y * sinA;
+    mf->yz = (1.0f - cosA) * yz - x * sinA;
     mf->zz = (1.0f - zz) * cosA + zz;
-    mf->zw = 0.0f;
+    mf->wz = 0.0f;
 
-    mf->wx = mf->wy = mf->wz = 0.0f;
+    mf->xw = mf->yw = mf->zw = 0.0f;
     mf->ww = 1.0f;
 }
 
@@ -642,27 +641,27 @@ void SkinMatrix_SetXRotation(MtxF* mf, s16 a) {
         cosA = 1.0f;
     }
 
-    mf->xy = 0.0f;
-    mf->xz = 0.0f;
-    mf->xw = 0.0f;
-
     mf->yx = 0.0f;
-    mf->yw = 0.0f;
-
     mf->zx = 0.0f;
-    mf->zw = 0.0f;
-
     mf->wx = 0.0f;
+
+    mf->xy = 0.0f;
     mf->wy = 0.0f;
+
+    mf->xz = 0.0f;
     mf->wz = 0.0f;
 
+    mf->xw = 0.0f;
+    mf->yw = 0.0f;
+    mf->zw = 0.0f;
+
     mf->xx = 1.0f;
     mf->ww = 1.0f;
 
     mf->yy = cosA;
     mf->zz = cosA;
-    mf->yz = sinA;
-    mf->zy = -sinA;
+    mf->zy = sinA;
+    mf->yz = -sinA;
 }
 
 void SkinMatrix_MulXRotation(MtxF* mf, s16 a) {
@@ -675,25 +674,25 @@ void SkinMatrix_MulXRotation(MtxF* mf, s16 a) {
         sinA = Math_SinS(a);
         cosA = Math_CosS(a);
 
-        ry = mf->yx;
-        rz = mf->zx;
-        mf->yx = ry * cosA + rz * sinA;
-        mf->zx = rz * cosA - ry * sinA;
+        ry = mf->xy;
+        rz = mf->xz;
+        mf->xy = ry * cosA + rz * sinA;
+        mf->xz = rz * cosA - ry * sinA;
 
         ry = mf->yy;
-        rz = mf->zy;
+        rz = mf->yz;
         mf->yy = ry * cosA + rz * sinA;
-        mf->zy = rz * cosA - ry * sinA;
+        mf->yz = rz * cosA - ry * sinA;
 
-        ry = mf->yz;
+        ry = mf->zy;
         rz = mf->zz;
-        mf->yz = ry * cosA + rz * sinA;
+        mf->zy = ry * cosA + rz * sinA;
         mf->zz = rz * cosA - ry * sinA;
 
-        ry = mf->yw;
-        rz = mf->zw;
-        mf->yw = ry * cosA + rz * sinA;
-        mf->zw = rz * cosA - ry * sinA;
+        ry = mf->wy;
+        rz = mf->wz;
+        mf->wy = ry * cosA + rz * sinA;
+        mf->wz = rz * cosA - ry * sinA;
     }
 }
 
@@ -709,27 +708,27 @@ void SkinMatrix_SetYRotation(MtxF* mf, s16 a) {
         cosA = 1.0f;
     }
 
-    mf->xy = 0.0f;
-    mf->xw = 0.0f;
-
     mf->yx = 0.0f;
-    mf->yz = 0.0f;
-    mf->yw = 0.0f;
+    mf->wx = 0.0f;
 
+    mf->xy = 0.0f;
     mf->zy = 0.0f;
-    mf->zw = 0.0f;
-
-    mf->wx = 0.0f;
     mf->wy = 0.0f;
+
+    mf->yz = 0.0f;
     mf->wz = 0.0f;
 
+    mf->xw = 0.0f;
+    mf->yw = 0.0f;
+    mf->zw = 0.0f;
+
     mf->yy = 1.0f;
     mf->ww = 1.0f;
 
     mf->xx = cosA;
     mf->zz = cosA;
-    mf->xz = -sinA;
-    mf->zx = sinA;
+    mf->zx = -sinA;
+    mf->xz = sinA;
 }
 
 void SkinMatrix_MulYRotation(MtxF* mf, s16 a) {
@@ -743,24 +742,24 @@ void SkinMatrix_MulYRotation(MtxF* mf, s16 a) {
         cosA = Math_CosS(a);
 
         rx = mf->xx;
-        rz = mf->zx;
+        rz = mf->xz;
         mf->xx = rx * cosA - rz * sinA;
-        mf->zx = rx * sinA + rz * cosA;
+        mf->xz = rx * sinA + rz * cosA;
 
-        rx = mf->xy;
-        rz = mf->zy;
-        mf->xy = rx * cosA - rz * sinA;
-        mf->zy = rx * sinA + rz * cosA;
+        rx = mf->yx;
+        rz = mf->yz;
+        mf->yx = rx * cosA - rz * sinA;
+        mf->yz = rx * sinA + rz * cosA;
 
-        rx = mf->xz;
+        rx = mf->zx;
         rz = mf->zz;
-        mf->xz = rx * cosA - rz * sinA;
+        mf->zx = rx * cosA - rz * sinA;
         mf->zz = rx * sinA + rz * cosA;
 
-        rx = mf->xw;
-        rz = mf->zw;
-        mf->xw = rx * cosA - rz * sinA;
-        mf->zw = rx * sinA + rz * cosA;
+        rx = mf->wx;
+        rz = mf->wz;
+        mf->wx = rx * cosA - rz * sinA;
+        mf->wz = rx * sinA + rz * cosA;
     }
 }
 
@@ -776,25 +775,25 @@ void SkinMatrix_SetZRotation(MtxF* mf, s16 a) {
         cosA = 1.0f;
     }
 
-    mf->xz = 0.0f;
-    mf->xw = 0.0f;
-
-    mf->yz = 0.0f;
-    mf->yw = 0.0f;
-
     mf->zx = 0.0f;
-    mf->zy = 0.0f;
-    mf->zw = 0.0f;
-
     mf->wx = 0.0f;
+
+    mf->zy = 0.0f;
     mf->wy = 0.0f;
+
+    mf->xz = 0.0f;
+    mf->yz = 0.0f;
     mf->wz = 0.0f;
 
+    mf->xw = 0.0f;
+    mf->yw = 0.0f;
+    mf->zw = 0.0f;
+
     mf->zz = 1.0f;
     mf->ww = 1.0f;
 
     mf->xx = cosA;
     mf->yy = cosA;
-    mf->xy = sinA;
-    mf->yx = -sinA;
+    mf->yx = sinA;
+    mf->xy = -sinA;
 }
diff --git a/src/code/z_sub_s.c b/src/code/z_sub_s.c
index 58812abc9..3e5443bc6 100644
--- a/src/code/z_sub_s.c
+++ b/src/code/z_sub_s.c
@@ -53,7 +53,7 @@ Gfx* SubS_DrawTransformFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void**
     Vec3f pos;
     Vec3s rot;
 
-    Matrix_StatePush();
+    Matrix_Push();
     limb = Lib_SegmentedToVirtual(skeleton[limbIndex]);
     limbIndex++;
     rot = jointTable[limbIndex];
@@ -63,8 +63,8 @@ Gfx* SubS_DrawTransformFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void**
     newDList = limbDList = limb->dList;
 
     if ((overrideLimbDraw == NULL) || !overrideLimbDraw(globalCtx, limbIndex, &newDList, &pos, &rot, actor, &gfx)) {
-        Matrix_JointPosition(&pos, &rot);
-        Matrix_StatePush();
+        Matrix_TranslateRotateZYX(&pos, &rot);
+        Matrix_Push();
 
         transformLimbDraw(globalCtx, limbIndex, actor, &gfx);
 
@@ -77,7 +77,7 @@ Gfx* SubS_DrawTransformFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void**
             Matrix_ToMtx(*mtx);
             (*mtx)++;
         }
-        Matrix_StatePop();
+        Matrix_Pop();
     }
     if (postLimbDraw != NULL) {
         postLimbDraw(globalCtx, limbIndex, &limbDList, &rot, actor, &gfx);
@@ -86,7 +86,7 @@ Gfx* SubS_DrawTransformFlexLimb(GlobalContext* globalCtx, s32 limbIndex, void**
         gfx = SubS_DrawTransformFlexLimb(globalCtx, limb->child, skeleton, jointTable, overrideLimbDraw, postLimbDraw,
                                          transformLimbDraw, actor, mtx, gfx);
     }
-    Matrix_StatePop();
+    Matrix_Pop();
     if (limb->sibling != LIMB_DONE) {
         gfx = SubS_DrawTransformFlexLimb(globalCtx, limb->sibling, skeleton, jointTable, overrideLimbDraw, postLimbDraw,
                                          transformLimbDraw, actor, mtx, gfx);
@@ -119,7 +119,7 @@ Gfx* SubS_DrawTransformFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jo
     }
 
     gSPSegment(gfx++, 0x0D, mtx);
-    Matrix_StatePush();
+    Matrix_Push();
     rootLimb = Lib_SegmentedToVirtual(skeleton[0]);
     pos.x = jointTable->x;
     pos.y = jointTable->y;
@@ -129,8 +129,8 @@ Gfx* SubS_DrawTransformFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jo
     limbDList = rootLimb->dList;
 
     if (overrideLimbDraw == NULL || !overrideLimbDraw(globalCtx, 1, &newDlist, &pos, &rot, actor, &gfx)) {
-        Matrix_JointPosition(&pos, &rot);
-        Matrix_StatePush();
+        Matrix_TranslateRotateZYX(&pos, &rot);
+        Matrix_Push();
 
         transformLimbDraw(globalCtx, 1, actor, &gfx);
 
@@ -143,7 +143,7 @@ Gfx* SubS_DrawTransformFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jo
             Matrix_ToMtx(mtx);
             mtx++;
         }
-        Matrix_StatePop();
+        Matrix_Pop();
     }
 
     if (postLimbDraw != NULL) {
@@ -154,7 +154,7 @@ Gfx* SubS_DrawTransformFlex(GlobalContext* globalCtx, void** skeleton, Vec3s* jo
         gfx = SubS_DrawTransformFlexLimb(globalCtx, rootLimb->child, skeleton, jointTable, overrideLimbDraw,
                                          postLimbDraw, transformLimbDraw, actor, &mtx, gfx);
     }
-    Matrix_StatePop();
+    Matrix_Pop();
     return gfx;
 }
 
@@ -187,9 +187,9 @@ s32 SubS_UpdateLimb(s16 newRotZ, s16 newRotY, Vec3f* pos, Vec3s* rot, s32 stepRo
     Vec3s newRot;
     MtxF curState;
 
-    Matrix_MultiplyVector3fByState(&zeroVec, &newPos);
-    Matrix_CopyCurrentState(&curState);
-    func_8018219C(&curState, &newRot, MTXMODE_NEW);
+    Matrix_MultVec3f(&zeroVec, &newPos);
+    Matrix_Get(&curState);
+    Matrix_MtxFToYXZRot(&curState, &newRot, MTXMODE_NEW);
     *pos = newPos;
 
     if (!stepRot && !overrideRot) {
@@ -538,7 +538,7 @@ void SubS_GenShadowTex(Vec3f bodyPartsPos[], Vec3f* worldPos, u8* tex, f32 tween
             pos.z = bodyPartPos->z - worldPos->z;
         }
 
-        Matrix_MultiplyVector3fByState(&pos, &startVec);
+        Matrix_MultVec3f(&pos, &startVec);
         startCol = 64.0f + startVec.x;
         startRow = 64.0f - startVec.z;
         SubS_FillShadowTex(startCol >> 1, startRow >> 1, tex, sizes[i]);
@@ -554,7 +554,7 @@ void SubS_DrawShadowTex(Actor* actor, GameState* gameState, u8* tex) {
     func_8012C28C(gfxCtx);
     gDPSetPrimColor(POLY_OPA_DISP++, 0, 0, 0, 0, 0, 100);
     gDPSetEnvColor(POLY_OPA_DISP++, 0, 0, 0, 0);
-    Matrix_InsertTranslation(actor->world.pos.x, 0.0f, actor->world.pos.z, MTXMODE_NEW);
+    Matrix_Translate(actor->world.pos.x, 0.0f, actor->world.pos.z, MTXMODE_NEW);
     Matrix_Scale(0.6f, 1.0f, 0.6f, MTXMODE_APPLY);
     gSPMatrix(POLY_OPA_DISP++, Matrix_NewMtx(gfxCtx), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
     gSPDisplayList(POLY_OPA_DISP++, gShadowDL);
diff --git a/src/code/z_view.c b/src/code/z_view.c
index d11a18201..915c196b7 100644
--- a/src/code/z_view.c
+++ b/src/code/z_view.c
@@ -262,15 +262,15 @@ s32 View_StepDistortion(View* view, Mtx* projectionMtx) {
             F32_LERPIMP(view->curDistortionScale.z, view->distortionScale.z, view->distortionSpeed);
     }
 
-    Matrix_FromRSPMatrix(projectionMtx, &projectionMtxF);
-    Matrix_SetCurrentState(&projectionMtxF);
-    Matrix_RotateStateAroundXAxis(view->curDistortionDirRot.x);
-    Matrix_InsertYRotation_f(view->curDistortionDirRot.y, MTXMODE_APPLY);
-    Matrix_InsertZRotation_f(view->curDistortionDirRot.z, MTXMODE_APPLY);
+    Matrix_MtxToMtxF(projectionMtx, &projectionMtxF);
+    Matrix_Put(&projectionMtxF);
+    Matrix_RotateXFApply(view->curDistortionDirRot.x);
+    Matrix_RotateYF(view->curDistortionDirRot.y, MTXMODE_APPLY);
+    Matrix_RotateZF(view->curDistortionDirRot.z, MTXMODE_APPLY);
     Matrix_Scale(view->curDistortionScale.x, view->curDistortionScale.y, view->curDistortionScale.z, MTXMODE_APPLY);
-    Matrix_InsertZRotation_f(-view->curDistortionDirRot.z, MTXMODE_APPLY);
-    Matrix_InsertYRotation_f(-view->curDistortionDirRot.y, MTXMODE_APPLY);
-    Matrix_RotateStateAroundXAxis(-view->curDistortionDirRot.x);
+    Matrix_RotateZF(-view->curDistortionDirRot.z, MTXMODE_APPLY);
+    Matrix_RotateYF(-view->curDistortionDirRot.y, MTXMODE_APPLY);
+    Matrix_RotateXFApply(-view->curDistortionDirRot.x);
     Matrix_ToMtx(projectionMtx);
 
     return true;
diff --git a/src/code/z_vr_box_draw.c b/src/code/z_vr_box_draw.c
index d7f99eed9..326fcb3e2 100644
--- a/src/code/z_vr_box_draw.c
+++ b/src/code/z_vr_box_draw.c
@@ -3,11 +3,11 @@
 Mtx* sSkyboxDrawMatrix;
 
 Mtx* SkyboxDraw_UpdateMatrix(SkyboxContext* skyboxCtx, f32 x, f32 y, f32 z) {
-    Matrix_InsertTranslation(x, y, z, MTXMODE_NEW);
+    Matrix_Translate(x, y, z, MTXMODE_NEW);
     Matrix_Scale(1.0f, 1.0f, 1.0f, MTXMODE_APPLY);
-    Matrix_RotateStateAroundXAxis(skyboxCtx->rotX);
-    Matrix_InsertYRotation_f(skyboxCtx->rotY, MTXMODE_APPLY);
-    Matrix_InsertZRotation_f(skyboxCtx->rotZ, MTXMODE_APPLY);
+    Matrix_RotateXFApply(skyboxCtx->rotX);
+    Matrix_RotateYF(skyboxCtx->rotY, MTXMODE_APPLY);
+    Matrix_RotateZF(skyboxCtx->rotZ, MTXMODE_APPLY);
     return Matrix_ToMtx(sSkyboxDrawMatrix);
 }
 
@@ -30,11 +30,11 @@ void SkyboxDraw_Draw(SkyboxContext* skyboxCtx, GraphicsContext* gfxCtx, s16 skyb
 
     sSkyboxDrawMatrix = GRAPH_ALLOC(gfxCtx, sizeof(Mtx));
 
-    Matrix_InsertTranslation(x, y, z, MTXMODE_NEW);
+    Matrix_Translate(x, y, z, MTXMODE_NEW);
     Matrix_Scale(1.0f, 1.0f, 1.0f, MTXMODE_APPLY);
-    Matrix_RotateStateAroundXAxis(skyboxCtx->rotX);
-    Matrix_InsertYRotation_f(skyboxCtx->rotY, MTXMODE_APPLY);
-    Matrix_InsertZRotation_f(skyboxCtx->rotZ, MTXMODE_APPLY);
+    Matrix_RotateXFApply(skyboxCtx->rotX);
+    Matrix_RotateYF(skyboxCtx->rotY, MTXMODE_APPLY);
+    Matrix_RotateZF(skyboxCtx->rotZ, MTXMODE_APPLY);
     Matrix_ToMtx(sSkyboxDrawMatrix);
 
     gSPMatrix(POLY_OPA_DISP++, sSkyboxDrawMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);