Implement LegoAnimNodeData::GetRotation (#661)

* WIP

* Improve matches

LEGO1/lego/legoomni/include/helicopter.h

@@ -5,18 +5,6 @@
 #include "islepathactor.h"
 #include "realtime/matrix.h"
 
-// SIZE 0x34
-class HelicopterSubclass {
-public:
-	inline HelicopterSubclass() : m_unk0x30(0) {}
-	MxResult FUN_100040a0(Vector4& p_v, float p_f);
-
-private:
-	Mx4DPointFloat m_unk0x00; // 0x00
-	Mx4DPointFloat m_unk0x18; // 0x18
-	undefined4 m_unk0x30;     // 0x30
-};
-
 // VTABLE: LEGO1 0x100d40f8
 // SIZE 0x230
 class Helicopter : public IslePathActor {
@@ -52,7 +40,7 @@ class Helicopter : public IslePathActor {
 	MxMatrix m_unk0x160;              // 0x160
 	MxMatrix m_unk0x1a8;              // 0x1a8
 	float m_unk0x1f0;                 // 0x1f0
-	HelicopterSubclass m_unk0x1f4; // 0x1f4
+	UnknownMx4DPointFloat m_unk0x1f4; // 0x1f4
 	HelicopterState* m_state;         // 0x228
 	MxAtomId m_script;                // 0x22c
 

LEGO1/lego/legoomni/src/actors/helicopter.cpp

@@ -366,51 +366,3 @@ void Helicopter::VTable0x70(float p_float)
 		}
 	}
 }
-
-// FUNCTION: LEGO1 0x100040a0
-MxResult HelicopterSubclass::FUN_100040a0(Vector4& p_v, float p_f)
-{
-	MxU32 state = m_unk0x30;
-	if (state == 1) {
-		p_v.EqualsImpl(m_unk0x00.GetData());
-		p_v[3] = acos(p_v[3]) * (1 - p_f) * 2.0;
-		return p_v.NormalizeQuaternion();
-	}
-	else if (state == 2) {
-		p_v.EqualsImpl(m_unk0x18.GetData());
-		p_v[3] = acos(p_v[3]) * p_f * 2.0;
-		return p_v.NormalizeQuaternion();
-	}
-	else if (state == 3) {
-		double d1 = p_v.Dot(&m_unk0x00, &m_unk0x18), d2;
-		if (d1 + 1 > 0.00001) {
-			if (1 - d1 > 0.00001) {
-				double d = acos(d1);
-				sin(d);
-				d1 = sin((1 - p_f) * d) / sin(d);
-				d2 = sin(p_f * d) / sin(d);
-			}
-			else {
-				d1 = 1 - p_f;
-				d2 = p_f;
-			}
-			for (MxS32 i = 0; i < 4; i++) {
-				p_v[i] = m_unk0x18[i] * d2 + m_unk0x00[i] * d1;
-			}
-			return SUCCESS;
-		}
-		p_v[0] = -m_unk0x00[1];
-		p_v[1] = m_unk0x00[1];
-		p_v[2] = -m_unk0x00[3];
-		p_v[3] = m_unk0x00[2];
-		d1 = sin((1 - p_f) * 1.570796326794895);
-		d2 = sin(p_f * 1.570796326794895);
-		for (MxS32 i = 0; i < 3; i++) {
-			p_v[i] = m_unk0x00[i] * d1 + p_v[i] * d2;
-		}
-		return SUCCESS;
-	}
-	else {
-		return FAILURE;
-	}
-}

LEGO1/lego/sources/anim/legoanim.cpp

