diff options
Diffstat (limited to 'src/raymath.h')
| -rw-r--r-- | src/raymath.h | 327 |
1 files changed, 225 insertions, 102 deletions
diff --git a/src/raymath.h b/src/raymath.h index 1dbebc07..fe0b8947 100644 --- a/src/raymath.h +++ b/src/raymath.h @@ -1,6 +1,6 @@ /********************************************************************************************** * -* raymath v1.0 - Math functions to work with Vector3, Matrix and Quaternions +* raymath v1.1 - Math functions to work with Vector3, Matrix and Quaternions * * CONFIGURATION: * @@ -19,7 +19,7 @@ * * LICENSE: zlib/libpng * -* Copyright (c) 2015 Ramon Santamaria (@raysan5) +* Copyright (c) 2015-2017 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. @@ -124,7 +124,7 @@ RMDEF Vector2 Vector2Zero(void); // Vector with c RMDEF Vector2 Vector2One(void); // Vector with components value 1.0f RMDEF Vector2 Vector2Add(Vector2 v1, Vector2 v2); // Add two vectors (v1 + v2) RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2); // Subtract two vectors (v1 - v2) -RMDEF float Vector2Lenght(Vector2 v); // Calculate vector lenght +RMDEF float Vector2Length(Vector2 v); // Calculate vector length RMDEF float Vector2DotProduct(Vector2 v1, Vector2 v2); // Calculate two vectors dot product RMDEF float Vector2Distance(Vector2 v1, Vector2 v2); // Calculate distance between two vectors RMDEF float Vector2Angle(Vector2 v1, Vector2 v2); // Calculate angle between two vectors in X-axis @@ -136,24 +136,27 @@ RMDEF void Vector2Normalize(Vector2 *v); // Normalize pro //------------------------------------------------------------------------------------ // Functions Declaration to work with Vector3 //------------------------------------------------------------------------------------ -RMDEF Vector3 VectorZero(void); // Vector with components value 0.0f -RMDEF Vector3 VectorOne(void); // Vector with components value 1.0f -RMDEF Vector3 VectorAdd(Vector3 v1, Vector3 v2); // Add two vectors -RMDEF Vector3 VectorSubtract(Vector3 v1, Vector3 v2); // Substract two vectors -RMDEF Vector3 VectorCrossProduct(Vector3 v1, Vector3 v2); // Calculate two vectors cross product -RMDEF Vector3 VectorPerpendicular(Vector3 v); // Calculate one vector perpendicular vector -RMDEF float VectorLength(const Vector3 v); // Calculate vector lenght -RMDEF float VectorDotProduct(Vector3 v1, Vector3 v2); // Calculate two vectors dot product -RMDEF float VectorDistance(Vector3 v1, Vector3 v2); // Calculate distance between two points -RMDEF void VectorScale(Vector3 *v, float scale); // Scale provided vector -RMDEF void VectorNegate(Vector3 *v); // Negate provided vector (invert direction) -RMDEF void VectorNormalize(Vector3 *v); // Normalize provided vector -RMDEF void VectorTransform(Vector3 *v, Matrix mat); // Transforms a Vector3 by a given Matrix -RMDEF Vector3 VectorLerp(Vector3 v1, Vector3 v2, float amount); // Calculate linear interpolation between two vectors -RMDEF Vector3 VectorReflect(Vector3 vector, Vector3 normal); // Calculate reflected vector to normal -RMDEF Vector3 VectorMin(Vector3 vec1, Vector3 vec2); // Return min value for each pair of components -RMDEF Vector3 VectorMax(Vector3 vec1, Vector3 vec2); // Return max value for each pair of components -RMDEF Vector3 VectorBarycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c); // Barycenter coords for p in triangle abc +RMDEF Vector3 Vector3Zero(void); // Vector with components value 0.0f +RMDEF Vector3 Vector3One(void); // Vector with components value 1.0f +RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2); // Add two vectors +RMDEF Vector3 Vector3Multiply(Vector3 v, float scalar); // Multiply vector by scalar +RMDEF Vector3 Vector3MultiplyV(Vector3 v1, Vector3 v2); // Multiply vector by vector +RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2); // Substract two vectors +RMDEF Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2); // Calculate two vectors cross product +RMDEF Vector3 Vector3Perpendicular(Vector3 v); // Calculate one vector perpendicular vector +RMDEF float Vector3Length(const Vector3 v); // Calculate vector length +RMDEF float Vector3DotProduct(Vector3 v1, Vector3 v2); // Calculate two vectors dot product +RMDEF float Vector3Distance(Vector3 v1, Vector3 v2); // Calculate distance between two points +RMDEF void Vector3Scale(Vector3 *v, float scale); // Scale provided vector +RMDEF void Vector3Negate(Vector3 *v); // Negate provided vector (invert direction) +RMDEF void Vector3Normalize(Vector3 *v); // Normalize provided vector +RMDEF void Vector3Transform(Vector3 *v, Matrix mat); // Transforms a Vector3 by a given Matrix +RMDEF Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount); // Calculate linear interpolation between two vectors +RMDEF Vector3 Vector3Reflect(Vector3 vector, Vector3 normal); // Calculate reflected vector to normal +RMDEF Vector3 Vector3Min(Vector3 vec1, Vector3 vec2); // Return min value for each pair of components +RMDEF Vector3 Vector3Max(Vector3 vec1, Vector3 vec2); // Return max value for each pair of components +RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c); // Barycenter coords for p in triangle abc +RMDEF float *Vector3ToFloat(Vector3 vec); // Returns Vector3 as float array //------------------------------------------------------------------------------------ // Functions Declaration to work with Matrix @@ -177,15 +180,20 @@ RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top, RMDEF Matrix MatrixPerspective(double fovy, double aspect, double near, double far); // Returns perspective projection matrix RMDEF Matrix MatrixOrtho(double left, double right, double bottom, double top, double near, double far); // Returns orthographic projection matrix RMDEF Matrix MatrixLookAt(Vector3 position, Vector3 target, Vector3 up); // Returns camera look-at matrix (view matrix) +RMDEF float *MatrixToFloat(Matrix mat); // Returns float array of Matrix data //------------------------------------------------------------------------------------ // Functions Declaration to work with Quaternions //------------------------------------------------------------------------------------ +RMDEF Quaternion QuaternionIdentity(void); // Returns identity quaternion RMDEF float QuaternionLength(Quaternion quat); // Compute the length of a quaternion RMDEF void QuaternionNormalize(Quaternion *q); // Normalize provided quaternion RMDEF void QuaternionInvert(Quaternion *quat); // Invert provided quaternion RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2); // Calculate two quaternion multiplication -RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float slerp); // Calculates spherical linear interpolation between two quaternions +RMDEF Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount); // Calculate linear interpolation between two quaternions +RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount); // Calculates spherical linear interpolation between two quaternions +RMDEF Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount); // Calculate slerp-optimized interpolation between two quaternions +RMDEF Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to); // Calculate quaternion based on the rotation from one vector to another RMDEF Quaternion QuaternionFromMatrix(Matrix matrix); // Returns a quaternion for a given rotation matrix RMDEF Matrix QuaternionToMatrix(Quaternion q); // Returns a matrix for a given quaternion RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle); // Returns rotation quaternion for an angle and axis @@ -236,8 +244,8 @@ RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2) return (Vector2){ v1.x - v2.x, v1.y - v2.y }; } -// Calculate vector lenght -RMDEF float Vector2Lenght(Vector2 v) +// Calculate vector length +RMDEF float Vector2Length(Vector2 v) { return sqrtf((v.x*v.x) + (v.y*v.y)); } @@ -287,7 +295,7 @@ RMDEF void Vector2Divide(Vector2 *v, float div) // Normalize provided vector RMDEF void Vector2Normalize(Vector2 *v) { - Vector2Divide(v, Vector2Lenght(*v)); + Vector2Divide(v, Vector2Length(*v)); } //---------------------------------------------------------------------------------- @@ -295,25 +303,47 @@ RMDEF void Vector2Normalize(Vector2 *v) //---------------------------------------------------------------------------------- // Vector with components value 0.0f -RMDEF Vector3 VectorZero(void) { return (Vector3){ 0.0f, 0.0f, 0.0f }; } +RMDEF Vector3 Vector3Zero(void) { return (Vector3){ 0.0f, 0.0f, 0.0f }; } // Vector with components value 1.0f -RMDEF Vector3 VectorOne(void) { return (Vector3){ 1.0f, 1.0f, 1.0f }; } +RMDEF Vector3 Vector3One(void) { return (Vector3){ 1.0f, 1.0f, 1.0f }; } // Add two vectors -RMDEF Vector3 VectorAdd(Vector3 v1, Vector3 v2) +RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2) { return (Vector3){ v1.x + v2.x, v1.y + v2.y, v1.z + v2.z }; } // Substract two vectors -RMDEF Vector3 VectorSubtract(Vector3 v1, Vector3 v2) +RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2) { return (Vector3){ v1.x - v2.x, v1.y - v2.y, v1.z - v2.z }; } +// Multiply vector by scalar +RMDEF Vector3 Vector3Multiply(Vector3 v, float scalar) +{ + v.x *= scalar; + v.y *= scalar; + v.z *= scalar; + + return v; +} + +// Multiply vector by vector +RMDEF Vector3 Vector3MultiplyV(Vector3 v1, Vector3 v2) +{ + Vector3 result; + + result.x = v1.x * v2.x; + result.y = v1.y * v2.y; + result.z = v1.z * v2.z; + + return result; +} + // Calculate two vectors cross product -RMDEF Vector3 VectorCrossProduct(Vector3 v1, Vector3 v2) +RMDEF Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2) { Vector3 result; @@ -325,7 +355,7 @@ RMDEF Vector3 VectorCrossProduct(Vector3 v1, Vector3 v2) } // Calculate one vector perpendicular vector -RMDEF Vector3 VectorPerpendicular(Vector3 v) +RMDEF Vector3 Vector3Perpendicular(Vector3 v) { Vector3 result; @@ -343,25 +373,25 @@ RMDEF Vector3 VectorPerpendicular(Vector3 v) cardinalAxis = (Vector3){0.0f, 0.0f, 1.0f}; } - result = VectorCrossProduct(v, cardinalAxis); + result = Vector3CrossProduct(v, cardinalAxis); return result; } -// Calculate vector lenght -RMDEF float VectorLength(const Vector3 v) +// Calculate vector length +RMDEF float Vector3Length(const Vector3 v) { return sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); } // Calculate two vectors dot product -RMDEF float VectorDotProduct(Vector3 v1, Vector3 v2) +RMDEF float Vector3DotProduct(Vector3 v1, Vector3 v2) { return (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z); } // Calculate distance between two vectors -RMDEF float VectorDistance(Vector3 v1, Vector3 v2) +RMDEF float Vector3Distance(Vector3 v1, Vector3 v2) { float dx = v2.x - v1.x; float dy = v2.y - v1.y; @@ -371,7 +401,7 @@ RMDEF float VectorDistance(Vector3 v1, Vector3 v2) } // Scale provided vector -RMDEF void VectorScale(Vector3 *v, float scale) +RMDEF void Vector3Scale(Vector3 *v, float scale) { v->x *= scale; v->y *= scale; @@ -379,7 +409,7 @@ RMDEF void VectorScale(Vector3 *v, float scale) } // Negate provided vector (invert direction) -RMDEF void VectorNegate(Vector3 *v) +RMDEF void Vector3Negate(Vector3 *v) { v->x = -v->x; v->y = -v->y; @@ -387,11 +417,11 @@ RMDEF void VectorNegate(Vector3 *v) } // Normalize provided vector -RMDEF void VectorNormalize(Vector3 *v) +RMDEF void Vector3Normalize(Vector3 *v) { float length, ilength; - length = VectorLength(*v); + length = Vector3Length(*v); if (length == 0.0f) length = 1.0f; @@ -403,8 +433,7 @@ RMDEF void VectorNormalize(Vector3 *v) } // Transforms a Vector3 by a given Matrix -// TODO: Review math (matrix transpose required?) -RMDEF void VectorTransform(Vector3 *v, Matrix mat) +RMDEF void Vector3Transform(Vector3 *v, Matrix mat) { float x = v->x; float y = v->y; @@ -416,7 +445,7 @@ RMDEF void VectorTransform(Vector3 *v, Matrix mat) }; // Calculate linear interpolation between two vectors -RMDEF Vector3 VectorLerp(Vector3 v1, Vector3 v2, float amount) +RMDEF Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount) { Vector3 result; @@ -428,7 +457,7 @@ RMDEF Vector3 VectorLerp(Vector3 v1, Vector3 v2, float amount) } // Calculate reflected vector to normal -RMDEF Vector3 VectorReflect(Vector3 vector, Vector3 normal) +RMDEF Vector3 Vector3Reflect(Vector3 vector, Vector3 normal) { // I is the original vector // N is the normal of the incident plane @@ -436,7 +465,7 @@ RMDEF Vector3 VectorReflect(Vector3 vector, Vector3 normal) Vector3 result; - float dotProduct = VectorDotProduct(vector, normal); + float dotProduct = Vector3DotProduct(vector, normal); result.x = vector.x - (2.0f*normal.x)*dotProduct; result.y = vector.y - (2.0f*normal.y)*dotProduct; @@ -446,7 +475,7 @@ RMDEF Vector3 VectorReflect(Vector3 vector, Vector3 normal) } // Return min value for each pair of components -RMDEF Vector3 VectorMin(Vector3 vec1, Vector3 vec2) +RMDEF Vector3 Vector3Min(Vector3 vec1, Vector3 vec2) { Vector3 result; @@ -458,7 +487,7 @@ RMDEF Vector3 VectorMin(Vector3 vec1, Vector3 vec2) } // Return max value for each pair of components -RMDEF Vector3 VectorMax(Vector3 vec1, Vector3 vec2) +RMDEF Vector3 Vector3Max(Vector3 vec1, Vector3 vec2) { Vector3 result; @@ -471,18 +500,18 @@ RMDEF Vector3 VectorMax(Vector3 vec1, Vector3 vec2) // Compute barycenter coordinates (u, v, w) for point p with respect to triangle (a, b, c) // NOTE: Assumes P is on the plane of the triangle -RMDEF Vector3 VectorBarycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c) +RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c) { //Vector v0 = b - a, v1 = c - a, v2 = p - a; - Vector3 v0 = VectorSubtract(b, a); - Vector3 v1 = VectorSubtract(c, a); - Vector3 v2 = VectorSubtract(p, a); - float d00 = VectorDotProduct(v0, v0); - float d01 = VectorDotProduct(v0, v1); - float d11 = VectorDotProduct(v1, v1); - float d20 = VectorDotProduct(v2, v0); - float d21 = VectorDotProduct(v2, v1); + Vector3 v0 = Vector3Subtract(b, a); + Vector3 v1 = Vector3Subtract(c, a); + Vector3 v2 = Vector3Subtract(p, a); + float d00 = Vector3DotProduct(v0, v0); + float d01 = Vector3DotProduct(v0, v1); + float d11 = Vector3DotProduct(v1, v1); + float d20 = Vector3DotProduct(v2, v0); + float d21 = Vector3DotProduct(v2, v1); float denom = d00*d11 - d01*d01; @@ -495,6 +524,18 @@ RMDEF Vector3 VectorBarycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c) return result; } +// Returns Vector3 as float array +RMDEF float *Vector3ToFloat(Vector3 vec) +{ + static float buffer[3]; + + buffer[0] = vec.x; + buffer[1] = vec.y; + buffer[2] = vec.z; + + return buffer; +} + //---------------------------------------------------------------------------------- // Module Functions Definition - Matrix math //---------------------------------------------------------------------------------- @@ -685,10 +726,10 @@ RMDEF Matrix MatrixSubstract(Matrix left, Matrix right) // Returns translation matrix RMDEF Matrix MatrixTranslate(float x, float y, float z) { - 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, - x, y, z, 1.0f }; + Matrix result = { 1.0f, 0.0f, 0.0f, x, + 0.0f, 1.0f, 0.0f, y, + 0.0f, 0.0f, 1.0f, z, + 0.0f, 0.0f, 0.0f, 1.0f }; return result; } @@ -813,22 +854,22 @@ RMDEF Matrix MatrixMultiply(Matrix left, Matrix right) { Matrix result; - result.m0 = right.m0*left.m0 + right.m1*left.m4 + right.m2*left.m8 + right.m3*left.m12; - result.m1 = right.m0*left.m1 + right.m1*left.m5 + right.m2*left.m9 + right.m3*left.m13; - result.m2 = right.m0*left.m2 + right.m1*left.m6 + right.m2*left.m10 + right.m3*left.m14; - result.m3 = right.m0*left.m3 + right.m1*left.m7 + right.m2*left.m11 + right.m3*left.m15; - result.m4 = right.m4*left.m0 + right.m5*left.m4 + right.m6*left.m8 + right.m7*left.m12; - result.m5 = right.m4*left.m1 + right.m5*left.m5 + right.m6*left.m9 + right.m7*left.m13; - result.m6 = right.m4*left.m2 + right.m5*left.m6 + right.m6*left.m10 + right.m7*left.m14; - result.m7 = right.m4*left.m3 + right.m5*left.m7 + right.m6*left.m11 + right.m7*left.m15; - result.m8 = right.m8*left.m0 + right.m9*left.m4 + right.m10*left.m8 + right.m11*left.m12; - result.m9 = right.m8*left.m1 + right.m9*left.m5 + right.m10*left.m9 + right.m11*left.m13; - result.m10 = right.m8*left.m2 + right.m9*left.m6 + right.m10*left.m10 + right.m11*left.m14; - result.m11 = right.m8*left.m3 + right.m9*left.m7 + right.m10*left.m11 + right.m11*left.m15; - result.m12 = right.m12*left.m0 + right.m13*left.m4 + right.m14*left.m8 + right.m15*left.m12; - result.m13 = right.m12*left.m1 + right.m13*left.m5 + right.m14*left.m9 + right.m15*left.m13; - result.m14 = right.m12*left.m2 + right.m13*left.m6 + right.m14*left.m10 + right.m15*left.m14; - result.m15 = right.m12*left.m3 + right.m13*left.m7 + right.m14*left.m11 + right.m15*left.m15; + result.m0 = left.m0*right.m0 + left.m1*right.m4 + left.m2*right.m8 + left.m3*right.m12; + result.m1 = left.m0*right.m1 + left.m1*right.m5 + left.m2*right.m9 + left.m3*right.m13; + result.m2 = left.m0*right.m2 + left.m1*right.m6 + left.m2*right.m10 + left.m3*right.m14; + result.m3 = left.m0*right.m3 + left.m1*right.m7 + left.m2*right.m11 + left.m3*right.m15; + result.m4 = left.m4*right.m0 + left.m5*right.m4 + left.m6*right.m8 + left.m7*right.m12; + result.m5 = left.m4*right.m1 + left.m5*right.m5 + left.m6*right.m9 + left.m7*right.m13; + result.m6 = left.m4*right.m2 + left.m5*right.m6 + left.m6*right.m10 + left.m7*right.m14; + result.m7 = left.m4*right.m3 + left.m5*right.m7 + left.m6*right.m11 + left.m7*right.m15; + result.m8 = left.m8*right.m0 + left.m9*right.m4 + left.m10*right.m8 + left.m11*right.m12; + result.m9 = left.m8*right.m1 + left.m9*right.m5 + left.m10*right.m9 + left.m11*right.m13; + result.m10 = left.m8*right.m2 + left.m9*right.m6 + left.m10*right.m10 + left.m11*right.m14; + result.m11 = left.m8*right.m3 + left.m9*right.m7 + left.m10*right.m11 + left.m11*right.m15; + result.m12 = left.m12*right.m0 + left.m13*right.m4 + left.m14*right.m8 + left.m15*right.m12; + result.m13 = left.m12*right.m1 + left.m13*right.m5 + left.m14*right.m9 + left.m15*right.m13; + result.m14 = left.m12*right.m2 + left.m13*right.m6 + left.m14*right.m10 + left.m15*right.m14; + result.m15 = left.m12*right.m3 + left.m13*right.m7 + left.m14*right.m11 + left.m15*right.m15; return result; } @@ -866,9 +907,10 @@ RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top, } // Returns perspective projection matrix +// NOTE: Angle should be provided in radians RMDEF Matrix MatrixPerspective(double fovy, double aspect, double near, double far) { - double top = near*tan(fovy*PI/360.0); + double top = near*tan(fovy*0.5); double right = top*aspect; return MatrixFrustum(-right, right, -top, top, near, far); @@ -908,37 +950,70 @@ RMDEF Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up) { Matrix result; - Vector3 z = VectorSubtract(eye, target); - VectorNormalize(&z); - Vector3 x = VectorCrossProduct(up, z); - VectorNormalize(&x); - Vector3 y = VectorCrossProduct(z, x); - VectorNormalize(&y); - + Vector3 z = Vector3Subtract(eye, target); + Vector3Normalize(&z); + Vector3 x = Vector3CrossProduct(up, z); + Vector3Normalize(&x); + Vector3 y = Vector3CrossProduct(z, x); + Vector3Normalize(&y); + result.m0 = x.x; result.m1 = x.y; result.m2 = x.z; - result.m3 = -((x.x*eye.x) + (x.y*eye.y) + (x.z*eye.z)); + result.m3 = 0.0f; result.m4 = y.x; result.m5 = y.y; result.m6 = y.z; - result.m7 = -((y.x*eye.x) + (y.y*eye.y) + (y.z*eye.z)); + result.m7 = 0.0f; result.m8 = z.x; result.m9 = z.y; result.m10 = z.z; - result.m11 = -((z.x*eye.x) + (z.y*eye.y) + (z.z*eye.z)); - result.m12 = 0.0f; - result.m13 = 0.0f; - result.m14 = 0.0f; + result.m11 = 0.0f; + result.m12 = eye.x; + result.m13 = eye.y; + result.m14 = eye.z; result.m15 = 1.0f; + MatrixInvert(&result); + return result; } +// Returns float array of matrix data +RMDEF float *MatrixToFloat(Matrix mat) +{ + static float buffer[16]; + + buffer[0] = mat.m0; + buffer[1] = mat.m1; + buffer[2] = mat.m2; + buffer[3] = mat.m3; + buffer[4] = mat.m4; + buffer[5] = mat.m5; + buffer[6] = mat.m6; + buffer[7] = mat.m7; + buffer[8] = mat.m8; + buffer[9] = mat.m9; + buffer[10] = mat.m10; + buffer[11] = mat.m11; + buffer[12] = mat.m12; + buffer[13] = mat.m13; + buffer[14] = mat.m14; + buffer[15] = mat.m15; + + return buffer; +} + //---------------------------------------------------------------------------------- // Module Functions Definition - Quaternion math //---------------------------------------------------------------------------------- +// Returns identity quaternion +RMDEF Quaternion QuaternionIdentity(void) +{ + return (Quaternion){ 0.0f, 0.0f, 0.0f, 1.0f }; +} + // Computes the length of a quaternion RMDEF float QuaternionLength(Quaternion quat) { @@ -995,6 +1070,19 @@ RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2) return result; } +// Calculate linear interpolation between two quaternions +RMDEF Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount) +{ + Quaternion result; + + result.x = q1.x + amount*(q2.x - q1.x); + result.y = q1.y + amount*(q2.y - q1.y); + result.z = q1.z + amount*(q2.z - q1.z); + result.w = q1.w + amount*(q2.w - q1.w); + + return result; +} + // Calculates spherical linear interpolation between two quaternions RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount) { @@ -1003,6 +1091,7 @@ RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount) float cosHalfTheta = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w; if (fabs(cosHalfTheta) >= 1.0f) result = q1; + else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount); else { float halfTheta = acos(cosHalfTheta); @@ -1030,6 +1119,37 @@ RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount) return result; } +// Calculate slerp-optimized interpolation between two quaternions +RMDEF Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount) +{ + Quaternion result = QuaternionLerp(q1, q2, amount); + QuaternionNormalize(&result); + + return result; +} + +// Calculate quaternion based on the rotation from one vector to another +RMDEF Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to) +{ + Quaternion q = { 0 }; + + float cos2Theta = Vector3DotProduct(from, to); + Vector3 cross = Vector3CrossProduct(from, to); + + q.x = cross.x; + q.y = cross.y; + q.z = cross.y; + q.w = 1.0f + cos2Theta; // NOTE: Added QuaternioIdentity() + + // Normalize to essentially nlerp the original and identity to 0.5 + QuaternionNormalize(&q); + + // Above lines are equivalent to: + //Quaternion result = QuaternionNlerp(q, QuaternionIdentity(), 0.5f); + + return q; +} + // Returns a quaternion for a given rotation matrix RMDEF Quaternion QuaternionFromMatrix(Matrix matrix) { @@ -1096,18 +1216,21 @@ RMDEF Matrix QuaternionToMatrix(Quaternion q) float x2 = x + x; float y2 = y + y; float z2 = z + z; + + float length = QuaternionLength(q); + float lengthSquared = length*length; - float xx = x*x2; - float xy = x*y2; - float xz = x*z2; + float xx = x*x2/lengthSquared; + float xy = x*y2/lengthSquared; + float xz = x*z2/lengthSquared; - float yy = y*y2; - float yz = y*z2; - float zz = z*z2; + float yy = y*y2/lengthSquared; + float yz = y*z2/lengthSquared; + float zz = z*z2/lengthSquared; - float wx = w*x2; - float wy = w*y2; - float wz = w*z2; + float wx = w*x2/lengthSquared; + float wy = w*y2/lengthSquared; + float wz = w*z2/lengthSquared; result.m0 = 1.0f - (yy + zz); result.m1 = xy - wz; @@ -1135,11 +1258,11 @@ RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle) { Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f }; - if (VectorLength(axis) != 0.0f) + if (Vector3Length(axis) != 0.0f) angle *= 0.5f; - VectorNormalize(&axis); + Vector3Normalize(&axis); float sinres = sinf(angle); float cosres = cosf(angle); @@ -1203,7 +1326,7 @@ RMDEF Quaternion QuaternionFromEuler(float roll, float pitch, float yaw) } // Return the Euler angles equivalent to quaternion (roll, pitch, yaw) -// NOTE: Angles are returned in a Vector3 struct and in degrees +// NOTE: Angles are returned in a Vector3 struct in degrees RMDEF Vector3 QuaternionToEuler(Quaternion q) { Vector3 v = { 0 }; |