From 75abedc711b9e57d0ebd3f5d5fdba5c5903c2b9d Mon Sep 17 00:00:00 2001
From: Kawe Mazidjatari <48657826+Mauler125@users.noreply.github.com>
Date: Fri, 8 Jul 2022 16:25:20 +0200
Subject: [PATCH] New custom math functions

* AngleCompose (compose angle from 2 angles).
* AngleLerp (linearly interpolate first angle to second).
* AngleInverse (invert input angle).
---
 r5dev/mathlib/mathlib.h        |  3 +++
 r5dev/mathlib/mathlib_base.cpp | 44 ++++++++++++++++++++++++++++++++++
 2 files changed, 47 insertions(+)

diff --git a/r5dev/mathlib/mathlib.h b/r5dev/mathlib/mathlib.h
index ad11b2f6..197b0b5c 100644
--- a/r5dev/mathlib/mathlib.h
+++ b/r5dev/mathlib/mathlib.h
@@ -1214,6 +1214,9 @@ int InsideOut(int nTotal, int nCounter);
 // FIXME: Vector versions.... the float versions will go away hopefully soon!
 //-----------------------------------------------------------------------------
 
+void AngleCompose(const QAngle& a1, const QAngle& a2, QAngle& out);
+void AngleLerp(const QAngle& a1, const QAngle& a2, float t, QAngle& out);
+void AngleInverse(const QAngle& angles, QAngle& out);
 void AngleVectors(const QAngle& angles, Vector3D* forward);
 void AngleVectors(const QAngle& angles, Vector3D* forward, Vector3D* right, Vector3D* up);
 void AngleVectorsTranspose(const QAngle& angles, Vector3D* forward, Vector3D* right, Vector3D* up);
diff --git a/r5dev/mathlib/mathlib_base.cpp b/r5dev/mathlib/mathlib_base.cpp
index 0d4ce678..a9894eaf 100644
--- a/r5dev/mathlib/mathlib_base.cpp
+++ b/r5dev/mathlib/mathlib_base.cpp
@@ -1002,6 +1002,50 @@ int __cdecl BoxOnPlaneSide(const float* emins, const float* emaxs, const cplane_
 	return sides;
 }
 
+//-----------------------------------------------------------------------------
+// Euler QAngle -> Euler QAngle Composed
+//-----------------------------------------------------------------------------
+
+void AngleCompose(const QAngle& a1, const QAngle& a2, QAngle& out)
+{
+	Quaternion q1, q2, q3;
+
+	AngleQuaternion(a1, q1);
+	AngleQuaternion(a2, q2);
+
+	QuaternionMult(q1, q2, q3);
+	QuaternionAngles(q3, out);
+}
+
+//-----------------------------------------------------------------------------
+// Euler QAngle -> Euler QAngle Lerped
+//-----------------------------------------------------------------------------
+
+void AngleLerp(const QAngle& a1, const QAngle& a2, float t, QAngle& out)
+{
+	Quaternion q1, q2, q3;
+
+	AngleQuaternion(a1, q1);
+	AngleQuaternion(a2, q2);
+
+	QuaternionSlerp(q1, q2, t, q3);
+	QuaternionAngles(q3, out);
+}
+
+//-----------------------------------------------------------------------------
+// Euler QAngle -> Euler QAngle Inverted
+//-----------------------------------------------------------------------------
+
+void AngleInverse(const QAngle& angles, QAngle& out)
+{
+	Quaternion q1, q2;
+
+	AngleQuaternion(angles, q1);
+
+	QuaternionInvert(q1, q2);
+	QuaternionAngles(q2, out);
+}
+
 //-----------------------------------------------------------------------------
 // Euler QAngle -> Basis Vectors
 //-----------------------------------------------------------------------------