Match Vector4::NormalizeQuaternion

isledecomp · Dec 25, 2024 · d5a6a84 · d5a6a84
1 parent 9263cf8
commit d5a6a84
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 18 deletions.
diff --git a/LEGO1/library_msvc.h b/LEGO1/library_msvc.h
@@ -675,6 +675,9 @@
 // LIBRARY: BETA10 0x100f9a50
 // _sin
 
+// LIBRARY: BETA10 0x100f9a5a
+// _cos
+
 // LIBRARY: BETA10 0x100f95d0
 // _sqrt
 

diff --git a/LEGO1/realtime/vector.h b/LEGO1/realtime/vector.h
@@ -467,38 +467,36 @@ class Vector4 : public Vector3 {
 // FUNCTION: BETA10 0x10048ad0
 inline int Vector4::NormalizeQuaternion()
 {
-	float* v = m_data;
-	float magnitude = v[0] * v[0] + v[2] * v[2] + v[1] * v[1];
-	if (magnitude > 0.0f) {
-		float theta = v[3] * 0.5f;
-		v[3] = cos(theta);
-		magnitude = sin(theta) / sqrt(magnitude);
-		Vector3::MulImpl(magnitude);
-		return 0;
-	}
+	float length = m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2];
 
-	return -1;
-}
+	if (length > 0.0f) {
+		float theta = m_data[3] * 0.5f;
+		float magnitude = sin((double) theta);
+		m_data[3] = cos((double) theta);
 
-inline static float QuaternionProductScalarPart(const float* bDat, const float* aDat)
-{
-	// We have no indication from the beta that this function exists,
-	// but it helps with the stack layout of Vector4::EqualsHamiltonProduct()
-	return aDat[3] * bDat[3] - (aDat[0] * bDat[0] + aDat[2] * bDat[2] + aDat[1] * bDat[1]);
+		magnitude = magnitude / (float) sqrt((double) length);
+		m_data[0] *= magnitude;
+		m_data[1] *= magnitude;
+		m_data[2] *= magnitude;
+		return 0;
+	}
+	else {
+		return -1;
+	}
 }
 
 // FUNCTION: LEGO1 0x10002bf0
 // FUNCTION: BETA10 0x10048c20
 inline int Vector4::EqualsHamiltonProduct(const Vector4& p_a, const Vector4& p_b)
 {
-	m_data[3] = QuaternionProductScalarPart(p_a.m_data, p_b.m_data);
+	m_data[3] = p_a.m_data[3] * p_b.m_data[3] -
+				(p_a.m_data[0] * p_b.m_data[0] + p_a.m_data[2] * p_b.m_data[2] + p_a.m_data[1] * p_b.m_data[1]);
 
 	Vector3::EqualsCrossImpl(p_a.m_data, p_b.m_data);
 
 	m_data[0] = p_b.m_data[3] * p_a.m_data[0] + p_a.m_data[3] * p_b.m_data[0] + m_data[0];
 	m_data[1] = p_b.m_data[1] * p_a.m_data[3] + p_a.m_data[1] * p_b.m_data[3] + m_data[1];
 	m_data[2] = p_b.m_data[2] * p_a.m_data[3] + p_a.m_data[2] * p_b.m_data[3] + m_data[2];
-
 	return 0;
 }