@@ -449,7 +449,7 @@ LegoResult LegoAnimNodeData::CreateLocalTransform(LegoFloat p_time, Matrix4& p_m
 
 // FUNCTION: LEGO1 0x100a0600
 inline void LegoAnimNodeData::GetTranslation(
-	LegoU32 p_numTranslationKeys,
+	LegoU16 p_numTranslationKeys,
 	LegoTranslationKey* p_translationKeys,
 	LegoFloat p_time,
 	Matrix4& p_matrix,
@@ -511,20 +511,72 @@ inline void LegoAnimNodeData::GetTranslation(
 	p_matrix.TranslateBy(&x, &y, &z);
 }
 
-// STUB: LEGO1 0x100a06f0
+// FUNCTION: LEGO1 0x100a06f0
 /*inline*/ void LegoAnimNodeData::GetRotation(
-	LegoU32 p_numRotationKeys,
+	LegoU16 p_numRotationKeys,
 	LegoRotationKey* p_rotationKeys,
 	LegoFloat p_time,
 	Matrix4& p_matrix,
 	LegoU32& p_old_index
 )
 {
-	// TODO
+	LegoU32 i, n;
+	n = FindKeys(p_time, p_numRotationKeys & USHRT_MAX, p_rotationKeys, sizeof(*p_rotationKeys), i, p_old_index);
+
+	switch (n) {
+	case 0:
+		return;
+	case 1:
+		if (p_rotationKeys[i].TestBit1()) {
+			p_matrix.FromQuaternion(Mx4DPointFloat(
+				p_rotationKeys[i].GetX(),
+				p_rotationKeys[i].GetY(),
+				p_rotationKeys[i].GetZ(),
+				p_rotationKeys[i].GetAngle()
+			));
+		}
+		break;
+	case 2:
+		Mx4DPointFloat a;
+		UnknownMx4DPointFloat b;
+
+		if (p_rotationKeys[i].TestBit1() || p_rotationKeys[i + 1].TestBit1()) {
+			a[0] = p_rotationKeys[i].GetX();
+			a[1] = p_rotationKeys[i].GetY();
+			a[2] = p_rotationKeys[i].GetZ();
+			a[3] = p_rotationKeys[i].GetAngle();
+
+			if (p_rotationKeys[i + 1].TestBit3()) {
+				p_matrix.FromQuaternion(a);
+				return;
+			}
+
+			Mx4DPointFloat c;
+			if (p_rotationKeys[i + 1].TestBit2()) {
+				c[0] = -p_rotationKeys[i + 1].GetX();
+				c[1] = -p_rotationKeys[i + 1].GetY();
+				c[2] = -p_rotationKeys[i + 1].GetZ();
+				c[3] = -p_rotationKeys[i + 1].GetAngle();
+			}
+			else {
+				c[0] = p_rotationKeys[i + 1].GetX();
+				c[1] = p_rotationKeys[i + 1].GetY();
+				c[2] = p_rotationKeys[i + 1].GetZ();
+				c[3] = p_rotationKeys[i + 1].GetAngle();
+			}
+
+			b.Unknown1(a);
+			b.Unknown2(c);
+			b.Unknown_100040a0(
+				p_matrix,
+				(p_time - p_rotationKeys[i].GetTime()) / (p_rotationKeys[i + 1].GetTime() - p_rotationKeys[i].GetTime())
+			);
+		}
+	}
 }
 
 inline void LegoAnimNodeData::GetScale(
-	LegoU32 p_numScaleKeys,
+	LegoU16 p_numScaleKeys,
 	LegoScaleKey* p_scaleKeys,
 	LegoFloat p_time,
 	Matrix4& p_matrix,

LEGO1/lego/sources/anim/legoanim.h

@@ -10,7 +10,9 @@
 class LegoAnimKey {
 public:
 	enum Flags {
-		c_bit1 = 0x01
+		c_bit1 = 0x01,
+		c_bit2 = 0x02,
+		c_bit3 = 0x04
 	};
 
 	LegoAnimKey();
@@ -18,6 +20,8 @@ class LegoAnimKey {
 	LegoFloat GetTime() { return m_time; }
 	void SetTime(LegoFloat p_time) { m_time = p_time; }
 	LegoU32 TestBit1() { return m_flags & c_bit1; }
+	LegoU32 TestBit2() { return m_flags & c_bit2; }
+	LegoU32 TestBit3() { return m_flags & c_bit3; }
 
 protected:
 	LegoU8 m_flags;   // 0x00
@@ -47,6 +51,14 @@ class LegoRotationKey : public LegoAnimKey {
 public:
 	LegoRotationKey();
 	LegoResult Read(LegoStorage* p_storage);
+	LegoFloat GetAngle() { return m_angle; }
+	void SetAngle(LegoFloat p_angle) { m_angle = p_angle; }
+	LegoFloat GetX() { return m_x; }
+	void SetX(LegoFloat p_x) { m_x = p_x; }
+	LegoFloat GetY() { return m_y; }
+	void SetY(LegoFloat p_y) { m_y = p_y; }
+	LegoFloat GetZ() { return m_z; }
+	void SetZ(LegoFloat p_z) { m_z = p_z; }
 
 protected:
 	LegoFloat m_angle; // 0x08
@@ -121,21 +133,21 @@ class LegoAnimNodeData : public LegoTreeNodeData {
 	LegoBool FUN_100a0990(LegoTime p_time) { return FUN_100a0990((LegoFloat) p_time); }
 
 	inline static void GetTranslation(
-		LegoU32 p_numTranslationKeys,
+		LegoU16 p_numTranslationKeys,
 		LegoTranslationKey* p_translationKeys,
 		LegoFloat p_time,
 		Matrix4& p_matrix,
 		LegoU32& p_old_index
 	);
 	/*inline*/ static void GetRotation(
-		LegoU32 p_numRotationKeys,
+		LegoU16 p_numRotationKeys,
 		LegoRotationKey* p_rotationKeys,
 		LegoFloat p_time,
 		Matrix4& p_matrix,
 		LegoU32& p_old_index
 	);
 	inline static void GetScale(
-		LegoU32 p_numScaleKeys,
+		LegoU16 p_numScaleKeys,
 		LegoScaleKey* p_scaleKeys,
 		LegoFloat p_time,
 		Matrix4& p_matrix,

LEGO1/library_msvc.h

@@ -18,6 +18,9 @@
 // LIBRARY: LEGO1 0x1008b020
 // ___CxxFrameHandler
 
+// LIBRARY: LEGO1 0x1008b3dc
+// __CIacos
+
 // LIBRARY: LEGO1 0x1008b400
 // _atol
 

LEGO1/mxgeometry/mxgeometry3d.h

@@ -1,6 +1,8 @@
 #ifndef MXGEOMETRY3D_H
 #define MXGEOMETRY3D_H
 
+#include "decomp.h"
+#include "realtime/matrix.h"
 #include "realtime/vector.h"
 
 // VTABLE: LEGO1 0x100d4488
@@ -15,13 +17,6 @@ class Mx3DPointFloat : public Vector3 {
 		m_elements[2] = p_z;
 	}
 
-	inline float GetX() { return m_data[0]; }
-	inline float GetY() { return m_data[1]; }
-	inline float GetZ() { return m_data[2]; }
-
-	inline float& operator[](size_t idx) { return m_data[idx]; }
-	inline const float& operator[](size_t idx) const { return m_data[idx]; }
-
 	// FUNCTION: LEGO1 0x100343a0
 	inline Mx3DPointFloat(const Mx3DPointFloat& p_other) : Vector3(m_elements) { EqualsImpl(p_other.m_data); }
 
@@ -31,6 +26,13 @@ class Mx3DPointFloat : public Vector3 {
 	// FUNCTION: LEGO1 0x10003c10
 	virtual void operator=(const Vector3& p_impl) { EqualsImpl(p_impl.m_data); } // vtable+0x88
 
+	inline float GetX() { return m_data[0]; }
+	inline float GetY() { return m_data[1]; }
+	inline float GetZ() { return m_data[2]; }
+
+	inline float& operator[](size_t idx) { return m_data[idx]; }
+	inline const float& operator[](size_t idx) const { return m_data[idx]; }
+
 	// FUNCTION: LEGO1 0x10010c00
 	inline Mx3DPointFloat& operator=(const Mx3DPointFloat& p_other)
 	{
@@ -54,9 +56,121 @@ class Mx3DPointFloat : public Vector3 {
 class Mx4DPointFloat : public Vector4 {
 public:
 	inline Mx4DPointFloat() : Vector4(m_elements) {}
+	inline Mx4DPointFloat(float p_x, float p_y, float p_z, float p_a) : Vector4(m_elements)
+	{
+		m_elements[0] = p_x;
+		m_elements[1] = p_y;
+		m_elements[2] = p_z;
+		m_elements[3] = p_a;
+	}
+
+	// FUNCTION: LEGO1 0x10003200
+	virtual void operator=(const Vector4& p_impl) { EqualsImpl(p_impl.m_data); } // vtable+0x98
+
+	inline float& operator[](size_t idx) { return m_data[idx]; }
+	inline const float& operator[](size_t idx) const { return m_data[idx]; }
 
 private:
-	float m_elements[4];
+	float m_elements[4]; // 0x08
 };
 
+// SIZE 0x34
+class UnknownMx4DPointFloat {
+public:
+	enum {
+		c_bit1 = 0x01,
+		c_bit2 = 0x02
+	};
+
+	UnknownMx4DPointFloat() : m_unk0x30(0) {}
+
+	inline void Unknown1(Vector4& p_v)
+	{
+		m_unk0x00 = p_v;
+		m_unk0x30 |= c_bit1;
+	}
+
+	inline void Unknown2(Vector4& p_v)
+	{
+		m_unk0x18 = p_v;
+		m_unk0x30 |= c_bit2;
+	}
+
+	inline int Unknown_100040a0(Matrix4& p_matrix, float p_f);
+	inline int FUN_100040a0(Vector4& p_v, float p_f);
+
+private:
+	Mx4DPointFloat m_unk0x00; // 0x00
+	Mx4DPointFloat m_unk0x18; // 0x18
+	undefined4 m_unk0x30;     // 0x30
+};
+
+int UnknownMx4DPointFloat::Unknown_100040a0(Matrix4& p_matrix, float p_f)
+{
+	float data[4];
+	Vector4 v(data);
+
+	if (FUN_100040a0(v, p_f) == 0) {
+		return p_matrix.FromQuaternion(v);
+	}
+	else {
+		return -1;
+	}
+}
+
+// FUNCTION: LEGO1 0x100040a0
+inline int UnknownMx4DPointFloat::FUN_100040a0(Vector4& p_v, float p_f)
+{
+	undefined4 state = m_unk0x30;
+
+	if (state == 1) {
+		p_v = m_unk0x00;
+		p_v[3] = (1.0 - p_f) * acos(p_v[3]) * 2.0;
+		return p_v.NormalizeQuaternion();
+	}
+	else if (state == 2) {
+		p_v = m_unk0x18;
+		p_v[3] = p_f * acos(p_v[3]) * 2.0;
+		return p_v.NormalizeQuaternion();
+	}
+	else if (state == 3) {
+		double d1 = p_v.Dot(&m_unk0x00, &m_unk0x18);
+		double d2;
+
+		if (d1 + 1.0 > 0.00001) {
+			if (1.0 - d1 > 0.00001) {
+				double d = acos(d1);
+				sin(d);
+				d1 = sin((1 - p_f) * d) / sin(d);
+				d2 = sin(p_f * d) / sin(d);
+			}
+			else {
+				d1 = 1.0 - p_f;
+				d2 = p_f;
+			}
+
+			for (int i = 0; i < 4; i++) {
+				p_v[i] = m_unk0x18[i] * d2 + m_unk0x00[i] * d1;
+			}
+		}
+		else {
+			p_v[0] = -m_unk0x00[1];
+			p_v[1] = m_unk0x00[1];
+			p_v[2] = -m_unk0x00[3];
+			p_v[3] = m_unk0x00[2];
+			d1 = sin((1.0 - p_f) * 1.570796326794895);
+			d2 = sin(p_f * 1.570796326794895);
+
+			for (int i = 0; i < 3; i++) {
+				p_v[i] = m_unk0x00[i] * d1 + p_v[i] * d2;
+			}
+		}
+
+		return 0;
+	}
+	else {
+		return -1;
+	}
+}
+
 #endif // MXGEOMETRY3D_H

LEGO1/realtime/vector.h

@@ -291,8 +291,8 @@ class Vector4 : public Vector3 {
 	// FUNCTION: LEGO1 0x10002ae0
 	virtual void SetMatrixProduct(Vector4* p_a, float* p_b) { SetMatrixProduct(p_a->m_data, p_b); } // vtable+0x88
 
-	inline virtual int NormalizeQuaternion();                            // vtable+90
+	inline virtual int NormalizeQuaternion();                            // vtable+0x90
-	inline virtual void UnknownQuaternionOp(Vector4* p_a, Vector4* p_b); // vtable+94
+	inline virtual void UnknownQuaternionOp(Vector4* p_a, Vector4* p_b); // vtable+0x94
 
 	// Vector3 overrides
 
@@ -376,6 +376,8 @@ class Vector4 : public Vector3 {
 		m_data[2] = *p_value;
 		m_data[3] = *p_value;
 	} // vtable+0x84
+
+	friend class Mx4DPointFloat;
 };
 
 // Note close yet, included because I'm at least confident I know what operation