diff --git a/Makefile b/Makefile index 0bf9221..3d3176d 100644 --- a/Makefile +++ b/Makefile @@ -21,7 +21,7 @@ VERSION := $(CITRO3D_MAJOR).$(CITRO3D_MINOR).$(CITRO3D_PATCH) # DATA is a list of directories containing data files # INCLUDES is a list of directories containing header files #--------------------------------------------------------------------------------- -TARGET := $(notdir $(CURDIR)) +TARGET := citro3d BUILD := build SOURCES := source \ source/maths diff --git a/include/c3d/maths.h b/include/c3d/maths.h index e7986fa..215a4ce 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -489,6 +489,12 @@ void Mtx_PerspStereoTilt(C3D_Mtx* mtx, float fovy, float aspect, float near, flo * @param[in] isLeftHanded If true, output matrix is left-handed. If false, output matrix is right-handed. */ void Mtx_LookAt(C3D_Mtx* out, C3D_FVec cameraPosition, C3D_FVec cameraTarget, C3D_FVec cameraUpVector, bool isLeftHanded); + +/** + *@brief Transposes the matrix. Row => Column, and vice versa. + *@param[in,out] out Output matrix. + */ +void Mtx_Transpose(C3D_Mtx* out); ///@} ///@name Quaternion Math @@ -621,6 +627,14 @@ C3D_FQuat Quat_RotateZ(C3D_FQuat q, float r, bool bRightSide); */ void Mtx_FromQuat(C3D_Mtx* m, C3D_FQuat q); +/** + * @brief Get Quaternion equivalent to 4x4 matrix + * @note If the matrix is orthogonal or special orthogonal, where determinant(matrix) = +1.0f, then the matrix can be converted. + * @param[in] m Input Matrix + * @return Generated Quaternion + */ +C3D_FQuat Quat_FromMtx(const C3D_Mtx* m); + /** * @brief Identity Quaternion * @return Identity Quaternion @@ -677,4 +691,22 @@ static inline C3D_FVec FVec3_CrossQuat(C3D_FVec v, C3D_FQuat q) * @return C3D_FQuat The Quaternion equivalent with the pitch, yaw, and roll orientations applied. */ C3D_FQuat Quat_FromPitchYawRoll(float pitch, float yaw, float roll, bool bRightSide); + +/** + * @brief Quaternion Look At + * @param[in] source C3D_FVec Starting position. Origin of rotation. + * @param[in] target C3D_FVec Target position to orient towards. + * @param[in] forwardVector C3D_FVec The Up vector. + * @param[in] upVector C3D_FVec The Up vector. + * @return Quaternion rotation. + */ +C3D_FQuat Quat_LookAt(C3D_FVec source, C3D_FVec target, C3D_FVec forwardVector, C3D_FVec upVector); + +/** + * @brief Quaternion, created from a given axis and angle in radians. + * @param[in] axis C3D_FVec The axis to rotate around at. + * @param[in] angle float The angle to rotate. Unit: Radians + * @return Quaternion rotation based on the axis and angle. Axis doesn't have to be orthogonal. + */ +C3D_FQuat Quat_FromAxisAngle(C3D_FVec axis, float angle); ///@} diff --git a/source/maths/mtx_transpose.c b/source/maths/mtx_transpose.c new file mode 100644 index 0000000..47ad6fc --- /dev/null +++ b/source/maths/mtx_transpose.c @@ -0,0 +1,15 @@ +#include + +void Mtx_Transpose(C3D_Mtx* out) +{ + float swap; + for (int i = 0; i <= 2; i++) + { + for (int j = 2-i; j >= 0; j--) + { + swap = out->r[i].c[j]; + out->r[i].c[j] = out->r[3-j].c[3-i]; + out->r[3-j].c[3-i] = swap; + } + } +} diff --git a/source/maths/quat_fromaxisangle.c b/source/maths/quat_fromaxisangle.c new file mode 100644 index 0000000..6edd542 --- /dev/null +++ b/source/maths/quat_fromaxisangle.c @@ -0,0 +1,9 @@ +#include + +C3D_FQuat Quat_FromAxisAngle(C3D_FVec axis, float angle) +{ + float halfAngle = angle / 2.0f; + float scale = sinf(halfAngle); + axis = Quat_Normalize(axis); + return Quat_New(axis.x * scale, axis.y * scale, axis.z * scale, cosf(halfAngle)); +} diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c new file mode 100644 index 0000000..0a34c1c --- /dev/null +++ b/source/maths/quat_frommtx.c @@ -0,0 +1,55 @@ +#include + +C3D_FQuat Quat_FromMtx(const C3D_Mtx* m) +{ + //Taken from Gamasutra: + //http://www.gamasutra.com/view/feature/131686/rotating_objects_using_quaternions.php + //Expanded upon from: + //http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ + + //Variables we need. + float trace, sqrtTrace; + C3D_FQuat q; + + //Check the main diagonal of the passed-in matrix for positive/negative signs. + trace = m->r[0].x + m->r[1].y + m->r[2].z; + if (trace > 0.0f) + { + //Diagonal is positive. + sqrtTrace = sqrtf(trace + 1.0f); + q.w = sqrtTrace / 2.0f; + sqrtTrace = 0.5 / sqrtTrace; + q.x = (m->r[1].z - m->r[2].y) * sqrtTrace; + q.y = (m->r[2].x - m->r[0].z) * sqrtTrace; + q.z = (m->r[0].y - m->r[1].x) * sqrtTrace; + } + else + { + //Diagonal is negative or equals to zero. We need to identify which major diagonal element has the greatest value. + if (m->r[0].x > m->r[1].y && m->r[0].x > m->r[2].z) + { + sqrtTrace = 2.0f * sqrtf(1.0f + m->r[0].x - m->r[1].y - m->r[2].z); + q.w = (m->r[2].y - m->r[1].z) / sqrtTrace; + q.x = 0.25f * sqrtTrace; + q.y = (m->r[0].y - m->r[1].x) / sqrtTrace; + q.z = (m->r[0].z - m->r[2].x) / sqrtTrace; + } + else if (m->r[1].y > m->r[2].z) + { + sqrtTrace = 2.0f * sqrtf(1.0f + m->r[1].y - m->r[0].x - m->r[2].z); + q.w = (m->r[0].z - m->r[2].x) / sqrtTrace; + q.x = (m->r[0].y - m->r[1].x) / sqrtTrace; + q.y = 0.25f * sqrtTrace; + q.z = (m->r[1].z - m->r[2].y) / sqrtTrace; + } + else + { + sqrtTrace = 2.0f * sqrtf(1.0f + m->r[2].z - m->r[0].x - m->r[1].y); + q.w = (m->r[1].x - m->r[0].y) / sqrtTrace; + q.x = (m->r[0].z - m->r[2].x) / sqrtTrace; + q.y = (m->r[1].z - m->r[2].y) / sqrtTrace; + q.z = 0.25f * sqrtTrace; + } + } + return q; +} diff --git a/source/maths/quat_lookat.c b/source/maths/quat_lookat.c new file mode 100644 index 0000000..0ba4c5c --- /dev/null +++ b/source/maths/quat_lookat.c @@ -0,0 +1,18 @@ +#include +#include + +C3D_FQuat Quat_LookAt(C3D_FVec source, C3D_FVec target, C3D_FVec forwardVector, C3D_FVec upVector) +{ + C3D_FVec forward = FVec3_Normalize(FVec3_Subtract(target, source)); + float dot = FVec3_Dot(forwardVector, forward); + + //Instead of checking at very high precision, this check includes margin of rounding errors for floats. + if (dot + 1.0f < 0.0001f) //If dot product is -1.0f, the resulting quaternion is rotated 180 degrees on the Up axis. + return Quat_FromAxisAngle(upVector, M_TAU/2.0f); + if (dot - 1.0f > -0.0001f) //If dot product is 1.0f, the resulting quaternion is an identity quaternion; there's no rotation. + return Quat_Identity(); + + float rotationAngle = acosf(dot); + C3D_FVec rotationAxis = FVec3_Cross(forwardVector, forward); + return Quat_FromAxisAngle(rotationAxis, rotationAngle); +} diff --git a/test/3ds/source/main.cpp b/test/3ds/source/main.cpp index decdebd..018509a 100644 --- a/test/3ds/source/main.cpp +++ b/test/3ds/source/main.cpp @@ -7,6 +7,7 @@ #include #include <3ds.h> #include +#include #include "vshader_shbin.h" @@ -840,6 +841,104 @@ void ortho_test() C3D_RenderTargetDelete(tex); } +void transpose_test() +{ + consoleClear(); + C3D_Mtx modelView, check; + + Mtx_Identity(&modelView); + Mtx_Translate(&modelView, ((float)(rand() % 100)) + 5.0f, ((float)(rand() % 100)) + 5.0f, ((float)(rand() % 100)) + 5.0f, true); + Mtx_RotateX(&modelView, (float)(rand() % 180) * (acos(-1)/180.0f), true); + Mtx_RotateY(&modelView, (float)(rand() % 180) * (acos(-1)/180.0f), true); + Mtx_RotateZ(&modelView, (float)(rand() % 180) * (acos(-1)/180.0f), true); + Mtx_Copy(&check, &modelView); + + std::printf("Random Translation:\n"); + for (int i = 0; i < 16; i++) + { + std::printf("%2.2f ", modelView.m[i]); + if (i % 4 == 3) + std::printf("\n"); + } + + Mtx_Transpose(&modelView); + + std::printf("Random Translation Transposed:\n"); + for (int i = 0; i < 16; i++) + { + std::printf("%2.2f ", modelView.m[i]); + if (i % 4 == 3) + std::printf("\n"); + } + + Mtx_Transpose(&modelView); + + std::printf("Rand-Trans Transposed Transposed:\n"); + for (int i = 0; i < 16; i++) + { + std::printf("%2.2f ", modelView.m[i]); + if (i % 4 == 3) + std::printf("\n"); + } + + bool transposeFailCheck = false; + for (int i = 0; i < 16; i++) + { + if (modelView.m[i] != check.m[i]) + { + transposeFailCheck = true; + break; + } + } + + bool transInvFailCheck = false; + Mtx_Inverse(&modelView); + Mtx_Transpose(&modelView); + Mtx_Transpose(&check); + Mtx_Inverse(&check); + for (int i = 0; i < 16; i++) + { + if (fabsf(modelView.m[i] - check.m[i]) > 0.001f) + { + std::printf("%f != %f\n", modelView.m[i], check.m[i]); + transInvFailCheck = true; + break; + } + } + + std::printf("Transposed Inverse of RandMatrix:\n"); + for (int i = 0; i < 16; i++) + { + std::printf("%2.2f ", modelView.m[i]); + if (i % 4 == 3) + std::printf("\n"); + } + + std::printf("Inverse Transposed of RandMatrix:\n"); + for (int i = 0; i < 16; i++) + { + std::printf("%2.2f ", check.m[i]); + if (i % 4 == 3) + std::printf("\n"); + } + + std::printf("\n"); + std::printf("Transpose(Transpose(A)) = A? %s\n", (transposeFailCheck ? "False" : "True")); + std::printf("Inv(Trans(A))=Trans(Inv(A))? %s\n", (transInvFailCheck ? "False" : "True")); + + while(aptMainLoop()) + { + gspWaitForVBlank(); + + hidScanInput(); + u32 down = hidKeysDown(); + if(down & (KEY_START|KEY_SELECT)) + break; + } +} + + + typedef struct { const char *name; @@ -854,6 +953,7 @@ test_t tests[] = { "Mtx_Persp", persp_test, }, { "Mtx_PerspStereo", stereo_test, }, { "Mtx_Ortho", ortho_test, }, + { "Mtx_Transpose", transpose_test, }, }; const size_t num_tests = sizeof(tests)/sizeof(tests[0]); diff --git a/test/pc/main.cpp b/test/pc/main.cpp index 3a5d8a9..b39f908 100644 --- a/test/pc/main.cpp +++ b/test/pc/main.cpp @@ -737,6 +737,31 @@ check_matrix(generator_t &gen, distribution_t &dist) assert(Mtx_MultiplyFVecH(&m, FVec3_New(v.x, v.y, v.z)) == glm::mat4x3(g)*v); } + + // check matrix transpose + { + C3D_Mtx m; + glm::mat4 check; + + randomMatrix(m, gen, dist); + + //Reducing rounding errors, and copying the values over to the check matrix. + for(size_t i = 0; i < 16; ++i) + { + m.m[i] = static_cast(m.m[i]); + } + + check = loadMatrix(m); + + Mtx_Transpose(&m); + Mtx_Transpose(&m); + assert(m == glm::transpose(glm::transpose(check))); + + //Comparing inverse(transpose(m)) == transpose(inverse(m)) + Mtx_Transpose(&m); + Mtx_Inverse(&m); + assert(m == glm::transpose(glm::inverse(check))); + } } }