From 6eb0cacd0a18b79a4b6dc3b1219ff3c19fce0bd7 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Sat, 6 Aug 2016 23:02:17 -0400 Subject: [PATCH 1/5] Added Quat_FromMtx(). This converts a matrix to a quaternion. Quat_FromMtx() added. --- include/c3d/maths.h | 8 ++++++ source/maths/quat_frommtx.c | 53 +++++++++++++++++++++++++++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 source/maths/quat_frommtx.c diff --git a/include/c3d/maths.h b/include/c3d/maths.h index 1587231..9113cf3 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -621,6 +621,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[out] q Output Quaternion + * @param[in] m Input Matrix + */ +void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m); + /** * @brief Identity Quaternion * @return Identity Quaternion diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c new file mode 100644 index 0000000..eba7e96 --- /dev/null +++ b/source/maths/quat_frommtx.c @@ -0,0 +1,53 @@ +#include + +void Quat_FromMtx(C3D_FQuat* q, 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; + + //Check the main diagonal of the passed-in matrix for positive/negative signs. + trace = m.r[0].c[0] + m.r[1].c[1] + m.r[2].c[2]; + 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].c[2] - m.r[2].c[1]) * sqrtTrace; + q->y = (m.r[2].c[0] - m.r[0].c[2]) * sqrtTrace; + q->z = (m.r[0].c[1] - m.r[1].c[0]) * 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].c[0] > m.r[1].c[1] && m.r[0].c[0] > m.r[2].c[2]) + { + sqrtTrace = 2.0f * sqrtf(1.0f + m.r[0].c[0] - m.r[1].c[1] - m.r[2].c[2]); + q->w = (m.r[2].c[1] - m.r[1].c[2]) / sqrtTrace; + q->x = 0.25f * sqrtTrace; + q->y = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; + q->z = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + } + else if (m.r[1].c[1] > m.r[2].c[2]) + { + sqrtTrace = 2.0f * sqrtf(1.0f + m.r[1].c[1] - m.r[0].c[0] - m.r[2].c[2]); + q->w = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + q->x = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; + q->y = 0.25f * sqrtTrace; + q->z = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; + } + else + { + sqrtTrace = 2.0f * sqrtf(1.0f + m.r[2].c[2] - m.r[0].c[0] - m.r[1].c[1]); + q->w = (m.r[1].c[0] - m.r[0].c[1]) / sqrtTrace; + q->x = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + q->y = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; + q->z = 0.25f * sqrtTrace; + } + } +} From 2efedfe8d23bbe75d8cd3d9126105f93ddc43c35 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Sun, 7 Aug 2016 08:12:52 -0400 Subject: [PATCH 2/5] Changed function declaration to match what was requested. --- include/c3d/maths.h | 4 ++-- source/maths/quat_frommtx.c | 36 +++++++++++++++++++----------------- 2 files changed, 21 insertions(+), 19 deletions(-) diff --git a/include/c3d/maths.h b/include/c3d/maths.h index 9113cf3..76e3bdd 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -624,10 +624,10 @@ 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[out] q Output Quaternion * @param[in] m Input Matrix + * @return Generated Quaternion */ -void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m); +C3D_FQuat Quat_FromMtx(C3D_Mtx m); /** * @brief Identity Quaternion diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c index eba7e96..f9ae941 100644 --- a/source/maths/quat_frommtx.c +++ b/source/maths/quat_frommtx.c @@ -1,6 +1,6 @@ #include -void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m) +C3D_FQuat Quat_FromMtx(C3D_Mtx m) { //Taken from Gamasutra: //http://www.gamasutra.com/view/feature/131686/rotating_objects_using_quaternions.php @@ -9,6 +9,7 @@ void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m) //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].c[0] + m.r[1].c[1] + m.r[2].c[2]; @@ -16,11 +17,11 @@ void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m) { //Diagonal is positive. sqrtTrace = sqrtf(trace + 1.0f); - q->w = sqrtTrace / 2.0f; + q.w = sqrtTrace / 2.0f; sqrtTrace = 0.5 / sqrtTrace; - q->x = (m.r[1].c[2] - m.r[2].c[1]) * sqrtTrace; - q->y = (m.r[2].c[0] - m.r[0].c[2]) * sqrtTrace; - q->z = (m.r[0].c[1] - m.r[1].c[0]) * sqrtTrace; + q.x = (m.r[1].c[2] - m.r[2].c[1]) * sqrtTrace; + q.y = (m.r[2].c[0] - m.r[0].c[2]) * sqrtTrace; + q.z = (m.r[0].c[1] - m.r[1].c[0]) * sqrtTrace; } else { @@ -28,26 +29,27 @@ void Quat_FromMtx(C3D_FQuat* q, C3D_Mtx m) if (m.r[0].c[0] > m.r[1].c[1] && m.r[0].c[0] > m.r[2].c[2]) { sqrtTrace = 2.0f * sqrtf(1.0f + m.r[0].c[0] - m.r[1].c[1] - m.r[2].c[2]); - q->w = (m.r[2].c[1] - m.r[1].c[2]) / sqrtTrace; - q->x = 0.25f * sqrtTrace; - q->y = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; - q->z = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + q.w = (m.r[2].c[1] - m.r[1].c[2]) / sqrtTrace; + q.x = 0.25f * sqrtTrace; + q.y = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; + q.z = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; } else if (m.r[1].c[1] > m.r[2].c[2]) { sqrtTrace = 2.0f * sqrtf(1.0f + m.r[1].c[1] - m.r[0].c[0] - m.r[2].c[2]); - q->w = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; - q->x = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; - q->y = 0.25f * sqrtTrace; - q->z = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; + q.w = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + q.x = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; + q.y = 0.25f * sqrtTrace; + q.z = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; } else { sqrtTrace = 2.0f * sqrtf(1.0f + m.r[2].c[2] - m.r[0].c[0] - m.r[1].c[1]); - q->w = (m.r[1].c[0] - m.r[0].c[1]) / sqrtTrace; - q->x = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; - q->y = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; - q->z = 0.25f * sqrtTrace; + q.w = (m.r[1].c[0] - m.r[0].c[1]) / sqrtTrace; + q.x = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + q.y = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; + q.z = 0.25f * sqrtTrace; } } + return q; } From b18b44bbf8e3153c733df8218886aec3cf4fb350 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Sun, 7 Aug 2016 08:17:48 -0400 Subject: [PATCH 3/5] Passing matrix by reference. --- include/c3d/maths.h | 2 +- source/maths/quat_frommtx.c | 38 ++++++++++++++++++------------------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/include/c3d/maths.h b/include/c3d/maths.h index 76e3bdd..bfccae6 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -627,7 +627,7 @@ void Mtx_FromQuat(C3D_Mtx* m, C3D_FQuat q); * @param[in] m Input Matrix * @return Generated Quaternion */ -C3D_FQuat Quat_FromMtx(C3D_Mtx m); +C3D_FQuat Quat_FromMtx(C3D_Mtx* m); /** * @brief Identity Quaternion diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c index f9ae941..b1981f3 100644 --- a/source/maths/quat_frommtx.c +++ b/source/maths/quat_frommtx.c @@ -1,6 +1,6 @@ #include -C3D_FQuat Quat_FromMtx(C3D_Mtx m) +C3D_FQuat Quat_FromMtx(C3D_Mtx* m) { //Taken from Gamasutra: //http://www.gamasutra.com/view/feature/131686/rotating_objects_using_quaternions.php @@ -12,42 +12,42 @@ C3D_FQuat Quat_FromMtx(C3D_Mtx m) C3D_FQuat q; //Check the main diagonal of the passed-in matrix for positive/negative signs. - trace = m.r[0].c[0] + m.r[1].c[1] + m.r[2].c[2]; + trace = m->r[0].c[0] + m->r[1].c[1] + m->r[2].c[2]; 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].c[2] - m.r[2].c[1]) * sqrtTrace; - q.y = (m.r[2].c[0] - m.r[0].c[2]) * sqrtTrace; - q.z = (m.r[0].c[1] - m.r[1].c[0]) * sqrtTrace; + q.x = (m->r[1].c[2] - m->r[2].c[1]) * sqrtTrace; + q.y = (m->r[2].c[0] - m->r[0].c[2]) * sqrtTrace; + q.z = (m->r[0].c[1] - m->r[1].c[0]) * 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].c[0] > m.r[1].c[1] && m.r[0].c[0] > m.r[2].c[2]) + if (m->r[0].c[0] > m->r[1].c[1] && m->r[0].c[0] > m->r[2].c[2]) { - sqrtTrace = 2.0f * sqrtf(1.0f + m.r[0].c[0] - m.r[1].c[1] - m.r[2].c[2]); - q.w = (m.r[2].c[1] - m.r[1].c[2]) / sqrtTrace; + sqrtTrace = 2.0f * sqrtf(1.0f + m->r[0].c[0] - m->r[1].c[1] - m->r[2].c[2]); + q.w = (m->r[2].c[1] - m->r[1].c[2]) / sqrtTrace; q.x = 0.25f * sqrtTrace; - q.y = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; - q.z = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; + q.y = (m->r[0].c[1] - m->r[1].c[0]) / sqrtTrace; + q.z = (m->r[0].c[2] - m->r[2].c[0]) / sqrtTrace; } - else if (m.r[1].c[1] > m.r[2].c[2]) + else if (m->r[1].c[1] > m->r[2].c[2]) { - sqrtTrace = 2.0f * sqrtf(1.0f + m.r[1].c[1] - m.r[0].c[0] - m.r[2].c[2]); - q.w = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; - q.x = (m.r[0].c[1] - m.r[1].c[0]) / sqrtTrace; + sqrtTrace = 2.0f * sqrtf(1.0f + m->r[1].c[1] - m->r[0].c[0] - m->r[2].c[2]); + q.w = (m->r[0].c[2] - m->r[2].c[0]) / sqrtTrace; + q.x = (m->r[0].c[1] - m->r[1].c[0]) / sqrtTrace; q.y = 0.25f * sqrtTrace; - q.z = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; + q.z = (m->r[1].c[2] - m->r[2].c[1]) / sqrtTrace; } else { - sqrtTrace = 2.0f * sqrtf(1.0f + m.r[2].c[2] - m.r[0].c[0] - m.r[1].c[1]); - q.w = (m.r[1].c[0] - m.r[0].c[1]) / sqrtTrace; - q.x = (m.r[0].c[2] - m.r[2].c[0]) / sqrtTrace; - q.y = (m.r[1].c[2] - m.r[2].c[1]) / sqrtTrace; + sqrtTrace = 2.0f * sqrtf(1.0f + m->r[2].c[2] - m->r[0].c[0] - m->r[1].c[1]); + q.w = (m->r[1].c[0] - m->r[0].c[1]) / sqrtTrace; + q.x = (m->r[0].c[2] - m->r[2].c[0]) / sqrtTrace; + q.y = (m->r[1].c[2] - m->r[2].c[1]) / sqrtTrace; q.z = 0.25f * sqrtTrace; } } From fa348aadf3d0194d190bbc46b0b19ebab31b26f3 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Sun, 7 Aug 2016 09:58:02 -0400 Subject: [PATCH 4/5] Adding const as directed. --- include/c3d/maths.h | 2 +- source/maths/quat_frommtx.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/c3d/maths.h b/include/c3d/maths.h index bfccae6..8f100a5 100644 --- a/include/c3d/maths.h +++ b/include/c3d/maths.h @@ -627,7 +627,7 @@ void Mtx_FromQuat(C3D_Mtx* m, C3D_FQuat q); * @param[in] m Input Matrix * @return Generated Quaternion */ -C3D_FQuat Quat_FromMtx(C3D_Mtx* m); +C3D_FQuat Quat_FromMtx(const C3D_Mtx* m); /** * @brief Identity Quaternion diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c index b1981f3..64bafc5 100644 --- a/source/maths/quat_frommtx.c +++ b/source/maths/quat_frommtx.c @@ -1,6 +1,6 @@ #include -C3D_FQuat Quat_FromMtx(C3D_Mtx* m) +C3D_FQuat Quat_FromMtx(const C3D_Mtx* m) { //Taken from Gamasutra: //http://www.gamasutra.com/view/feature/131686/rotating_objects_using_quaternions.php From ade43ca4d2b16fe39d038e907b9a7cf030fc4446 Mon Sep 17 00:00:00 2001 From: Thompson Lee Date: Sun, 7 Aug 2016 10:01:02 -0400 Subject: [PATCH 5/5] Converted c[] to xyzw format. Should not be accidentally transposing the matrix. --- source/maths/quat_frommtx.c | 36 ++++++++++++++++++------------------ 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/source/maths/quat_frommtx.c b/source/maths/quat_frommtx.c index 64bafc5..0a34c1c 100644 --- a/source/maths/quat_frommtx.c +++ b/source/maths/quat_frommtx.c @@ -12,42 +12,42 @@ C3D_FQuat Quat_FromMtx(const C3D_Mtx* m) C3D_FQuat q; //Check the main diagonal of the passed-in matrix for positive/negative signs. - trace = m->r[0].c[0] + m->r[1].c[1] + m->r[2].c[2]; + 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].c[2] - m->r[2].c[1]) * sqrtTrace; - q.y = (m->r[2].c[0] - m->r[0].c[2]) * sqrtTrace; - q.z = (m->r[0].c[1] - m->r[1].c[0]) * 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].c[0] > m->r[1].c[1] && m->r[0].c[0] > m->r[2].c[2]) + 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].c[0] - m->r[1].c[1] - m->r[2].c[2]); - q.w = (m->r[2].c[1] - m->r[1].c[2]) / sqrtTrace; + 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].c[1] - m->r[1].c[0]) / sqrtTrace; - q.z = (m->r[0].c[2] - m->r[2].c[0]) / 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].c[1] > m->r[2].c[2]) + else if (m->r[1].y > m->r[2].z) { - sqrtTrace = 2.0f * sqrtf(1.0f + m->r[1].c[1] - m->r[0].c[0] - m->r[2].c[2]); - q.w = (m->r[0].c[2] - m->r[2].c[0]) / sqrtTrace; - q.x = (m->r[0].c[1] - m->r[1].c[0]) / sqrtTrace; + 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].c[2] - m->r[2].c[1]) / sqrtTrace; + q.z = (m->r[1].z - m->r[2].y) / sqrtTrace; } else { - sqrtTrace = 2.0f * sqrtf(1.0f + m->r[2].c[2] - m->r[0].c[0] - m->r[1].c[1]); - q.w = (m->r[1].c[0] - m->r[0].c[1]) / sqrtTrace; - q.x = (m->r[0].c[2] - m->r[2].c[0]) / sqrtTrace; - q.y = (m->r[1].c[2] - m->r[2].c[1]) / sqrtTrace; + 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; } }