Pushed a ton of interpolation fixesfps-interpolation-2

4 files changed, 87 insertions, 24 deletions

diff --git a/src/port/interpolation/FrameInterpolation.cpp b/src/port/interpolation/FrameInterpolation.cpp
index 12b57f4a9..5ac52c5c0 100644
--- a/src/port/interpolation/FrameInterpolation.cpp
+++ b/src/port/interpolation/FrameInterpolation.cpp
@@ -8,6 +8,7 @@
 #include <math_util.h>
 #include <math_util_2.h>
 #include "FrameInterpolation.h"
+#include "matrix.h"
 
 /*
 Frame interpolation.
@@ -59,6 +60,8 @@ enum class Op {
     MatrixTranslate,
     MatrixScale,
     MatrixRotate1Coord,
+    MatrixMult4x4,
+    MatrixPosRotXYZ,
     MatrixMultVec3fNoTranslate,
     MatrixMultVec3f,
     MatrixMtxFToMtx,
@@ -90,14 +93,13 @@ union Data {
 
     struct {
         Mat4* matrix;
-        Vec3f b;
+        Vec3fInterp b;
     } matrix_translate, matrix_scale;
 
     struct {
         Mat4* matrix;
         u32 coord;
-        f32 value;
-        u8 mode;
+        s16 value;
     } matrix_rotate_1_coord;
 
     struct {
@@ -113,6 +115,17 @@ union Data {
     } matrix_vec_no_translate;
 
     struct {
+        Mat4* dest;
+        Mat4 mtx1;
+        Mat4 mtx2;
+    } matrix_mult_4x4;
+
+    struct {
+        Vec3fInterp pos;
+        Vec3sInterp orientation;
+    } matrix_pos_rot_xyz;
+
+    struct {
         Mat4* matrix;
         Vec3f translation;
         Vec3s rotation;
@@ -215,6 +228,10 @@ struct InterpolateCtx {
         return w * o + step * n;
     }
 
+    s16 lerp_s16(s16 o, s16 n) {
+        return w * o + step * n;
+    }
+
     void lerp_vec3f(Vec3f* res, Vec3f* o, Vec3f* n) {
         *res[0] = lerp(*o[0], *n[0]);
         *res[1] = lerp(*o[1], *n[1]);
@@ -332,20 +349,29 @@ struct InterpolateCtx {
                             break;
 
                         case Op::MatrixTranslate:
-                            // Matrix_Translate(gInterpolationMatrix, lerp(old_op.matrix_translate.x,
-                            // new_op.matrix_translate.x),
-                            //                  lerp(old_op.matrix_translate.y, new_op.matrix_translate.y),
-                            //                  lerp(old_op.matrix_translate.z, new_op.matrix_translate.z),
-                            //                  new_op.matrix_translate.mode);
 
                             Vec3f temp;
 
-                            temp[0] = lerp(old_op.matrix_translate.b[0], new_op.matrix_translate.b[0]);
-                            temp[1] = lerp(old_op.matrix_translate.b[1], new_op.matrix_translate.b[1]);
-                            temp[2] = lerp(old_op.matrix_translate.b[2], new_op.matrix_translate.b[2]);
+                            temp[0] = lerp(old_op.matrix_translate.b.x, new_op.matrix_translate.b.x);
+                            temp[1] = lerp(old_op.matrix_translate.b.y, new_op.matrix_translate.b.y);
+                            temp[2] = lerp(old_op.matrix_translate.b.z, new_op.matrix_translate.b.z);
 
                             mtxf_translate(*gInterpolationMatrix, temp);
                             break;
+                        case Op::MatrixPosRotXYZ:
+                            Vec3f tempF;
+                            Vec3s tempS;
+
+                            tempF[0] = lerp(old_op.matrix_pos_rot_xyz.pos.x, new_op.matrix_pos_rot_xyz.pos.x);
+                            tempF[1] = lerp(old_op.matrix_pos_rot_xyz.pos.y, new_op.matrix_pos_rot_xyz.pos.y);
+                            tempF[2] = lerp(old_op.matrix_pos_rot_xyz.pos.z, new_op.matrix_pos_rot_xyz.pos.z);
+
+                            tempS[0] = lerp(old_op.matrix_pos_rot_xyz.orientation.x, new_op.matrix_pos_rot_xyz.orientation.x);
+                            tempS[1] = lerp(old_op.matrix_pos_rot_xyz.orientation.y, new_op.matrix_pos_rot_xyz.orientation.y);
+                            tempS[2] = lerp(old_op.matrix_pos_rot_xyz.orientation.z, new_op.matrix_pos_rot_xyz.orientation.z);
+
+                            mtxf_pos_rotation_xyz(*gInterpolationMatrix, tempF, tempS);
+                            break;
 
                         case Op::MatrixScale:
                             // Matrix_Scale(gInterpolationMatrix, lerp(old_op.matrix_scale.x, new_op.matrix_scale.x),
@@ -355,20 +381,19 @@ struct InterpolateCtx {
                             break;
 
                         case Op::MatrixRotate1Coord: {
-                            float v = interpolate_angle(old_op.matrix_rotate_1_coord.value,
+                            s16 v = interpolate_angle(old_op.matrix_rotate_1_coord.value,
                                                         new_op.matrix_rotate_1_coord.value);
-                            u8 mode = new_op.matrix_rotate_1_coord.mode;
                             switch (new_op.matrix_rotate_1_coord.coord) {
                                 case 0:
-                                    // Matrix_RotateX(gInterpolationMatrix, v, mode);
+                                    mtxf_rotate_x(*gInterpolationMatrix, v);
                                     break;
 
                                 case 1:
-                                    // Matrix_RotateY(gInterpolationMatrix, v, mode);
+                                    mtxf_rotate_y(*gInterpolationMatrix, v);
                                     break;
 
                                 case 2:
-                                    // Matrix_RotateZ(gInterpolationMatrix, v, mode);
+                                    mtxf_s16_rotate_z(*gInterpolationMatrix, v);
                                     break;
                             }
                             break;
@@ -530,7 +555,7 @@ void FrameInterpolation_RecordMatrixTranslate(Mat4* matrix, Vec3f b) {
     if (!is_recording)
         return;
     
-    append(Op::MatrixTranslate).matrix_translate = { matrix, b[0] };
+    append(Op::MatrixTranslate).matrix_translate = { matrix, *((Vec3fInterp*) &b) };
 }
 
 void FrameInterpolation_RecordMatrixScale(Mat4* matrix, f32 x, f32 y, f32 z, u8 mode) {
@@ -542,7 +567,16 @@ void FrameInterpolation_RecordMatrixScale(Mat4* matrix, f32 x, f32 y, f32 z, u8
 void FrameInterpolation_RecordMatrixMultVec3fNoTranslate(Mat4* matrix, Vec3f src, Vec3f dest) {
     if (!is_recording)
         return;
-    // append(Op::MatrixMultVec3fNoTranslate).matrix_vec_no_translate = { matrix, src, dest };
+    //append(Op::MatrixMultVec3fNoTranslate).matrix_vec_no_translate = { matrix, src, dest };
+}
+
+// Make a template for deref
+
+
+void FrameInterpolation_RecordMatrixPosRotXYZ(Mat4 out, Vec3f pos, Vec3s orientation) {
+    if (!is_recording)
+        return;
+    append(Op::MatrixPosRotXYZ).matrix_pos_rot_xyz = { *((Vec3fInterp*) &pos), *((Vec3sInterp*) &orientation) };
 }
 
 void FrameInterpolation_RecordMatrixMultVec3f(Mat4* matrix, Vec3f src, Vec3f dest) {
@@ -551,10 +585,10 @@ void FrameInterpolation_RecordMatrixMultVec3f(Mat4* matrix, Vec3f src, Vec3f des
     // append(Op::MatrixMultVec3f).matrix_vec_translate = { matrix, src, dest };
 }
 
-void FrameInterpolation_RecordMatrixRotate1Coord(Mat4* matrix, u32 coord, f32 value, u8 mode) {
+void FrameInterpolation_RecordMatrixRotate1Coord(Mat4* matrix, u32 coord, s16 value) {
     if (!is_recording)
         return;
-    append(Op::MatrixRotate1Coord).matrix_rotate_1_coord = { matrix, coord, value, mode };
+    append(Op::MatrixRotate1Coord).matrix_rotate_1_coord = { matrix, coord, value };
 }
 
 void FrameInterpolation_RecordMatrixMtxFToMtx(MtxF* src, Mtx* dest) {
diff --git a/src/port/interpolation/FrameInterpolation.h b/src/port/interpolation/FrameInterpolation.h
index 72a1c9146..b09816452 100644
--- a/src/port/interpolation/FrameInterpolation.h
+++ b/src/port/interpolation/FrameInterpolation.h
@@ -12,11 +12,8 @@
 std::unordered_map<Mtx*, MtxF> FrameInterpolation_Interpolate(float step);
 
 extern "C" {
-
 #endif
 
-
-
 void FrameInterpolation_ShouldInterpolateFrame(bool shouldInterpolate);
 
 void FrameInterpolation_StartRecord(void);
@@ -35,6 +32,8 @@ int FrameInterpolation_GetCameraEpoch(void);
 
 void FrameInterpolation_RecordActorPosRotMatrix(void);
 
+void FrameInterpolation_RecordMatrixPosRotXYZ(Mat4 out, Vec3f pos, Vec3s orientation);
+
 //void FrameInterpolation_RecordMatrixPush(Matrix** mtx);
 
 //void FrameInterpolation_RecordMatrixPop(Matrix** mtx);
@@ -45,7 +44,7 @@ void FrameInterpolation_RecordMatrixTranslate(Mat4* matrix, Vec3f b);
 
 //void FrameInterpolation_RecordMatrixScale(Matrix* matrix, f32 x, f32 y, f32 z, u8 mode);
 
-//void FrameInterpolation_RecordMatrixRotate1Coord(Matrix* matrix, u32 coord, f32 value, u8 mode);
+void FrameInterpolation_RecordMatrixRotate1Coord(Mat4* matrix, u32 coord, s16 value);
 
 void FrameInterpolation_RecordMatrixMtxFToMtx(MtxF* src, Mtx* dest);
 
diff --git a/src/port/interpolation/matrix.c b/src/port/interpolation/matrix.c
index 8e9e09fd8..4f8a7839b 100644
--- a/src/port/interpolation/matrix.c
+++ b/src/port/interpolation/matrix.c
@@ -33,6 +33,18 @@ void Matrix_InitPerspective(Gfx** dList) {
     Matrix_Copy(gGfxMatrix, &gIdentityMatrix);
 }
 
+void Matrix_InitOrtho(Gfx** dList) {
+    FrameInterpolation_RecordOpenChild("ortho", 0);
+    FrameInterpolation_RecordMarker(__FILE__, __LINE__);
+    guOrtho(gGfxMtx, -320.0f / 2, 320.0f / 2, -240.0f / 2, 240.0f / 2, 0.0f, 5.0f, 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();
+}
+
+
 // Copies src Matrix into dst
 void Matrix_Copy(Matrix* dst, Matrix* src) {
     int32_t i;
diff --git a/src/port/interpolation/matrix.h b/src/port/interpolation/matrix.h
index 41411c107..f71ef6ab0 100644
--- a/src/port/interpolation/matrix.h
+++ b/src/port/interpolation/matrix.h
@@ -10,6 +10,23 @@ typedef struct {
     float b;
 } Color;
 
+typedef struct {
+    float x;
+    float y;
+    float z;
+} Vec3fInterp;
+
+typedef struct {
+    s16 x;
+    s16 y;
+    s16 z;
+} Vec3sInterp;
+
+typedef struct {
+    f32 m1; f32 m2; f32 m3; f32 m4;
+    f32 m5; f32 m6; f32 m7; f32 m8;
+} Mat4Interp;
+
 #define M_PI    3.14159265358979323846f
 #define M_RTOD	(180.0f / M_PI)
 #define SQ(val) ((val) * (val))
@@ -50,6 +67,7 @@ extern Mtx gMainMatrixStack[];
 extern Mtx* gGfxMtx;
 
 void Matrix_InitPerspective(Gfx** dList);
+void Matrix_InitOrtho(Gfx** dList);
 void Matrix_Copy(Matrix* dst, Matrix* src);
 void Matrix_Push(Matrix** mtxStack);
 void Matrix_Pop(Matrix** mtxStack);