Added interpolation system

2 files changed, 151 insertions, 0 deletions

diff --git a/src/sys/sys_lib.c b/src/sys/sys_lib.c
index d94ec37c..6d78cc77 100644
--- a/src/sys/sys_lib.c
+++ b/src/sys/sys_lib.c
@@ -1,4 +1,5 @@
 #include "sys.h"
+#include "port/interpolation/FrameInterpolation.h"
 
 s32 Lib_vsPrintf(char* dst, const char* fmt, va_list args) {
     return vsprintf(dst, fmt, args);
@@ -84,21 +85,25 @@ void Lib_QuickSort(u8* first, u32 length, u32 size, CompareFunc cFunc) {
 void Lib_InitPerspective(Gfx** dList) {
     u16 norm;
 
+    FrameInterpolation_RecordOpenChild("perspective", 0);
     guPerspective(gGfxMtx, &norm, gFovY, (f32) SCREEN_WIDTH / SCREEN_HEIGHT, gProjectNear, gProjectFar, 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);
+    FrameInterpolation_RecordCloseChild();
 }
 
 void Lib_InitOrtho(Gfx** dList) {
+    FrameInterpolation_RecordOpenChild("ortho", 0);
     guOrtho(gGfxMtx, -SCREEN_WIDTH / 2, SCREEN_WIDTH / 2, -SCREEN_HEIGHT / 2, SCREEN_HEIGHT / 2, gProjectNear,
             gProjectFar, 1.0f);
     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);
+    FrameInterpolation_RecordCloseChild();
 }
 
 void Lib_DmaRead(void* src, void* dst, s32 size) {
diff --git a/src/sys/sys_matrix.c b/src/sys/sys_matrix.c
index 6e478835..064ca7d0 100644
--- a/src/sys/sys_matrix.c
+++ b/src/sys/sys_matrix.c
@@ -1,5 +1,6 @@
 #include "sys.h"
 
+#include "port/interpolation/FrameInterpolation.h"
 #define qs1616(e) ((s32) ((e) *0x00010000))
 
 #define IPART(x) ((qs1616(x) >> 16) & 0xFFFF)
@@ -46,17 +47,152 @@ void Matrix_Copy(Matrix* dst, Matrix* src) {
 
 // Makes a copy of the stack's current matrix and puts it on the top of the stack
 void Matrix_Push(Matrix** mtxStack) {
+    FrameInterpolation_RecordMatrixPush();
     Matrix_Copy(*mtxStack + 1, *mtxStack);
     (*mtxStack)++;
 }
 
 // Removes the top matrix of the stack
 void Matrix_Pop(Matrix** mtxStack) {
+    FrameInterpolation_RecordMatrixPop();
     (*mtxStack)--;
 }
 
+/**
+ * Matrix multiplication, dest = mfA * mfB.
+ * mfA and dest should not be the same matrix.
+ */
+void Matrix_MtxFMtxFMult(MtxF* mfB, MtxF* mfA, MtxF* dest) {
+    f32 rx;
+    f32 ry;
+    f32 rz;
+    f32 rw;
+
+    //---COL1---
+    f32 cx = mfB->xx;
+    f32 cy = mfB->xy;
+    f32 cz = mfB->xz;
+    f32 cw = mfB->xw;
+    //--------
+
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->xx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->xy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->xz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->xw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    //---2Col---
+    cx = mfB->yx;
+    cy = mfB->yy;
+    cz = mfB->yz;
+    cw = mfB->yw;
+    //--------
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->yx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->yy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->yz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->yw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    //---3Col---
+    cx = mfB->zx;
+    cy = mfB->zy;
+    cz = mfB->zz;
+    cw = mfB->zw;
+    //--------
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->zx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->zy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->zz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->zw = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    //---4Col---
+    cx = mfB->wx;
+    cy = mfB->wy;
+    cz = mfB->wz;
+    cw = mfB->ww;
+    //--------
+    rx = mfA->xx;
+    ry = mfA->yx;
+    rz = mfA->zx;
+    rw = mfA->wx;
+    dest->wx = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xy;
+    ry = mfA->yy;
+    rz = mfA->zy;
+    rw = mfA->wy;
+    dest->wy = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xz;
+    ry = mfA->yz;
+    rz = mfA->zz;
+    rw = mfA->wz;
+    dest->wz = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+
+    rx = mfA->xw;
+    ry = mfA->yw;
+    rz = mfA->zw;
+    rw = mfA->ww;
+    dest->ww = (cx * rx) + (cy * ry) + (cz * rz) + (cw * rw);
+}
+
 // Copies tf into mtx (MTXF_NEW) or applies it to mtx (MTXF_APPLY)
 void Matrix_Mult(Matrix* mtx, Matrix* tf, u8 mode) {
+    FrameInterpolation_RecordMatrixMult(tf, mode);
     f32 rx;
     f32 ry;
     f32 rz;
@@ -109,6 +245,7 @@ void Matrix_Mult(Matrix* mtx, Matrix* tf, u8 mode) {
 
 // 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) {
+    FrameInterpolation_RecordMatrixTranslate(x, y, z, mode);
     f32 rx;
     f32 ry;
     s32 i;
@@ -132,6 +269,7 @@ void Matrix_Translate(Matrix* mtx, f32 x, f32 y, f32 z, u8 mode) {
 
 // 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) {
+    FrameInterpolation_RecordMatrixScale(xScale, yScale, zScale, mode);
     f32 rx;
     f32 ry;
     s32 i;
@@ -157,6 +295,7 @@ void Matrix_Scale(Matrix* mtx, f32 xScale, f32 yScale, f32 zScale, u8 mode) {
 
 // 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) {
+    FrameInterpolation_RecordMatrixRotate1Coord(0, angle, mode);
     f32 cs;
     f32 sn;
     f32 ry;
@@ -185,6 +324,7 @@ void Matrix_RotateX(Matrix* mtx, f32 angle, u8 mode) {
 
 // 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) {
+    FrameInterpolation_RecordMatrixRotate1Coord(1, angle, mode);
     f32 cs;
     f32 sn;
     f32 rx;
@@ -213,6 +353,7 @@ void Matrix_RotateY(Matrix* mtx, f32 angle, u8 mode) {
 
 // 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) {
+    FrameInterpolation_RecordMatrixRotate1Coord(2, angle, mode);
     f32 cs;
     f32 sn;
     f32 rx;
@@ -242,6 +383,7 @@ void Matrix_RotateZ(Matrix* mtx, f32 angle, u8 mode) {
 // 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) {
+//    FrameInterpolation_RecordMatrixRotateAxis()
     f32 rx;
     f32 ry;
     f32 rz;
@@ -336,17 +478,20 @@ void Matrix_RotateAxis(Matrix* mtx, f32 angle, f32 axisX, f32 axisY, f32 axisZ,
 
 // Converts the current Gfx matrix to a Mtx
 void Matrix_ToMtx(Mtx* dest) {
+    FrameInterpolation_RecordMatrixMtxFToMtx(gGfxMatrix->m, dest);
     // LTODO: We need to validate this
     guMtxF2L(gGfxMatrix->m, dest);
 }
 
 // Converts the Mtx src to a Matrix, putting the result in dest
 void Matrix_FromMtx(Mtx* src, Matrix* dest) {
+    FrameInterpolation_RecordMatrixMtxFToMtx(src, dest);
     guMtxF2L(src->m, dest->m);
 }
 
 // Applies the transform matrix mtx to the vector src, putting the result in dest
 void Matrix_MultVec3f(Matrix* mtx, Vec3f* src, Vec3f* dest) {
+    FrameInterpolation_RecordMatrixMultVec3f(*src, *dest);
     dest->x = (mtx->m[0][0] * src->x) + (mtx->m[1][0] * src->y) + (mtx->m[2][0] * src->z) + mtx->m[3][0];
     dest->y = (mtx->m[0][1] * src->x) + (mtx->m[1][1] * src->y) + (mtx->m[2][1] * src->z) + mtx->m[3][1];
     dest->z = (mtx->m[0][2] * src->x) + (mtx->m[1][2] * src->y) + (mtx->m[2][2] * src->z) + mtx->m[3][2];
@@ -355,6 +500,7 @@ void Matrix_MultVec3f(Matrix* mtx, Vec3f* src, Vec3f* dest) {
 // 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) {
+    FrameInterpolation_RecordMatrixMultVec3fNoTranslate(*src, *dest);
     dest->x = (mtx->m[0][0] * src->x) + (mtx->m[1][0] * src->y) + (mtx->m[2][0] * src->z);
     dest->y = (mtx->m[0][1] * src->x) + (mtx->m[1][1] * src->y) + (mtx->m[2][1] * src->z);
     dest->z = (mtx->m[0][2] * src->x) + (mtx->m[1][2] * src->y) + (mtx->m[2][2] * src->z);