diff options
| author | hitomi kirigri <[email protected]> | 2022-07-26 20:25:31 +0800 |
|---|---|---|
| committer | GitHub <[email protected]> | 2022-07-26 14:25:31 +0200 |
| commit | 64710e60307165a9ffb98e4b576eee40a567a31d (patch) | |
| tree | 0c28edd72e1e26fa11aa4db160b971e01cd0fefd /src | |
| parent | d78177d2464d60221eaeefd1831d4834f48e6f59 (diff) | |
| download | raylib-64710e60307165a9ffb98e4b576eee40a567a31d.tar.gz raylib-64710e60307165a9ffb98e4b576eee40a567a31d.zip | |
fix QuaternionFromMatrix & QuaternionEquals (#2591)
Co-authored-by: kirigiri hitomi <[email protected]>
Diffstat (limited to 'src')
| -rw-r--r-- | src/raymath.h | 74 |
1 files changed, 54 insertions, 20 deletions
diff --git a/src/raymath.h b/src/raymath.h index 22fa1442..d292af1b 100644 --- a/src/raymath.h +++ b/src/raymath.h @@ -1802,30 +1802,60 @@ RMAPI Quaternion QuaternionFromMatrix(Matrix mat) { Quaternion result = { 0 }; - if ((mat.m0 > mat.m5) && (mat.m0 > mat.m10)) + float fourWSquaredMinus1 = mat.m0 + mat.m5 + mat.m10; + float fourXSquaredMinus1 = mat.m0 - mat.m5 - mat.m10; + float fourYSquaredMinus1 = mat.m5 - mat.m0 - mat.m10; + float fourZSquaredMinus1 = mat.m10 - mat.m0 - mat.m5; + + int biggestIndex = 0; + float fourBiggestSquaredMinus1 = fourWSquaredMinus1; + if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) { - float s = sqrtf(1.0f + mat.m0 - mat.m5 - mat.m10)*2; + fourBiggestSquaredMinus1 = fourXSquaredMinus1; + biggestIndex = 1; + } - result.x = 0.25f*s; - result.y = (mat.m4 + mat.m1)/s; - result.z = (mat.m2 + mat.m8)/s; - result.w = (mat.m9 - mat.m6)/s; + if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) + { + fourBiggestSquaredMinus1 = fourYSquaredMinus1; + biggestIndex = 2; } - else if (mat.m5 > mat.m10) + + if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) { - float s = sqrtf(1.0f + mat.m5 - mat.m0 - mat.m10)*2; - result.x = (mat.m4 + mat.m1)/s; - result.y = 0.25f*s; - result.z = (mat.m9 + mat.m6)/s; - result.w = (mat.m2 - mat.m8)/s; + fourBiggestSquaredMinus1 = fourZSquaredMinus1; + biggestIndex = 3; } - else + + float biggestVal = sqrtf(fourBiggestSquaredMinus1 + 1.0f) * 0.5f; + float mult = 0.25f / biggestVal; + + switch (biggestIndex) { - float s = sqrtf(1.0f + mat.m10 - mat.m0 - mat.m5)*2; - result.x = (mat.m2 + mat.m8)/s; - result.y = (mat.m9 + mat.m6)/s; - result.z = 0.25f*s; - result.w = (mat.m4 - mat.m1)/s; + case 0: + result.w = biggestVal; + result.x = (mat.m6 - mat.m9) * mult; + result.y = (mat.m8 - mat.m2) * mult; + result.z = (mat.m1 - mat.m4) * mult; + break; + case 1: + result.x = biggestVal; + result.w = (mat.m6 - mat.m9) * mult; + result.y = (mat.m1 + mat.m4) * mult; + result.z = (mat.m8 + mat.m2) * mult; + break; + case 2: + result.y = biggestVal; + result.w = (mat.m8 - mat.m2) * mult; + result.x = (mat.m1 + mat.m4) * mult; + result.z = (mat.m6 + mat.m9) * mult; + break; + case 3: + result.z = biggestVal; + result.w = (mat.m1 - mat.m4) * mult; + result.x = (mat.m8 + mat.m2) * mult; + result.y = (mat.m6 + mat.m9) * mult; + break; } return result; @@ -2009,10 +2039,14 @@ RMAPI Quaternion QuaternionTransform(Quaternion q, Matrix mat) // Check whether two given quaternions are almost equal RMAPI int QuaternionEquals(Quaternion p, Quaternion q) { - int result = ((fabsf(p.x - q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && + int result = (((fabsf(p.x - q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && ((fabsf(p.y - q.y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) && ((fabsf(p.z - q.z)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z))))) && - ((fabsf(p.w - q.w)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.w), fabsf(q.w))))); + ((fabsf(p.w - q.w)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.w), fabsf(q.w)))))) || + (((fabsf(p.x + q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && + ((fabsf(p.y + q.y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) && + ((fabsf(p.z + q.z)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z))))) && + ((fabsf(p.w + q.w)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.w), fabsf(q.w)))))); return result; } |