xCutscene: Equivalent match for xcsCalcAnimMatrices

Author: JoshSanch

src/SB/Core/x/xCutscene.cpp

@@ -7,6 +7,9 @@
 
 #include "iCutscene.h"
 #include "iModel.h"
+#include "zCamera.h"
+#include "zGlobals.h"
+#include "iAnim.h"
 
 #include <types.h>
 #include <string.h>
@@ -287,6 +290,84 @@ float std::logf(float x)
     return (float)log((double)x);
 }
 
+static void xcsCalcAnimMatrices(RwMatrixTag* animMat, RpAtomic* model, xCutsceneAnimHdr* ahdr, F32 time, U32 tworoot)
+{
+    xQuat quatresult[65];
+    xVec3 tranresult[65];
+
+    void* afile = &ahdr[1];
+    iAnimEval(afile, time, 0x0, tranresult, quatresult);
+
+    if (iModelNumBones(model) != 0)
+    {
+        xMat4x3Identity((xMat4x3*)animMat);
+        animMat->pos.x = ahdr->Translate[0];
+        animMat->pos.y = ahdr->Translate[1];
+        animMat->pos.z = ahdr->Translate[2];
+
+        
+        if (tworoot)
+        {
+            xMat4x3 m1;
+            xMat4x3 m2;
+
+            quatresult[1].s = -quatresult[1].s;
+            quatresult[2].s = -quatresult[2].s;
+            xQuatToMat(&quatresult[1], &m1);
+            xQuatToMat(&quatresult[2], &m2);
+
+            m1.pos = tranresult[1];
+            m2.pos = tranresult[2];
+
+            xMat4x3Mul(&m1, &m2, &m1);
+
+            tranresult[2].x = 0.0f;
+            tranresult[2].y = 0.0f;
+            tranresult[2].z = 0.0f;
+            quatresult[2].v.x = 0.0f;
+            quatresult[2].v.y = 0.0f;
+            quatresult[2].v.z = 0.0f;
+            quatresult[2].s = 1.0f;
+
+            xQuatFromMat(&quatresult[1], &m1);
+
+            quatresult[1].s = -quatresult[1].s;
+            m1.pos = tranresult[1];
+        }
+
+        U32 numbone = iModelNumBones(model);
+        U32 boneidx = 0;
+        xQuat* qqq = quatresult;
+        xVec3* ttt = tranresult;
+        while (boneidx < numbone && boneidx <= ahdr->RootIndex)
+        {
+            animMat->pos.x += tranresult[boneidx].x;
+            animMat->pos.y += tranresult[boneidx].y;
+            animMat->pos.z += tranresult[boneidx].z;
+            
+            tranresult[boneidx].x = 0.0f;
+            tranresult[boneidx].y = 0.0f;
+            tranresult[boneidx].z = 0.0f;
+            
+            if (FABS(quatresult[boneidx].s) < 0.9999f)
+            {
+                break;
+            }
+
+            boneidx++;
+        }
+
+        iModelAnimMatrices(model, quatresult, tranresult, &animMat[1]);
+    }
+    else
+    {
+        xQuatToMat(quatresult, (xMat4x3*)animMat);
+        animMat->pos.x = tranresult[0].x + ahdr->Translate[0];
+        animMat->pos.y = tranresult[0].y + ahdr->Translate[1];
+        animMat->pos.z = tranresult[0].z + ahdr->Translate[2];
+    }
+}
+
 void xVec3Lerp(xVec3* out, const xVec3* a, const xVec3* b, float alpha)
 {
     out->x = a->x + (b->x - a->x) * alpha;