UnrealXR/raylib
2
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

fix QuaternionFromMatrix & QuaternionEquals (#2591)

Browse source 
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
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 }; 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 s = sqrtf(1.0f + mat.m0 - mat.m5 - mat.m10)*2; float fourYSquaredMinus1 = mat.m5 - mat.m0 - mat.m10;
float fourZSquaredMinus1 = mat.m10 - mat.m0 - mat.m5;
result.x = 0.25f*s; int biggestIndex = 0;
result.y = (mat.m4 + mat.m1)/s; float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
result.z = (mat.m2 + mat.m8)/s; if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
result.w = (mat.m9 - mat.m6)/s;
}
else if (mat.m5 > mat.m10)
{ {
float s = sqrtf(1.0f + mat.m5 - mat.m0 - mat.m10)*2; fourBiggestSquaredMinus1 = fourXSquaredMinus1;
result.x = (mat.m4 + mat.m1)/s; biggestIndex = 1;
result.y = 0.25f*s;
result.z = (mat.m9 + mat.m6)/s;
result.w = (mat.m2 - mat.m8)/s;
} }
else
if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
{ {
float s = sqrtf(1.0f + mat.m10 - mat.m0 - mat.m5)*2; fourBiggestSquaredMinus1 = fourYSquaredMinus1;
result.x = (mat.m2 + mat.m8)/s; biggestIndex = 2;
result.y = (mat.m9 + mat.m6)/s; }
result.z = 0.25f*s;
result.w = (mat.m4 - mat.m1)/s; 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; return result;
@ -2009,10 +2039,14 @@ RMAPI Quaternion QuaternionTransform(Quaternion q, Matrix mat)
// Check whether two given quaternions are almost equal // Check whether two given quaternions are almost equal
RMAPI int QuaternionEquals(Quaternion p, Quaternion q) 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.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.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; return result;
} }