From 709297133191c13f1aca0aa8bb6efabdf0901683 Mon Sep 17 00:00:00 2001
From: Shiny Metagross <30511800+ShinyMetagross@users.noreply.github.com>
Date: Fri, 28 Oct 2022 12:52:10 -0700
Subject: [PATCH] Bone manipulation updates

- Factored in parent bone rotations to check if a bone needs updating
- Implemented multiply Quaternion functions to TVector4
- Converted Euler rotations in A_ManipulateBone to degrees
---
 src/common/models/models_iqm.cpp  | 14 +++++++++++---
 src/common/utility/TRS.h          |  7 +++++++
 src/common/utility/vectors.h      | 19 +++++++++++++++++++
 src/playsim/p_actionfunctions.cpp | 16 ++++++++++------
 4 files changed, 47 insertions(+), 9 deletions(-)

diff --git a/src/common/models/models_iqm.cpp b/src/common/models/models_iqm.cpp
index 521a545abc..fc4d8123b1 100644
--- a/src/common/models/models_iqm.cpp
+++ b/src/common/models/models_iqm.cpp
@@ -540,6 +540,7 @@ const TArray<VSMatrix> IQMModel::CalculateBones(int frame1, int frame2, double i
 	swapYZ[3 + 3 * 4] = 1.0f;
 
 	TArray<VSMatrix> bones(numbones, true);
+	TArray<bool> modifiedBone(numbones, true);
 	for (int i = 0; i < numbones; i++)
 	{
 		TRS bone;
@@ -551,14 +552,14 @@ const TArray<VSMatrix> IQMModel::CalculateBones(int frame1, int frame2, double i
 			if (i < actor->boneManipulationData->boneComponentsOld.Size())
 			{
 				from.translation += actor->boneManipulationData->boneComponentsOld[i].translation;
-				from.rotation += actor->boneManipulationData->boneComponentsOld[i].rotation;
+				from.rotation *= actor->boneManipulationData->boneComponentsOld[i].rotation;
 				from.scaling += actor->boneManipulationData->boneComponentsOld[i].scaling;
 			}
 
 			if (i < actor->boneManipulationData->boneComponentsNew.Size())
 			{
 				to.translation += actor->boneManipulationData->boneComponentsNew[i].translation;
-				to.rotation += actor->boneManipulationData->boneComponentsNew[i].rotation;
+				to.rotation *= actor->boneManipulationData->boneComponentsNew[i].rotation;
 				to.scaling += actor->boneManipulationData->boneComponentsNew[i].scaling;
 			}
 		}
@@ -573,14 +574,21 @@ const TArray<VSMatrix> IQMModel::CalculateBones(int frame1, int frame2, double i
 		bone.rotation.MakeUnit();
 		bone.scaling = from.scaling * invt + to.scaling * t;
 
-		if (actor->boneComponentData->trscomponents[i].Equals(bone))
+		if (Joints[i].Parent >= 0 && modifiedBone[Joints[i].Parent])
+		{
+			actor->boneComponentData->trscomponents[i] = bone;
+			modifiedBone[i] = true;
+		}
+		else if (actor->boneComponentData->trscomponents[i].Equals(bone))
 		{
 			bones[i] = actor->boneComponentData->trsmatrix[i];
+			modifiedBone[i] = false;
 			continue;
 		}
 		else
 		{
 			actor->boneComponentData->trscomponents[i] = bone;
+			modifiedBone[i] = true;
 		}
 
 		VSMatrix m;
diff --git a/src/common/utility/TRS.h b/src/common/utility/TRS.h
index d94e262fb2..c39a870ad3 100644
--- a/src/common/utility/TRS.h
+++ b/src/common/utility/TRS.h
@@ -41,6 +41,13 @@ public:
 	FVector4 rotation;
 	FVector3 scaling;
 
+	TRS()
+	{
+		translation = FVector3(0,0,0);
+		rotation = FVector4(0,0,0,1);
+		scaling = FVector3(0,0,0);
+	}
+
 	bool Equals(TRS& compare)
 	{
 		return compare.translation == this->translation && compare.rotation == this->rotation && compare.scaling == this->scaling;
diff --git a/src/common/utility/vectors.h b/src/common/utility/vectors.h
index 52cbdadf3b..e65e69607d 100644
--- a/src/common/utility/vectors.h
+++ b/src/common/utility/vectors.h
@@ -833,6 +833,25 @@ struct TVector4
 		return TVector4(v.X * scalar, v.Y * scalar, v.Z * scalar, v.W * scalar);
 	}
 
+	// Multiply as Quaternion
+	TVector4& operator*= (const TVector4& v)
+	{
+		X = v.W * X + v.X * W + v.Y * Z - v.Z * Y;
+		Y = v.W * Y + v.Y * W + v.Z * X - v.X * Z;
+		Z = v.W * Z + v.Z * W + v.X * Y - v.Y * X;
+		W = v.W * W - v.X * X - v.Y * Y - v.Z * Z;
+		return *this;
+	}
+
+	friend TVector4 operator* (const TVector4& v1, const TVector4& v2)
+	{
+		return TVector4(v2.W * v1.X + v2.X * v1.W + v2.Y * v1.Z - v1.Z * v1.Y,
+			v2.W * v1.Y + v2.Y * v1.W + v2.Z * v1.X - v2.X * v1.Z,
+			v2.W * v1.Z + v2.Z * v1.W + v2.X * v1.Y - v2.Y * v1.X,
+			v2.W * v1.W - v2.X * v1.X - v2.Y * v1.Y - v2.Z * v1.Z
+		);
+	}
+
 	// Scalar division
 	TVector4 &operator/= (vec_t scalar)
 	{
diff --git a/src/playsim/p_actionfunctions.cpp b/src/playsim/p_actionfunctions.cpp
index 0e94ae5dd9..af0bfb57ea 100644
--- a/src/playsim/p_actionfunctions.cpp
+++ b/src/playsim/p_actionfunctions.cpp
@@ -5246,12 +5246,16 @@ DEFINE_ACTION_FUNCTION(AActor, A_ManipulateBone)
 
 	if (flags & BM_USEEULER)
 	{
-		double cr = cos(rotationZ * 0.5);
-		double sr = sin(rotationZ * 0.5);
-		double cp = cos(rotationY * 0.5);
-		double sp = sin(rotationY * 0.5);
-		double cy = cos(rotationX * 0.5);
-		double sy = sin(rotationX * 0.5);
+		rotationX = rotationX * 3.14159265359f / 180.0f;
+		rotationY = rotationY * 3.14159265359f / 180.0f;
+		rotationZ = rotationZ * 3.14159265359f / 180.0f;
+
+		double cr = cos(rotationX * 0.5);
+		double sr = sin(rotationX * 0.5);
+		double cp = cos(rotationZ * 0.5);
+		double sp = sin(rotationZ * 0.5);
+		double cy = cos(rotationY * 0.5);
+		double sy = sin(rotationY * 0.5);
 
 		rotationW = cr * cp * cy + sr * sp * sy;
 		rotationX = sr * cp * cy - cr * sp * sy;