From 49f1dad6b9deeb769e384ae547fca9f64b90bf79 Mon Sep 17 00:00:00 2001 From: jussi Date: Fri, 19 Aug 2022 13:38:09 +0300 Subject: Moved to raylib 4.2.0. Renamed some directory functions to raylib 4.2.0 conventions. Removed GenMeshBinormals and GetRayCollisionModel. Sound and music pan. --- include/raymath.h | 511 +++++++++++++++++++++++++++++++++++++++++------------- 1 file changed, 386 insertions(+), 125 deletions(-) (limited to 'include/raymath.h') diff --git a/include/raymath.h b/include/raymath.h index 9714962..fbe4cea 100644 --- a/include/raymath.h +++ b/include/raymath.h @@ -18,14 +18,14 @@ * - Functions are always self-contained, no function use another raymath function inside, * required code is directly re-implemented inside * - Functions input parameters are always received by value (2 unavoidable exceptions) -* - Functions use always a "result" anmed variable for return +* - Functions use always a "result" variable for return * - Functions are always defined inline * - Angles are always in radians (DEG2RAD/RAD2DEG macros provided for convenience) * * * LICENSE: zlib/libpng * -* Copyright (c) 2015-2021 Ramon Santamaria (@raysan5) +* Copyright (c) 2015-2022 Ramon Santamaria (@raysan5) * * This software is provided "as-is", without any express or implied warranty. In no event * will the authors be held liable for any damages arising from the use of this software. @@ -77,6 +77,10 @@ #define PI 3.14159265358979323846f #endif +#ifndef EPSILON + #define EPSILON 0.000001f +#endif + #ifndef DEG2RAD #define DEG2RAD (PI/180.0f) #endif @@ -154,7 +158,7 @@ typedef struct float16 { float v[16]; } float16; -#include // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), fminf(), fmaxf(), fabs() +#include // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), floor(), fminf(), fmaxf(), fabs() //---------------------------------------------------------------------------------- // Module Functions Definition - Utils math @@ -189,7 +193,23 @@ RMAPI float Normalize(float value, float start, float end) // Remap input value within input range to output range RMAPI float Remap(float value, float inputStart, float inputEnd, float outputStart, float outputEnd) { - float result =(value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart; + float result = (value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart; + + return result; +} + +// Wrap input value from min to max +RMAPI float Wrap(float value, float min, float max) +{ + float result = value - (max - min)*floorf((value - min)/(max - min)); + + return result; +} + +// Check whether two given floats are almost equal +RMAPI int FloatEquals(float x, float y) +{ + int result = (fabsf(x - y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y)))); return result; } @@ -278,11 +298,19 @@ RMAPI float Vector2Distance(Vector2 v1, Vector2 v2) return result; } +// Calculate square distance between two vectors +RMAPI float Vector2DistanceSqr(Vector2 v1, Vector2 v2) +{ + float result = ((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y)); + + return result; +} + // Calculate angle from two vectors RMAPI float Vector2Angle(Vector2 v1, Vector2 v2) { float result = atan2f(v2.y, v2.x) - atan2f(v1.y, v1.x); - + return result; } @@ -334,6 +362,21 @@ RMAPI Vector2 Vector2Normalize(Vector2 v) return result; } +// Transforms a Vector2 by a given Matrix +RMAPI Vector2 Vector2Transform(Vector2 v, Matrix mat) +{ + Vector2 result = { 0 }; + + float x = v.x; + float y = v.y; + float z = 0; + + result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12; + result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13; + + return result; +} + // Calculate linear interpolation between two vectors RMAPI Vector2 Vector2Lerp(Vector2 v1, Vector2 v2, float amount) { @@ -363,8 +406,11 @@ RMAPI Vector2 Vector2Rotate(Vector2 v, float angle) { Vector2 result = { 0 }; - result.x = v.x*cosf(angle) - v.y*sinf(angle); - result.y = v.x*sinf(angle) + v.y*cosf(angle); + float cosres = cosf(angle); + float sinres = sinf(angle); + + result.x = v.x*cosres - v.y*sinres; + result.y = v.x*sinres + v.y*cosres; return result; } @@ -388,6 +434,62 @@ RMAPI Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance) return result; } +// Invert the given vector +RMAPI Vector2 Vector2Invert(Vector2 v) +{ + Vector2 result = { 1.0f/v.x, 1.0f/v.y }; + + return result; +} + +// Clamp the components of the vector between +// min and max values specified by the given vectors +RMAPI Vector2 Vector2Clamp(Vector2 v, Vector2 min, Vector2 max) +{ + Vector2 result = { 0 }; + + result.x = fminf(max.x, fmaxf(min.x, v.x)); + result.y = fminf(max.y, fmaxf(min.y, v.y)); + + return result; +} + +// Clamp the magnitude of the vector between two min and max values +RMAPI Vector2 Vector2ClampValue(Vector2 v, float min, float max) +{ + Vector2 result = v; + + float length = (v.x*v.x) + (v.y*v.y); + if (length > 0.0f) + { + length = sqrtf(length); + + if (length < min) + { + float scale = min/length; + result.x = v.x*scale; + result.y = v.y*scale; + } + else if (length > max) + { + float scale = max/length; + result.x = v.x*scale; + result.y = v.y*scale; + } + } + + return result; +} + +// Check whether two given vectors are almost equal +RMAPI int Vector2Equals(Vector2 p, Vector2 q) +{ + 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))))); + + return result; +} + //---------------------------------------------------------------------------------- // Module Functions Definition - Vector3 math //---------------------------------------------------------------------------------- @@ -472,14 +574,14 @@ RMAPI Vector3 Vector3Perpendicular(Vector3 v) float min = (float) fabs(v.x); Vector3 cardinalAxis = {1.0f, 0.0f, 0.0f}; - if (fabs(v.y) < min) + if (fabsf(v.y) < min) { min = (float) fabs(v.y); Vector3 tmp = {0.0f, 1.0f, 0.0f}; cardinalAxis = tmp; } - if (fabs(v.z) < min) + if (fabsf(v.z) < min) { Vector3 tmp = {0.0f, 0.0f, 1.0f}; cardinalAxis = tmp; @@ -530,16 +632,29 @@ RMAPI float Vector3Distance(Vector3 v1, Vector3 v2) return result; } +// Calculate square distance between two vectors +RMAPI float Vector3DistanceSqr(Vector3 v1, Vector3 v2) +{ + float result = 0.0f; + + float dx = v2.x - v1.x; + float dy = v2.y - v1.y; + float dz = v2.z - v1.z; + result = dx*dx + dy*dy + dz*dz; + + return result; +} + // Calculate angle between two vectors RMAPI float Vector3Angle(Vector3 v1, Vector3 v2) { float result = 0.0f; - + Vector3 cross = { v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x }; float len = sqrtf(cross.x*cross.x + cross.y*cross.y + cross.z*cross.z); float dot = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z); result = atan2f(len, dot); - + return result; } @@ -638,6 +753,58 @@ RMAPI Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q) return result; } +// Rotates a vector around an axis +RMAPI Vector3 Vector3RotateByAxisAngle(Vector3 v, Vector3 axis, float angle) +{ + // Using Euler-Rodrigues Formula + // Ref.: https://en.wikipedia.org/w/index.php?title=Euler%E2%80%93Rodrigues_formula + + Vector3 result = v; + + // Vector3Normalize(axis); + float length = sqrtf(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z); + if (length == 0.0f) length = 1.0f; + float ilength = 1.0f / length; + axis.x *= ilength; + axis.y *= ilength; + axis.z *= ilength; + + angle /= 2.0f; + float a = sinf(angle); + float b = axis.x * a; + float c = axis.y * a; + float d = axis.z * a; + a = cosf(angle); + Vector3 w = { b, c, d }; + + // Vector3CrossProduct(w, v) + Vector3 wv = { w.y * v.z - w.z * v.y, w.z * v.x - w.x * v.z, w.x * v.y - w.y * v.x }; + + // Vector3CrossProduct(w, wv) + Vector3 wwv = { w.y * wv.z - w.z * wv.y, w.z * wv.x - w.x * wv.z, w.x * wv.y - w.y * wv.x }; + + // Vector3Scale(wv, 2 * a) + a *= 2; + wv.x *= a; + wv.y *= a; + wv.z *= a; + + // Vector3Scale(wwv, 2) + wwv.x *= 2; + wwv.y *= 2; + wwv.z *= 2; + + result.x += wv.x; + result.y += wv.y; + result.z += wv.z; + + result.x += wwv.x; + result.y += wwv.y; + result.z += wwv.z; + + return result; +} + // Calculate linear interpolation between two vectors RMAPI Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount) { @@ -812,6 +979,92 @@ RMAPI float3 Vector3ToFloatV(Vector3 v) return buffer; } +// Invert the given vector +RMAPI Vector3 Vector3Invert(Vector3 v) +{ + Vector3 result = { 1.0f/v.x, 1.0f/v.y, 1.0f/v.z }; + + return result; +} + +// Clamp the components of the vector between +// min and max values specified by the given vectors +RMAPI Vector3 Vector3Clamp(Vector3 v, Vector3 min, Vector3 max) +{ + Vector3 result = { 0 }; + + result.x = fminf(max.x, fmaxf(min.x, v.x)); + result.y = fminf(max.y, fmaxf(min.y, v.y)); + result.z = fminf(max.z, fmaxf(min.z, v.z)); + + return result; +} + +// Clamp the magnitude of the vector between two values +RMAPI Vector3 Vector3ClampValue(Vector3 v, float min, float max) +{ + Vector3 result = v; + + float length = (v.x*v.x) + (v.y*v.y) + (v.z*v.z); + if (length > 0.0f) + { + length = sqrtf(length); + + if (length < min) + { + float scale = min/length; + result.x = v.x*scale; + result.y = v.y*scale; + result.z = v.z*scale; + } + else if (length > max) + { + float scale = max/length; + result.x = v.x*scale; + result.y = v.y*scale; + result.z = v.z*scale; + } + } + + return result; +} + +// Check whether two given vectors are almost equal +RMAPI int Vector3Equals(Vector3 p, Vector3 q) +{ + 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))))); + + return result; +} + +// Compute the direction of a refracted ray where v specifies the +// normalized direction of the incoming ray, n specifies the +// normalized normal vector of the interface of two optical media, +// and r specifies the ratio of the refractive index of the medium +// from where the ray comes to the refractive index of the medium +// on the other side of the surface +RMAPI Vector3 Vector3Refract(Vector3 v, Vector3 n, float r) +{ + Vector3 result = { 0 }; + + float dot = v.x*n.x + v.y*n.y + v.z*n.z; + float d = 1.0f - r*r*(1.0f - dot*dot); + + if (d >= 0.0f) + { + d = sqrtf(d); + v.x = r*v.x - (r*dot + d)*n.x; + v.y = r*v.y - (r*dot + d)*n.y; + v.z = r*v.z - (r*dot + d)*n.z; + + result = v; + } + + return result; +} + //---------------------------------------------------------------------------------- // Module Functions Definition - Matrix math //---------------------------------------------------------------------------------- @@ -917,45 +1170,6 @@ RMAPI Matrix MatrixInvert(Matrix mat) return result; } -// Normalize provided matrix -RMAPI Matrix MatrixNormalize(Matrix mat) -{ - Matrix result = { 0 }; - - // Cache the matrix values (speed optimization) - float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3; - float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7; - float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11; - float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15; - - // MatrixDeterminant(mat) - float det = a30*a21*a12*a03 - a20*a31*a12*a03 - a30*a11*a22*a03 + a10*a31*a22*a03 + - a20*a11*a32*a03 - a10*a21*a32*a03 - a30*a21*a02*a13 + a20*a31*a02*a13 + - a30*a01*a22*a13 - a00*a31*a22*a13 - a20*a01*a32*a13 + a00*a21*a32*a13 + - a30*a11*a02*a23 - a10*a31*a02*a23 - a30*a01*a12*a23 + a00*a31*a12*a23 + - a10*a01*a32*a23 - a00*a11*a32*a23 - a20*a11*a02*a33 + a10*a21*a02*a33 + - a20*a01*a12*a33 - a00*a21*a12*a33 - a10*a01*a22*a33 + a00*a11*a22*a33; - - result.m0 = mat.m0/det; - result.m1 = mat.m1/det; - result.m2 = mat.m2/det; - result.m3 = mat.m3/det; - result.m4 = mat.m4/det; - result.m5 = mat.m5/det; - result.m6 = mat.m6/det; - result.m7 = mat.m7/det; - result.m8 = mat.m8/det; - result.m9 = mat.m9/det; - result.m10 = mat.m10/det; - result.m11 = mat.m11/det; - result.m12 = mat.m12/det; - result.m13 = mat.m13/det; - result.m14 = mat.m14/det; - result.m15 = mat.m15/det; - - return result; -} - // Get identity matrix RMAPI Matrix MatrixIdentity(void) { @@ -1099,7 +1313,8 @@ RMAPI Matrix MatrixRotate(Vector3 axis, float angle) return result; } -// Get x-rotation matrix (angle in radians) +// Get x-rotation matrix +// NOTE: Angle must be provided in radians RMAPI Matrix MatrixRotateX(float angle) { Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, @@ -1111,14 +1326,15 @@ RMAPI Matrix MatrixRotateX(float angle) float sinres = sinf(angle); result.m5 = cosres; - result.m6 = -sinres; - result.m9 = sinres; + result.m6 = sinres; + result.m9 = -sinres; result.m10 = cosres; return result; } -// Get y-rotation matrix (angle in radians) +// Get y-rotation matrix +// NOTE: Angle must be provided in radians RMAPI Matrix MatrixRotateY(float angle) { Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, @@ -1130,14 +1346,15 @@ RMAPI Matrix MatrixRotateY(float angle) float sinres = sinf(angle); result.m0 = cosres; - result.m2 = sinres; - result.m8 = -sinres; + result.m2 = -sinres; + result.m8 = sinres; result.m10 = cosres; return result; } -// Get z-rotation matrix (angle in radians) +// Get z-rotation matrix +// NOTE: Angle must be provided in radians RMAPI Matrix MatrixRotateZ(float angle) { Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, @@ -1149,74 +1366,76 @@ RMAPI Matrix MatrixRotateZ(float angle) float sinres = sinf(angle); result.m0 = cosres; - result.m1 = -sinres; - result.m4 = sinres; + result.m1 = sinres; + result.m4 = -sinres; result.m5 = cosres; return result; } -// Get xyz-rotation matrix (angles in radians) -RMAPI Matrix MatrixRotateXYZ(Vector3 ang) +// Get xyz-rotation matrix +// NOTE: Angle must be provided in radians +RMAPI Matrix MatrixRotateXYZ(Vector3 angle) { Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity() - float cosz = cosf(-ang.z); - float sinz = sinf(-ang.z); - float cosy = cosf(-ang.y); - float siny = sinf(-ang.y); - float cosx = cosf(-ang.x); - float sinx = sinf(-ang.x); + float cosz = cosf(-angle.z); + float sinz = sinf(-angle.z); + float cosy = cosf(-angle.y); + float siny = sinf(-angle.y); + float cosx = cosf(-angle.x); + float sinx = sinf(-angle.x); result.m0 = cosz*cosy; - result.m4 = (cosz*siny*sinx) - (sinz*cosx); - result.m8 = (cosz*siny*cosx) + (sinz*sinx); + result.m1 = (cosz*siny*sinx) - (sinz*cosx); + result.m2 = (cosz*siny*cosx) + (sinz*sinx); - result.m1 = sinz*cosy; + result.m4 = sinz*cosy; result.m5 = (sinz*siny*sinx) + (cosz*cosx); - result.m9 = (sinz*siny*cosx) - (cosz*sinx); + result.m6 = (sinz*siny*cosx) - (cosz*sinx); - result.m2 = -siny; - result.m6 = cosy*sinx; + result.m8 = -siny; + result.m9 = cosy*sinx; result.m10= cosy*cosx; return result; } -// Get zyx-rotation matrix (angles in radians) -RMAPI Matrix MatrixRotateZYX(Vector3 ang) +// Get zyx-rotation matrix +// NOTE: Angle must be provided in radians +RMAPI Matrix MatrixRotateZYX(Vector3 angle) { Matrix result = { 0 }; - float cz = cosf(ang.z); - float sz = sinf(ang.z); - float cy = cosf(ang.y); - float sy = sinf(ang.y); - float cx = cosf(ang.x); - float sx = sinf(ang.x); + float cz = cosf(angle.z); + float sz = sinf(angle.z); + float cy = cosf(angle.y); + float sy = sinf(angle.y); + float cx = cosf(angle.x); + float sx = sinf(angle.x); result.m0 = cz*cy; - result.m1 = cz*sy*sx - cx*sz; - result.m2 = sz*sx + cz*cx*sy; - result.m3 = 0; + result.m4 = cz*sy*sx - cx*sz; + result.m8 = sz*sx + cz*cx*sy; + result.m12 = 0; - result.m4 = cy*sz; + result.m1 = cy*sz; result.m5 = cz*cx + sz*sy*sx; - result.m6 = cx*sz*sy - cz*sx; - result.m7 = 0; + result.m9 = cx*sz*sy - cz*sx; + result.m13 = 0; - result.m8 = -sy; - result.m9 = cy*sx; + result.m2 = -sy; + result.m6 = cy*sx; result.m10 = cy*cx; - result.m11 = 0; - - result.m12 = 0; - result.m13 = 0; result.m14 = 0; + + result.m3 = 0; + result.m7 = 0; + result.m11 = 0; result.m15 = 1; return result; @@ -1266,7 +1485,7 @@ RMAPI Matrix MatrixFrustum(double left, double right, double bottom, double top, } // Get perspective projection matrix -// NOTE: Angle should be provided in radians +// NOTE: Fovy angle must be provided in radians RMAPI Matrix MatrixPerspective(double fovy, double aspect, double near, double far) { Matrix result = { 0 }; @@ -1475,10 +1694,9 @@ RMAPI Quaternion QuaternionInvert(Quaternion q) { Quaternion result = q; - float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); - float lengthSq = length*length; + float lengthSq = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w; - if (lengthSq != 0.0) + if (lengthSq != 0.0f) { float invLength = 1.0f/lengthSq; @@ -1512,12 +1730,10 @@ RMAPI Quaternion QuaternionScale(Quaternion q, float mul) { Quaternion result = { 0 }; - float qax = q.x, qay = q.y, qaz = q.z, qaw = q.w; - - result.x = qax*mul + qaw*mul + qay*mul - qaz*mul; - result.y = qay*mul + qaw*mul + qaz*mul - qax*mul; - result.z = qaz*mul + qaw*mul + qax*mul - qay*mul; - result.w = qaw*mul - qax*mul - qay*mul - qaz*mul; + result.x = q.x*mul; + result.y = q.y*mul; + result.z = q.z*mul; + result.w = q.w*mul; return result; } @@ -1581,14 +1797,14 @@ RMAPI Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount) cosHalfTheta = -cosHalfTheta; } - if (fabs(cosHalfTheta) >= 1.0f) result = q1; + if (fabsf(cosHalfTheta) >= 1.0f) result = q1; else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount); else { float halfTheta = acosf(cosHalfTheta); float sinHalfTheta = sqrtf(1.0f - cosHalfTheta*cosHalfTheta); - if (fabs(sinHalfTheta) < 0.001f) + if (fabsf(sinHalfTheta) < 0.001f) { result.x = (q1.x*0.5f + q2.x*0.5f); result.y = (q1.y*0.5f + q2.y*0.5f); @@ -1643,30 +1859,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; @@ -1706,7 +1952,7 @@ RMAPI Matrix QuaternionToMatrix(Quaternion q) } // Get rotation quaternion for an angle and axis -// NOTE: angle must be provided in radians +// NOTE: Angle must be provided in radians RMAPI Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle) { Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f }; @@ -1754,7 +2000,7 @@ RMAPI Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle) // Get the rotation angle and axis for a given quaternion RMAPI void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle) { - if (fabs(q.w) > 1.0f) + if (fabsf(q.w) > 1.0f) { // QuaternionNormalize(q); float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); @@ -1847,4 +2093,19 @@ RMAPI Quaternion QuaternionTransform(Quaternion q, Matrix mat) return result; } +// 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))))) && + ((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.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; +} + #endif // RAYMATH_H -- cgit v1.2.3