Pushed a ton of interpolation fixesfps-interpolation-2

10 files changed, 121 insertions, 28 deletions

diff --git a/include/common_structs.h b/include/common_structs.h
index b5efa387a..fa18c39c9 100644
--- a/include/common_structs.h
+++ b/include/common_structs.h
@@ -28,7 +28,7 @@ typedef struct {
 // In the data to use "A, B, Z, R" instead of hex numbers.
 typedef enum { A = 0x80, B = 0x40, Z = 0x20, R = 0x10 } GhostController;
 
-/***  types.h  ***/
+/***  types.h  ***/ 
 
 typedef struct {
     /* 0x0 */ s16 waypointStart;
diff --git a/src/actors/train/render.inc.c b/src/actors/train/render.inc.c
index 825aa4cd9..4a2618f98 100644
--- a/src/actors/train/render.inc.c
+++ b/src/actors/train/render.inc.c
@@ -2,6 +2,8 @@
 #include <libultra/gbi.h>
 #include <main.h>
 #include <assets/kalimari_desert_data.h>
+#include <port/interpolation/matrix.h>
+#include <port/interpolation/FrameInterpolation.h>
 
 /**
  * @brief Renders the train engine actor.
@@ -19,6 +21,8 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
     Mat4 objectMtx;
     Mat4 resultMtx;
 
+    FrameInterpolation_RecordOpenChild(actor, 0);
+
     f32 distance = is_within_render_distance(camera->pos, actor->pos, camera->rot[1], 2500.0f,
                                              gCameraZoom[camera - camera1], 9000000.0f);
 
@@ -39,6 +43,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
     mtxf_pos_rotation_xyz(mainMtx, actor->pos, actor->rot);
     maxObjectsReached = render_set_position(mainMtx, 0) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -56,6 +61,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
         gSPDisplayList(gDisplayListHead++, d_course_kalimari_desert_dl_1E480);
     }
     if (1440000.0f < distance) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -69,6 +75,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -82,6 +89,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -95,6 +103,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -108,6 +117,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -121,6 +131,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -134,6 +145,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -147,6 +159,7 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
@@ -160,12 +173,14 @@ void render_actor_train_engine(Camera* camera, struct TrainCar* actor) {
 
     maxObjectsReached = render_set_position(resultMtx, 3) == 0;
     if (maxObjectsReached) {
+        FrameInterpolation_RecordCloseChild();
         return;
     }
 
     gSPDisplayList(gDisplayListHead++, d_course_kalimari_desert_dl_22D70);
     gSPPopMatrix(gDisplayListHead++, G_MTX_MODELVIEW);
     gSPSetGeometryMode(gDisplayListHead++, G_CULL_BACK);
+    FrameInterpolation_RecordCloseChild();
 }
 
 void render_actor_train_tender(Camera* camera, struct TrainCar* actor) {
diff --git a/src/engine/vehicles/Train.cpp b/src/engine/vehicles/Train.cpp
index f4a5a4f9d..5339912c0 100644
--- a/src/engine/vehicles/Train.cpp
+++ b/src/engine/vehicles/Train.cpp
@@ -153,7 +153,6 @@ void ATrain::SyncComponents(TrainCarStuff* trainCar, s16 orientationY) {
 }
 
 void ATrain::Tick() {
-    FrameInterpolation_StartRecord();
     f32 temp_f20;
     TrainCarStuff* car;
     u16 oldWaypointIndex;
@@ -224,8 +223,6 @@ void ATrain::Tick() {
             sync_train_components(car, orientationYUpdate);
         }
     }
-
-    FrameInterpolation_StopRecord();
 }
 
 void ATrain::VehicleCollision(s32 playerId, Player* player) {
diff --git a/src/main.c b/src/main.c
index c83077f03..d7697c584 100644
--- a/src/main.c
+++ b/src/main.c
@@ -48,6 +48,7 @@
 #include "port/Game.h"
 #include "engine/Matrix.h"
 #include "port/interpolation/matrix.h"
+#include "port/interpolation/FrameInterpolation.h"
 
 // Declarations (not in this file)
 void func_80091B78(void);
@@ -943,6 +944,7 @@ void game_state_handler(void) {
         gGamestateNext = ENDING;
     }
 #endif
+    FrameInterpolation_StartRecord();
     switch (gGamestate) {
         case 7:
             game_init_clear_framebuffer();
@@ -970,6 +972,7 @@ void game_state_handler(void) {
             credits_loop();
             break;
     }
+    FrameInterpolation_StopRecord();
 }
 
 void interrupt_gfx_sptask(void) {
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);
diff --git a/src/racing/math_util.c b/src/racing/math_util.c
index 6f9aba302..2f7e907c9 100644
--- a/src/racing/math_util.c
+++ b/src/racing/math_util.c
@@ -9,6 +9,7 @@
 #include "memory.h"
 #include "engine/Matrix.h"
 #include "port/Game.h"
+#include <port/interpolation/FrameInterpolation.h>
 #include <port/interpolation/matrix.h>
 
 #pragma intrinsic(sqrtf, fabs)
@@ -331,6 +332,7 @@ void func_802B5794(Mat4 mtx, Vec3f from, Vec3f to) {
 
 // create a rotation matrix around the x axis
 void mtxf_rotate_x(Mat4 mat, s16 angle) {
+    FrameInterpolation_RecordMatrixRotate1Coord(&mat, 0, angle);
     f32 sin_theta = sins(angle);
     f32 cos_theta = coss(angle);
 
@@ -350,6 +352,7 @@ void mtxf_rotate_x(Mat4 mat, s16 angle) {
 
 // create a rotation matrix around the y axis
 void mtxf_rotate_y(Mat4 mat, s16 angle) {
+    FrameInterpolation_RecordMatrixRotate1Coord(&mat, 1, angle);
     f32 sin_theta = sins(angle);
     f32 cos_theta = coss(angle);
 
@@ -369,6 +372,7 @@ void mtxf_rotate_y(Mat4 mat, s16 angle) {
 
 // create a rotation matrix around the z axis
 void mtxf_s16_rotate_z(Mat4 mat, s16 angle) {
+    FrameInterpolation_RecordMatrixRotate1Coord(&mat, 2, angle);
     f32 sin_theta = sins(angle);
     f32 cos_theta = coss(angle);
 
@@ -475,6 +479,8 @@ void mtxf_scale(Mat4 mat, f32 coef) {
 
 // look like create a translation and rotation matrix with arg1 position and arg2 rotation
 void mtxf_pos_rotation_xyz(Mat4 out, Vec3f pos, Vec3s orientation) {
+    FrameInterpolation_RecordMatrixPosRotXYZ(out, pos, orientation);
+
     f32 sine1;
     f32 cosine1;
     f32 sine2;
@@ -825,6 +831,7 @@ void func_802B6D58(Mat4 arg0, Vec3f arg1, Vec3f arg2) {
 }
 
 void mtxf_multiplication(Mat4 dest, Mat4 mat1, Mat4 mat2) {
+    // FrameInterpolation_RecordMatrixMult4x4(dest, mat1, mat2);
     Mat4 product;
     product[0][0] =
         (mat1[0][0] * mat2[0][0]) + (mat1[0][1] * mat2[1][0]) + (mat1[0][2] * mat2[2][0]) + (mat1[0][3] * mat2[3][0]);
diff --git a/src/racing/math_util.h b/src/racing/math_util.h
index d5d7bf3c4..43ccc249a 100644
--- a/src/racing/math_util.h
+++ b/src/racing/math_util.h
@@ -12,6 +12,10 @@
 
 #define sqr(x) ((x) * (x))
 
+#ifdef __cplusplus
+extern "C" {
+#endif
+
 // Here to appease the pragma gods
 double fabs(double x);
 
@@ -72,4 +76,8 @@ extern s32 D_802B91C0[];
 extern Vec3f D_802B91C8;
 // extern Mtx gIdentityMatrix;
 
+#ifdef __cplusplus
+}
+#endif
+
 #endif // MATH_UTIL_H