summaryrefslogtreecommitdiff
path: root/include/raymath.h
diff options
context:
space:
mode:
Diffstat (limited to 'include/raymath.h')
-rw-r--r--include/raymath.h511
1 files changed, 386 insertions, 125 deletions
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 <math.h> // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), fminf(), fmaxf(), fabs()
+#include <math.h> // 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