From e429379bc34a6de38bab59494467a05fef580822 Mon Sep 17 00:00:00 2001
From: TokyoSU <77746747+TokyoSU@users.noreply.github.com>
Date: Tue, 15 Mar 2022 20:05:30 +0100
Subject: [PATCH] Decompiled Move3DPosTo3DPos()

Finished collide.cpp.
PS: GetCollisionInfo() was already decompiled by @asasas9500 on PR #27
---
 game/collide.cpp | 58 +++++++++++++++++++++++++++++++++++++++++++++++-
 game/collide.h   |  2 +-
 2 files changed, 58 insertions(+), 2 deletions(-)

diff --git a/game/collide.cpp b/game/collide.cpp
index d4c0b7b..cbb2698 100644
--- a/game/collide.cpp
+++ b/game/collide.cpp
@@ -591,6 +591,62 @@ BOOL __cdecl MoveLaraPosition(PHD_VECTOR* pos, ITEM_INFO* item, ITEM_INFO* larai
 	return distance < 128 || Move3DPosTo3DPos(&laraitem->pos, &newpos, 16, 2 * PHD_DEGREE);
 }
 
+BOOL __cdecl Move3DPosTo3DPos(PHD_3DPOS* src, PHD_3DPOS* dest, int velocity, __int16 angleAdder) {
+	int x, y, z, distance;
+	short xRot, yRot, zRot;
+
+	x = dest->x - src->x;
+	y = dest->y - src->y;
+	z = dest->z - src->z;
+	distance = phd_sqrt(SQR(z) + SQR(y) + SQR(x));
+	if (velocity < distance) {
+		src->x += velocity * x / distance;
+		src->y += velocity * y / distance;
+		src->z += velocity * z / distance;
+	} else {
+		src->x = dest->x;
+		src->y = dest->y;
+		src->z = dest->z;
+	}
+
+	xRot = dest->rotX - src->rotX;
+	yRot = dest->rotY - src->rotY;
+	zRot = dest->rotZ - src->rotZ;
+	if (xRot <= angleAdder) {
+		if (xRot >= -angleAdder) {
+			src->rotX = dest->rotX;
+		} else {
+			src->rotX -= angleAdder;
+		}
+	} else {
+		src->rotX += angleAdder;
+	}
+	if (yRot <= angleAdder) {
+		if (yRot >= -angleAdder) {
+			src->rotY = dest->rotY;
+		} else {
+			src->rotY -= angleAdder;
+		}
+	} else {
+		src->rotY += angleAdder;
+	}
+	if (zRot <= angleAdder) {
+		if (zRot >= -angleAdder) {
+			src->rotZ = dest->rotZ;
+		} else {
+			src->rotZ -= angleAdder;
+		}
+	} else {
+		src->rotZ += angleAdder;
+	}
+	return src->x == dest->x
+		&& src->y == dest->y
+		&& src->z == dest->z
+		&& src->rotX == dest->rotX
+		&& src->rotY == dest->rotY
+		&& src->rotZ == dest->rotZ;
+}
+
 /*
  * Inject function
  */
@@ -616,5 +672,5 @@ void Inject_Collide() {
 	INJECT(0x00413DF0, TestLaraPosition);
 	INJECT(0x00413F30, AlignLaraPosition);
 	INJECT(0x00414070, MoveLaraPosition);
-//	INJECT(0x00414200, Move3DPosTo3DPos);
+	INJECT(0x00414200, Move3DPosTo3DPos);
 }
diff --git a/game/collide.h b/game/collide.h
index 4729c78..40b1865 100644
--- a/game/collide.h
+++ b/game/collide.h
@@ -52,6 +52,6 @@ BOOL __cdecl TestBoundsCollide(ITEM_INFO* item, ITEM_INFO* laraitem, int radius)
 BOOL __cdecl TestLaraPosition(__int16* bounds, ITEM_INFO* item, ITEM_INFO* laraitem); // 0x00413DF0
 void __cdecl AlignLaraPosition(PHD_VECTOR* pos, ITEM_INFO* item, ITEM_INFO* laraitem); // 0x00413F30
 BOOL __cdecl MoveLaraPosition(PHD_VECTOR* pos, ITEM_INFO* item, ITEM_INFO* laraitem); // 0x00414070
-#define Move3DPosTo3DPos ((BOOL(__cdecl*)(PHD_3DPOS*, PHD_3DPOS*, int, __int16)) 0x00414200)
+BOOL __cdecl Move3DPosTo3DPos(PHD_3DPOS* src, PHD_3DPOS* dest, int velocity, __int16 angleAdder); // 0x00414200
 
 #endif // COLLIDE_H_INCLUDED