1 files changed, 69 insertions, 24 deletions

diff --git a/src/code/code_800430A0.c b/src/code/code_800430A0.c
index 29eaae1e1..9673fc86a 100644
--- a/src/code/code_800430A0.c
+++ b/src/code/code_800430A0.c
@@ -1,23 +1,64 @@
 #include "global.h"
+#include "vt.h"
 
-#pragma GLOBAL_ASM("asm/non_matchings/code/code_800430A0/func_800430A0.s")
+void func_800430A0(CollisionContext* colCtx, s32 bgId, Actor* actor) {
+    MtxF prevTransform;
+    MtxF prevTransformInv;
+    MtxF curTransform;
+    Vec3f pos;
+    Vec3f tempPos;
 
-void func_800432A0(CollisionContext* colCtx, u32 floorPolySource, Actor* actor) {
-    if (func_8003E934(floorPolySource) != 0) {
-        s16 v1 = colCtx->dyna.actorMeshArr[floorPolySource].rot2.y - colCtx->dyna.actorMeshArr[floorPolySource].rot1.y;
+    if (DynaPoly_IsBgIdBgActor(bgId)) {
+        SkinMatrix_SetScaleRotateYRPTranslate(
+            &prevTransform, colCtx->dyna.bgActors[bgId].prevTransform.scale.x,
+            colCtx->dyna.bgActors[bgId].prevTransform.scale.y, colCtx->dyna.bgActors[bgId].prevTransform.scale.z,
+            colCtx->dyna.bgActors[bgId].prevTransform.rot.x, colCtx->dyna.bgActors[bgId].prevTransform.rot.y,
+            colCtx->dyna.bgActors[bgId].prevTransform.rot.z, colCtx->dyna.bgActors[bgId].prevTransform.pos.x,
+            colCtx->dyna.bgActors[bgId].prevTransform.pos.y, colCtx->dyna.bgActors[bgId].prevTransform.pos.z);
+        if (SkinMatrix_Invert(&prevTransform, &prevTransformInv) != 2) {
+            SkinMatrix_SetScaleRotateYRPTranslate(
+                &curTransform, colCtx->dyna.bgActors[bgId].curTransform.scale.x,
+                colCtx->dyna.bgActors[bgId].curTransform.scale.y, colCtx->dyna.bgActors[bgId].curTransform.scale.z,
+                colCtx->dyna.bgActors[bgId].curTransform.rot.x, colCtx->dyna.bgActors[bgId].curTransform.rot.y,
+                colCtx->dyna.bgActors[bgId].curTransform.rot.z, colCtx->dyna.bgActors[bgId].curTransform.pos.x,
+                colCtx->dyna.bgActors[bgId].curTransform.pos.y, colCtx->dyna.bgActors[bgId].curTransform.pos.z);
+            SkinMatrix_Vec3fMtxFMultXYZ(&prevTransformInv, &actor->posRot.pos, &tempPos);
+            SkinMatrix_Vec3fMtxFMultXYZ(&curTransform, &tempPos, &pos);
+            actor->posRot.pos = pos;
+            if (BGCHECK_XYZ_ABSMAX <= pos.x || pos.x <= -BGCHECK_XYZ_ABSMAX || BGCHECK_XYZ_ABSMAX <= pos.y ||
+                pos.y <= -BGCHECK_XYZ_ABSMAX || BGCHECK_XYZ_ABSMAX <= pos.z || pos.z <= -BGCHECK_XYZ_ABSMAX) {
 
-        if (actor->id == 0) {
-            ((Player*)actor)->currentYaw += v1;
+                osSyncPrintf(VT_FGCOL(RED));
+                // @bug file and line are not passed to osSyncPrintf
+                // Position is not valid
+                osSyncPrintf(
+                    "BGCheckCollection_typicalActorPos():位置が妥当ではありません。\npos (%f,%f,%f) file:%s line:%d\n",
+                    pos.x, pos.y, pos.z);
+                osSyncPrintf(VT_RST);
+            }
+        }
+    }
+}
+
+/**
+ * Rotate actor
+ */
+void func_800432A0(CollisionContext* colCtx, s32 bgId, Actor* actor) {
+    if (DynaPoly_IsBgIdBgActor(bgId)) {
+        s16 rot = colCtx->dyna.bgActors[bgId].curTransform.rot.y - colCtx->dyna.bgActors[bgId].prevTransform.rot.y;
+
+        if (actor->id == ACTOR_PLAYER) {
+            ((Player*)actor)->currentYaw += rot;
         }
 
-        actor->shape.rot.y += v1;
-        actor->posRot.rot.y += v1;
+        actor->shape.rot.y += rot;
+        actor->posRot.rot.y += rot;
     }
 }
 
-void func_80043334(CollisionContext* colCtx, Actor* actor, u32 floorPolySource) {
-    if (func_8003E934(floorPolySource) != 0) {
-        DynaPolyActor* dynaActor = DynaPolyInfo_GetActor(colCtx, floorPolySource);
+void func_80043334(CollisionContext* colCtx, Actor* actor, s32 bgId) {
+    if (DynaPoly_IsBgIdBgActor(bgId)) {
+        DynaPolyActor* dynaActor = DynaPoly_GetActor(colCtx, bgId);
         if (dynaActor != NULL) {
             func_800434A8(dynaActor);
 
@@ -28,33 +69,37 @@ void func_80043334(CollisionContext* colCtx, Actor* actor, u32 floorPolySource)
     }
 }
 
-s32 func_800433A4(CollisionContext* colCtx, u32 floorPolySource, Actor* actor) {
-    s32 sp24 = 0;
+/**
+ * Transform actor's position
+ * `actor` is the actor to update
+ */
+s32 func_800433A4(CollisionContext* colCtx, s32 bgId, Actor* actor) {
+    s32 result = false;
     DynaPolyActor* dynaActor;
 
-    if (func_8003E934(floorPolySource) == 0) {
-        return 0;
+    if (DynaPoly_IsBgIdBgActor(bgId) == false) {
+        return false;
     }
 
-    if ((colCtx->dyna.flags[floorPolySource] & 2) || !(colCtx->dyna.flags[floorPolySource] & 1)) {
-        return 0;
+    if ((colCtx->dyna.bgActorFlags[bgId] & 2) || !(colCtx->dyna.bgActorFlags[bgId] & 1)) {
+        return false;
     }
 
-    dynaActor = DynaPolyInfo_GetActor(colCtx, floorPolySource);
+    dynaActor = (DynaPolyActor*)DynaPoly_GetActor(colCtx, bgId);
 
     if (dynaActor == NULL) {
-        return 0;
+        return false;
     }
 
     if (dynaActor->unk_15C & 1) {
-        func_800430A0(colCtx, floorPolySource, actor);
-        sp24 = 1;
+        func_800430A0(colCtx, bgId, actor);
+        result = true;
     }
 
     if (dynaActor->unk_15C & 2) {
-        func_800432A0(colCtx, floorPolySource, actor);
-        sp24 = 1;
+        func_800432A0(colCtx, bgId, actor);
+        result = true;
     }
 
-    return sp24;
+    return result;
 }