From eeda9c7738b729f8ef282e831a484f9cd958c1c1 Mon Sep 17 00:00:00 2001
From: Megamouse <studienricky89@googlemail.com>
Date: Sun, 8 Mar 2026 13:56:49 +0100
Subject: [PATCH] cellGem: improve quaternion rotation

rotate_vector caused wrong results for default_orientation and 0,0,x
---
 rpcs3/Emu/Io/ps_move_data.cpp | 46 +++++++++++++++++------------------
 rpcs3/Emu/Io/ps_move_data.h   | 23 ++++++++++++++++++
 2 files changed, 46 insertions(+), 23 deletions(-)

diff --git a/rpcs3/Emu/Io/ps_move_data.cpp b/rpcs3/Emu/Io/ps_move_data.cpp
index 0a167eed39..bf35f26a19 100644
--- a/rpcs3/Emu/Io/ps_move_data.cpp
+++ b/rpcs3/Emu/Io/ps_move_data.cpp
@@ -24,34 +24,34 @@ void ps_move_data::reset_sensors()
 	angaccel_world = {};
 }
 
+ps_move_data::vect<3> ps_move_data::rotate_vector(const vect<4>& q, const vect<3>& v)
+{
+	const auto cross = [](const vect<3>& a, const vect<3>& b)
+	{
+		return vect<3>({
+			a.y() * b.z() - a.z() * b.y(),
+			a.z() * b.x() - a.x() * b.z(),
+			a.x() * b.y() - a.y() * b.x()
+		});
+	};
+
+	// q = (x, y, z, w)
+	const vect<3> q_vec({q.x(), q.y(), q.z()});
+
+	// t = 2 * cross(q_vec, v)
+	const vect<3> t = cross(q_vec, v) * 2.0f;
+
+	// v' = v + w * t + cross(q_vec, t)
+	const vect<3> v_prime = v + t * q.w() + cross(q_vec, t);
+
+	return v_prime;
+}
+
 void ps_move_data::update_orientation(f32 delta_time)
 {
 	if (!delta_time)
 		return;
 
-	// Rotate vector v by quaternion q
-	const auto rotate_vector = [](const vect<4>& q, const vect<3>& v)
-	{
-		const vect<4> qv({0.0f, v.x(), v.y(), v.z()});
-		const vect<4> q_inv({q.w(), -q.x(), -q.y(), -q.z()});
-
-		// t = q * v
-		vect<4> t;
-		t.w() = -q.x() * qv.x() - q.y() * qv.y() - q.z() * qv.z();
-		t.x() =  q.w() * qv.x() + q.y() * qv.z() - q.z() * qv.y();
-		t.y() =  q.w() * qv.y() - q.x() * qv.z() + q.z() * qv.x();
-		t.z() =  q.w() * qv.z() + q.x() * qv.y() - q.y() * qv.x();
-
-		// r = t * q_inv
-		vect<4> r;
-		r.w() = -t.x() * q_inv.x() - t.y() * q_inv.y() - t.z() * q_inv.z();
-		r.x() =  t.w() * q_inv.x() + t.y() * q_inv.z() - t.z() * q_inv.y();
-		r.y() =  t.w() * q_inv.y() - t.x() * q_inv.z() + t.z() * q_inv.x();
-		r.z() =  t.w() * q_inv.z() + t.x() * q_inv.y() - t.y() * q_inv.x();
-
-		return vect<3>({r.x(), r.y(), r.z()});
-	};
-
 	if constexpr (use_imu_for_velocity)
 	{
 		// Gravity in world frame
diff --git a/rpcs3/Emu/Io/ps_move_data.h b/rpcs3/Emu/Io/ps_move_data.h
index 1ae30f5c66..2470e47e8c 100644
--- a/rpcs3/Emu/Io/ps_move_data.h
+++ b/rpcs3/Emu/Io/ps_move_data.h
@@ -15,6 +15,26 @@ struct ps_move_data
 		template <typename I>
 		const T& operator[](I i) const { return data[i]; }
 
+		vect<Size, T> operator*(f32 s) const
+		{
+			vect<Size, T> result = *this;
+			for (int i = 0; i < Size; ++i)
+			{
+				result[i] *= s;
+			}
+			return result;
+		}
+
+		vect<Size, T> operator+(const vect<Size, T>& other) const
+		{
+			vect<Size, T> result = *this;
+			for (int i = 0; i < Size; ++i)
+			{
+				result[i] += other[i];
+			}
+			return result;
+		}
+
 		T x() const requires (Size >= 1) { return data[0]; }
 		T y() const requires (Size >= 2) { return data[1]; }
 		T z() const requires (Size >= 3) { return data[2]; }
@@ -72,4 +92,7 @@ struct ps_move_data
 	void reset_sensors();
 	void update_orientation(f32 delta_time);
 	void update_velocity(u64 timestamp, be_t<f32> pos_world[4]);
+
+	// Rotate vector v by quaternion q
+	static vect<3> rotate_vector(const vect<4>& q, const vect<3>& v);
 };