From ee8fa9b0e11c1e790e03d1e4076342169369a92a Mon Sep 17 00:00:00 2001
From: LHoG <1476261+lhog@users.noreply.github.com>
Date: Sun, 9 Feb 2025 19:26:09 +0100
Subject: [PATCH] Actually use quaternions for CSolidObject::UpdateDirVectors()
 and CSolidObject::SetHeadingFromDirection()

---
 rts/Sim/Objects/SolidObject.cpp | 38 ++++++++++-----------------------
 rts/System/Quaternion.cpp       | 28 +++++++++++++++++++++++-
 rts/System/Quaternion.h         |  9 ++++++++
 3 files changed, 47 insertions(+), 28 deletions(-)

diff --git a/rts/Sim/Objects/SolidObject.cpp b/rts/Sim/Objects/SolidObject.cpp
index b5ad624b34..7da3ae545b 100644
--- a/rts/Sim/Objects/SolidObject.cpp
+++ b/rts/Sim/Objects/SolidObject.cpp
@@ -11,6 +11,7 @@
 #include "Sim/MoveTypes/MoveDefHandler.h"
 #include "Game/GameHelper.h"
 #include "System/SpringMath.h"
+#include "System/Quaternion.h"
 
 #include "System/Misc/TracyDefs.h"
 
@@ -380,14 +381,11 @@ void CSolidObject::SetHeadingFromDirection() {
 	// undo UpdateDirVectors transformation
 
 	// construct quaternion to describe rotation from uDir to UpVector
-	float4 quat{ -updir.z, 0.0f, updir.x, 1.0f + updir.y }; // same angle as in UpdateDirVectors, but inverted axis
-	float qN = quat.SqLength() + quat.w * quat.w;
-	quat *= math::isqrt(qN);
+	CQuaternion quat(-updir.z, 0.0f, updir.x, 1.0f + updir.y); // same angle as in UpdateDirVectors, but inverted axis
+	quat.ANormalize();
 
-	const float3 fDir =
-		2.0f * quat.dot(frontdir) * static_cast<float3>(quat) +
-		(quat.w * quat.w - quat.dot(quat)) * frontdir +
-		2.0f * quat.w * quat.cross(frontdir);
+	const float3 fDir = quat * frontdir;
+	assert(epscmp(fDir.y, 0.0f, float3::cmp_eps()));
 
 	heading = GetHeadingFromVector(fDir.x, fDir.z);
 }
@@ -408,27 +406,13 @@ void CSolidObject::UpdateDirVectors(const float3& uDir)
 	const float3 rDir = float3{ -fDir.z, 0.0f, fDir.x };
 
 	// construct quaternion to describe rotation from UpVector to uDir
+	// can use CQuaternion::MakeFrom(const float3& v1, const float3& v2);
+	// but simplified given UpVector is trivial
+	CQuaternion quat(uDir.z, 0.0f, -uDir.x, 1.0f + uDir.y);
+	quat.ANormalize();
 
-	// https://raw.org/proof/quaternion-from-two-vectors/
-	// const float dp = UpVector.dot(uDir);
-	// const float3 axis = UpVector.cross(uDir);
-	// float4 quat { axis, 1.0f + dp };
-
-	// same as above, but simplified given UpVector is trivial
-	float4 quat{ uDir.z, 0.0f, -uDir.x, 1.0f + uDir.y };
-	float qN = quat.SqLength() + quat.w * quat.w;
-	quat *= math::isqrt(qN);
-
-	frontdir =
-		2.0f * quat.dot(fDir) * static_cast<float3>(quat) +
-		(quat.w * quat.w - quat.dot(quat)) * fDir +
-		2.0f * quat.w * quat.cross(fDir);
-
-	rightdir =
-		2.0f * quat.dot(rDir) * static_cast<float3>(quat) +
-		(quat.w * quat.w - quat.dot(quat)) * rDir +
-		2.0f * quat.w * quat.cross(rDir);
-
+	frontdir = quat * fDir;
+	rightdir = quat * rDir;
 	updir = uDir;
 }
 
diff --git a/rts/System/Quaternion.cpp b/rts/System/Quaternion.cpp
index 8191ec9a29..f306b1215f 100644
--- a/rts/System/Quaternion.cpp
+++ b/rts/System/Quaternion.cpp
@@ -259,7 +259,18 @@ CQuaternion& CQuaternion::Normalize()
 	if unlikely(sqn < float3::nrm_eps())
 		return *this;
 
-	*this *= InvSqrt(sqn);
+	*this /= math::sqrt(sqn);
+
+	return *this;
+}
+
+CQuaternion& CQuaternion::ANormalize()
+{
+	const float sqn = SqNorm();
+	if unlikely(sqn < float3::nrm_eps())
+		return *this;
+
+	*this *= math::isqrt(sqn);
 
 	return *this;
 }
@@ -387,6 +398,21 @@ CQuaternion& CQuaternion::operator*=(float f)
 	return *this;
 }
 
+CQuaternion& CQuaternion::operator/=(float f)
+{
+	if unlikely(epscmp(f, 0.0f, float3::cmp_eps()))
+		return *this;
+
+	f = 1.0f / f;
+
+	x *= f;
+	y *= f;
+	z *= f;
+	r *= f;
+
+	return *this;
+}
+
 float CQuaternion::SqNorm() const {
 	return (x * x + y * y + z * z + r * r);
 }
diff --git a/rts/System/Quaternion.h b/rts/System/Quaternion.h
index e915855e8b..50726a9dda 100644
--- a/rts/System/Quaternion.h
+++ b/rts/System/Quaternion.h
@@ -62,6 +62,7 @@ class alignas(16) CQuaternion
 public:
 	bool Normalized() const;
 	CQuaternion& Normalize();
+	CQuaternion& ANormalize();
 	constexpr CQuaternion& Conjugate() { x = -x; y = -y; z = -z; return *this; }
 	CQuaternion  Inverse() const;
 	CQuaternion& InverseInPlace();
@@ -94,8 +95,16 @@ class alignas(16) CQuaternion
 		return CQuaternion(x - rhs.x, y - rhs.y, z - rhs.z, r - rhs.r);
 	}
 
+	float3 operator*(const float3& arg) const {
+		return Rotate(arg);
+	}
+	float4 operator*(const float4& arg) const {
+		return Rotate(arg);
+	}
+
 	CQuaternion operator*(const CQuaternion& rhs) const;
 	CQuaternion& operator*=(float f);
+	CQuaternion& operator/=(float f);
 
 	bool operator==(const CQuaternion& rhs) const { return  equals(rhs); } //aproximate
 	bool operator!=(const CQuaternion& rhs) const { return !equals(rhs); } //aproximate