Interpolate Horse Animation (#1061)

4 files changed, 202 insertions, 79 deletions

diff --git a/mm/src/code/stubs.c b/mm/src/code/stubs.c
index da40b1a1a..196ff3eec 100644
--- a/mm/src/code/stubs.c
+++ b/mm/src/code/stubs.c
@@ -167,6 +167,27 @@ void gSPSegment(void* value, int segNum, uintptr_t target) {
     __gSPSegment(value, segNum, target);
 }
 
+void gSPSegmentInterp(void* value, int segNum, uintptr_t target) {
+    char* imgData = (char*)target;
+
+    int res = ResourceMgr_OTRSigCheck(imgData);
+
+    // OTRTODO: Disabled for now to fix an issue with HD Textures.
+    // With HD textures, we need to pass the path to F3D, not the raw texture data.
+    // Otherwise the needed metadata is not available for proper rendering...
+    // This should *not* cause any crashes, but some testing may be needed...
+    // UPDATE: To maintain compatability it will still do the old behavior if the resource is a display list.
+    // That should not affect HD textures.
+    if (res) {
+        uintptr_t desiredTarget = (uintptr_t)ResourceMgr_LoadIfDListByName(imgData);
+
+        if (desiredTarget != NULL)
+            target = desiredTarget;
+    }
+
+    __gSPSegmentInterp(value, segNum, target);
+}
+
 void gSPSegmentLoadRes(void* value, int segNum, uintptr_t target) {
     char* imgData = (char*)target;
 
diff --git a/mm/src/code/z_skelanime.c b/mm/src/code/z_skelanime.c
index 924d368f9..676718d7b 100644
--- a/mm/src/code/z_skelanime.c
+++ b/mm/src/code/z_skelanime.c
@@ -620,30 +620,93 @@ void SkelAnime_DrawTransformFlexOpa(PlayState* play, void** skeleton, Vec3s* joi
     CLOSE_DISPS(play->state.gfxCtx);
 }
 
+// TODO: Put this in a dedicated math file
+// Actually there might already be something that accomplishes this...
+static s16 LerpS16(float a, float b, float t) {
+    return (s16)(a + (b - a) * t);
+}
+
 /**
  * Copies frame data from the frame data table, indexed by the joint index table.
  * Indices below staticIndexMax are copied from that entry in the static frame data table.
  * Indices above staticIndexMax are offsets to a frame data array indexed by the frame.
  */
-void SkelAnime_GetFrameData(AnimationHeader* animation, s32 frame, s32 limbCount, Vec3s* frameTable) {
+void SkelAnime_GetFrameData(AnimationHeader* animation, float frame, float animFrameCount, float animSpeed,
+                            s32 limbCount, Vec3s* frameTable, Vec3s* interpFrameTable, bool isSkinnedSkeleton) {
     if (ResourceMgr_OTRSigCheck(animation))
         animation = ResourceMgr_LoadAnimByName(animation);
 
-    AnimationHeader* animHeader = Lib_SegmentedToVirtual(animation);
-    JointIndex* jointIndices = Lib_SegmentedToVirtual(animHeader->jointIndices);
-    s16* frameData = Lib_SegmentedToVirtual(animHeader->frameData);
-    s16* dynamicData = &frameData[frame];
-    s32 i;
-    u16 staticIndexMax = animHeader->staticIndexMax;
+    if (animSpeed != 0) {
+        // TODO: This feels wrong, but it prevents graphical issues
+        // Need to give this some proper thought later
+        if (animSpeed > 0)
+            animSpeed = 1;
+        else
+            animSpeed = -1;
+    }
+
+    float fpsDiv = Ship_GetInterpolationFPS() / 20.0f;
+
+    Vec3s* originalFrameTable = frameTable;
+    int interpolatedFrameCount = isSkinnedSkeleton ? Ship_GetInterpolationFrameCount() : 1;
+
+    for (int j = 0; j < Ship_GetInterpolationFrameCount(); j++) {
+        if (j > 0)
+            frameTable = &interpFrameTable[limbCount * (j - 1)];
+
+        float framePerc = frame - (s32)frame;
+        s32 frameBase = (s32)frame;
+        s32 frameBaseNext = (s32)(frame + animSpeed);
+
+        if (frameBaseNext < frameBase) {
+            int tmp = frameBase;
+            frameBase = frameBaseNext;
+            frameBaseNext = tmp;
+        }
+
+        if (animFrameCount != 0) {
+            frameBase = fmodf(frameBase, animFrameCount);
+            frameBaseNext = fmodf(frameBaseNext, animFrameCount);
+        }
+
+        if (frameBase < 0)
+            frameBase = animFrameCount - 1;
+
+        if (frameBaseNext < 0)
+            frameBaseNext = animFrameCount - 1;
 
-    for (i = 0; i < limbCount; i++) {
-        // Debug prints here, this is needed to prevent loop unrolling
-        if ((frameTable == NULL) || (jointIndices == NULL) || (dynamicData == NULL)) {}
-        frameTable->x = jointIndices->x >= staticIndexMax ? dynamicData[jointIndices->x] : frameData[jointIndices->x];
-        frameTable->y = jointIndices->y >= staticIndexMax ? dynamicData[jointIndices->y] : frameData[jointIndices->y];
-        frameTable->z = jointIndices->z >= staticIndexMax ? dynamicData[jointIndices->z] : frameData[jointIndices->z];
-        jointIndices++;
-        frameTable++;
+        AnimationHeader* animHeader = Lib_SegmentedToVirtual(animation);
+        JointIndex* jointIndices = Lib_SegmentedToVirtual(animHeader->jointIndices);
+        s16* frameData = Lib_SegmentedToVirtual(animHeader->frameData);
+        s16* dynamicData = &frameData[frameBase];
+        s16* dynamicDataNext = &frameData[frameBaseNext];
+        s32 i;
+        u16 staticIndexMax = animHeader->staticIndexMax;
+
+        for (i = 0; i < limbCount; i++) {
+            s16 dynamicDataX = 0;
+            s16 dynamicDataY = 0;
+            s16 dynamicDataZ = 0;
+
+            if (isSkinnedSkeleton) {
+                dynamicDataX = LerpS16(dynamicData[jointIndices->x], dynamicDataNext[jointIndices->x], framePerc);
+                dynamicDataY = LerpS16(dynamicData[jointIndices->y], dynamicDataNext[jointIndices->y], framePerc);
+                dynamicDataZ = LerpS16(dynamicData[jointIndices->z], dynamicDataNext[jointIndices->z], framePerc);
+            } else {
+                dynamicDataX = dynamicData[jointIndices->x];
+                dynamicDataY = dynamicData[jointIndices->y];
+                dynamicDataZ = dynamicData[jointIndices->z];
+            }
+
+            frameTable->x = jointIndices->x >= staticIndexMax ? dynamicDataX : frameData[jointIndices->x];
+            frameTable->y = jointIndices->y >= staticIndexMax ? dynamicDataY : frameData[jointIndices->y];
+            frameTable->z = jointIndices->z >= staticIndexMax ? dynamicDataZ : frameData[jointIndices->z];
+
+            jointIndices++;
+            frameTable++;
+        }
+
+        frame += animSpeed / fpsDiv;
     }
 }
 
@@ -1265,6 +1328,8 @@ void SkelAnime_InitPlayer(PlayState* play, SkelAnime* skelAnime, FlexSkeletonHea
         skelAnime->morphTable = (void*)ALIGN16((uintptr_t)morphTableBuffer);
     }
 
+    skelAnime->extraJointTable = ZeldaArena_Malloc(allocSize * MAX_INTERP_FRAMES);
+
     PlayerAnimation_Change(play, skelAnime, animation, 1.0f, 0.0f, 0.0f, ANIMMODE_LOOP, 0.0f);
 }
 
@@ -1587,6 +1652,9 @@ void SkelAnime_Init(PlayState* play, SkelAnime* skelAnime, SkeletonHeader* skele
         skelAnime->morphTable = morphTable;
     }
 
+    skelAnime->extraJointTable =
+        ZeldaArena_Malloc(sizeof(*skelAnime->jointTable) * skelAnime->limbCount * MAX_INTERP_FRAMES);
+
     if (animation != NULL) {
         Animation_PlayLoop(skelAnime, animation);
     }
@@ -1617,6 +1685,9 @@ void SkelAnime_InitFlex(PlayState* play, SkelAnime* skelAnime, FlexSkeletonHeade
         skelAnime->morphTable = morphTable;
     }
 
+    skelAnime->extraJointTable =
+        ZeldaArena_Malloc(sizeof(*skelAnime->jointTable) * skelAnime->limbCount * MAX_INTERP_FRAMES);
+
     if (animation != NULL) {
         Animation_PlayLoop(skelAnime, animation);
     }
@@ -1639,6 +1710,11 @@ void SkelAnime_InitSkin(GameState* gameState, SkelAnime* skelAnime, SkeletonHead
     skelAnime->jointTable = ZeldaArena_Malloc(sizeof(*skelAnime->jointTable) * skelAnime->limbCount);
     skelAnime->morphTable = ZeldaArena_Malloc(sizeof(*skelAnime->morphTable) * skelAnime->limbCount);
 
+    skelAnime->extraJointTable =
+        ZeldaArena_Malloc(sizeof(*skelAnime->jointTable) * skelAnime->limbCount * MAX_INTERP_FRAMES);
+
+    skelAnime->isSkinned = true;
+
     // Debug prints here, required to match.
     if (1) {}
 
@@ -1723,9 +1799,12 @@ s32 SkelAnime_MorphTaper(SkelAnime* skelAnime) {
  * Gets frame data for the current frame as modified by morphTable and advances the morph
  */
 void SkelAnime_AnimateFrame(SkelAnime* skelAnime) {
-    Vec3s nextjointTable[100];
+    Vec3s nextjointTable[100 * MAX_INTERP_FRAMES];
+
+    SkelAnime_GetFrameData(skelAnime->animation, skelAnime->curFrame, skelAnime->animLength, skelAnime->playSpeed,
+                           skelAnime->limbCount, skelAnime->jointTable, skelAnime->extraJointTable,
+                           skelAnime->isSkinned);
 
-    SkelAnime_GetFrameData(skelAnime->animation, skelAnime->curFrame, skelAnime->limbCount, skelAnime->jointTable);
     if (skelAnime->mode & ANIM_INTERP) {
         s32 frame = skelAnime->curFrame;
         f32 partialFrame = skelAnime->curFrame - frame;
@@ -1734,7 +1813,8 @@ void SkelAnime_AnimateFrame(SkelAnime* skelAnime) {
         if (frame >= (s32)skelAnime->animLength) {
             frame = 0;
         }
-        SkelAnime_GetFrameData(skelAnime->animation, frame, skelAnime->limbCount, nextjointTable);
+        SkelAnime_GetFrameData(skelAnime->animation, frame, skelAnime->animLength, skelAnime->playSpeed,
+                               skelAnime->limbCount, nextjointTable, skelAnime->extraJointTable, skelAnime->isSkinned);
         SkelAnime_InterpFrameTable(skelAnime->limbCount, skelAnime->jointTable, skelAnime->jointTable, nextjointTable,
                                    partialFrame);
     }
@@ -1791,7 +1871,9 @@ s32 SkelAnime_Once(SkelAnime* skelAnime) {
     f32 updateRate = gFramerateDivisorThird;
 
     if (skelAnime->curFrame == skelAnime->endFrame) {
-        SkelAnime_GetFrameData(skelAnime->animation, skelAnime->curFrame, skelAnime->limbCount, skelAnime->jointTable);
+        SkelAnime_GetFrameData(skelAnime->animation, skelAnime->curFrame, skelAnime->animLength, skelAnime->playSpeed,
+                               skelAnime->limbCount, skelAnime->jointTable, skelAnime->extraJointTable,
+                               skelAnime->isSkinned);
         SkelAnime_AnimateFrame(skelAnime);
         return true;
     }
@@ -1842,13 +1924,16 @@ void Animation_ChangeImpl(SkelAnime* skelAnime, AnimationHeader* animation, f32
             } else {
                 skelAnime->update.normal = SkelAnime_Morph;
             }
-            SkelAnime_GetFrameData(animation, startFrame, skelAnime->limbCount, skelAnime->morphTable);
+            SkelAnime_GetFrameData(animation, startFrame, skelAnime->animLength, skelAnime->playSpeed,
+                                   skelAnime->limbCount, skelAnime->morphTable, skelAnime->extraJointTable,
+                                   skelAnime->isSkinned);
         }
         skelAnime->morphWeight = 1.0f;
         skelAnime->morphRate = 1.0f / morphFrames;
     } else {
         SkelAnime_SetUpdate(skelAnime);
-        SkelAnime_GetFrameData(animation, startFrame, skelAnime->limbCount, skelAnime->jointTable);
+        SkelAnime_GetFrameData(animation, startFrame, skelAnime->animLength, skelAnime->playSpeed, skelAnime->limbCount,
+                               skelAnime->jointTable, skelAnime->extraJointTable, skelAnime->isSkinned);
         skelAnime->morphWeight = 0.0f;
     }
 
diff --git a/mm/src/code/z_skin.c b/mm/src/code/z_skin.c
index 5569c886b..77b5f0515 100644
--- a/mm/src/code/z_skin.c
+++ b/mm/src/code/z_skin.c
@@ -1,9 +1,10 @@
 #include "prevent_bss_reordering.h"
 #include "global.h"
 #include "z64skin.h"
+#include "BenPort.h"
 
 // 60 is an arbitrary number which specifies the max amount of limbs per skeleton this system supports
-MtxF gSkinLimbMatrices[60];
+MtxF gSkinLimbMatrices[60 * MAX_INTERP_FRAMES];
 
 static s32 sBssPad;
 
@@ -75,56 +76,67 @@ void Skin_ApplyLimbModifications(GraphicsContext* gfxCtx, Skin* skin, s32 limbIn
 
     vtxEntry = &skin->vtxTable[limbIndex];
 
-    vtxBuf = vtxEntry->buf[vtxEntry->index];
     modifCount = data->limbModifCount;
 
-    for (modif = modifications; modif < &modifications[modifCount]; modif++) {
-        Vec3f spAC;
-        Vec3f spA0;
-
-        skinVertices = (SkinVertex*)Lib_SegmentedToVirtual(modif->skinVertices);
-        limbTransformations = (SkinTransformation*)Lib_SegmentedToVirtual(modif->limbTransformations);
-        transformCount = modif->transformCount;
-
-        if (transformCount == 1) {
-            spAC.x = limbTransformations[0].x;
-            spAC.y = limbTransformations[0].y;
-            spAC.z = limbTransformations[0].z;
-
-            SkinMatrix_Vec3fMtxFMultXYZ(&gSkinLimbMatrices[limbTransformations[0].limbIndex], &spAC, &spDC);
-        } else if (arg3) {
-            transformationEntry = &limbTransformations[modif->unk_04];
-
-            spA0.x = transformationEntry->x;
-            spA0.y = transformationEntry->y;
-            spA0.z = transformationEntry->z;
-            SkinMatrix_Vec3fMtxFMultXYZ(&gSkinLimbMatrices[transformationEntry->limbIndex], &spA0, &spDC);
-        } else {
-            spDC.x = 0.0f;
-            spDC.y = 0.0f;
-            spDC.z = 0.0f;
-
-            for (transformationEntry = limbTransformations; transformationEntry < &limbTransformations[transformCount];
-                 transformationEntry++) {
-                scale = transformationEntry->scale * 0.01f;
-
-                sp88.x = transformationEntry->x;
-                sp88.y = transformationEntry->y;
-                sp88.z = transformationEntry->z;
-
-                SkinMatrix_Vec3fMtxFMultXYZ(&gSkinLimbMatrices[transformationEntry->limbIndex], &sp88, &spD0);
-
-                spDC.x += spD0.x * scale;
-                spDC.y += spD0.y * scale;
-                spDC.z += spD0.z * scale;
+    int limbCount = skin->limbCount + 1;
+
+    for (int interpIdx = 0; interpIdx < Ship_GetInterpolationFrameCount(); interpIdx++) {
+        vtxBuf = vtxEntry->buf[vtxEntry->index + (interpIdx * 2)];
+
+        for (modif = modifications; modif < &modifications[modifCount]; modif++) {
+            Vec3f spAC;
+            Vec3f spA0;
+
+            skinVertices = (SkinVertex*)Lib_SegmentedToVirtual(modif->skinVertices);
+            limbTransformations = (SkinTransformation*)Lib_SegmentedToVirtual(modif->limbTransformations);
+            transformCount = modif->transformCount;
+
+            if (transformCount == 1) {
+                spAC.x = limbTransformations[0].x;
+                spAC.y = limbTransformations[0].y;
+                spAC.z = limbTransformations[0].z;
+
+                SkinMatrix_Vec3fMtxFMultXYZ(
+                    &gSkinLimbMatrices[(interpIdx * limbCount) + limbTransformations[0].limbIndex], &spAC, &spDC);
+            } else if (arg3) {
+                transformationEntry = &limbTransformations[modif->unk_04];
+
+                spA0.x = transformationEntry->x;
+                spA0.y = transformationEntry->y;
+                spA0.z = transformationEntry->z;
+                SkinMatrix_Vec3fMtxFMultXYZ(
+                    &gSkinLimbMatrices[(interpIdx * limbCount) + transformationEntry->limbIndex], &spA0, &spDC);
+            } else {
+                spDC.x = 0.0f;
+                spDC.y = 0.0f;
+                spDC.z = 0.0f;
+
+                for (transformationEntry = limbTransformations;
+                     transformationEntry < &limbTransformations[transformCount]; transformationEntry++) {
+                    scale = transformationEntry->scale * 0.01f;
+
+                    sp88.x = transformationEntry->x;
+                    sp88.y = transformationEntry->y;
+                    sp88.z = transformationEntry->z;
+
+                    SkinMatrix_Vec3fMtxFMultXYZ(
+                        &gSkinLimbMatrices[(interpIdx * limbCount) + transformationEntry->limbIndex], &sp88, &spD0);
+
+                    spDC.x += spD0.x * scale;
+                    spDC.y += spD0.y * scale;
+                    spDC.z += spD0.z * scale;
+                }
             }
+
+            Skin_UpdateVertices(
+                &gSkinLimbMatrices[(interpIdx * limbCount) + limbTransformations[modif->unk_04].limbIndex],
+                skinVertices, modif, vtxBuf, &spDC);
         }
 
-        Skin_UpdateVertices(&gSkinLimbMatrices[limbTransformations[modif->unk_04].limbIndex], skinVertices, modif,
-                            vtxBuf, &spDC);
+        gSPSegmentInterp(POLY_OPA_DISP++, 0x08 + (interpIdx * 0x10), vtxBuf);
     }
 
-    gSPSegment(POLY_OPA_DISP++, 0x08, vtxEntry->buf[vtxEntry->index]);
+    // gSPSegment(POLY_OPA_DISP++, 0x08, vtxEntry->buf[vtxEntry->index]);
 
     vtxEntry->index = (vtxEntry->index == 0);
 
@@ -192,7 +204,13 @@ void Skin_DrawImpl(Actor* actor, PlayState* play, Skin* skin, SkinPostDraw postD
     OPEN_DISPS(gfxCtx);
 
     if (!(drawFlags & SKIN_DRAW_FLAG_CUSTOM_TRANSFORMS)) {
-        Skin_ApplyAnimTransformations(skin, gSkinLimbMatrices, actor, setTranslation);
+        int limbCount = skin->limbCount + 1;
+
+        Skin_ApplyAnimTransformations(skin, gSkinLimbMatrices, actor, setTranslation, skin->skelAnime.jointTable);
+
+        for (int i = 0; i < Ship_GetInterpolationFrameCount(); i++)
+            Skin_ApplyAnimTransformations(skin, gSkinLimbMatrices + (limbCount * (i + 1)), actor, setTranslation,
+                                          &skin->skelAnime.extraJointTable[limbCount * i]);
     }
 
     skeleton = Lib_SegmentedToVirtual(skin->skeletonHeader->segment);
diff --git a/mm/src/code/z_skin_awb.c b/mm/src/code/z_skin_awb.c
index ab2f4dc3a..3a7a5b0fd 100644
--- a/mm/src/code/z_skin_awb.c
+++ b/mm/src/code/z_skin_awb.c
@@ -71,8 +71,8 @@ void Skin_Init(GameState* gameState, Skin* skin, SkeletonHeader* skeletonHeader,
             (((SkinLimb*)Lib_SegmentedToVirtual(skeleton[i]))->segment == NULL)) {
             vtxEntry->index = 0;
 
-            vtxEntry->buf[0] = NULL;
-            vtxEntry->buf[1] = NULL;
+            for (int j = 0; j < ARRAY_COUNT(vtxEntry->buf); j++)
+                vtxEntry->buf[j] = NULL;
         } else {
             SkinAnimatedLimbData* animatedLimbData =
                 Lib_SegmentedToVirtual((((SkinLimb*)Lib_SegmentedToVirtual(skeleton[i]))->segment));
@@ -80,8 +80,9 @@ void Skin_Init(GameState* gameState, Skin* skin, SkeletonHeader* skeletonHeader,
             { s32 tmp; }
 
             vtxEntry->index = 0;
-            vtxEntry->buf[0] = ZeldaArena_Malloc(animatedLimbData->totalVtxCount * sizeof(Vtx));
-            vtxEntry->buf[1] = ZeldaArena_Malloc(animatedLimbData->totalVtxCount * sizeof(Vtx));
+
+            for (int j = 0; j < ARRAY_COUNT(skin->vtxTable->buf); j++)
+                vtxEntry->buf[j] = ZeldaArena_Malloc(animatedLimbData->totalVtxCount * sizeof(Vtx));
 
             Skin_InitAnimatedLimb(gameState, skin, i);
         }
@@ -98,13 +99,11 @@ void Skin_Free(GameState* gameState, Skin* skin) {
         s32 i;
 
         for (i = 0; i < skin->limbCount; i++) {
-            if (skin->vtxTable[i].buf[0] != NULL) {
-                ZeldaArena_Free(skin->vtxTable[i].buf[0]);
-                skin->vtxTable[i].buf[0] = NULL;
-            }
-            if (skin->vtxTable[i].buf[1] != NULL) {
-                ZeldaArena_Free(skin->vtxTable[i].buf[1]);
-                skin->vtxTable[i].buf[1] = NULL;
+            for (int j = 0; j < ARRAY_COUNT(skin->vtxTable->buf); j++) {
+                if (skin->vtxTable[i].buf[j] != NULL) {
+                    ZeldaArena_Free(skin->vtxTable[i].buf[j]);
+                    skin->vtxTable[i].buf[j] = NULL;
+                }
             }
         }
 
@@ -152,7 +151,7 @@ s32 func_801387D4(Skin* skin, SkinLimb** skeleton, MtxF* limbMatrices, u8 parent
 /**
  * Recursively applies matrix tranformations to each limb
  */
-s32 Skin_ApplyAnimTransformations(Skin* skin, MtxF* limbMatrices, Actor* actor, s32 setTranslation) {
+s32 Skin_ApplyAnimTransformations(Skin* skin, MtxF* limbMatrices, Actor* actor, s32 setTranslation, Vec3s* jointRot) {
     s32 i;
     s32 pad;
     f32 yRot;
@@ -163,7 +162,6 @@ s32 Skin_ApplyAnimTransformations(Skin* skin, MtxF* limbMatrices, Actor* actor,
     f32 xTransl;
     f32 zTransl;
     SkinLimb** skeleton = Lib_SegmentedToVirtual(skin->skeletonHeader->segment);
-    Vec3s* jointRot = skin->skelAnime.jointTable;
 
     jointRot++;
     xRot = jointRot->x;
@@ -194,6 +192,7 @@ s32 Skin_ApplyAnimTransformations(Skin* skin, MtxF* limbMatrices, Actor* actor,
         yRot = jointRot->y;
         zRot = jointRot->z;
         jointRot++;
+
         SkinMatrix_SetRotateRPYTranslate(&limbMatrices[i], xRot, yRot, zRot, xTransl, yTransl, zTransl);
     }