mirrors
/
raylib
mirror of https://github.com/raysan5/raylib
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fix QuaternionFromMatrix & QuaternionEquals (#2591) 

Co-authored-by: kirigiri hitomi <chenjw@hiscene.com>
This commit is contained in:
hitomi kirigri 2022-07-26 20:25:31 +08:00 committed by GitHub
parent d78177d246
commit 64710e6030
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 56 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

78
src/raymath.h
View File

@ -1802,30 +1802,60 @@ RMAPI Quaternion QuaternionFromMatrix(Matrix mat)
{
Quaternion result = { 0 };
if ((mat.m0 > mat.m5) && (mat.m0 > mat.m10))
{
float s = sqrtf(1.0f + mat.m0 - mat.m5 - mat.m10)*2;
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;
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;
}
else if (mat.m5 > mat.m10)
int biggestIndex = 0;
float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
if (fourXSquaredMinus1 > 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 = fourXSquaredMinus1;
biggestIndex = 1;
}
else
if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
{
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;
fourBiggestSquaredMinus1 = fourYSquaredMinus1;
biggestIndex = 2;
}
if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
{
fourBiggestSquaredMinus1 = fourZSquaredMinus1;
biggestIndex = 3;
}
float biggestVal = sqrtf(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;
float mult = 0.25f / biggestVal;
switch (biggestIndex)
{
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;
}