diff --git a/examples/UnitTests/source/Main.cpp b/examples/UnitTests/source/Main.cpp index 2db54b84..7a4bf226 100644 --- a/examples/UnitTests/source/Main.cpp +++ b/examples/UnitTests/source/Main.cpp @@ -1,6 +1,7 @@ #include #include "utest.h" -#include "Test_CVector.h" +//#include "Test_CVector.h" +#include "Test_PluginSA_CMatrix.h" using namespace plugin; diff --git a/examples/UnitTests/source/Test_PluginSA_CMatrix.h b/examples/UnitTests/source/Test_PluginSA_CMatrix.h new file mode 100644 index 00000000..b42b288d --- /dev/null +++ b/examples/UnitTests/source/Test_PluginSA_CMatrix.h @@ -0,0 +1,695 @@ +#pragma once +#include +#include "utest.h" +#include + +using namespace plugin; + +constexpr int32_t cfloatingPointAcceptableULP = 6; + +static void PrintBytes(const void* const buffer, size_t size) { + const uint8_t* const bytes = (uint8_t*)buffer; + uint8_t pad = 0; + for (size_t i = 0; i < size; i++) { + printf("0x%.2X, ", bytes[i]); + pad++; + if (pad >= 16) { + printf("\n"); + pad = 0; + } + } + printf("\n"); +} + +static void RandomData(void* data, size_t size) { + unsigned char *ptr = reinterpret_cast(data); + for (size_t i = 0; i < size; i++) { + ptr[i] = rand() % 256; + } +} + +static float RandomFloat(float min, float max) { + // Generate a random float between 0.0 and 1.0 + float scale = static_cast(rand()) / static_cast(RAND_MAX); + // Scale and shift to the desired range [min, max] + return min + scale * (max - min); +} + +// "Unit in the last place(ULP)" for testing equality of floats +static inline int32_t FloatToOrderedInt(float f) { + int32_t i; + std::memcpy(&i, &f, sizeof(float)); + + // Make lexicographically ordered as twos-complement integer + if (i < 0) + i = 0x80000000 - i; + + return i; +} + +static bool AlmostEqualULP(float a, float b, int maxUlps) { + // Handle NaN + if (std::isnan(a) || std::isnan(b)) + return false; + + // Handle infinities + if (std::isinf(a) || std::isinf(b)) + return a == b; + + int32_t ia = FloatToOrderedInt(a); + int32_t ib = FloatToOrderedInt(b); + + if (std::abs(ia - ib) > maxUlps) { + printf("threshold reached %d\n", std::abs(ia - ib)); + } + + return std::abs(ia - ib) <= maxUlps; +} +////////////////////////// + +static bool IsFloatApproximatelyEqual(float a, float b) { + return AlmostEqualULP(a, b, cfloatingPointAcceptableULP); +} + +static bool IsVectorApproximatelyEqual(CVector &a, CVector &b) { + return IsFloatApproximatelyEqual(a.x, b.x) + && IsFloatApproximatelyEqual(a.y, b.y) + && IsFloatApproximatelyEqual(a.z, b.z); +} + +static bool IsMatrixApproximatelyEqual(CMatrix &a, CMatrix &b) { + return a.m_pAttachMatrix == b.m_pAttachMatrix + && a.m_bOwnsAttachedMatrix == b.m_bOwnsAttachedMatrix + && IsVectorApproximatelyEqual(a.GetRight(), b.GetRight()) + && IsVectorApproximatelyEqual(a.GetForward(), b.GetForward()) + && IsVectorApproximatelyEqual(a.GetUp(), b.GetUp()) + && IsVectorApproximatelyEqual(a.pos, b.pos); +} + +static void RandomizeComponents(CMatrix &matrix) { + matrix.GetRight().x = RandomFloat(-1.0f, 1.0f); + matrix.GetRight().y = RandomFloat(-1.0f, 1.0f); + matrix.GetRight().z = RandomFloat(-1.0f, 1.0f); + matrix.GetRight().Normalise(); + + matrix.GetForward().x = RandomFloat(-1.0f, 1.0f); + matrix.GetForward().y = RandomFloat(-1.0f, 1.0f); + matrix.GetForward().z = RandomFloat(-1.0f, 1.0f); + matrix.GetForward().Normalise(); + + matrix.GetUp().x = RandomFloat(-1.0f, 1.0f); + matrix.GetUp().y = RandomFloat(-1.0f, 1.0f); + matrix.GetUp().z = RandomFloat(-1.0f, 1.0f); + matrix.GetUp().Normalise(); + + matrix.pos.x = RandomFloat(-2000.0f, 2000.0f); + matrix.pos.y = RandomFloat(-2000.0f, 2000.0f); + matrix.pos.z = RandomFloat(-2000.0f, 2000.0f); +} + +static void PrintMatrix(CMatrix &matrix) { + printf("Right: %f %f %f\n", matrix.GetRight().x, matrix.GetRight().y, matrix.GetRight().z); + printf("Forward: %f %f %f\n", matrix.GetForward().x, matrix.GetForward().y, matrix.GetForward().z); + printf("Up: %f %f %f\n", matrix.GetUp().x, matrix.GetUp().y, matrix.GetUp().z); + printf("Pos: %f %f %f\n", matrix.pos.x, matrix.pos.y, matrix.pos.z); + printf("m_pAttachMatrix=%p m_bOwnsAttachedMatrix=%hhu\n", matrix.m_pAttachMatrix, matrix.m_bOwnsAttachedMatrix); +} + +UTEST(CMatrix, ctor_CopyMatrix) +{ + auto copied = CMatrix(); + RandomData(&copied, sizeof(CMatrix)); + + auto modern = CMatrix(copied); + auto legacy = CMatrix(); + ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59BCF0)(&legacy, copied); + EXPECT_EQ(modern == legacy, true); + + // garbage/dummy values require manual cleanup + copied.m_pAttachMatrix = nullptr; + copied.m_bOwnsAttachedMatrix = false; + modern.m_pAttachMatrix = nullptr; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_pAttachMatrix = nullptr; + legacy.m_bOwnsAttachedMatrix = false; +} + +UTEST(CMatrix, ctor_CopyRwMatrixThenDelete) +{ + // need to figure out a way to correctly mock test DTOR without causing exception errors + + auto copied = RwMatrix(); + RandomData(&copied, sizeof(RwMatrix)); + + { + CMatrix modern = CMatrix(&copied, false); + CMatrix legacy = CMatrix(); + ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59C050)(&legacy, &copied, false); + EXPECT_EQ(modern == legacy, true); + ((void (__thiscall *)(CMatrix *))0x59ACD0)(&legacy); + } + + //{ + // CMatrix modern = CMatrix(&copied, true); + // CMatrix legacy = CMatrix(); + // ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59C050)(&legacy, &copied, true); + // EXPECT_EQ(modern == legacy, true); + // ((void (__thiscall *)(CMatrix *))0x59ACD0)(&legacy); + //} +} + +UTEST(CMatrix, AttachDetach) +{ + auto attached = RwMatrix(); + RandomData(&attached, sizeof(RwMatrix)); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern.Attach(&attached, false); + ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59BD10)(&legacy, &attached, false); + EXPECT_EQ(modern == legacy, true); + modern.Detach(); + ((void (__thiscall *)(CMatrix *))0x59ACF0)(&legacy); + EXPECT_EQ(modern == legacy, true); + + // need to figure out a way to correctly mock test this without causing exception errors + //modern.Attach(&attached, true); + //((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59BD10)(&legacy, &attached, true); + //EXPECT_EQ(modern == legacy, true); + //modern.Detach(); + //((void (__thiscall *)(CMatrix *))0x59ACF0)(&legacy); + //EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, CopyOnlyMatrix) +{ + auto copied = CMatrix(); + RandomData(&copied, sizeof(CMatrix)); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern.CopyOnlyMatrix(copied); + ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59ADD0)(&legacy, copied); + EXPECT_EQ(modern == legacy, true); + + // garbage/dummy values require manual cleanup + copied.m_pAttachMatrix = nullptr; + modern.m_pAttachMatrix = nullptr; + legacy.m_pAttachMatrix = nullptr; + copied.m_bOwnsAttachedMatrix = false; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_bOwnsAttachedMatrix = false; +} + +UTEST(CMatrix, UpdateMatrix) +{ + auto copied = RwMatrix(); + auto attached = RwMatrix(); + RandomData(&copied, sizeof(RwMatrix)); + RandomData(&attached, sizeof(RwMatrix)); + + auto modern = CMatrix(&attached, false); + auto legacy = CMatrix(); + ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59BD10)(&legacy, &attached, false); + + modern.UpdateMatrix(&copied); + plugin::CallMethod<0x59AD20, CMatrix*, RwMatrix*>(&legacy, &copied); + EXPECT_EQ(modern == legacy, true); + + modern.Update(); + ((void (__thiscall *)(CMatrix *))0x59BB60)(&legacy); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, UpdateRW) +{ + auto copied_modern = RwMatrix(); + auto copied_legacy = RwMatrix(); + auto attached_modern = RwMatrix(); + auto attached_legacy = RwMatrix(); + + auto modern = CMatrix(&attached_modern, false); + auto legacy = CMatrix(&attached_legacy, false); + RandomData(&modern, sizeof(RwMatrix)); + memcpy(&legacy, &modern, sizeof(RwMatrix)); + + modern.UpdateRW(); + ((void (__thiscall *)(CMatrix *))0x59BBB0)(&legacy); + EXPECT_EQ(std::memcmp(&copied_modern, &copied_legacy, sizeof(RwMatrix)), 0); + EXPECT_EQ(std::memcmp(&attached_modern, &attached_legacy, sizeof(RwMatrix)), 0); + + modern.UpdateRW(&copied_modern); + ((void (__thiscall *)(CMatrix *, RwMatrix *))0x59AD70)(&legacy, &copied_legacy); + EXPECT_EQ(std::memcmp(&copied_modern, &copied_legacy, sizeof(RwMatrix)), 0); + EXPECT_EQ(std::memcmp(&attached_modern, &attached_legacy, sizeof(RwMatrix)), 0); +} + +UTEST(CMatrix, ResetOrientation) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern.ResetOrientation(); + ((void (__thiscall *)(CMatrix *))0x59AEA0)(&legacy); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetUnity) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern.SetUnity(); + ((void (__thiscall *)(CMatrix *))0x59AE70)(&legacy); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetScale) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float x = 123.4f, y = 567.8f, z = 910.1f; + + modern.SetScale(13.0); + ((void (__thiscall *)(CMatrix *, float))0x59AED0)(&legacy, 13.0); + EXPECT_EQ(modern == legacy, true); + + modern.SetScale(x, y, z); + ((void (__thiscall *)(CMatrix *, float, float, float))0x59AF00)(&legacy, x, y, z); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetTranslateOnly) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float x = 123.4f, y = 567.8f, z = 910.1f; + + modern.SetTranslateOnly(x, y, z); + ((void (__thiscall *)(CMatrix *, float, float, float))0x59AF80)(&legacy, x, y, z); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetTranslate) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float x = 123.4f, y = 567.8f, z = 910.1f; + + modern.SetTranslate(x, y, z); + ((void (__thiscall *)(CMatrix *, float, float, float))0x59AF40)(&legacy, x, y, z); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, SetRotateXOnly) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.SetRotateXOnly(value); + ((void (__thiscall *)(CMatrix *, float))0x59AFA0)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetRotateYOnly) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.SetRotateYOnly(value); + ((void (__thiscall *)(CMatrix *, float))0x59AFE0)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetRotateZOnly) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.SetRotateZOnly(value); + ((void (__thiscall *)(CMatrix *, float))0x59B020)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetRotateX) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.SetRotateX(value); + ((void (__thiscall *)(CMatrix *, float))0x59B060)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetRotateY) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.SetRotateY(value); + ((void (__thiscall *)(CMatrix *, float))0x59B0A0)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetRotateZ) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.SetRotateZ(value); + ((void (__thiscall *)(CMatrix *, float))0x59B0E0)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, SetRotate_Vector) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float x = RandomFloat(-3.14f, 3.14f); + float y = RandomFloat(-3.14f, 3.14f); + float z = RandomFloat(-3.14f, 3.14f); + modern.SetRotate(x, y, z); + ((void (__thiscall *)(CMatrix *, float, float, float))0x59B120)(&legacy, x, y, z); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, SetRotate_Quat) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + CQuaternion quat = {{RandomFloat(-1.0f, 1.0f), RandomFloat(-1.0f, 1.0f), RandomFloat(-1.0f, 1.0f)}, RandomFloat(-1.0f, 1.0f)}; + modern.SetRotate(quat); + ((void (__thiscall *)(CMatrix *, CQuaternion const&))0x59BBF0)(&legacy, quat); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, MultiplyRotation) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + CVector vector = CVector(RandomFloat(-3.14f, 3.14f), RandomFloat(-3.14f, 3.14f), RandomFloat(-3.14f, 3.14f)); + modern.MultiplyRotation(vector); + plugin::CallAndReturn(legacy, vector); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, MultiplyTransposedRotation) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + CVector vector = CVector(RandomFloat(-3.14f, 3.14f), RandomFloat(-3.14f, 3.14f), RandomFloat(-3.14f, 3.14f)); + modern.MultiplyTransposedRotation(vector); + plugin::CallAndReturn(vector, legacy); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, RotateX) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.RotateX(value); + ((void (__thiscall *)(CMatrix *, float))0x59B1E0)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, RotateY) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.RotateY(value); + ((void (__thiscall *)(CMatrix *, float))0x59B2C0)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, RotateZ) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float value = RandomFloat(-3.14f, 3.14f); + modern.RotateZ(value); + ((void (__thiscall *)(CMatrix *, float))0x59B390)(&legacy, value); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, Rotate) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float x = RandomFloat(-3.14f, 3.14f); + float y = RandomFloat(-3.14f, 3.14f); + float z = RandomFloat(-3.14f, 3.14f); + modern.Rotate(x, y, z); + ((void (__thiscall *)(CMatrix *, float, float, float))0x59B460)(&legacy, x, y, z); + EXPECT_EQ(modern == legacy, true); +} + +UTEST(CMatrix, Reorthogonalise) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + RandomizeComponents(modern); + memcpy(&legacy, &modern, sizeof(CMatrix)); + + modern.Reorthogonalise(); + ((void (__thiscall *)(CMatrix *))0x59B6A0)(&legacy); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, CopyToRwMatrix) +{ + auto copied_modern = RwMatrix(); + auto copied_legacy = RwMatrix(); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + RandomData(&modern, sizeof(RwMatrix)); + memcpy(&legacy, &modern, sizeof(RwMatrix)); + + modern.CopyToRwMatrix(&copied_modern); + ((void (__thiscall *)(CMatrix *, RwMatrix *))0x59B8B0)(&legacy, &copied_legacy); + EXPECT_EQ(std::memcmp(&copied_modern, &copied_legacy, sizeof(RwMatrix)), 0); +} + +UTEST(CMatrix, Scale) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + RandomizeComponents(modern); + memcpy(&legacy, &modern, sizeof(CMatrix)); + + const float x = RandomFloat(2.0, 100.0f); + const float y = RandomFloat(2.0, 100.0f); + const float z = RandomFloat(2.0, 100.0f); + + modern.Scale(x, y, z); + plugin::CallMethod<0x5A2E60, CMatrix *, float, float, float>(&legacy, x, y, z); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); + + modern.Scale(x); + plugin::CallMethod<0x459350, CMatrix *, float>(&legacy, x); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, ForceUpVector) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + RandomizeComponents(modern); + memcpy(&legacy, &modern, sizeof(CMatrix)); + + CVector vector = {RandomFloat(2.0, 100.0f), RandomFloat(2.0, 100.0f), RandomFloat(2.0, 100.0f)}; + + modern.ForceUpVector(vector); + plugin::CallMethod<0x59B7E0, CMatrix *, CVector>(&legacy, vector); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, ConvertToEulerAngles) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + RandomizeComponents(modern); + memcpy(&legacy, &modern, sizeof(CMatrix)); + + float modern_initial, modern_intermediate, modern_final; + float legacy_initial, legacy_intermediate, legacy_final; + for (unsigned char flags = 0; flags < 24; flags++) { + modern.ConvertToEulerAngles(modern_initial, modern_intermediate, modern_final, static_cast(flags)); + plugin::CallMethod<0x59A840, const CMatrix*, float*, float*, float*, CMatrix::eMatrixEulerFlags>( + &legacy, &legacy_initial, &legacy_intermediate, &legacy_final, static_cast(flags)); + EXPECT_EQ(IsFloatApproximatelyEqual(modern_initial, legacy_initial), true); + EXPECT_EQ(IsFloatApproximatelyEqual(modern_intermediate, legacy_intermediate), true); + EXPECT_EQ(IsFloatApproximatelyEqual(modern_final, legacy_final), true); + } +} + +UTEST(CMatrix, ConvertFromEulerAngles) +{ + auto modern = CMatrix(); + auto legacy = CMatrix(); + + float initialAngle = RandomFloat(-3.14f, 3.14f); + float intermediateAngle = RandomFloat(-3.14f, 3.14f); + float finalAngle = RandomFloat(-3.14f, 3.14f); + + for (unsigned char flags = 0; flags < 24; flags++) { + modern.ConvertFromEulerAngles(initialAngle, intermediateAngle, finalAngle, static_cast(flags)); + plugin::CallMethod<0x59AA40, CMatrix*, float, float, float, CMatrix::eMatrixEulerFlags>( + &legacy, initialAngle, intermediateAngle, finalAngle, static_cast(flags)); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); + } +} + +UTEST(CMatrix, StoreOperator) +{ + auto copied = CMatrix(); + RandomData(&copied, sizeof(CMatrix)); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern = copied; + ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59BBC0)(&legacy, copied); + EXPECT_EQ(modern == legacy, true); + + // garbage/dummy values require manual cleanup + copied.m_pAttachMatrix = nullptr; + copied.m_bOwnsAttachedMatrix = false; + modern.m_pAttachMatrix = nullptr; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_pAttachMatrix = nullptr; + legacy.m_bOwnsAttachedMatrix = false; +} + +UTEST(CMatrix, AddEqualsOperator) +{ + auto copied = CMatrix(); + RandomizeComponents(copied); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + RandomizeComponents(modern); + memcpy(&legacy, &modern, sizeof(CMatrix)); + + modern += copied; + ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59ADF0)(&legacy, copied); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); + + // garbage/dummy values require manual cleanup + copied.m_pAttachMatrix = nullptr; + copied.m_bOwnsAttachedMatrix = false; + modern.m_pAttachMatrix = nullptr; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_pAttachMatrix = nullptr; + legacy.m_bOwnsAttachedMatrix = false; +} + +UTEST(CMatrix, MultiplyEqualsOperator) +{ + auto copied = CMatrix(); + RandomizeComponents(copied); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + RandomizeComponents(modern); + memcpy(&legacy, &modern, sizeof(CMatrix)); + + modern *= copied; + ((void (__thiscall *)(CMatrix *, CMatrix const&))0x411A80)(&legacy, copied); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); + + // garbage/dummy values require manual cleanup + copied.m_pAttachMatrix = nullptr; + copied.m_bOwnsAttachedMatrix = false; + modern.m_pAttachMatrix = nullptr; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_pAttachMatrix = nullptr; + legacy.m_bOwnsAttachedMatrix = false; +} + +UTEST(CMatrix, MultiplyMatrixOperator) +{ + auto matrix1 = CMatrix(); + auto matrix2 = CMatrix(); + RandomizeComponents(matrix1); + RandomizeComponents(matrix2); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern = matrix1 * matrix2; + ((void(__cdecl *)(CMatrix*, CMatrix const&, CMatrix const&))0x59BE30)(&legacy, matrix1, matrix2); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); + + // garbage/dummy values require manual cleanup + matrix1.m_pAttachMatrix = nullptr; + matrix1.m_bOwnsAttachedMatrix = false; + matrix2.m_pAttachMatrix = nullptr; + matrix2.m_bOwnsAttachedMatrix = false; + modern.m_pAttachMatrix = nullptr; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_pAttachMatrix = nullptr; + legacy.m_bOwnsAttachedMatrix = false; +} + +UTEST(CMatrix, MultiplyVectorOperator) +{ + auto matrix = CMatrix(); + CVector vector = {RandomFloat(2.0, 100.0f), RandomFloat(2.0, 100.0f), RandomFloat(2.0, 100.0f)}; + RandomizeComponents(matrix); + + CVector modern = matrix * vector; + CVector legacy = {0.0f, 0.0f, 0.0f}; + ((void(__cdecl *)(CVector*, CMatrix const&, CVector const&))0x59C890)(&legacy, matrix, vector); + EXPECT_EQ(IsVectorApproximatelyEqual(modern, legacy), true); +} + +UTEST(CMatrix, AddMatrixOperator) +{ + auto matrix1 = CMatrix(); + auto matrix2 = CMatrix(); + RandomizeComponents(matrix1); + RandomizeComponents(matrix2); + + auto modern = CMatrix(); + auto legacy = CMatrix(); + + modern = matrix1 + matrix2; + ((void(__cdecl *)(CMatrix*, CMatrix const&, CMatrix const&))0x59BFA0)(&legacy, matrix1, matrix2); + EXPECT_EQ(IsMatrixApproximatelyEqual(modern, legacy), true); + + // garbage/dummy values require manual cleanup + matrix1.m_pAttachMatrix = nullptr; + matrix1.m_bOwnsAttachedMatrix = false; + matrix2.m_pAttachMatrix = nullptr; + matrix2.m_bOwnsAttachedMatrix = false; + modern.m_pAttachMatrix = nullptr; + modern.m_bOwnsAttachedMatrix = false; + legacy.m_pAttachMatrix = nullptr; + legacy.m_bOwnsAttachedMatrix = false; +} diff --git a/plugin_sa/game_sa/CMatrix.cpp b/plugin_sa/game_sa/CMatrix.cpp index f81ff810..57dc9bf9 100644 --- a/plugin_sa/game_sa/CMatrix.cpp +++ b/plugin_sa/game_sa/CMatrix.cpp @@ -5,208 +5,751 @@ Do not delete this comment block. Respect others' work! */ #include "CMatrix.h" +#include "rwplcore.h" -CMatrix::CMatrix(CMatrix const& matrix) +#include + +// The some parts of the game's mathematical computations uses double precision. +// But since the discrepancy on using float instead of double is low +// Then It's reasonable to use float to maximize performance in exchange for minimal tradeoff in precision. + +CMatrix::CMatrix(CMatrix const& matrix) +: m_pAttachMatrix(nullptr), m_bOwnsAttachedMatrix(false) { - ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59BCF0)(this, matrix); + // ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59BCF0)(this, matrix); + + CopyOnlyMatrix(matrix); } // like previous + attach -CMatrix::CMatrix(RwMatrix *matrix, bool temporary) +CMatrix::CMatrix(RwMatrix *matrix, bool temporary) : m_pAttachMatrix(nullptr) { - ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59C050)(this, matrix, temporary); + // ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59C050)(this, matrix, temporary); + + Attach(matrix, temporary); } -// destructor detaches matrix if attached -CMatrix::~CMatrix() +// destructor detaches matrix if attached +CMatrix::~CMatrix(void) { - ((void (__thiscall *)(CMatrix *))0x59ACD0)(this); + // ((void (__thiscall *)(CMatrix *))0x59ACD0)(this); + + Detach(); } void CMatrix::Attach(RwMatrix *matrix, bool temporary) { - ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59BD10)(this, matrix, temporary); + // ((void (__thiscall *)(CMatrix *, RwMatrix *, bool))0x59BD10)(this, matrix, temporary); + + Detach(); + + m_pAttachMatrix = matrix; + m_bOwnsAttachedMatrix = temporary; + + Update(); } -void CMatrix::Detach() +void CMatrix::Detach(void) { - ((void (__thiscall *)(CMatrix *))0x59ACF0)(this); + // ((void (__thiscall *)(CMatrix *))0x59ACF0)(this); + + if (m_bOwnsAttachedMatrix && m_pAttachMatrix) { + RwMatrixDestroy(m_pAttachMatrix); + } + m_pAttachMatrix = nullptr; } // copy base RwMatrix to another matrix void CMatrix::CopyOnlyMatrix(CMatrix const& matrix) { - ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59ADD0)(this, matrix); + // ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59ADD0)(this, matrix); + + // *reinterpret_cast(this) = reinterpret_cast(matrix); + std::memcpy(this, &matrix, sizeof(RwMatrix)); } // update RwMatrix with attaching matrix. This doesn't check if attaching matrix is present, so use it only if you know it is present. // Using UpdateRW() is more safe since it perform this check. -void CMatrix::Update() +void CMatrix::Update(void) { - ((void (__thiscall *)(CMatrix *))0x59BB60)(this); + // ((void (__thiscall *)(CMatrix *))0x59BB60)(this); + + UpdateMatrix(m_pAttachMatrix); } // update RwMatrix with attaching matrix. -void CMatrix::UpdateRW() +void CMatrix::UpdateRW(void) { - ((void (__thiscall *)(CMatrix *))0x59BBB0)(this); + // ((void (__thiscall *)(CMatrix *))0x59BBB0)(this); + + if (m_pAttachMatrix) { + CMatrix::UpdateRW(m_pAttachMatrix); + } } // update RwMatrix with this matrix void CMatrix::UpdateRW(RwMatrix *matrix) { - ((void (__thiscall *)(CMatrix *, RwMatrix *))0x59AD70)(this, matrix); + // ((void (__thiscall *)(CMatrix *, RwMatrix *))0x59AD70)(this, matrix); + + matrix->right = reinterpret_cast(GetRight()); + matrix->up = reinterpret_cast(GetForward()); + matrix->at = reinterpret_cast(GetUp()); + matrix->pos = reinterpret_cast(pos); + + RwMatrixUpdate(matrix); } -void CMatrix::SetUnity() +void CMatrix::UpdateMatrix(RwMatrix* rwMatrix) { - ((void (__thiscall *)(CMatrix *))0x59AE70)(this); + // plugin::CallMethod<0x59AD20, CMatrix*, RwMatrix*>(this, rwMatrix); + + GetRight() = reinterpret_cast(rwMatrix->right); + GetForward() = reinterpret_cast(rwMatrix->up); + GetUp() = reinterpret_cast(rwMatrix->at); + pos = reinterpret_cast(rwMatrix->pos); } -void CMatrix::ResetOrientation() +void CMatrix::ResetOrientation(void) { - ((void (__thiscall *)(CMatrix *))0x59AEA0)(this); + //((void (__thiscall *)(CMatrix *))0x59AEA0)(this); + + GetRight() .Set(1.0f, 0.0f, 0.0f); + GetForward().Set(0.0f, 1.0f, 0.0f); + GetUp() .Set(0.0f, 0.0f, 1.0f); } -void CMatrix::SetScale(float scale) +void CMatrix::SetUnity(void) { - ((void (__thiscall *)(CMatrix *, float))0x59AED0)(this, scale); + //((void (__thiscall *)(CMatrix *))0x59AE70)(this); + + SetScale(1.0f); } // scale on three axes -void CMatrix::SetScale(float x, float y, float z) +void CMatrix::SetScale(float right, float forward, float up) { - ((void (__thiscall *)(CMatrix *, float, float, float))0x59AF00)(this, x, y, z); + //((void (__thiscall *)(CMatrix *, float, float, float))0x59AF00)(this, x, y, z); + + GetRight() .Set(right, 0.0f, 0.0f); + GetForward().Set( 0.0f, forward, 0.0f); + GetUp() .Set( 0.0f, 0.0f, up); + + pos.Set(0.0f); +} + +void CMatrix::SetScale(CVector const &scale) { + SetScale(scale.x, scale.y, scale.z); +} + +void CMatrix::SetScale(float scale) +{ + //((void (__thiscall *)(CMatrix *, float))0x59AED0)(this, scale); + + SetScale(scale, scale, scale); } void CMatrix::SetTranslateOnly(float x, float y, float z) { - ((void (__thiscall *)(CMatrix *, float, float, float))0x59AF80)(this, x, y, z); + //((void (__thiscall *)(CMatrix *, float, float, float))0x59AF80)(this, x, y, z); + + pos.Set(x, y, z); +} + +void CMatrix::SetTranslateOnly(CVector const &newPos) { + //SetTranslateOnly(newPos.x, newPos.y, newPos.z); + + pos = newPos; } // like previous + reset orientation void CMatrix::SetTranslate(float x, float y, float z) { - ((void (__thiscall *)(CMatrix *, float, float, float))0x59AF40)(this, x, y, z); + //((void (__thiscall *)(CMatrix *, float, float, float))0x59AF40)(this, x, y, z); + + ResetOrientation(); + pos.Set(x, y, z); } -void CMatrix::SetRotateXOnly(float angle) -{ - ((void (__thiscall *)(CMatrix *, float))0x59AFA0)(this, angle); +void CMatrix::SetTranslate(CVector const &newPos) { + SetTranslate(newPos.x, newPos.y, newPos.z); } -void CMatrix::SetRotateYOnly(float angle) +void CMatrix::SetRotateXOnly(float pitch) { - ((void (__thiscall *)(CMatrix *, float))0x59AFE0)(this, angle); + //((void (__thiscall *)(CMatrix *, float))0x59AFA0)(this, pitch); + + const float fSin = std::sin(pitch); + const float fCos = std::cos(pitch); + + GetRight() .Set(1.0f, 0.0f, 0.0f); + GetForward().Set(0.0f, fCos, fSin); + GetUp() .Set(0.0f, -fSin, fCos); } -void CMatrix::SetRotateZOnly(float angle) +void CMatrix::SetRotateYOnly(float roll) { - ((void (__thiscall *)(CMatrix *, float))0x59B020)(this, angle); + //((void (__thiscall *)(CMatrix *, float))0x59AFE0)(this, roll); + + const float fSin = sin(roll); + const float fCos = cos(roll); + + GetRight() .Set(fCos, 0.0F, -fSin); + GetForward().Set(0.0F, 1.0F, 0.0F); + GetUp() .Set(fSin, 0.0F, fCos); } -void CMatrix::SetRotateX(float angle) +void CMatrix::SetRotateZOnly(float yaw) { - ((void (__thiscall *)(CMatrix *, float))0x59B060)(this, angle); + //((void (__thiscall *)(CMatrix *, float))0x59B020)(this, yaw); + + const float fSin = sin(yaw); + const float fCos = cos(yaw); + + GetRight() .Set( fCos, fSin, 0.0F); + GetForward().Set(-fSin, fCos, 0.0F); + GetUp() .Set( 0.0F, 0.0F, 1.0F); } -void CMatrix::SetRotateY(float angle) -{ - ((void (__thiscall *)(CMatrix *, float))0x59B0A0)(this, angle); +void CMatrix::SetRotateOnly(float pitch, float roll, float yaw) +{ + // precompute trigo ratios + const float sX = sin(pitch); + const float cX = cos(pitch); + const float sY = sin(roll); + const float cY = cos(roll); + const float sZ = sin(yaw); + const float cZ = cos(yaw); + + const float sX_sY = sX * sY; + const float sX_cY = sX * cY; + + GetRight() .Set(cY*cZ - sX_sY*sZ, cY*sZ + sX_sY*cZ, -cX*sY); + GetForward().Set( -cX*sZ, cX*cZ, sX); + GetUp() .Set(sY*cZ + sX_cY*sZ, sY*sZ - sX_cY*cZ, cX*cY); } -void CMatrix::SetRotateZ(float angle) -{ - ((void (__thiscall *)(CMatrix *, float))0x59B0E0)(this, angle); +void CMatrix::SetRotateOnly(CVector const &rotation) { + SetRotateOnly(rotation.x, rotation.y, rotation.z); } -// set rotate on 3 axes -void CMatrix::SetRotate(float x, float y, float z) +void CMatrix::SetRotateX(float pitch) { - ((void (__thiscall *)(CMatrix *, float, float, float))0x59B120)(this, x, y, z); + //((void (__thiscall *)(CMatrix *, float))0x59B060)(this, pitch); + + CMatrix::SetRotateXOnly(pitch); + pos.Set(0.0f, 0.0f, 0.0f); } -void CMatrix::Translate(float x, float y, float z) +void CMatrix::SetRotateY(float roll) { - pos.x += x; - pos.y += y; - pos.z += z; + //((void (__thiscall *)(CMatrix *, float))0x59B0A0)(this, roll); + + CMatrix::SetRotateYOnly(roll); + pos.Set(0.0f, 0.0f, 0.0f); } -void CMatrix::RotateX(float angle) +void CMatrix::SetRotateZ(float yaw) { - ((void (__thiscall *)(CMatrix *, float))0x59B1E0)(this, angle); + //((void (__thiscall *)(CMatrix *, float))0x59B0E0)(this, yaw); + + CMatrix::SetRotateZOnly(yaw); + pos.Set(0.0f, 0.0f, 0.0f); } -void CMatrix::RotateY(float angle) +void CMatrix::SetRotate(float pitch, float roll, float yaw) { - ((void (__thiscall *)(CMatrix *, float))0x59B2C0)(this, angle); + //((void (__thiscall *)(CMatrix *, float, float, float))0x59B120)(this, pitch, roll, yaw); + + SetRotateOnly(pitch, roll, yaw); + pos.Set(0.0f, 0.0f, 0.0f); +} + +void CMatrix::SetRotate(CVector const &rotation) { + SetRotate(rotation.x, rotation.y, rotation.z); } -void CMatrix::RotateZ(float angle) +void CMatrix::SetRotate(CQuaternion const& quat) { - ((void (__thiscall *)(CMatrix *, float))0x59B390)(this, angle); + //((void (__thiscall *)(CMatrix *, CQuaternion const&))0x59BBF0)(this, quat); + + const CVector vecImag2 = quat.imag + quat.imag; + + const float x2x = vecImag2.x * quat.imag.x; + const float y2x = vecImag2.y * quat.imag.x; + const float z2x = vecImag2.z * quat.imag.x; + + const float y2y = vecImag2.y * quat.imag.y; + const float z2y = vecImag2.z * quat.imag.y; + const float z2z = vecImag2.z * quat.imag.z; + + const float x2r = vecImag2.x * quat.real; + const float y2r = vecImag2.y * quat.real; + const float z2r = vecImag2.z * quat.real; + + GetRight().Set (1.0F - z2z - y2y, z2r + y2x, z2x - y2r); + GetForward().Set( y2x - z2r, 1.0F - z2z - x2x, x2r + z2y); + GetUp().Set ( y2r + z2x, z2y - x2r, 1.0F - y2y - x2x); +} + +// column-vector convention +// converts local(relative) -> global(world) +CVector CMatrix::MultiplyRotation(CVector const &direction) const { + // return plugin::CallAndReturn(matrix, vec); + return direction.x * GetRight() + direction.y * GetForward() + direction.z * GetUp(); +} + +// row-vector convention +// converts global(world) -> local(relative) +CVector CMatrix::MultiplyTransposedRotation(CVector const& direction) const { + //return plugin::CallAndReturn(direction, matrix); + const CVector &right = GetRight(); + const CVector &forward = GetForward(); + const CVector &up = GetUp(); + return direction.x * CVector(right.x, forward.x, up.x) + + direction.y * CVector(right.y, forward.y, up.y) + + direction.z * CVector(right.z, forward.z, up.z); +} + +void CMatrix::Translate(CVector const &offset) { + pos += offset; } -// rotate on 3 axes -void CMatrix::Rotate(float x, float y, float z) +void CMatrix::Translate(float x, float y, float z) { - ((void (__thiscall *)(CMatrix *, float, float, float))0x59B460)(this, x, y, z); + pos.x += x; + pos.y += y; + pos.z += z; +} + +void CMatrix::RotateX(float pitch, bool keepPos) +{ + //((void (__thiscall *)(CMatrix *, float))0x59B1E0)(this, pitch); + + // x' = x + // y' = c*y - s*z + // z' = s*y + c*z + + const float c = std::cos(pitch); + const float s = std::sin(pitch); + + CVector &right = GetRight(); + CVector &forward = GetForward(); + CVector &up = GetUp(); + + auto rotate = [&](CVector& direction) + { + const float y = direction.y; + const float z = direction.z; + direction.y = c * y - s * z; + direction.z = s * y + c * z; + }; + + rotate(right); + rotate(up); + rotate(at); + + if (!keepPos) { + rotate(pos); + } +} + +void CMatrix::RotateY(float roll, bool keepPos) +{ + //((void (__thiscall *)(CMatrix *, float))0x59B2C0)(this, roll); + + // x' = s*z + c*x + // y' = y + // z' = c*z - s*x + + const float c = std::cos(roll); + const float s = std::sin(roll); + + auto rotate = [&](CVector& v) + { + const float x = v.x; + const float z = v.z; + v.x = s * z + c * x; + v.z = c * z - s * x; + }; + + rotate(right); + rotate(up); + rotate(at); + + if (!keepPos) { + rotate(pos); + } +} + +void CMatrix::RotateZ(float yaw, bool keepPos) +{ + //((void (__thiscall *)(CMatrix *, float))0x59B390)(this, yaw); + + // x' = c*x - s*y + // y' = s*x + c*y + // z' = z + + const float c = std::cos(yaw); + const float s = std::sin(yaw); + + auto rotate = [&](CVector& v) + { + const float x = v.x; + const float y = v.y; + v.x = c * x - s * y; + v.y = s * x + c * y; + }; + + rotate(right); + rotate(up); + rotate(at); + + if (!keepPos) { + rotate(pos); + } +} + +// This function is the straightforward version of: +// this->ConvertToEulerAngles(yaw, roll, pitch, CMatrix::INTRINSIC | CMatrix::SEQUENCE_ZYX) +void CMatrix::Rotate(float pitch, float roll, float yaw, bool keepPos) +{ + // ((void (__thiscall *)(CMatrix *, float, float, float))0x59B460)(this, pitch, roll, yaw); + + const float cx = std::cos(pitch); + const float sx = std::sin(pitch); + const float cy = std::cos(roll); + const float sy = std::sin(roll); + const float cz = std::cos(yaw); + const float sz = std::sin(yaw); + + // Rotation matrix rows (intrinsic ZYX) + const CVector r0( + cy * cz, + cy * sz, + -sy + ); + + const CVector r1( + sx * sy * cz - cx * sz, + sx * sy * sz + cx * cz, + sx * cy + ); + + const CVector r2( + cx * sy * cz + sx * sz, + cx * sy * sz - sx * cz, + cx * cy + ); + + auto rotate = [&](const CVector& v) + { + return CVector( + r0.x * v.x + r0.y * v.y + r0.z * v.z, + r1.x * v.x + r1.y * v.y + r1.z * v.z, + r2.x * v.x + r2.y * v.y + r2.z * v.z + ); + }; + + right = rotate(right); + up = rotate(up); + at = rotate(at); + + if (!keepPos) { + pos = rotate(pos); + } + +} + +void CMatrix::Rotate(CVector const &rotation, bool keepPos) { + Rotate(rotation.x, rotation.y, rotation.z); } void CMatrix::Reorthogonalise() { - ((void (__thiscall *)(CMatrix *))0x59B6A0)(this); + //((void (__thiscall *)(CMatrix *))0x59B6A0)(this); + + CVector &right = GetRight(); + CVector &forward = GetForward(); + CVector &up = GetUp(); + + up = right.Cross(forward); + up.Normalise(); + right = forward.Cross(up); + right.Normalise(); + forward = up.Cross(right); + //forward.Normalise(); // unecessary, guaranteed to be normalized } -// similar to UpdateRW(RwMatrixTag *) void CMatrix::CopyToRwMatrix(RwMatrix *matrix) { - ((void (__thiscall *)(CMatrix *, RwMatrix *))0x59B8B0)(this, matrix); + //((void (__thiscall *)(CMatrix *, RwMatrix *))0x59B8B0)(this, matrix); + + UpdateRW(matrix); } -void CMatrix::SetRotate(CQuaternion const& quat) -{ - ((void (__thiscall *)(CMatrix *, CQuaternion const&))0x59BBF0)(this, quat); +void CMatrix::Scale(float x, float y, float z) { + //plugin::CallMethod<0x5A2E60, CMatrix *, float, float, float>(this, x, y, z); + + CVector &right = GetRight(); + CVector &forward = GetForward(); + CVector &up = GetUp(); + + right.x *= x; + right.y *= y; + right.z *= z; + + forward.x *= x; + forward.y *= y; + forward.z *= z; + + up.x *= x; + up.y *= y; + up.z *= z; } -void CMatrix::Scale(float scale) { - plugin::CallMethod<0x459350, CMatrix *, float>(this, scale); +void CMatrix::Scale(CVector const &scale) { + Scale(scale.x, scale.y, scale.z); } -void CMatrix::Scale(float x, float y, float z) { - plugin::CallMethod<0x459350, CMatrix *, float, float, float>(this, x, y, z); +void CMatrix::Scale(float scale) { + //plugin::CallMethod<0x459350, CMatrix *, float>(this, scale); + + Scale(scale, scale, scale); +} + +void CMatrix::ForceUpVector(CVector const &vecUp) { + //plugin::CallMethod<0x59B7E0, CMatrix *, CVector>(this, vecUp); + + CVector &right = GetRight(); + CVector &forward = GetForward(); + CVector &up = GetUp(); + + right = forward.Cross(vecUp); + forward = vecUp.Cross(right); + up = vecUp; +} + +void CMatrix::ForceUpVector(float x, float y, float z) { + ForceUpVector(CVector(x, y, z)); +} + +// flags must be between 0-23 +void CMatrix::ConvertToEulerAngles(float &initialAngle, float &intermediateAngle, float &finalAngle, CMatrix::eMatrixEulerFlags flags) const { + //plugin::CallMethod<0x59A840, const CMatrix*, float*, float*, float*, eMatrixEulerFlags>(this, &initial, &intermediate, &final, flags); + + constexpr float gimbalLockThreshold = 0.0000019073486f; + + const float matrix[3][3] = { + { GetRight().x, GetRight().y, GetRight().z }, + { GetForward().x, GetForward().y, GetForward().z }, + { GetUp().x, GetUp().y, GetUp().z } + }; + + const bool swap2ndAnd3rdSeq = (flags & __SWAP_2ND_3RD_SEQ) != 0; + + // compute permutation indeces + constexpr unsigned char BYTE_866D9C[4] = { 0, 1, 2, 0 }; + constexpr unsigned char BYTE_866D94[5] = { 1, 2, 0, 1, 0 }; + const unsigned char rowIndex = BYTE_866D9C[ flags >> 3 & 3u ]; + const unsigned char idxA = BYTE_866D94[ rowIndex - swap2ndAnd3rdSeq + 1 ]; + const unsigned char idxB = BYTE_866D94[ rowIndex + swap2ndAnd3rdSeq ]; + // The permutation indeces computation of the game as seen above seems a bit incorrect when idxA or idxB becomes 4 + // Looks like a minor oversight in the original code, so the computation below corrects it + // but for now I will leave it commented out + // unsigned char idx1 = ((flags >> 3) & 3); // primaryAxisIndex + // if (idx1 >= 3) idx1 -= 3; + // unsigned char idx2 = (1 + idx1 + swap2ndAnd3rdSeq); + // if (idx2 >= 3) idx2 -= 3; + // unsigned char idx3 = (2 + idx1 - swap2ndAnd3rdSeq); + // if (idx3 >= 3) idx3 -= 3; + + if (flags & eMatrixEulerFlags::EXTRINSIC) { + float A = matrix[rowIndex][idxA]; + float B = matrix[rowIndex][idxB]; + float hypotenuse = std::sqrt(B*B + A*A); + + if (hypotenuse > gimbalLockThreshold) { + initialAngle = std::atan2(B, A); + intermediateAngle = std::atan2(hypotenuse, matrix[rowIndex][rowIndex]); + finalAngle = std::atan2(matrix[idxB][rowIndex], -matrix[idxA][rowIndex]); + } else { + initialAngle = std::atan2(-matrix[idxB][idxA], matrix[idxB][idxB]); + intermediateAngle = std::atan2(-matrix[idxA][rowIndex], hypotenuse); + finalAngle = 0.0f; + } + } else { + float A = matrix[idxB][rowIndex]; + float diag = matrix[rowIndex][rowIndex]; + float hypotenuse = std::sqrt(A*A + diag*diag); + + if (hypotenuse > gimbalLockThreshold) { + initialAngle = std::atan2(matrix[idxA][idxB], matrix[idxA][idxA]); + intermediateAngle = std::atan2(-matrix[idxA][rowIndex], hypotenuse); + finalAngle = std::atan2(A, diag); + } else { + initialAngle = std::atan2(-matrix[idxB][idxA], matrix[idxB][idxB]); + intermediateAngle = std::atan2(-matrix[idxA][rowIndex], hypotenuse); + finalAngle = 0.0f; + } + } + + if (swap2ndAnd3rdSeq) { + initialAngle *= -1.0f; + intermediateAngle *= -1.0f; + finalAngle *= -1.0f; + } + + if (flags & eMatrixEulerFlags::SWAP_1ST_3RD_VALUES) { + std::swap(initialAngle, finalAngle); + } +} + +// flags must be between 0-23 +void CMatrix::ConvertFromEulerAngles(float initialAngle, float intermediateAngle, float finalAngle, CMatrix::eMatrixEulerFlags flags) { + //plugin::CallMethod<0x59AA40, CMatrix*, float, float, float, eMatrixEulerFlags>(this, initial, intermediate, final, flags); + + const bool swap2ndAnd3rdSeq = (flags & __SWAP_2ND_3RD_SEQ) != 0; + + constexpr unsigned char BYTE_866D9C[4] = { 0, 1, 2, 0 }; + constexpr unsigned char BYTE_866D94[5] = { 1, 2, 0, 1, 0 }; + const unsigned char idx1 = BYTE_866D9C[ flags >> 3 & 3u ]; + const unsigned char idx3 = BYTE_866D94[ idx1 - swap2ndAnd3rdSeq + 1 ]; + const unsigned char idx2 = BYTE_866D94[ idx1 + swap2ndAnd3rdSeq ]; + // The permutation indeces computation of the game as seen above seems a bit incorrect when idxA or idxB becomes 4 + // Looks like a minor oversight in the original code, so the computation below corrects it + // but for now I will leave it commented out + // unsigned char idx1 = ((flags >> 3) & 3); // primaryAxisIndex + // if (idx1 >= 3) idx1 -= 3; + // unsigned char idx2 = (1 + idx1 + swap2ndAnd3rdSeq); + // if (idx2 >= 3) idx2 -= 3; + // unsigned char idx3 = (2 + idx1 - swap2ndAnd3rdSeq); + // if (idx3 >= 3) idx3 -= 3; + + float matrix[3][3]; + + if (flags & eMatrixEulerFlags::SWAP_1ST_3RD_VALUES) { + std::swap(initialAngle, finalAngle); + } + + if (swap2ndAnd3rdSeq) { + initialAngle *= -1.0f; + intermediateAngle *= -1.0f; + finalAngle *= -1.0f; + } + + const float cX = cos(initialAngle); + const float cY = cos(intermediateAngle); + const float cZ = cos(finalAngle); + const float sX = sin(initialAngle); + const float sY = sin(intermediateAngle); + const float sZ = sin(finalAngle); + const float cX_cZ = cX * cZ; + const float cX_sZ = cX * sZ; + const float sX_cZ = sX * cZ; + const float sX_sZ = sX * sZ; + if (flags & eMatrixEulerFlags::EXTRINSIC) { + matrix[idx1][idx1] = cY; + matrix[idx1][idx2] = sX * sY; + matrix[idx1][idx3] = cX * sY; + + matrix[idx2][idx1] = sY * sZ; + matrix[idx2][idx2] = cX_cZ - sX_sZ * cY; + matrix[idx2][idx3] = -(cX_sZ * cY) - sX_cZ; + + matrix[idx3][idx1] = -(sY * cZ); + matrix[idx3][idx2] = sX_cZ * cY + cX_sZ; + matrix[idx3][idx3] = cX_cZ * cY - sX_sZ; + } else { + matrix[idx1][idx1] = cZ * cY; + matrix[idx1][idx2] = sX_cZ * sY - cX_sZ; + matrix[idx1][idx3] = cX_cZ * sY + sX_sZ; + + matrix[idx2][idx1] = sZ * cY; + matrix[idx2][idx2] = sX_sZ * sY + cX_cZ; + matrix[idx2][idx3] = cX_sZ * sY - sX_cZ; + + matrix[idx3][idx1] = -sY; + matrix[idx3][idx2] = sX * cY; + matrix[idx3][idx3] = cY * cX; + } + + GetRight().Set (matrix[0][0], matrix[0][1], matrix[0][2]); + GetForward().Set(matrix[1][0], matrix[1][1], matrix[1][2]); + GetUp().Set (matrix[2][0], matrix[2][1], matrix[2][2]); } void CMatrix::operator=(CMatrix const& rvalue) { - ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59BBC0)(this, rvalue); + //((void (__thiscall *)(CMatrix *, CMatrix const&))0x59BBC0)(this, rvalue); + + CopyOnlyMatrix(rvalue); + UpdateRW(); } void CMatrix::operator+=(CMatrix const& rvalue) { - ((void (__thiscall *)(CMatrix *, CMatrix const&))0x59ADF0)(this, rvalue); + //((void (__thiscall *)(CMatrix *, CMatrix const&))0x59ADF0)(this, rvalue); + + GetRight() += rvalue.GetRight(); + GetForward() += rvalue.GetForward(); + GetUp() += rvalue.GetUp(); + pos += rvalue.pos; } void CMatrix::operator*=(CMatrix const& rvalue) { - ((void (__thiscall *)(CMatrix *, CMatrix const&))0x411A80)(this, rvalue); + //((void (__thiscall *)(CMatrix *, CMatrix const&))0x411A80)(this, rvalue); + + *this = (*this * rvalue); } CMatrix operator*(CMatrix const&a, CMatrix const&b) { - CMatrix result; - ((void(__cdecl *)(CMatrix*, CMatrix const&, CMatrix const&))0x59BE30)(&result, a, b); + CMatrix result = CMatrix(); + + //((void(__cdecl *)(CMatrix*, CMatrix const&, CMatrix const&))0x59BE30)(&result, a, b); + + CVector const &a_right = a.GetRight(); + CVector const &a_forward = a.GetForward(); + CVector const &a_up = a.GetUp(); + + CVector const &b_right = b.GetRight(); + CVector const &b_forward = b.GetForward(); + CVector const &b_up = b.GetUp(); + + result.GetRight() = a_right * b_right.x + a_forward * b_right.y + a_up * b_right.z; + result.GetForward() = a_right * b_forward.x + a_forward * b_forward.y + a_up * b_forward.z; + result.GetUp() = a_right * b_up.x + a_forward * b_up.y + a_up * b_up.z; + result.pos = a_right * b.pos.x + a_forward * b.pos.y + a_up * b.pos.z + a.pos; + return result; } CVector operator*(CMatrix const&a, CVector const&b) { - CVector result; - ((void(__cdecl *)(CVector*, CMatrix const&, CVector const&))0x59C890)(&result, a, b); - return result; + //CVector result; + //((void(__cdecl *)(CVector*, CMatrix const&, CVector const&))0x59C890)(&result, a, b); + //return result; + + return a.MultiplyRotation(b) + a.pos; } CMatrix operator+(CMatrix const&a, CMatrix const&b) { CMatrix result; - ((void(__cdecl *)(CMatrix*, CMatrix const&, CMatrix const&))0x59BFA0)(&result, a, b); + //((void(__cdecl *)(CMatrix*, CMatrix const&, CMatrix const&))0x59BFA0)(&result, a, b); + + result.GetRight() = a.GetRight() + b.GetRight(); + result.GetForward() = a.GetForward() + b.GetForward(); + result.GetUp() = a.GetUp() + b.GetUp(); + result.pos = a.pos + b.pos; + return result; -} \ No newline at end of file +} + +bool operator==(CMatrix const&a, CMatrix const&b) { + return a.m_pAttachMatrix == b.m_pAttachMatrix + && a.m_bOwnsAttachedMatrix == b.m_bOwnsAttachedMatrix + && a.GetRight() == b.GetRight() + && a.GetForward() == b.GetForward() + && a.GetUp() == b.GetUp() + && a.pos == b.pos; +} diff --git a/plugin_sa/game_sa/CMatrix.h b/plugin_sa/game_sa/CMatrix.h index 12b8f76b..7835a449 100644 --- a/plugin_sa/game_sa/CMatrix.h +++ b/plugin_sa/game_sa/CMatrix.h @@ -11,12 +11,38 @@ class CMatrix { public: + // unused, but retained here as layout for the enum below + // struct tMatrixEulerFlags { + // bool swapXZ: 1; + // bool isExtrinsic: 1; // false = intrinsic, true = extrinsic + // bool swapYZ: 1; + // unsigned char primaryAxisIndex: 2; // index (0, 1, 2) into byte_866D9C[] that selects primary axis/order + // }; // { true, false, true, 2} = 0x15 = 21U: is Always used by the game + enum eMatrixEulerFlags : unsigned char { + SWAP_1ST_3RD_VALUES = 0x01, // The sequence is unaffected, but the Initial and Final values were swapped + __SWAP_2ND_3RD_SEQ = 0x04, // no need to use this flag as it is already applied at the sequence flags stated below + + INTRINSIC = 0x0, + EXTRINSIC = 0x2, + + // X = Pitch, Y = Roll, Z = Yaw + SEQUENCE_XYZ = 0x00, + SEQUENCE_YZX = 0x08, + SEQUENCE_ZXY = 0x10, + SEQUENCE_XZY = SEQUENCE_XYZ | __SWAP_2ND_3RD_SEQ, + SEQUENCE_YXZ = SEQUENCE_YZX | __SWAP_2ND_3RD_SEQ, + SEQUENCE_ZYX = SEQUENCE_ZXY | __SWAP_2ND_3RD_SEQ, + + DEFAULT = SEQUENCE_ZYX | SWAP_1ST_3RD_VALUES, + }; + VALIDATE_SIZE(eMatrixEulerFlags, 1); + // RwV3d-like: - CVector right; + CVector right; // x-axis unsigned int flags; - CVector up; + CVector up; // y-axis, the member name is supposed to be "forward" unsigned int pad1; - CVector at; + CVector at; // z-axis, the member name is supposed to be "up" unsigned int pad2; CVector pos; unsigned int pad3; @@ -36,33 +62,52 @@ class CMatrix { void Attach(RwMatrix *matrix, bool temporary); void Detach(); void CopyOnlyMatrix(CMatrix const& matrix); // copy base RwMatrix to another matrix - void Update(); // update RwMatrix with attaching matrix. This doesn't check if attaching matrix is present, so use it only if you know it is present. - // Using UpdateRW() is more safe since it perform this check. - void UpdateRW(); // update RwMatrix with attaching matrix. - void UpdateRW(RwMatrix *matrix); // update RwMatrix with this matrix - void SetUnity(); + void CopyToRwMatrix(RwMatrix *matrix); // similar to UpdateRW(RwMatrixTag *) + void ConvertToEulerAngles(float &initialAngle, float &intermediateAngle, float &finalAngle, eMatrixEulerFlags flags) const; + void ConvertFromEulerAngles(float initialAngle, float intermediateAngle, float finalAngle, eMatrixEulerFlags flags); + void ForceUpVector(CVector const &vecUp); + void ForceUpVector(float x, float y, float z); void ResetOrientation(); + void Scale(float scale); + void Scale(CVector const &scale); + void Scale(float x, float y, float z); void SetScale(float scale); // set (scaled) - void SetScale(float x, float y, float z); // set (scaled) + void SetScale(CVector const &scale); + void SetScale(float right, float forward, float up); // set (scaled) + void SetUnity(); + void SetTranslateOnly(CVector const &pos); void SetTranslateOnly(float x, float y, float z); + void SetTranslate(CVector const &pos); void SetTranslate(float x, float y, float z); // like previous + reset orientation - void SetRotateXOnly(float angle); - void SetRotateYOnly(float angle); - void SetRotateZOnly(float angle); - void SetRotateX(float angle); - void SetRotateY(float angle); - void SetRotateZ(float angle); - void SetRotate(float x, float y, float z); // set rotate on 3 axes - void RotateX(float angle); - void RotateY(float angle); - void RotateZ(float angle); - void Rotate(float x, float y, float z); // rotate on 3 axes + void SetRotateXOnly(float pitch); + void SetRotateYOnly(float roll); + void SetRotateZOnly(float yaw); + void SetRotateOnly(CVector const &rotation); + void SetRotateOnly(float pitch, float roll, float yaw); // sets the rotation on 3 axes + void SetRotateX(float pitch); + void SetRotateY(float roll); + void SetRotateZ(float yaw); + void SetRotate(CVector const &rotation); + void SetRotate(float pitch, float roll, float yaw); // sets the rotation on 3 axes + resets the position to origin(0, 0, 0) + void SetRotate(CQuaternion const& quat); + void RotateX(float pitch, bool keepPos = false); + void RotateY(float roll, bool keepPos = false); + void RotateZ(float yaw, bool keepPos = false); + void Rotate(CVector const &rotation, bool keepPos = false); + // This function is the straightforward version of: + // this->ConvertToEulerAngles(yaw, roll, pitch, CMatrix::INTRINSIC | CMatrix::SEQUENCE_ZYX) + void Rotate(float pitch, float roll, float yaw, bool keepPos = false); + + CVector MultiplyRotation(CVector const &direction) const; + CVector MultiplyTransposedRotation(CVector const& direction) const; + void Translate(CVector const &offset); void Translate(float x, float y, float z); // move the position void Reorthogonalise(); - void CopyToRwMatrix(RwMatrix *matrix); // similar to UpdateRW(RwMatrixTag *) - void SetRotate(CQuaternion const& quat); - void Scale(float scale); - void Scale(float x, float y, float z); + void Update(); // update RwMatrix with attaching matrix. This doesn't check if attaching matrix is present, so use it only if you know it is present. + // Using UpdateRW() is more safe since it perform this check. + void UpdateRW(); // update RwMatrix with attaching matrix. + void UpdateRW(RwMatrix *matrix); // update RwMatrix with this matrix + void UpdateMatrix(RwMatrix* rwMatrix); void operator=(CMatrix const& right); void operator+=(CMatrix const& right); void operator*=(CMatrix const& right); @@ -84,4 +129,5 @@ VALIDATE_SIZE(CMatrix, 0x48); CMatrix operator*(CMatrix const&a, CMatrix const&b); CVector operator*(CMatrix const&a, CVector const&b); -CMatrix operator+(CMatrix const&a, CMatrix const&b); \ No newline at end of file +CMatrix operator+(CMatrix const&a, CMatrix const&b); +bool operator==(CMatrix const&a, CMatrix const&b); diff --git a/plugin_sa/game_sa/common.cpp b/plugin_sa/game_sa/common.cpp index 9f619894..e899dfa9 100644 --- a/plugin_sa/game_sa/common.cpp +++ b/plugin_sa/game_sa/common.cpp @@ -66,15 +66,23 @@ bool InTwoPlayersMode() return ((bool (__cdecl *)())0x441390)(); } -CVector VectorSub(CVector const& from, CVector const& what) -{ - return ((CVector (__cdecl *)(CVector const&, CVector const&))0x40FE60)(from, what); -} - -CVector Multiply3x3(CMatrix const& matrix, CVector const& vec) -{ - return ((CVector (__cdecl *)(CMatrix const&, CVector const&))0x59C790)(matrix, vec); -} +// obsolete! Use CVector::operator-() or CVector::Diff instead +// CVector VectorSub(CVector const& from, CVector const& what) +// { +// return ((CVector (__cdecl *)(CVector const&, CVector const&))0x40FE60)(from, what); +// } + +// obsolete! Use CMatrix::MultiplyRotation instead +// CVector Multiply3x3(CMatrix const& matrix, CVector const& vec) +// { +// return plugin::CallAndReturn(matrix, vec); +// } + +// obsolete! Use CMatrix::MultiplyTransposedRotation instead +// CVector MultiplyTransposed3x3(CMatrix const& matrix, CVector const& vec) +// { +// return plugin::CallAndReturn(vec, matrix); +// } CWanted * FindPlayerWanted(int playerId) { diff --git a/plugin_sa/game_sa/common.h b/plugin_sa/game_sa/common.h index 25636847..3abade1d 100644 --- a/plugin_sa/game_sa/common.h +++ b/plugin_sa/game_sa/common.h @@ -40,10 +40,9 @@ CPlayerPed * FindPlayerPed(int playerId = -1); CVehicle * FindPlayerVehicle(int playerId = -1, bool bIncludeRemote = false); // 2 players are playing bool InTwoPlayersMode(); -// vectorsub -CVector VectorSub(CVector const& from, CVector const& what); -// matrix mul -CVector Multiply3x3(CMatrix const& matrix, CVector const& vec); +// CVector VectorSub(CVector const& from, CVector const& what); // obsolete! Use CVector::operator-() or CVector::Diff instead +// CVector Multiply3x3(CMatrix const& matrix, CVector const& vector); // obsolete! Use CMatrix::Multiply instead +// CVector MultiplyTransposed3x3(CMatrix const& matrix, CVector const& vector); // obsolete! Use CMatrix::TransposedMultiply instead // returns player wanted CWanted * FindPlayerWanted(int playerId = -1);