From 6382347f731aa5a5190d3fc5c9e2074549501146 Mon Sep 17 00:00:00 2001
From: jonschz <17198703+jonschz@users.noreply.github.com>
Date: Fri, 3 May 2024 20:08:58 +0200
Subject: [PATCH] Refactor Matrix4::ToQuaternion() (#879)

* Refactor Matrix4::ToQuaternion()

* fix CI errors

* Apply feedback by @foxtacles

* run formatter

---------

Co-authored-by: jonschz <jonschz@users.noreply.github.com>
---
 LEGO1/realtime/matrix.h | 58 +++++++++++++++++++----------------------
 1 file changed, 27 insertions(+), 31 deletions(-)

diff --git a/LEGO1/realtime/matrix.h b/LEGO1/realtime/matrix.h
index 02f724b1..82c3166a 100644
--- a/LEGO1/realtime/matrix.h
+++ b/LEGO1/realtime/matrix.h
@@ -151,48 +151,44 @@ class Matrix4 {
 	float (*m_data)[4];
 };
 
-// Not close, Ghidra struggles understinging this method so it will have to
-// be manually worked out. Included since I at least figured out what it was
-// doing with rotateIndex and what overall operation it's trying to do.
-// STUB: LEGO1 0x10002550
+// FUNCTION: LEGO1 0x10002550
 inline void Matrix4::ToQuaternion(Vector4& p_outQuat)
 {
-	/*
-	float trace = m_data[0] + m_data[5] + m_data[10];
+	float trace = m_data[0][0] + m_data[1][1] + m_data[2][2];
 	if (trace > 0) {
 		trace = sqrt(trace + 1.0);
-		p_outQuat->GetData()[3] = trace * 0.5f;
-		p_outQuat->GetData()[0] = (m_data[9] - m_data[6]) * trace;
-		p_outQuat->GetData()[1] = (m_data[2] - m_data[8]) * trace;
-		p_outQuat->GetData()[2] = (m_data[4] - m_data[1]) * trace;
-		return;
+		p_outQuat[3] = trace * 0.5f;
+		trace = 0.5f / trace;
+		p_outQuat[0] = (m_data[2][1] - m_data[1][2]) * trace;
+		p_outQuat[1] = (m_data[0][2] - m_data[2][0]) * trace;
+		p_outQuat[2] = (m_data[1][0] - m_data[0][1]) * trace;
 	}
+	else {
 
-	// ~GLOBAL: LEGO1 0x100d4090
-	static int rotateIndex[] = {1, 2, 0};
+		// GLOBAL: LEGO1 0x100d4090
+		static int rotateIndex[] = {1, 2, 0};
 
-	// Largest element along the trace
-	int largest = m_data[0] < m_data[5];
-	if (*Element(largest, largest) < m_data[10])
-		largest = 2;
+		// Largest element along the trace
+		int largest = 0;
+		if (m_data[0][0] < m_data[1][1]) {
+			largest = 1;
+		}
+		if (*Element(largest, largest) < m_data[2][2]) {
+			largest = 2;
+		}
 
-	int next = rotateIndex[largest];
-	int nextNext = rotateIndex[next];
-	float valueA = *Element(nextNext, nextNext);
-	float valueB = *Element(next, next);
-	float valueC = *Element(largest, largest);
+		int next = rotateIndex[largest];
+		int nextNext = rotateIndex[next];
 
-	// Above is somewhat decomped, below is pure speculation since the automatic
-	// decomp becomes very garbled.
-	float traceValue = sqrt(valueA - valueB - valueC + 1.0);
+		float trace = sqrt(*Element(largest, largest) - (*Element(nextNext, nextNext) + *Element(next, next)) + 1.0);
 
-	p_outQuat->GetData()[largest] = traceValue * 0.5f;
-	traceValue = 0.5f / traceValue;
+		p_outQuat[largest] = trace * 0.5f;
+		trace = 0.5f / trace;
 
-	p_outQuat->GetData()[3] = (m_data[next + 4 * nextNext] - m_data[nextNext + 4 * next]) * traceValue;
-	p_outQuat->GetData()[next] = (m_data[next + 4 * largest] + m_data[largest + 4 * next]) * traceValue;
-	p_outQuat->GetData()[nextNext] = (m_data[nextNext + 4 * largest] + m_data[largest + 4 * nextNext]) * traceValue;
-	*/
+		p_outQuat[3] = (*Element(nextNext, next) - *Element(next, nextNext)) * trace;
+		p_outQuat[next] = (*Element(largest, next) + *Element(next, largest)) * trace;
+		p_outQuat[nextNext] = (*Element(largest, nextNext) + *Element(nextNext, largest)) * trace;
+	}
 }
 
 // FUNCTION: LEGO1 0x10002710