Part 0 of moving to a better matrix system

12 files changed, 540 insertions, 12 deletions

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 34b110bc7..d9b159d36 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -188,6 +188,7 @@ file(GLOB_RECURSE ALL_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR}
     "src/enhancements/freecam/*.h"
     "src/engine/*.cpp"
     "src/engine/*.h"
+    "src/port/interpolation/*.c"
 )
 
 list(APPEND ALL_FILES ${ALL_FILES_ROOT})
diff --git a/src/engine/vehicles/Train.cpp b/src/engine/vehicles/Train.cpp
index a3952742d..f4a5a4f9d 100644
--- a/src/engine/vehicles/Train.cpp
+++ b/src/engine/vehicles/Train.cpp
@@ -4,6 +4,8 @@
 #include "Train.h"
 #include <vector>
 
+#include "port/interpolation/FrameInterpolation.h"
+
 extern "C" {
 #include "macros.h"
 #include "main.h"
@@ -151,6 +153,7 @@ void ATrain::SyncComponents(TrainCarStuff* trainCar, s16 orientationY) {
 }
 
 void ATrain::Tick() {
+    FrameInterpolation_StartRecord();
     f32 temp_f20;
     TrainCarStuff* car;
     u16 oldWaypointIndex;
@@ -221,6 +224,8 @@ void ATrain::Tick() {
             sync_train_components(car, orientationYUpdate);
         }
     }
+
+    FrameInterpolation_StopRecord();
 }
 
 void ATrain::VehicleCollision(s32 playerId, Player* player) {
diff --git a/src/enhancements/collision_viewer.c b/src/enhancements/collision_viewer.c
index fd7c542e0..50aab44dc 100644
--- a/src/enhancements/collision_viewer.c
+++ b/src/enhancements/collision_viewer.c
@@ -26,7 +26,7 @@ void render_collision(void) {
     gDPSetCombineMode(gDisplayListHead++, G_CC_SHADE, G_CC_SHADE);
 
     // Set matrix
-    gSPMatrix(gDisplayListHead++, &gIdentityMatrix, G_MTX_MODELVIEW | G_MTX_LOAD | G_MTX_NOPUSH);
+    // gSPMatrix(gDisplayListHead++, &gIdentityMatrix, G_MTX_MODELVIEW | G_MTX_LOAD | G_MTX_NOPUSH);
 
     for (size_t i = 0; i < gCollisionMeshCount; i++) {
         // Load vertices for this tri
diff --git a/src/gbiMacro.c b/src/gbiMacro.c
index 5fa0ab171..ac7602c12 100644
--- a/src/gbiMacro.c
+++ b/src/gbiMacro.c
@@ -18,5 +18,5 @@ UNUSED void gfx_func_80040D00(void) {
     guOrtho(&gGfxPool->mtxScreen, 0.0f, SCREEN_WIDTH, 0.0f, SCREEN_HEIGHT, -1.0f, 1.0f, 1.0f);
     gSPPerspNormalize(gDisplayListHead++, 0xFFFF);
     gSPMatrix(gDisplayListHead++, VIRTUAL_TO_PHYSICAL(gGfxPool), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_PROJECTION);
-    gSPMatrix(gDisplayListHead++, VIRTUAL_TO_PHYSICAL(&gIdentityMatrix), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+    // gSPMatrix(gDisplayListHead++, VIRTUAL_TO_PHYSICAL(&gIdentityMatrix), G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
 }
diff --git a/src/main.c b/src/main.c
index 7b7e05cda..c83077f03 100644
--- a/src/main.c
+++ b/src/main.c
@@ -47,6 +47,7 @@
 #include "engine/wasm.h"
 #include "port/Game.h"
 #include "engine/Matrix.h"
+#include "port/interpolation/matrix.h"
 
 // Declarations (not in this file)
 void func_80091B78(void);
@@ -495,6 +496,9 @@ void rendering_init(void) {
     set_segment_base_addr_x64(1, gGfxPool);
     gGfxSPTask = &gGfxPool->spTask;
     gDisplayListHead = gGfxPool->gfxPool;
+    gGfxMtx = &gMainMatrixStack[0];
+    gGfxMatrix = &sGfxMatrixStack[0];
+    gCalcMatrix = &sCalcMatrixStack[0];
     init_rcp();
     clear_framebuffer(0);
     end_master_display_list();
@@ -647,7 +651,7 @@ void calculate_updaterate(void) {
     s32 total;
 
     // Get target FPS from configuration variable
-    s32 targetFPS = CVarGetInteger("gInterpolationFPS", 30);
+    s32 targetFPS = 30;
 
     if (targetFPS < 60) {
         targetFPS = 30;
diff --git a/src/port/Engine.cpp b/src/port/Engine.cpp
index debfad703..58e3920ea 100644
--- a/src/port/Engine.cpp
+++ b/src/port/Engine.cpp
@@ -388,6 +388,9 @@ void GameEngine::ProcessGfxCommands(Gfx* commands) {
         wnd->SetMaximumFrameLatency(1);
     }
     RunCommands(commands, mtx_replacements);
+
+    last_fps = fps;
+    last_update_rate = 2;
 }
 
 // Audio
diff --git a/src/port/interpolation/matrix.c b/src/port/interpolation/matrix.c
new file mode 100644
index 000000000..1ebcc1326
--- /dev/null
+++ b/src/port/interpolation/matrix.c
@@ -0,0 +1,437 @@
+#include <libultraship.h>
+#include <math.h>
+#include "matrix.h"
+
+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);
+Matrix gIdentityMatrix = { {
+    { 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 },
+} };
+
+Matrix* gGfxMatrix;
+Matrix sGfxMatrixStack[0x20];
+Matrix* gCalcMatrix;
+Matrix sCalcMatrixStack[0x20];
+
+Mtx gMainMatrixStack[0x480];
+Mtx* gGfxMtx;
+
+void Matrix_InitPerspective(Gfx** dList) {
+    u16 norm;
+    float near = 10.0f;
+    float far = 12800.0f;
+    float fov = 45.0f;
+
+    guPerspective(gGfxMtx, &norm, fov, 320.0f / 240.0f, near, far, 1.0f);
+    gSPPerspNormalize((*dList)++, norm);
+    gSPMatrix((*dList)++, gGfxMtx++, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_PROJECTION);
+    guLookAt(gGfxMtx, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, -12800.0f, 0.0f, 1.0f, 0.0f);
+    gSPMatrix((*dList)++, gGfxMtx++, G_MTX_NOPUSH | G_MTX_MUL | G_MTX_PROJECTION);
+    Matrix_Copy(gGfxMatrix, &gIdentityMatrix);
+}
+
+// Copies src Matrix into dst
+void Matrix_Copy(Matrix* dst, Matrix* src) {
+    int32_t i;
+
+    for (i = 0; i < 4; i++) {
+        dst->mf[i][0] = src->mf[i][0];
+        dst->mf[i][1] = src->mf[i][1];
+        dst->mf[i][2] = src->mf[i][2];
+        dst->mf[i][3] = src->mf[i][3];
+    }
+}
+
+// Makes a copy of the stack's current matrix and puts it on the top of the stack
+void Matrix_Push(Matrix** mtxStack) {
+    Matrix_Copy(*mtxStack + 1, *mtxStack);
+    (*mtxStack)++;
+}
+
+// Removes the top matrix of the stack
+void Matrix_Pop(Matrix** mtxStack) {
+    (*mtxStack)--;
+}
+
+// Copies tf into mtx (MTXF_NEW) or applies it to mtx (MTXF_APPLY)
+void Matrix_Mult(Matrix* mtx, Matrix* tf, u8 mode) {
+    f32 rx;
+    f32 ry;
+    f32 rz;
+    f32 rw;
+    s32 i0;
+    s32 i1;
+    s32 i2;
+    s32 i3;
+
+    if (mode == 1) {
+        rx = mtx->mf[0][0];
+        ry = mtx->mf[1][0];
+        rz = mtx->mf[2][0];
+        rw = mtx->mf[3][0];
+
+        for (i0 = 0; i0 < 4; i0++) {
+            mtx->mf[i0][0] = (rx * tf->mf[i0][0]) + (ry * tf->mf[i0][1]) + (rz * tf->mf[i0][2]) + (rw * tf->mf[i0][3]);
+        }
+
+        rx = mtx->mf[0][1];
+        ry = mtx->mf[1][1];
+        rz = mtx->mf[2][1];
+        rw = mtx->mf[3][1];
+
+        for (i1 = 0; i1 < 4; i1++) {
+            mtx->mf[i1][1] = (rx * tf->mf[i1][0]) + (ry * tf->mf[i1][1]) + (rz * tf->mf[i1][2]) + (rw * tf->mf[i1][3]);
+        }
+
+        rx = mtx->mf[0][2];
+        ry = mtx->mf[1][2];
+        rz = mtx->mf[2][2];
+        rw = mtx->mf[3][2];
+
+        for (i2 = 0; i2 < 4; i2++) {
+            mtx->mf[i2][2] = (rx * tf->mf[i2][0]) + (ry * tf->mf[i2][1]) + (rz * tf->mf[i2][2]) + (rw * tf->mf[i2][3]);
+        }
+
+        rx = mtx->mf[0][3];
+        ry = mtx->mf[1][3];
+        rz = mtx->mf[2][3];
+        rw = mtx->mf[3][3];
+
+        for (i3 = 0; i3 < 4; i3++) {
+            mtx->mf[i3][3] = (rx * tf->mf[i3][0]) + (ry * tf->mf[i3][1]) + (rz * tf->mf[i3][2]) + (rw * tf->mf[i3][3]);
+        }
+    } else {
+        Matrix_Copy(mtx, tf);
+    }
+}
+
+// Creates a translation matrix in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY)
+void Matrix_Translate(Matrix* mtx, f32 x, f32 y, f32 z, u8 mode) {
+    f32 rx;
+    f32 ry;
+    s32 i;
+
+    if (mode == 1) {
+        for (i = 0; i < 4; i++) {
+            rx = mtx->mf[0][i];
+            ry = mtx->mf[1][i];
+
+            mtx->mf[3][i] += (rx * x) + (ry * y) + (mtx->mf[2][i] * z);
+        }
+    } else {
+        mtx->mf[3][0] = x;
+        mtx->mf[3][1] = y;
+        mtx->mf[3][2] = z;
+        mtx->mf[0][1] = mtx->mf[0][2] = mtx->mf[0][3] = mtx->mf[1][0] = mtx->mf[1][2] = mtx->mf[1][3] = mtx->mf[2][0] =
+            mtx->mf[2][1] = mtx->mf[2][3] = 0.0f;
+        mtx->mf[0][0] = mtx->mf[1][1] = mtx->mf[2][2] = mtx->mf[3][3] = 1.0f;
+    }
+}
+
+// Creates a scale matrix in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY)
+void Matrix_Scale(Matrix* mtx, f32 xScale, f32 yScale, f32 zScale, u8 mode) {
+    f32 rx;
+    f32 ry;
+    s32 i;
+
+    if (mode == 1) {
+        for (i = 0; i < 4; i++) {
+            rx = mtx->mf[0][i];
+            ry = mtx->mf[1][i];
+
+            mtx->mf[0][i] = rx * xScale;
+            mtx->mf[1][i] = ry * yScale;
+            mtx->mf[2][i] *= zScale;
+        }
+    } else {
+        mtx->mf[0][0] = xScale;
+        mtx->mf[1][1] = yScale;
+        mtx->mf[2][2] = zScale;
+        mtx->mf[0][1] = mtx->mf[0][2] = mtx->mf[0][3] = mtx->mf[1][0] = mtx->mf[1][2] = mtx->mf[1][3] = mtx->mf[2][0] =
+            mtx->mf[2][1] = mtx->mf[2][3] = mtx->mf[3][0] = mtx->mf[3][1] = mtx->mf[3][2] = 0.0f;
+        mtx->mf[3][3] = 1.0f;
+    }
+}
+
+// Creates rotation matrix about the X axis in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY)
+void Matrix_RotateX(Matrix* mtx, f32 angle, u8 mode) {
+    f32 cs;
+    f32 sn;
+    f32 ry;
+    f32 rz;
+    s32 i;
+
+    sn = sinf(angle);
+    cs = cosf(angle);
+    if (mode == 1) {
+        for (i = 0; i < 4; i++) {
+            ry = mtx->mf[1][i];
+            rz = mtx->mf[2][i];
+
+            mtx->mf[1][i] = (ry * cs) + (rz * sn);
+            mtx->mf[2][i] = (rz * cs) - (ry * sn);
+        }
+    } else {
+        mtx->mf[1][1] = mtx->mf[2][2] = cs;
+        mtx->mf[1][2] = sn;
+        mtx->mf[2][1] = -sn;
+        mtx->mf[0][0] = mtx->mf[3][3] = 1.0f;
+        mtx->mf[0][1] = mtx->mf[0][2] = mtx->mf[0][3] = mtx->mf[1][0] = mtx->mf[1][3] = mtx->mf[2][0] = mtx->mf[2][3] =
+            mtx->mf[3][0] = mtx->mf[3][1] = mtx->mf[3][2] = 0.0f;
+    }
+}
+
+// Creates rotation matrix about the Y axis in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY)
+void Matrix_RotateY(Matrix* mtx, f32 angle, u8 mode) {
+    f32 cs;
+    f32 sn;
+    f32 rx;
+    f32 rz;
+    s32 i;
+
+    sn = sinf(angle);
+    cs = cosf(angle);
+    if (mode == 1) {
+        for (i = 0; i < 4; i++) {
+            rx = mtx->mf[0][i];
+            rz = mtx->mf[2][i];
+
+            mtx->mf[0][i] = (rx * cs) - (rz * sn);
+            mtx->mf[2][i] = (rx * sn) + (rz * cs);
+        }
+    } else {
+        mtx->mf[0][0] = mtx->mf[2][2] = cs;
+        mtx->mf[0][2] = -sn;
+        mtx->mf[2][0] = sn;
+        mtx->mf[1][1] = mtx->mf[3][3] = 1.0f;
+        mtx->mf[0][1] = mtx->mf[0][3] = mtx->mf[1][0] = mtx->mf[1][2] = mtx->mf[1][3] = mtx->mf[2][1] = mtx->mf[2][3] =
+            mtx->mf[3][0] = mtx->mf[3][1] = mtx->mf[3][2] = 0.0f;
+    }
+}
+
+// Creates rotation matrix about the Z axis in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY)
+void Matrix_RotateZ(Matrix* mtx, f32 angle, u8 mode) {
+    f32 cs;
+    f32 sn;
+    f32 rx;
+    f32 ry;
+    s32 i;
+
+    sn = sinf(angle);
+    cs = cosf(angle);
+    if (mode == 1) {
+        for (i = 0; i < 4; i++) {
+            rx = mtx->mf[0][i];
+            ry = mtx->mf[1][i];
+
+            mtx->mf[0][i] = (rx * cs) + (ry * sn);
+            mtx->mf[1][i] = (ry * cs) - (rx * sn);
+        }
+    } else {
+        mtx->mf[0][0] = mtx->mf[1][1] = cs;
+        mtx->mf[0][1] = sn;
+        mtx->mf[1][0] = -sn;
+        mtx->mf[2][2] = mtx->mf[3][3] = 1.0f;
+        mtx->mf[0][2] = mtx->mf[0][3] = mtx->mf[1][2] = mtx->mf[1][3] = mtx->mf[2][0] = mtx->mf[2][1] = mtx->mf[2][3] =
+            mtx->mf[3][0] = mtx->mf[3][1] = mtx->mf[3][2] = 0.0f;
+    }
+}
+
+// Creates rotation matrix about a given vector axis in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY).
+// The vector specifying the axis does not need to be a unit vector.
+void Matrix_RotateAxis(Matrix* mtx, f32 angle, f32 axisX, f32 axisY, f32 axisZ, u8 mode) {
+    f32 rx;
+    f32 ry;
+    f32 rz;
+    f32 norm;
+    f32 cxx;
+    f32 cyx;
+    f32 czx;
+    f32 cxy;
+    f32 cyy;
+    f32 czy;
+    f32 cxz;
+    f32 cyz;
+    f32 czz;
+    f32 xx;
+    f32 yy;
+    f32 zz;
+    f32 xy;
+    f32 yz;
+    f32 xz;
+    f32 sinA;
+    f32 cosA;
+
+    norm = sqrtf((axisX * axisX) + (axisY * axisY) + (axisZ * axisZ));
+    if (norm != 0.0) {
+        axisX /= norm;
+        axisY /= norm;
+        axisZ /= norm;
+        sinA = sinf(angle);
+        cosA = cosf(angle);
+        xx = axisX * axisX;
+        yy = axisY * axisY;
+        zz = axisZ * axisZ;
+        xy = axisX * axisY;
+        yz = axisY * axisZ;
+        xz = axisX * axisZ;
+
+        if (mode == 1) {
+            cxx = (1.0f - xx) * cosA + xx;
+            cyx = (1.0f - cosA) * xy + axisZ * sinA;
+            czx = (1.0f - cosA) * xz - axisY * sinA;
+
+            cxy = (1.0f - cosA) * xy - axisZ * sinA;
+            cyy = (1.0f - yy) * cosA + yy;
+            czy = (1.0f - cosA) * yz + axisX * sinA;
+
+            cxz = (1.0f - cosA) * xz + axisY * sinA;
+            cyz = (1.0f - cosA) * yz - axisX * sinA;
+            czz = (1.0f - zz) * cosA + zz;
+
+            // loop doesn't seem to work here.
+            rx = mtx->mf[0][0];
+            ry = mtx->mf[0][1];
+            rz = mtx->mf[0][2];
+            mtx->mf[0][0] = (rx * cxx) + (ry * cxy) + (rz * cxz);
+            mtx->mf[0][1] = (rx * cyx) + (ry * cyy) + (rz * cyz);
+            mtx->mf[0][2] = (rx * czx) + (ry * czy) + (rz * czz);
+
+            rx = mtx->mf[1][0];
+            ry = mtx->mf[1][1];
+            rz = mtx->mf[1][2];
+            mtx->mf[1][0] = (rx * cxx) + (ry * cxy) + (rz * cxz);
+            mtx->mf[1][1] = (rx * cyx) + (ry * cyy) + (rz * cyz);
+            mtx->mf[1][2] = (rx * czx) + (ry * czy) + (rz * czz);
+
+            rx = mtx->mf[2][0];
+            ry = mtx->mf[2][1];
+            rz = mtx->mf[2][2];
+            mtx->mf[2][0] = (rx * cxx) + (ry * cxy) + (rz * cxz);
+            mtx->mf[2][1] = (rx * cyx) + (ry * cyy) + (rz * cyz);
+            mtx->mf[2][2] = (rx * czx) + (ry * czy) + (rz * czz);
+        } else {
+            mtx->mf[0][0] = (1.0f - xx) * cosA + xx;
+            mtx->mf[0][1] = (1.0f - cosA) * xy + axisZ * sinA;
+            mtx->mf[0][2] = (1.0f - cosA) * xz - axisY * sinA;
+            mtx->mf[0][3] = 0.0f;
+
+            mtx->mf[1][0] = (1.0f - cosA) * xy - axisZ * sinA;
+            mtx->mf[1][1] = (1.0f - yy) * cosA + yy;
+            mtx->mf[1][2] = (1.0f - cosA) * yz + axisX * sinA;
+            mtx->mf[1][3] = 0.0f;
+
+            mtx->mf[2][0] = (1.0f - cosA) * xz + axisY * sinA;
+            mtx->mf[2][1] = (1.0f - cosA) * yz - axisX * sinA;
+            mtx->mf[2][2] = (1.0f - zz) * cosA + zz;
+            mtx->mf[2][3] = 0.0f;
+
+            mtx->mf[3][0] = mtx->mf[3][1] = mtx->mf[3][2] = 0.0f;
+            mtx->mf[3][3] = 1.0f;
+        }
+    }
+}
+
+// Converts the current Gfx matrix to a Mtx
+void Matrix_ToMtx(Mtx* dest) {
+    // LTODO: We need to validate this
+    guMtxF2L(gGfxMatrix->mf, dest);
+}
+
+// Converts the Mtx src to a Matrix, putting the result in dest
+void Matrix_FromMtx(Mtx* src, Matrix* dest) {
+    guMtxF2L(src->m, dest->mf);
+}
+
+// Applies the transform matrix mtx to the vector src, putting the result in dest
+void Matrix_MultVec3f(Matrix* mtx, Vec3f* src, Vec3f* dest) {
+    dest->x = (mtx->mf[0][0] * src->x) + (mtx->mf[1][0] * src->y) + (mtx->mf[2][0] * src->z) + mtx->mf[3][0];
+    dest->y = (mtx->mf[0][1] * src->x) + (mtx->mf[1][1] * src->y) + (mtx->mf[2][1] * src->z) + mtx->mf[3][1];
+    dest->z = (mtx->mf[0][2] * src->x) + (mtx->mf[1][2] * src->y) + (mtx->mf[2][2] * src->z) + mtx->mf[3][2];
+}
+
+// Applies the linear part of the transformation matrix mtx to the vector src, ignoring any translation that mtx might
+// have. Puts the result in dest.
+void Matrix_MultVec3fNoTranslate(Matrix* mtx, Vec3f* src, Vec3f* dest) {
+    dest->x = (mtx->mf[0][0] * src->x) + (mtx->mf[1][0] * src->y) + (mtx->mf[2][0] * src->z);
+    dest->y = (mtx->mf[0][1] * src->x) + (mtx->mf[1][1] * src->y) + (mtx->mf[2][1] * src->z);
+    dest->z = (mtx->mf[0][2] * src->x) + (mtx->mf[1][2] * src->y) + (mtx->mf[2][2] * src->z);
+}
+
+// Expresses the rotational part of the transform mtx as Tait-Bryan angles, in the yaw-pitch-roll (intrinsic YXZ)
+// convention used in worldspace calculations
+void Matrix_GetYRPAngles(Matrix* mtx, Vec3f* rot) {
+    Matrix invYP;
+    Vec3f origin = { 0.0f, 0.0f, 0.0f };
+    Vec3f originP;
+    Vec3f zHat = { 0.0f, 0.0f, 1.0f };
+    Vec3f zHatP;
+    Vec3f xHat = { 1.0f, 0.0f, 0.0f };
+    Vec3f xHatP;
+
+    Matrix_MultVec3fNoTranslate(mtx, &origin, &originP);
+    Matrix_MultVec3fNoTranslate(mtx, &zHat, &zHatP);
+    Matrix_MultVec3fNoTranslate(mtx, &xHat, &xHatP);
+    zHatP.x -= originP.x;
+    zHatP.y -= originP.y;
+    zHatP.z -= originP.z;
+    xHatP.x -= originP.x;
+    xHatP.y -= originP.y;
+    xHatP.z -= originP.z;
+    rot->y = atan2f(zHatP.x, zHatP.z);
+    rot->x = -atan2f(zHatP.y, sqrtf(SQ(zHatP.x) + SQ(zHatP.z)));
+    Matrix_RotateX(&invYP, -rot->x, MTXF_NEW);
+    Matrix_RotateY(&invYP, -rot->y, MTXF_APPLY);
+    Matrix_MultVec3fNoTranslate(&invYP, &xHatP, &xHat);
+    rot->x *= M_RTOD;
+    rot->y *= M_RTOD;
+    rot->z = atan2f(xHat.y, xHat.x) * M_RTOD;
+}
+
+// Expresses the rotational part of the transform mtx as Tait-Bryan angles, in the extrinsic XYZ convention used in
+// modelspace calculations
+void Matrix_GetXYZAngles(Matrix* mtx, Vec3f* rot) {
+    Matrix invYZ;
+    Vec3f origin = { 0.0f, 0.0f, 0.0f };
+    Vec3f originP;
+    Vec3f xHat = { 1.0f, 0.0f, 0.0f };
+    Vec3f xHatP;
+    Vec3f yHat = { 0.0f, 1.0f, 0.0f };
+    Vec3f yHatP;
+
+    Matrix_MultVec3fNoTranslate(mtx, &origin, &originP);
+    Matrix_MultVec3fNoTranslate(mtx, &xHat, &xHatP);
+    Matrix_MultVec3fNoTranslate(mtx, &yHat, &yHatP);
+    xHatP.x -= originP.x;
+    xHatP.y -= originP.y;
+    xHatP.z -= originP.z;
+    yHatP.x -= originP.x;
+    yHatP.y -= originP.y;
+    yHatP.z -= originP.z;
+    rot->z = atan2f(xHatP.y, xHatP.x);
+    rot->y = -atan2f(xHatP.z, sqrtf(SQ(xHatP.x) + SQ(xHatP.y)));
+    Matrix_RotateY(&invYZ, -rot->y, MTXF_NEW);
+    Matrix_RotateZ(&invYZ, -rot->z, MTXF_APPLY);
+    Matrix_MultVec3fNoTranslate(&invYZ, &yHatP, &yHat);
+    rot->x = atan2f(yHat.z, yHat.y) * M_RTOD;
+    rot->y *= M_RTOD;
+    rot->z *= M_RTOD;
+}
+
+// Creates a look-at matrix from Eye, At, and Up in mtx (MTXF_NEW) or applies one to mtx (MTXF_APPLY).
+// A look-at matrix is a rotation-translation matrix that maps y to Up, z to (At - Eye), and translates to Eye
+void Matrix_LookAt(Matrix* mtx, f32 xEye, f32 yEye, f32 zEye, f32 xAt, f32 yAt, f32 zAt, f32 xUp, f32 yUp, f32 zUp,
+                   u8 mode) {
+    Matrix lookAt;
+
+    guLookAtF(lookAt.mf, xEye, yEye, zEye, xAt, yAt, zAt, xUp, yUp, zUp);
+    Matrix_Mult(mtx, &lookAt, mode);
+}
+
+// Converts the current Gfx matrix to a Mtx and sets it to the display list
+void Matrix_SetGfxMtx(Gfx** gfx) {
+    Matrix_ToMtx(gGfxMtx);
+    gSPMatrix((*gfx)++, gGfxMtx++, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+}
\ No newline at end of file
diff --git a/src/port/interpolation/matrix.h b/src/port/interpolation/matrix.h
new file mode 100644
index 000000000..85636cb94
--- /dev/null
+++ b/src/port/interpolation/matrix.h
@@ -0,0 +1,75 @@
+#pragma once
+
+#define MTXF_NEW 0
+#define MTXF_APPLY 1
+
+typedef struct {
+    float r;
+    float g;
+    float b;
+} Color;
+
+#define M_PI    3.14159265358979323846f
+#define M_RTOD	(180.0f / M_PI)
+#define SQ(val) ((val) * (val))
+
+#define qs1616(e) ((s32) ((e) *0x00010000))
+
+#define IPART(x) ((qs1616(x) >> 16) & 0xFFFF)
+#define FPART(x) (qs1616(x) & 0xFFFF)
+
+#define gdSPDefMtx(xx, yx, zx, wx, xy, yy, zy, wy, xz, yz, zz, wz, xw, yw, zw, ww)                                 \
+    {                                                                                                              \
+        {                                                                                                          \
+            (IPART(xx) << 0x10) | IPART(xy), (IPART(xz) << 0x10) | IPART(xw), (IPART(yx) << 0x10) | IPART(yy),     \
+                (IPART(yz) << 0x10) | IPART(yw), (IPART(zx) << 0x10) | IPART(zy), (IPART(zz) << 0x10) | IPART(zw), \
+                (IPART(wx) << 0x10) | IPART(wy), (IPART(wz) << 0x10) | IPART(ww), (FPART(xx) << 0x10) | FPART(xy), \
+                (FPART(xz) << 0x10) | FPART(xw), (FPART(yx) << 0x10) | FPART(yy), (FPART(yz) << 0x10) | FPART(yw), \
+                (FPART(zx) << 0x10) | FPART(zy), (FPART(zz) << 0x10) | FPART(zw), (FPART(wx) << 0x10) | FPART(wy), \
+                (FPART(wz) << 0x10) | FPART(ww),                                                                   \
+        }                                                                                                          \
+    }
+
+
+typedef MtxF Matrix;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern Mtx gIdentityMtx;
+extern Matrix gIdentityMatrix;
+
+extern Matrix* gGfxMatrix;
+extern Matrix sGfxMatrixStack[];
+extern Matrix* gCalcMatrix;
+extern Matrix sCalcMatrixStack[];
+
+extern Mtx gMainMatrixStack[];
+extern Mtx* gGfxMtx;
+
+void Matrix_InitPerspective(Gfx** dList);
+void Matrix_Copy(Matrix* dst, Matrix* src);
+void Matrix_Push(Matrix** mtxStack);
+void Matrix_Pop(Matrix** mtxStack);
+void Matrix_Mult(Matrix* mtx, Matrix* tf, u8 mode);
+void Matrix_Translate(Matrix* mtx, f32 x, f32 y, f32 z, u8 mode);
+void Matrix_Scale(Matrix* mtx, f32 xScale, f32 yScale, f32 zScale, u8 mode);
+void Matrix_RotateX(Matrix* mtx, f32 angle, u8 mode);
+void Matrix_RotateY(Matrix* mtx, f32 angle, u8 mode);
+void Matrix_RotateZ(Matrix* mtx, f32 angle, u8 mode);
+void Matrix_RotateAxis(Matrix* mtx, f32 angle, f32 axisX, f32 axisY, f32 axisZ, u8 mode);
+void Matrix_ToMtx(Mtx* dest);
+void Matrix_FromMtx(Mtx* src, Matrix* dest);
+void Matrix_MultVec3f(Matrix* mtx, Vec3f* src, Vec3f* dest);
+void Matrix_MultVec3fNoTranslate(Matrix* mtx, Vec3f* src, Vec3f* dest);
+void Matrix_GetYRPAngles(Matrix* mtx, Vec3f* rot);
+void Matrix_GetXYZAngles(Matrix* mtx, Vec3f* rot);
+void Matrix_LookAt(Matrix* mtx, f32 xEye, f32 yEye, f32 zEye, f32 xAt, f32 yAt, f32 zAt, f32 xUp, f32 yUp, f32 zUp,
+                   u8 mode);
+void Matrix_SetGfxMtx(Gfx** gfx);
+void Lights_SetOneLight(Gfx** dList, s32 dirX, s32 dirY, s32 dirZ, s32 colR, s32 colG, s32 colB, s32 ambR, s32 ambG, s32 ambB);
+
+#ifdef __cplusplus
+}
+#endif
\ No newline at end of file
diff --git a/src/racing/framebuffer_effects.c b/src/racing/framebuffer_effects.c
index 9a166b448..ccb3460e3 100644
--- a/src/racing/framebuffer_effects.c
+++ b/src/racing/framebuffer_effects.c
@@ -37,7 +37,7 @@ void FB_CreateFramebuffers(void) {
 void FB_CopyToFramebuffer(Gfx** gfxP, s32 fb_src, s32 fb_dest, u8 oncePerFrame, u8* hasCopied) {
     Gfx* gfx = *gfxP;
 
-    gSPMatrix(gfx++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+    // gSPMatrix(gfx++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
 
     gDPSetOtherMode(gfx++,
                     G_AD_DISABLE | G_CD_DISABLE | G_CK_NONE | G_TC_FILT | G_TF_POINT | G_TT_NONE | G_TL_TILE |
@@ -110,7 +110,7 @@ void FB_WriteFramebufferSliceToCPU(Gfx** gfxP, void* buffer, u8 byteSwap) {
 void FB_DrawFromFramebuffer(Gfx** gfxP, s32 fb, u8 alpha) {
     Gfx* gfx = *gfxP;
 
-    gSPMatrix(gfx++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+    // gSPMatrix(gfx++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
 
     gDPSetEnvColor(gfx++, 255, 255, 255, alpha);
 
diff --git a/src/racing/math_util.c b/src/racing/math_util.c
index 934865b08..6f9aba302 100644
--- a/src/racing/math_util.c
+++ b/src/racing/math_util.c
@@ -9,15 +9,16 @@
 #include "memory.h"
 #include "engine/Matrix.h"
 #include "port/Game.h"
+#include <port/interpolation/matrix.h>
 
 #pragma intrinsic(sqrtf, fabs)
 
 s32 D_802B91C0[2] = { 13, 13 };
 Vec3f D_802B91C8 = { 0.0f, 0.0f, 0.0f };
 
-Mtx gIdentityMatrix = {
-    toFixedPointMatrix(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0),
-};
+// Mtx gIdentityMatrix = {
+//     toFixedPointMatrix(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0),
+// };
 
 // This functions looks similar to a segment of code from func_802A4A0C in skybox_and_splitscreen.c
 UNUSED s32 func_802B4F60(UNUSED s32 arg0, Vec3f arg1, UNUSED s32 arg2, UNUSED f32 arg3, UNUSED f32 arg4) {
diff --git a/src/racing/math_util.h b/src/racing/math_util.h
index b767e9068..d5d7bf3c4 100644
--- a/src/racing/math_util.h
+++ b/src/racing/math_util.h
@@ -70,6 +70,6 @@ f32 is_within_render_distance(Vec3f, Vec3f, u16, f32, f32, f32);
 
 extern s32 D_802B91C0[];
 extern Vec3f D_802B91C8;
-extern Mtx gIdentityMatrix;
+// extern Mtx gIdentityMatrix;
 
 #endif // MATH_UTIL_H
diff --git a/src/racing/skybox_and_splitscreen.c b/src/racing/skybox_and_splitscreen.c
index 77b1c03ff..b83b573c1 100644
--- a/src/racing/skybox_and_splitscreen.c
+++ b/src/racing/skybox_and_splitscreen.c
@@ -402,14 +402,16 @@ void func_802A487C(Vtx* arg0, UNUSED struct UnkStruct_800DC5EC* arg1, UNUSED s32
 
     init_rdp();
     if (GetCourse() != GetRainbowRoad()) {
-
+        FrameInterpolation_RecordOpenChild("perspective", 0);
+        FrameInterpolation_RecordMarker(__FILE__, __LINE__);
         gDPSetRenderMode(gDisplayListHead++, G_RM_OPA_SURF, G_RM_OPA_SURF2);
         gSPClearGeometryMode(gDisplayListHead++, G_ZBUFFER | G_LIGHTING);
         guOrtho(&gGfxPool->mtxScreen, 0.0f, SCREEN_WIDTH, 0.0f, SCREEN_HEIGHT, 0.0f, 5.0f, 1.0f);
         gSPPerspNormalize(gDisplayListHead++, 0xFFFF);
         gSPMatrix(gDisplayListHead++, VIRTUAL_TO_PHYSICAL(&gGfxPool->mtxScreen),
                   G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_PROJECTION);
-        gSPMatrix(gDisplayListHead++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+        // gSPMatrix(gDisplayListHead++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+        // Matrix_
         gSPVertex(gDisplayListHead++, &arg0[4], 4, 0);
         gSP2Triangles(gDisplayListHead++, 0, 3, 1, 0, 1, 3, 2, 0);
     }
@@ -476,7 +478,7 @@ void func_802A4A0C(Vtx* vtx, struct UnkStruct_800DC5EC* arg1, UNUSED s32 arg2, U
     gSPPerspNormalize(gDisplayListHead++, 0xFFFF);
     gSPMatrix(gDisplayListHead++, VIRTUAL_TO_PHYSICAL(&gGfxPool->mtxScreen),
               G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_PROJECTION);
-    gSPMatrix(gDisplayListHead++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
+    // gSPMatrix(gDisplayListHead++, &gIdentityMatrix, G_MTX_NOPUSH | G_MTX_LOAD | G_MTX_MODELVIEW);
     gSPVertex(gDisplayListHead++, &vtx[0], 4, 0);
     gSP2Triangles(gDisplayListHead++, 0, 3, 1, 0, 1, 3, 2, 0);
     if (GetCourse() == GetRainbowRoad()) {