git.sesse.net Git - pistorm/blob - raylib/raymath.h

   1 /**********************************************************************************************
   2 *
   3 *   raymath v1.2 - Math functions to work with Vector3, Matrix and Quaternions
   4 *
   5 *   CONFIGURATION:
   6 *
   7 *   #define RAYMATH_IMPLEMENTATION
   8 *       Generates the implementation of the library into the included file.
   9 *       If not defined, the library is in header only mode and can be included in other headers
  10 *       or source files without problems. But only ONE file should hold the implementation.
  11 *
  12 *   #define RAYMATH_HEADER_ONLY
  13 *       Define static inline functions code, so #include header suffices for use.
  14 *       This may use up lots of memory.
  15 *
  16 *   #define RAYMATH_STANDALONE
  17 *       Avoid raylib.h header inclusion in this file.
  18 *       Vector3 and Matrix data types are defined internally in raymath module.
  19 *
  20 *
  21 *   LICENSE: zlib/libpng
  22 *
  23 *   Copyright (c) 2015-2021 Ramon Santamaria (@raysan5)
  24 *
  25 *   This software is provided "as-is", without any express or implied warranty. In no event
  26 *   will the authors be held liable for any damages arising from the use of this software.
  27 *
  28 *   Permission is granted to anyone to use this software for any purpose, including commercial
  29 *   applications, and to alter it and redistribute it freely, subject to the following restrictions:
  30 *
  31 *     1. The origin of this software must not be misrepresented; you must not claim that you
  32 *     wrote the original software. If you use this software in a product, an acknowledgment
  33 *     in the product documentation would be appreciated but is not required.
  34 *
  35 *     2. Altered source versions must be plainly marked as such, and must not be misrepresented
  36 *     as being the original software.
  37 *
  38 *     3. This notice may not be removed or altered from any source distribution.
  39 *
  40 **********************************************************************************************/
  41
  42 #ifndef RAYMATH_H
  43 #define RAYMATH_H
  44
  45 //#define RAYMATH_STANDALONE      // NOTE: To use raymath as standalone lib, just uncomment this line
  46 //#define RAYMATH_HEADER_ONLY     // NOTE: To compile functions as static inline, uncomment this line
  47
  48 #ifndef RAYMATH_STANDALONE
  49     #include "raylib.h"           // Required for structs: Vector3, Matrix
  50 #endif
  51
  52 #if defined(RAYMATH_IMPLEMENTATION) && defined(RAYMATH_HEADER_ONLY)
  53     #error "Specifying both RAYMATH_IMPLEMENTATION and RAYMATH_HEADER_ONLY is contradictory"
  54 #endif
  55
  56 #if defined(RAYMATH_IMPLEMENTATION)
  57     #if defined(_WIN32) && defined(BUILD_LIBTYPE_SHARED)
  58         #define RMDEF __declspec(dllexport) extern inline // We are building raylib as a Win32 shared library (.dll).
  59     #elif defined(_WIN32) && defined(USE_LIBTYPE_SHARED)
  60         #define RMDEF __declspec(dllimport)         // We are using raylib as a Win32 shared library (.dll)
  61     #else
  62         #define RMDEF extern inline // Provide external definition
  63     #endif
  64 #elif defined(RAYMATH_HEADER_ONLY)
  65     #define RMDEF static inline // Functions may be inlined, no external out-of-line definition
  66 #else
  67     #if defined(__TINYC__)
  68         #define RMDEF static inline // plain inline not supported by tinycc (See issue #435)
  69     #else
  70         #define RMDEF inline        // Functions may be inlined or external definition used
  71     #endif
  72 #endif
  73
  74 //----------------------------------------------------------------------------------
  75 // Defines and Macros
  76 //----------------------------------------------------------------------------------
  77 #ifndef PI
  78     #define PI 3.14159265358979323846f
  79 #endif
  80
  81 #ifndef DEG2RAD
  82     #define DEG2RAD (PI/180.0f)
  83 #endif
  84
  85 #ifndef RAD2DEG
  86     #define RAD2DEG (180.0f/PI)
  87 #endif
  88
  89 // Return float vector for Matrix
  90 #ifndef MatrixToFloat
  91     #define MatrixToFloat(mat) (MatrixToFloatV(mat).v)
  92 #endif
  93
  94 // Return float vector for Vector3
  95 #ifndef Vector3ToFloat
  96     #define Vector3ToFloat(vec) (Vector3ToFloatV(vec).v)
  97 #endif
  98
  99 //----------------------------------------------------------------------------------
 100 // Types and Structures Definition
 101 //----------------------------------------------------------------------------------
 102
 103 #if defined(RAYMATH_STANDALONE)
 104     // Vector2 type
 105     typedef struct Vector2 {
 106         float x;
 107         float y;
 108     } Vector2;
 109
 110     // Vector3 type
 111     typedef struct Vector3 {
 112         float x;
 113         float y;
 114         float z;
 115     } Vector3;
 116
 117     // Vector4 type
 118     typedef struct Vector4 {
 119         float x;
 120         float y;
 121         float z;
 122         float w;
 123     } Vector4;
 124
 125     // Quaternion type
 126     typedef Vector4 Quaternion;
 127
 128     // Matrix type (OpenGL style 4x4 - right handed, column major)
 129     typedef struct Matrix {
 130         float m0, m4, m8, m12;
 131         float m1, m5, m9, m13;
 132         float m2, m6, m10, m14;
 133         float m3, m7, m11, m15;
 134     } Matrix;
 135 #endif
 136
 137 // NOTE: Helper types to be used instead of array return types for *ToFloat functions
 138 typedef struct float3 { float v[3]; } float3;
 139 typedef struct float16 { float v[16]; } float16;
 140
 141 #include <math.h>       // Required for: sinf(), cosf(), sqrtf(), tan(), fabs()
 142
 143 //----------------------------------------------------------------------------------
 144 // Module Functions Definition - Utils math
 145 //----------------------------------------------------------------------------------
 146
 147 // Clamp float value
 148 RMDEF float Clamp(float value, float min, float max)
 149 {
 150     const float res = value < min ? min : value;
 151     return res > max ? max : res;
 152 }
 153
 154 // Calculate linear interpolation between two floats
 155 RMDEF float Lerp(float start, float end, float amount)
 156 {
 157     return start + amount*(end - start);
 158 }
 159
 160 // Normalize input value within input range
 161 RMDEF float Normalize(float value, float start, float end)
 162 {
 163     return (value - start)/(end - start);
 164 }
 165
 166 // Remap input value within input range to output range
 167 RMDEF float Remap(float value, float inputStart, float inputEnd, float outputStart, float outputEnd)
 168 {
 169     return (value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart;
 170 }
 171
 172 //----------------------------------------------------------------------------------
 173 // Module Functions Definition - Vector2 math
 174 //----------------------------------------------------------------------------------
 175
 176 // Vector with components value 0.0f
 177 RMDEF Vector2 Vector2Zero(void)
 178 {
 179     Vector2 result = { 0.0f, 0.0f };
 180     return result;
 181 }
 182
 183 // Vector with components value 1.0f
 184 RMDEF Vector2 Vector2One(void)
 185 {
 186     Vector2 result = { 1.0f, 1.0f };
 187     return result;
 188 }
 189
 190 // Add two vectors (v1 + v2)
 191 RMDEF Vector2 Vector2Add(Vector2 v1, Vector2 v2)
 192 {
 193     Vector2 result = { v1.x + v2.x, v1.y + v2.y };
 194     return result;
 195 }
 196
 197 // Add vector and float value
 198 RMDEF Vector2 Vector2AddValue(Vector2 v, float add)
 199 {
 200     Vector2 result = { v.x + add, v.y + add };
 201     return result;
 202 }
 203
 204 // Subtract two vectors (v1 - v2)
 205 RMDEF Vector2 Vector2Subtract(Vector2 v1, Vector2 v2)
 206 {
 207     Vector2 result = { v1.x - v2.x, v1.y - v2.y };
 208     return result;
 209 }
 210
 211 // Subtract vector by float value
 212 RMDEF Vector2 Vector2SubtractValue(Vector2 v, float sub)
 213 {
 214     Vector2 result = { v.x - sub, v.y - sub };
 215     return result;
 216 }
 217
 218 // Calculate vector length
 219 RMDEF float Vector2Length(Vector2 v)
 220 {
 221     float result = sqrtf((v.x*v.x) + (v.y*v.y));
 222     return result;
 223 }
 224
 225 // Calculate vector square length
 226 RMDEF float Vector2LengthSqr(Vector2 v)
 227 {
 228     float result = (v.x*v.x) + (v.y*v.y);
 229     return result;
 230 }
 231
 232 // Calculate two vectors dot product
 233 RMDEF float Vector2DotProduct(Vector2 v1, Vector2 v2)
 234 {
 235     float result = (v1.x*v2.x + v1.y*v2.y);
 236     return result;
 237 }
 238
 239 // Calculate distance between two vectors
 240 RMDEF float Vector2Distance(Vector2 v1, Vector2 v2)
 241 {
 242     float result = sqrtf((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y));
 243     return result;
 244 }
 245
 246 // Calculate angle from two vectors in X-axis
 247 RMDEF float Vector2Angle(Vector2 v1, Vector2 v2)
 248 {
 249     float result = atan2f(v2.y - v1.y, v2.x - v1.x)*(180.0f/PI);
 250     if (result < 0) result += 360.0f;
 251     return result;
 252 }
 253
 254 // Scale vector (multiply by value)
 255 RMDEF Vector2 Vector2Scale(Vector2 v, float scale)
 256 {
 257     Vector2 result = { v.x*scale, v.y*scale };
 258     return result;
 259 }
 260
 261 // Multiply vector by vector
 262 RMDEF Vector2 Vector2Multiply(Vector2 v1, Vector2 v2)
 263 {
 264     Vector2 result = { v1.x*v2.x, v1.y*v2.y };
 265     return result;
 266 }
 267
 268 // Negate vector
 269 RMDEF Vector2 Vector2Negate(Vector2 v)
 270 {
 271     Vector2 result = { -v.x, -v.y };
 272     return result;
 273 }
 274
 275 // Divide vector by vector
 276 RMDEF Vector2 Vector2Divide(Vector2 v1, Vector2 v2)
 277 {
 278     Vector2 result = { v1.x/v2.x, v1.y/v2.y };
 279     return result;
 280 }
 281
 282 // Normalize provided vector
 283 RMDEF Vector2 Vector2Normalize(Vector2 v)
 284 {
 285     Vector2 result = Vector2Scale(v, 1/Vector2Length(v));
 286     return result;
 287 }
 288
 289 // Calculate linear interpolation between two vectors
 290 RMDEF Vector2 Vector2Lerp(Vector2 v1, Vector2 v2, float amount)
 291 {
 292     Vector2 result = { 0 };
 293
 294     result.x = v1.x + amount*(v2.x - v1.x);
 295     result.y = v1.y + amount*(v2.y - v1.y);
 296
 297     return result;
 298 }
 299
 300 // Calculate reflected vector to normal
 301 RMDEF Vector2 Vector2Reflect(Vector2 v, Vector2 normal)
 302 {
 303     Vector2 result = { 0 };
 304
 305     float dotProduct = Vector2DotProduct(v, normal);
 306
 307     result.x = v.x - (2.0f*normal.x)*dotProduct;
 308     result.y = v.y - (2.0f*normal.y)*dotProduct;
 309
 310     return result;
 311 }
 312
 313 // Rotate Vector by float in Degrees.
 314 RMDEF Vector2 Vector2Rotate(Vector2 v, float degs)
 315 {
 316     float rads = degs*DEG2RAD;
 317     Vector2 result = {v.x*cosf(rads) - v.y*sinf(rads) , v.x*sinf(rads) + v.y*cosf(rads) };
 318     return result;
 319 }
 320
 321 // Move Vector towards target
 322 RMDEF Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance)
 323 {
 324     Vector2 result = { 0 };
 325     float dx = target.x - v.x;
 326     float dy = target.y - v.y;
 327     float value = (dx*dx) + (dy*dy);
 328
 329     if ((value == 0) || ((maxDistance >= 0) && (value <= maxDistance*maxDistance))) result = target;
 330
 331     float dist = sqrtf(value);
 332
 333     result.x = v.x + dx/dist*maxDistance;
 334     result.y = v.y + dy/dist*maxDistance;
 335
 336     return result;
 337 }
 338
 339 //----------------------------------------------------------------------------------
 340 // Module Functions Definition - Vector3 math
 341 //----------------------------------------------------------------------------------
 342
 343 // Vector with components value 0.0f
 344 RMDEF Vector3 Vector3Zero(void)
 345 {
 346     Vector3 result = { 0.0f, 0.0f, 0.0f };
 347     return result;
 348 }
 349
 350 // Vector with components value 1.0f
 351 RMDEF Vector3 Vector3One(void)
 352 {
 353     Vector3 result = { 1.0f, 1.0f, 1.0f };
 354     return result;
 355 }
 356
 357 // Add two vectors
 358 RMDEF Vector3 Vector3Add(Vector3 v1, Vector3 v2)
 359 {
 360     Vector3 result = { v1.x + v2.x, v1.y + v2.y, v1.z + v2.z };
 361     return result;
 362 }
 363
 364 // Add vector and float value
 365 RMDEF Vector3 Vector3AddValue(Vector3 v, float add)
 366 {
 367     Vector3 result = { v.x + add, v.y + add, v.z + add };
 368     return result;
 369 }
 370
 371 // Subtract two vectors
 372 RMDEF Vector3 Vector3Subtract(Vector3 v1, Vector3 v2)
 373 {
 374     Vector3 result = { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z };
 375     return result;
 376 }
 377
 378 // Subtract vector by float value
 379 RMDEF Vector3 Vector3SubtractValue(Vector3 v, float sub)
 380 {
 381     Vector3 result = { v.x - sub, v.y - sub, v.z - sub };
 382     return result;
 383 }
 384
 385 // Multiply vector by scalar
 386 RMDEF Vector3 Vector3Scale(Vector3 v, float scalar)
 387 {
 388     Vector3 result = { v.x*scalar, v.y*scalar, v.z*scalar };
 389     return result;
 390 }
 391
 392 // Multiply vector by vector
 393 RMDEF Vector3 Vector3Multiply(Vector3 v1, Vector3 v2)
 394 {
 395     Vector3 result = { v1.x*v2.x, v1.y*v2.y, v1.z*v2.z };
 396     return result;
 397 }
 398
 399 // Calculate two vectors cross product
 400 RMDEF Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2)
 401 {
 402     Vector3 result = { v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x };
 403     return result;
 404 }
 405
 406 // Calculate one vector perpendicular vector
 407 RMDEF Vector3 Vector3Perpendicular(Vector3 v)
 408 {
 409     Vector3 result = { 0 };
 410
 411     float min = (float) fabs(v.x);
 412     Vector3 cardinalAxis = {1.0f, 0.0f, 0.0f};
 413
 414     if (fabs(v.y) < min)
 415     {
 416         min = (float) fabs(v.y);
 417         Vector3 tmp = {0.0f, 1.0f, 0.0f};
 418         cardinalAxis = tmp;
 419     }
 420
 421     if (fabs(v.z) < min)
 422     {
 423         Vector3 tmp = {0.0f, 0.0f, 1.0f};
 424         cardinalAxis = tmp;
 425     }
 426
 427     result = Vector3CrossProduct(v, cardinalAxis);
 428
 429     return result;
 430 }
 431
 432 // Calculate vector length
 433 RMDEF float Vector3Length(const Vector3 v)
 434 {
 435     float result = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z);
 436     return result;
 437 }
 438
 439 // Calculate vector square length
 440 RMDEF float Vector3LengthSqr(const Vector3 v)
 441 {
 442     float result = v.x*v.x + v.y*v.y + v.z*v.z;
 443     return result;
 444 }
 445
 446 // Calculate two vectors dot product
 447 RMDEF float Vector3DotProduct(Vector3 v1, Vector3 v2)
 448 {
 449     float result = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z);
 450     return result;
 451 }
 452
 453 // Calculate distance between two vectors
 454 RMDEF float Vector3Distance(Vector3 v1, Vector3 v2)
 455 {
 456     float dx = v2.x - v1.x;
 457     float dy = v2.y - v1.y;
 458     float dz = v2.z - v1.z;
 459     float result = sqrtf(dx*dx + dy*dy + dz*dz);
 460     return result;
 461 }
 462
 463 // Negate provided vector (invert direction)
 464 RMDEF Vector3 Vector3Negate(Vector3 v)
 465 {
 466     Vector3 result = { -v.x, -v.y, -v.z };
 467     return result;
 468 }
 469
 470 // Divide vector by vector
 471 RMDEF Vector3 Vector3Divide(Vector3 v1, Vector3 v2)
 472 {
 473     Vector3 result = { v1.x/v2.x, v1.y/v2.y, v1.z/v2.z };
 474     return result;
 475 }
 476
 477 // Normalize provided vector
 478 RMDEF Vector3 Vector3Normalize(Vector3 v)
 479 {
 480     Vector3 result = v;
 481
 482     float length, ilength;
 483     length = Vector3Length(v);
 484     if (length == 0.0f) length = 1.0f;
 485     ilength = 1.0f/length;
 486
 487     result.x *= ilength;
 488     result.y *= ilength;
 489     result.z *= ilength;
 490
 491     return result;
 492 }
 493
 494 // Orthonormalize provided vectors
 495 // Makes vectors normalized and orthogonal to each other
 496 // Gram-Schmidt function implementation
 497 RMDEF void Vector3OrthoNormalize(Vector3 *v1, Vector3 *v2)
 498 {
 499     *v1 = Vector3Normalize(*v1);
 500     Vector3 vn = Vector3CrossProduct(*v1, *v2);
 501     vn = Vector3Normalize(vn);
 502     *v2 = Vector3CrossProduct(vn, *v1);
 503 }
 504
 505 // Transforms a Vector3 by a given Matrix
 506 RMDEF Vector3 Vector3Transform(Vector3 v, Matrix mat)
 507 {
 508     Vector3 result = { 0 };
 509     float x = v.x;
 510     float y = v.y;
 511     float z = v.z;
 512
 513     result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12;
 514     result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13;
 515     result.z = mat.m2*x + mat.m6*y + mat.m10*z + mat.m14;
 516
 517     return result;
 518 }
 519
 520 // Transform a vector by quaternion rotation
 521 RMDEF Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q)
 522 {
 523     Vector3 result = { 0 };
 524
 525     result.x = v.x*(q.x*q.x + q.w*q.w - q.y*q.y - q.z*q.z) + v.y*(2*q.x*q.y - 2*q.w*q.z) + v.z*(2*q.x*q.z + 2*q.w*q.y);
 526     result.y = v.x*(2*q.w*q.z + 2*q.x*q.y) + v.y*(q.w*q.w - q.x*q.x + q.y*q.y - q.z*q.z) + v.z*(-2*q.w*q.x + 2*q.y*q.z);
 527     result.z = v.x*(-2*q.w*q.y + 2*q.x*q.z) + v.y*(2*q.w*q.x + 2*q.y*q.z)+ v.z*(q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z);
 528
 529     return result;
 530 }
 531
 532 // Calculate linear interpolation between two vectors
 533 RMDEF Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount)
 534 {
 535     Vector3 result = { 0 };
 536
 537     result.x = v1.x + amount*(v2.x - v1.x);
 538     result.y = v1.y + amount*(v2.y - v1.y);
 539     result.z = v1.z + amount*(v2.z - v1.z);
 540
 541     return result;
 542 }
 543
 544 // Calculate reflected vector to normal
 545 RMDEF Vector3 Vector3Reflect(Vector3 v, Vector3 normal)
 546 {
 547     // I is the original vector
 548     // N is the normal of the incident plane
 549     // R = I - (2*N*( DotProduct[ I,N] ))
 550
 551     Vector3 result = { 0 };
 552
 553     float dotProduct = Vector3DotProduct(v, normal);
 554
 555     result.x = v.x - (2.0f*normal.x)*dotProduct;
 556     result.y = v.y - (2.0f*normal.y)*dotProduct;
 557     result.z = v.z - (2.0f*normal.z)*dotProduct;
 558
 559     return result;
 560 }
 561
 562 // Return min value for each pair of components
 563 RMDEF Vector3 Vector3Min(Vector3 v1, Vector3 v2)
 564 {
 565     Vector3 result = { 0 };
 566
 567     result.x = fminf(v1.x, v2.x);
 568     result.y = fminf(v1.y, v2.y);
 569     result.z = fminf(v1.z, v2.z);
 570
 571     return result;
 572 }
 573
 574 // Return max value for each pair of components
 575 RMDEF Vector3 Vector3Max(Vector3 v1, Vector3 v2)
 576 {
 577     Vector3 result = { 0 };
 578
 579     result.x = fmaxf(v1.x, v2.x);
 580     result.y = fmaxf(v1.y, v2.y);
 581     result.z = fmaxf(v1.z, v2.z);
 582
 583     return result;
 584 }
 585
 586 // Compute barycenter coordinates (u, v, w) for point p with respect to triangle (a, b, c)
 587 // NOTE: Assumes P is on the plane of the triangle
 588 RMDEF Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c)
 589 {
 590     //Vector v0 = b - a, v1 = c - a, v2 = p - a;
 591
 592     Vector3 v0 = Vector3Subtract(b, a);
 593     Vector3 v1 = Vector3Subtract(c, a);
 594     Vector3 v2 = Vector3Subtract(p, a);
 595     float d00 = Vector3DotProduct(v0, v0);
 596     float d01 = Vector3DotProduct(v0, v1);
 597     float d11 = Vector3DotProduct(v1, v1);
 598     float d20 = Vector3DotProduct(v2, v0);
 599     float d21 = Vector3DotProduct(v2, v1);
 600
 601     float denom = d00*d11 - d01*d01;
 602
 603     Vector3 result = { 0 };
 604
 605     result.y = (d11*d20 - d01*d21)/denom;
 606     result.z = (d00*d21 - d01*d20)/denom;
 607     result.x = 1.0f - (result.z + result.y);
 608
 609     return result;
 610 }
 611
 612 // Returns Vector3 as float array
 613 RMDEF float3 Vector3ToFloatV(Vector3 v)
 614 {
 615     float3 buffer = { 0 };
 616
 617     buffer.v[0] = v.x;
 618     buffer.v[1] = v.y;
 619     buffer.v[2] = v.z;
 620
 621     return buffer;
 622 }
 623
 624 //----------------------------------------------------------------------------------
 625 // Module Functions Definition - Matrix math
 626 //----------------------------------------------------------------------------------
 627
 628 // Compute matrix determinant
 629 RMDEF float MatrixDeterminant(Matrix mat)
 630 {
 631     // Cache the matrix values (speed optimization)
 632     float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
 633     float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
 634     float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
 635     float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
 636
 637     float result = a30*a21*a12*a03 - a20*a31*a12*a03 - a30*a11*a22*a03 + a10*a31*a22*a03 +
 638                    a20*a11*a32*a03 - a10*a21*a32*a03 - a30*a21*a02*a13 + a20*a31*a02*a13 +
 639                    a30*a01*a22*a13 - a00*a31*a22*a13 - a20*a01*a32*a13 + a00*a21*a32*a13 +
 640                    a30*a11*a02*a23 - a10*a31*a02*a23 - a30*a01*a12*a23 + a00*a31*a12*a23 +
 641                    a10*a01*a32*a23 - a00*a11*a32*a23 - a20*a11*a02*a33 + a10*a21*a02*a33 +
 642                    a20*a01*a12*a33 - a00*a21*a12*a33 - a10*a01*a22*a33 + a00*a11*a22*a33;
 643
 644     return result;
 645 }
 646
 647 // Returns the trace of the matrix (sum of the values along the diagonal)
 648 RMDEF float MatrixTrace(Matrix mat)
 649 {
 650     float result = (mat.m0 + mat.m5 + mat.m10 + mat.m15);
 651     return result;
 652 }
 653
 654 // Transposes provided matrix
 655 RMDEF Matrix MatrixTranspose(Matrix mat)
 656 {
 657     Matrix result = { 0 };
 658
 659     result.m0 = mat.m0;
 660     result.m1 = mat.m4;
 661     result.m2 = mat.m8;
 662     result.m3 = mat.m12;
 663     result.m4 = mat.m1;
 664     result.m5 = mat.m5;
 665     result.m6 = mat.m9;
 666     result.m7 = mat.m13;
 667     result.m8 = mat.m2;
 668     result.m9 = mat.m6;
 669     result.m10 = mat.m10;
 670     result.m11 = mat.m14;
 671     result.m12 = mat.m3;
 672     result.m13 = mat.m7;
 673     result.m14 = mat.m11;
 674     result.m15 = mat.m15;
 675
 676     return result;
 677 }
 678
 679 // Invert provided matrix
 680 RMDEF Matrix MatrixInvert(Matrix mat)
 681 {
 682     Matrix result = { 0 };
 683
 684     // Cache the matrix values (speed optimization)
 685     float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3;
 686     float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7;
 687     float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11;
 688     float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15;
 689
 690     float b00 = a00*a11 - a01*a10;
 691     float b01 = a00*a12 - a02*a10;
 692     float b02 = a00*a13 - a03*a10;
 693     float b03 = a01*a12 - a02*a11;
 694     float b04 = a01*a13 - a03*a11;
 695     float b05 = a02*a13 - a03*a12;
 696     float b06 = a20*a31 - a21*a30;
 697     float b07 = a20*a32 - a22*a30;
 698     float b08 = a20*a33 - a23*a30;
 699     float b09 = a21*a32 - a22*a31;
 700     float b10 = a21*a33 - a23*a31;
 701     float b11 = a22*a33 - a23*a32;
 702
 703     // Calculate the invert determinant (inlined to avoid double-caching)
 704     float invDet = 1.0f/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06);
 705
 706     result.m0 = (a11*b11 - a12*b10 + a13*b09)*invDet;
 707     result.m1 = (-a01*b11 + a02*b10 - a03*b09)*invDet;
 708     result.m2 = (a31*b05 - a32*b04 + a33*b03)*invDet;
 709     result.m3 = (-a21*b05 + a22*b04 - a23*b03)*invDet;
 710     result.m4 = (-a10*b11 + a12*b08 - a13*b07)*invDet;
 711     result.m5 = (a00*b11 - a02*b08 + a03*b07)*invDet;
 712     result.m6 = (-a30*b05 + a32*b02 - a33*b01)*invDet;
 713     result.m7 = (a20*b05 - a22*b02 + a23*b01)*invDet;
 714     result.m8 = (a10*b10 - a11*b08 + a13*b06)*invDet;
 715     result.m9 = (-a00*b10 + a01*b08 - a03*b06)*invDet;
 716     result.m10 = (a30*b04 - a31*b02 + a33*b00)*invDet;
 717     result.m11 = (-a20*b04 + a21*b02 - a23*b00)*invDet;
 718     result.m12 = (-a10*b09 + a11*b07 - a12*b06)*invDet;
 719     result.m13 = (a00*b09 - a01*b07 + a02*b06)*invDet;
 720     result.m14 = (-a30*b03 + a31*b01 - a32*b00)*invDet;
 721     result.m15 = (a20*b03 - a21*b01 + a22*b00)*invDet;
 722
 723     return result;
 724 }
 725
 726 // Normalize provided matrix
 727 RMDEF Matrix MatrixNormalize(Matrix mat)
 728 {
 729     Matrix result = { 0 };
 730
 731     float det = MatrixDeterminant(mat);
 732
 733     result.m0 = mat.m0/det;
 734     result.m1 = mat.m1/det;
 735     result.m2 = mat.m2/det;
 736     result.m3 = mat.m3/det;
 737     result.m4 = mat.m4/det;
 738     result.m5 = mat.m5/det;
 739     result.m6 = mat.m6/det;
 740     result.m7 = mat.m7/det;
 741     result.m8 = mat.m8/det;
 742     result.m9 = mat.m9/det;
 743     result.m10 = mat.m10/det;
 744     result.m11 = mat.m11/det;
 745     result.m12 = mat.m12/det;
 746     result.m13 = mat.m13/det;
 747     result.m14 = mat.m14/det;
 748     result.m15 = mat.m15/det;
 749
 750     return result;
 751 }
 752
 753 // Returns identity matrix
 754 RMDEF Matrix MatrixIdentity(void)
 755 {
 756     Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f,
 757                       0.0f, 1.0f, 0.0f, 0.0f,
 758                       0.0f, 0.0f, 1.0f, 0.0f,
 759                       0.0f, 0.0f, 0.0f, 1.0f };
 760
 761     return result;
 762 }
 763
 764 // Add two matrices
 765 RMDEF Matrix MatrixAdd(Matrix left, Matrix right)
 766 {
 767     Matrix result = MatrixIdentity();
 768
 769     result.m0 = left.m0 + right.m0;
 770     result.m1 = left.m1 + right.m1;
 771     result.m2 = left.m2 + right.m2;
 772     result.m3 = left.m3 + right.m3;
 773     result.m4 = left.m4 + right.m4;
 774     result.m5 = left.m5 + right.m5;
 775     result.m6 = left.m6 + right.m6;
 776     result.m7 = left.m7 + right.m7;
 777     result.m8 = left.m8 + right.m8;
 778     result.m9 = left.m9 + right.m9;
 779     result.m10 = left.m10 + right.m10;
 780     result.m11 = left.m11 + right.m11;
 781     result.m12 = left.m12 + right.m12;
 782     result.m13 = left.m13 + right.m13;
 783     result.m14 = left.m14 + right.m14;
 784     result.m15 = left.m15 + right.m15;
 785
 786     return result;
 787 }
 788
 789 // Subtract two matrices (left - right)
 790 RMDEF Matrix MatrixSubtract(Matrix left, Matrix right)
 791 {
 792     Matrix result = MatrixIdentity();
 793
 794     result.m0 = left.m0 - right.m0;
 795     result.m1 = left.m1 - right.m1;
 796     result.m2 = left.m2 - right.m2;
 797     result.m3 = left.m3 - right.m3;
 798     result.m4 = left.m4 - right.m4;
 799     result.m5 = left.m5 - right.m5;
 800     result.m6 = left.m6 - right.m6;
 801     result.m7 = left.m7 - right.m7;
 802     result.m8 = left.m8 - right.m8;
 803     result.m9 = left.m9 - right.m9;
 804     result.m10 = left.m10 - right.m10;
 805     result.m11 = left.m11 - right.m11;
 806     result.m12 = left.m12 - right.m12;
 807     result.m13 = left.m13 - right.m13;
 808     result.m14 = left.m14 - right.m14;
 809     result.m15 = left.m15 - right.m15;
 810
 811     return result;
 812 }
 813
 814 // Returns two matrix multiplication
 815 // NOTE: When multiplying matrices... the order matters!
 816 RMDEF Matrix MatrixMultiply(Matrix left, Matrix right)
 817 {
 818     Matrix result = { 0 };
 819
 820     result.m0 = left.m0*right.m0 + left.m1*right.m4 + left.m2*right.m8 + left.m3*right.m12;
 821     result.m1 = left.m0*right.m1 + left.m1*right.m5 + left.m2*right.m9 + left.m3*right.m13;
 822     result.m2 = left.m0*right.m2 + left.m1*right.m6 + left.m2*right.m10 + left.m3*right.m14;
 823     result.m3 = left.m0*right.m3 + left.m1*right.m7 + left.m2*right.m11 + left.m3*right.m15;
 824     result.m4 = left.m4*right.m0 + left.m5*right.m4 + left.m6*right.m8 + left.m7*right.m12;
 825     result.m5 = left.m4*right.m1 + left.m5*right.m5 + left.m6*right.m9 + left.m7*right.m13;
 826     result.m6 = left.m4*right.m2 + left.m5*right.m6 + left.m6*right.m10 + left.m7*right.m14;
 827     result.m7 = left.m4*right.m3 + left.m5*right.m7 + left.m6*right.m11 + left.m7*right.m15;
 828     result.m8 = left.m8*right.m0 + left.m9*right.m4 + left.m10*right.m8 + left.m11*right.m12;
 829     result.m9 = left.m8*right.m1 + left.m9*right.m5 + left.m10*right.m9 + left.m11*right.m13;
 830     result.m10 = left.m8*right.m2 + left.m9*right.m6 + left.m10*right.m10 + left.m11*right.m14;
 831     result.m11 = left.m8*right.m3 + left.m9*right.m7 + left.m10*right.m11 + left.m11*right.m15;
 832     result.m12 = left.m12*right.m0 + left.m13*right.m4 + left.m14*right.m8 + left.m15*right.m12;
 833     result.m13 = left.m12*right.m1 + left.m13*right.m5 + left.m14*right.m9 + left.m15*right.m13;
 834     result.m14 = left.m12*right.m2 + left.m13*right.m6 + left.m14*right.m10 + left.m15*right.m14;
 835     result.m15 = left.m12*right.m3 + left.m13*right.m7 + left.m14*right.m11 + left.m15*right.m15;
 836
 837     return result;
 838 }
 839
 840 // Returns translation matrix
 841 RMDEF Matrix MatrixTranslate(float x, float y, float z)
 842 {
 843     Matrix result = { 1.0f, 0.0f, 0.0f, x,
 844                       0.0f, 1.0f, 0.0f, y,
 845                       0.0f, 0.0f, 1.0f, z,
 846                       0.0f, 0.0f, 0.0f, 1.0f };
 847
 848     return result;
 849 }
 850
 851 // Create rotation matrix from axis and angle
 852 // NOTE: Angle should be provided in radians
 853 RMDEF Matrix MatrixRotate(Vector3 axis, float angle)
 854 {
 855     Matrix result = { 0 };
 856
 857     float x = axis.x, y = axis.y, z = axis.z;
 858
 859     float lengthSquared = x*x + y*y + z*z;
 860
 861     if ((lengthSquared != 1.0f) && (lengthSquared != 0.0f))
 862     {
 863         float inverseLength = 1.0f/sqrtf(lengthSquared);
 864         x *= inverseLength;
 865         y *= inverseLength;
 866         z *= inverseLength;
 867     }
 868
 869     float sinres = sinf(angle);
 870     float cosres = cosf(angle);
 871     float t = 1.0f - cosres;
 872
 873     result.m0  = x*x*t + cosres;
 874     result.m1  = y*x*t + z*sinres;
 875     result.m2  = z*x*t - y*sinres;
 876     result.m3  = 0.0f;
 877
 878     result.m4  = x*y*t - z*sinres;
 879     result.m5  = y*y*t + cosres;
 880     result.m6  = z*y*t + x*sinres;
 881     result.m7  = 0.0f;
 882
 883     result.m8  = x*z*t + y*sinres;
 884     result.m9  = y*z*t - x*sinres;
 885     result.m10 = z*z*t + cosres;
 886     result.m11 = 0.0f;
 887
 888     result.m12 = 0.0f;
 889     result.m13 = 0.0f;
 890     result.m14 = 0.0f;
 891     result.m15 = 1.0f;
 892
 893     return result;
 894 }
 895
 896 // Returns x-rotation matrix (angle in radians)
 897 RMDEF Matrix MatrixRotateX(float angle)
 898 {
 899     Matrix result = MatrixIdentity();
 900
 901     float cosres = cosf(angle);
 902     float sinres = sinf(angle);
 903
 904     result.m5 = cosres;
 905     result.m6 = -sinres;
 906     result.m9 = sinres;
 907     result.m10 = cosres;
 908
 909     return result;
 910 }
 911
 912 // Returns y-rotation matrix (angle in radians)
 913 RMDEF Matrix MatrixRotateY(float angle)
 914 {
 915     Matrix result = MatrixIdentity();
 916
 917     float cosres = cosf(angle);
 918     float sinres = sinf(angle);
 919
 920     result.m0 = cosres;
 921     result.m2 = sinres;
 922     result.m8 = -sinres;
 923     result.m10 = cosres;
 924
 925     return result;
 926 }
 927
 928 // Returns z-rotation matrix (angle in radians)
 929 RMDEF Matrix MatrixRotateZ(float angle)
 930 {
 931     Matrix result = MatrixIdentity();
 932
 933     float cosres = cosf(angle);
 934     float sinres = sinf(angle);
 935
 936     result.m0 = cosres;
 937     result.m1 = -sinres;
 938     result.m4 = sinres;
 939     result.m5 = cosres;
 940
 941     return result;
 942 }
 943
 944
 945 // Returns xyz-rotation matrix (angles in radians)
 946 RMDEF Matrix MatrixRotateXYZ(Vector3 ang)
 947 {
 948     Matrix result = MatrixIdentity();
 949
 950     float cosz = cosf(-ang.z);
 951     float sinz = sinf(-ang.z);
 952     float cosy = cosf(-ang.y);
 953     float siny = sinf(-ang.y);
 954     float cosx = cosf(-ang.x);
 955     float sinx = sinf(-ang.x);
 956
 957     result.m0 = cosz*cosy;
 958     result.m4 = (cosz*siny*sinx) - (sinz*cosx);
 959     result.m8 = (cosz*siny*cosx) + (sinz*sinx);
 960
 961     result.m1 = sinz*cosy;
 962     result.m5 = (sinz*siny*sinx) + (cosz*cosx);
 963     result.m9 = (sinz*siny*cosx) - (cosz*sinx);
 964
 965     result.m2 = -siny;
 966     result.m6 = cosy*sinx;
 967     result.m10= cosy*cosx;
 968
 969     return result;
 970 }
 971
 972 // Returns zyx-rotation matrix (angles in radians)
 973 RMDEF Matrix MatrixRotateZYX(Vector3 ang)
 974 {
 975     Matrix result = { 0 };
 976
 977     float cz = cosf(ang.z);
 978     float sz = sinf(ang.z);
 979     float cy = cosf(ang.y);
 980     float sy = sinf(ang.y);
 981     float cx = cosf(ang.x);
 982     float sx = sinf(ang.x);
 983
 984     result.m0 = cz*cy;
 985     result.m1 = cz*sy*sx - cx*sz;
 986     result.m2 = sz*sx + cz*cx*sy;
 987     result.m3 = 0;
 988
 989     result.m4 = cy*sz;
 990     result.m5 = cz*cx + sz*sy*sx;
 991     result.m6 = cx*sz*sy - cz*sx;
 992     result.m7 = 0;
 993
 994     result.m8 = -sy;
 995     result.m9 = cy*sx;
 996     result.m10 = cy*cx;
 997     result.m11 = 0;
 998
 999     result.m12 = 0;
1000     result.m13 = 0;
1001     result.m14 = 0;
1002     result.m15 = 1;
1003
1004     return result;
1005 }
1006
1007 // Returns scaling matrix
1008 RMDEF Matrix MatrixScale(float x, float y, float z)
1009 {
1010     Matrix result = { x, 0.0f, 0.0f, 0.0f,
1011                       0.0f, y, 0.0f, 0.0f,
1012                       0.0f, 0.0f, z, 0.0f,
1013                       0.0f, 0.0f, 0.0f, 1.0f };
1014
1015     return result;
1016 }
1017
1018 // Returns perspective projection matrix
1019 RMDEF Matrix MatrixFrustum(double left, double right, double bottom, double top, double near, double far)
1020 {
1021     Matrix result = { 0 };
1022
1023     float rl = (float)(right - left);
1024     float tb = (float)(top - bottom);
1025     float fn = (float)(far - near);
1026
1027     result.m0 = ((float) near*2.0f)/rl;
1028     result.m1 = 0.0f;
1029     result.m2 = 0.0f;
1030     result.m3 = 0.0f;
1031
1032     result.m4 = 0.0f;
1033     result.m5 = ((float) near*2.0f)/tb;
1034     result.m6 = 0.0f;
1035     result.m7 = 0.0f;
1036
1037     result.m8 = ((float)right + (float)left)/rl;
1038     result.m9 = ((float)top + (float)bottom)/tb;
1039     result.m10 = -((float)far + (float)near)/fn;
1040     result.m11 = -1.0f;
1041
1042     result.m12 = 0.0f;
1043     result.m13 = 0.0f;
1044     result.m14 = -((float)far*(float)near*2.0f)/fn;
1045     result.m15 = 0.0f;
1046
1047     return result;
1048 }
1049
1050 // Returns perspective projection matrix
1051 // NOTE: Angle should be provided in radians
1052 RMDEF Matrix MatrixPerspective(double fovy, double aspect, double near, double far)
1053 {
1054     double top = near*tan(fovy*0.5);
1055     double right = top*aspect;
1056     Matrix result = MatrixFrustum(-right, right, -top, top, near, far);
1057
1058     return result;
1059 }
1060
1061 // Returns orthographic projection matrix
1062 RMDEF Matrix MatrixOrtho(double left, double right, double bottom, double top, double near, double far)
1063 {
1064     Matrix result = { 0 };
1065
1066     float rl = (float)(right - left);
1067     float tb = (float)(top - bottom);
1068     float fn = (float)(far - near);
1069
1070     result.m0 = 2.0f/rl;
1071     result.m1 = 0.0f;
1072     result.m2 = 0.0f;
1073     result.m3 = 0.0f;
1074     result.m4 = 0.0f;
1075     result.m5 = 2.0f/tb;
1076     result.m6 = 0.0f;
1077     result.m7 = 0.0f;
1078     result.m8 = 0.0f;
1079     result.m9 = 0.0f;
1080     result.m10 = -2.0f/fn;
1081     result.m11 = 0.0f;
1082     result.m12 = -((float)left + (float)right)/rl;
1083     result.m13 = -((float)top + (float)bottom)/tb;
1084     result.m14 = -((float)far + (float)near)/fn;
1085     result.m15 = 1.0f;
1086
1087     return result;
1088 }
1089
1090 // Returns camera look-at matrix (view matrix)
1091 RMDEF Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up)
1092 {
1093     Matrix result = { 0 };
1094
1095     Vector3 z = Vector3Subtract(eye, target);
1096     z = Vector3Normalize(z);
1097     Vector3 x = Vector3CrossProduct(up, z);
1098     x = Vector3Normalize(x);
1099     Vector3 y = Vector3CrossProduct(z, x);
1100
1101     result.m0 = x.x;
1102     result.m1 = y.x;
1103     result.m2 = z.x;
1104     result.m3 = 0.0f;
1105     result.m4 = x.y;
1106     result.m5 = y.y;
1107     result.m6 = z.y;
1108     result.m7 = 0.0f;
1109     result.m8 = x.z;
1110     result.m9 = y.z;
1111     result.m10 = z.z;
1112     result.m11 = 0.0f;
1113     result.m12 = -Vector3DotProduct(x, eye);
1114     result.m13 = -Vector3DotProduct(y, eye);
1115     result.m14 = -Vector3DotProduct(z, eye);
1116     result.m15 = 1.0f;
1117
1118     return result;
1119 }
1120
1121 // Returns float array of matrix data
1122 RMDEF float16 MatrixToFloatV(Matrix mat)
1123 {
1124     float16 buffer = { 0 };
1125
1126     buffer.v[0] = mat.m0;
1127     buffer.v[1] = mat.m1;
1128     buffer.v[2] = mat.m2;
1129     buffer.v[3] = mat.m3;
1130     buffer.v[4] = mat.m4;
1131     buffer.v[5] = mat.m5;
1132     buffer.v[6] = mat.m6;
1133     buffer.v[7] = mat.m7;
1134     buffer.v[8] = mat.m8;
1135     buffer.v[9] = mat.m9;
1136     buffer.v[10] = mat.m10;
1137     buffer.v[11] = mat.m11;
1138     buffer.v[12] = mat.m12;
1139     buffer.v[13] = mat.m13;
1140     buffer.v[14] = mat.m14;
1141     buffer.v[15] = mat.m15;
1142
1143     return buffer;
1144 }
1145
1146 //----------------------------------------------------------------------------------
1147 // Module Functions Definition - Quaternion math
1148 //----------------------------------------------------------------------------------
1149
1150 // Add two quaternions
1151 RMDEF Quaternion QuaternionAdd(Quaternion q1, Quaternion q2)
1152 {
1153     Quaternion result = {q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w};
1154     return result;
1155 }
1156
1157 // Add quaternion and float value
1158 RMDEF Quaternion QuaternionAddValue(Quaternion q, float add)
1159 {
1160     Quaternion result = {q.x + add, q.y + add, q.z + add, q.w + add};
1161     return result;
1162 }
1163
1164 // Subtract two quaternions
1165 RMDEF Quaternion QuaternionSubtract(Quaternion q1, Quaternion q2)
1166 {
1167     Quaternion result = {q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w};
1168     return result;
1169 }
1170
1171 // Subtract quaternion and float value
1172 RMDEF Quaternion QuaternionSubtractValue(Quaternion q, float sub)
1173 {
1174     Quaternion result = {q.x - sub, q.y - sub, q.z - sub, q.w - sub};
1175     return result;
1176 }
1177
1178 // Returns identity quaternion
1179 RMDEF Quaternion QuaternionIdentity(void)
1180 {
1181     Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
1182     return result;
1183 }
1184
1185 // Computes the length of a quaternion
1186 RMDEF float QuaternionLength(Quaternion q)
1187 {
1188     float result = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w);
1189     return result;
1190 }
1191
1192 // Normalize provided quaternion
1193 RMDEF Quaternion QuaternionNormalize(Quaternion q)
1194 {
1195     Quaternion result = { 0 };
1196
1197     float length, ilength;
1198     length = QuaternionLength(q);
1199     if (length == 0.0f) length = 1.0f;
1200     ilength = 1.0f/length;
1201
1202     result.x = q.x*ilength;
1203     result.y = q.y*ilength;
1204     result.z = q.z*ilength;
1205     result.w = q.w*ilength;
1206
1207     return result;
1208 }
1209
1210 // Invert provided quaternion
1211 RMDEF Quaternion QuaternionInvert(Quaternion q)
1212 {
1213     Quaternion result = q;
1214     float length = QuaternionLength(q);
1215     float lengthSq = length*length;
1216
1217     if (lengthSq != 0.0)
1218     {
1219         float i = 1.0f/lengthSq;
1220
1221         result.x *= -i;
1222         result.y *= -i;
1223         result.z *= -i;
1224         result.w *= i;
1225     }
1226
1227     return result;
1228 }
1229
1230 // Calculate two quaternion multiplication
1231 RMDEF Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2)
1232 {
1233     Quaternion result = { 0 };
1234
1235     float qax = q1.x, qay = q1.y, qaz = q1.z, qaw = q1.w;
1236     float qbx = q2.x, qby = q2.y, qbz = q2.z, qbw = q2.w;
1237
1238     result.x = qax*qbw + qaw*qbx + qay*qbz - qaz*qby;
1239     result.y = qay*qbw + qaw*qby + qaz*qbx - qax*qbz;
1240     result.z = qaz*qbw + qaw*qbz + qax*qby - qay*qbx;
1241     result.w = qaw*qbw - qax*qbx - qay*qby - qaz*qbz;
1242
1243     return result;
1244 }
1245
1246 // Scale quaternion by float value
1247 RMDEF Quaternion QuaternionScale(Quaternion q, float mul)
1248 {
1249     Quaternion result = { 0 };
1250
1251     float qax = q.x, qay = q.y, qaz = q.z, qaw = q.w;
1252
1253     result.x = qax*mul + qaw*mul + qay*mul - qaz*mul;
1254     result.y = qay*mul + qaw*mul + qaz*mul - qax*mul;
1255     result.z = qaz*mul + qaw*mul + qax*mul - qay*mul;
1256     result.w = qaw*mul - qax*mul - qay*mul - qaz*mul;
1257
1258     return result;
1259 }
1260
1261 // Divide two quaternions
1262 RMDEF Quaternion QuaternionDivide(Quaternion q1, Quaternion q2)
1263 {
1264     Quaternion result = { q1.x/q2.x, q1.y/q2.y, q1.z/q2.z, q1.w/q2.w };
1265     return result;
1266 }
1267
1268 // Calculate linear interpolation between two quaternions
1269 RMDEF Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount)
1270 {
1271     Quaternion result = { 0 };
1272
1273     result.x = q1.x + amount*(q2.x - q1.x);
1274     result.y = q1.y + amount*(q2.y - q1.y);
1275     result.z = q1.z + amount*(q2.z - q1.z);
1276     result.w = q1.w + amount*(q2.w - q1.w);
1277
1278     return result;
1279 }
1280
1281 // Calculate slerp-optimized interpolation between two quaternions
1282 RMDEF Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount)
1283 {
1284     Quaternion result = QuaternionLerp(q1, q2, amount);
1285     result = QuaternionNormalize(result);
1286
1287     return result;
1288 }
1289
1290 // Calculates spherical linear interpolation between two quaternions
1291 RMDEF Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount)
1292 {
1293     Quaternion result = { 0 };
1294
1295     float cosHalfTheta =  q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w;
1296
1297     if (cosHalfTheta < 0)
1298     {
1299         q2.x = -q2.x; q2.y = -q2.y; q2.z = -q2.z; q2.w = -q2.w;
1300         cosHalfTheta = -cosHalfTheta;
1301     }
1302
1303     if (fabs(cosHalfTheta) >= 1.0f) result = q1;
1304     else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount);
1305     else
1306     {
1307         float halfTheta = acosf(cosHalfTheta);
1308         float sinHalfTheta = sqrtf(1.0f - cosHalfTheta*cosHalfTheta);
1309
1310         if (fabs(sinHalfTheta) < 0.001f)
1311         {
1312             result.x = (q1.x*0.5f + q2.x*0.5f);
1313             result.y = (q1.y*0.5f + q2.y*0.5f);
1314             result.z = (q1.z*0.5f + q2.z*0.5f);
1315             result.w = (q1.w*0.5f + q2.w*0.5f);
1316         }
1317         else
1318         {
1319             float ratioA = sinf((1 - amount)*halfTheta)/sinHalfTheta;
1320             float ratioB = sinf(amount*halfTheta)/sinHalfTheta;
1321
1322             result.x = (q1.x*ratioA + q2.x*ratioB);
1323             result.y = (q1.y*ratioA + q2.y*ratioB);
1324             result.z = (q1.z*ratioA + q2.z*ratioB);
1325             result.w = (q1.w*ratioA + q2.w*ratioB);
1326         }
1327     }
1328
1329     return result;
1330 }
1331
1332 // Calculate quaternion based on the rotation from one vector to another
1333 RMDEF Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to)
1334 {
1335     Quaternion result = { 0 };
1336
1337     float cos2Theta = Vector3DotProduct(from, to);
1338     Vector3 cross = Vector3CrossProduct(from, to);
1339
1340     result.x = cross.x;
1341     result.y = cross.y;
1342     result.z = cross.z;
1343     result.w = 1.0f + cos2Theta;     // NOTE: Added QuaternioIdentity()
1344
1345     // Normalize to essentially nlerp the original and identity to 0.5
1346     result = QuaternionNormalize(result);
1347
1348     // Above lines are equivalent to:
1349     //Quaternion result = QuaternionNlerp(q, QuaternionIdentity(), 0.5f);
1350
1351     return result;
1352 }
1353
1354 // Returns a quaternion for a given rotation matrix
1355 RMDEF Quaternion QuaternionFromMatrix(Matrix mat)
1356 {
1357     Quaternion result = { 0 };
1358
1359     if ((mat.m0 > mat.m5) && (mat.m0 > mat.m10))
1360     {
1361         float s = sqrtf(1.0f + mat.m0 - mat.m5 - mat.m10)*2;
1362
1363         result.x = 0.25f*s;
1364         result.y = (mat.m4 + mat.m1)/s;
1365         result.z = (mat.m2 + mat.m8)/s;
1366         result.w = (mat.m9 - mat.m6)/s;
1367     }
1368     else if (mat.m5 > mat.m10)
1369     {
1370         float s = sqrtf(1.0f + mat.m5 - mat.m0 - mat.m10)*2;
1371         result.x = (mat.m4 + mat.m1)/s;
1372         result.y = 0.25f*s;
1373         result.z = (mat.m9 + mat.m6)/s;
1374         result.w = (mat.m2 - mat.m8)/s;
1375     }
1376     else
1377     {
1378         float s  = sqrtf(1.0f + mat.m10 - mat.m0 - mat.m5)*2;
1379         result.x = (mat.m2 + mat.m8)/s;
1380         result.y = (mat.m9 + mat.m6)/s;
1381         result.z = 0.25f*s;
1382         result.w = (mat.m4 - mat.m1)/s;
1383     }
1384
1385     return result;
1386 }
1387
1388 // Returns a matrix for a given quaternion
1389 RMDEF Matrix QuaternionToMatrix(Quaternion q)
1390 {
1391     Matrix result = MatrixIdentity();
1392
1393     float a2 = 2*(q.x*q.x), b2=2*(q.y*q.y), c2=2*(q.z*q.z); //, d2=2*(q.w*q.w);
1394
1395     float ab = 2*(q.x*q.y), ac=2*(q.x*q.z), bc=2*(q.y*q.z);
1396     float ad = 2*(q.x*q.w), bd=2*(q.y*q.w), cd=2*(q.z*q.w);
1397
1398     result.m0 = 1 - b2 - c2;
1399     result.m1 = ab - cd;
1400     result.m2 = ac + bd;
1401
1402     result.m4 = ab + cd;
1403     result.m5 = 1 - a2 - c2;
1404     result.m6 = bc - ad;
1405
1406     result.m8 = ac - bd;
1407     result.m9 = bc + ad;
1408     result.m10 = 1 - a2 - b2;
1409
1410     return result;
1411 }
1412
1413 // Returns rotation quaternion for an angle and axis
1414 // NOTE: angle must be provided in radians
1415 RMDEF Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle)
1416 {
1417     Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f };
1418
1419     if (Vector3Length(axis) != 0.0f)
1420
1421     angle *= 0.5f;
1422
1423     axis = Vector3Normalize(axis);
1424
1425     float sinres = sinf(angle);
1426     float cosres = cosf(angle);
1427
1428     result.x = axis.x*sinres;
1429     result.y = axis.y*sinres;
1430     result.z = axis.z*sinres;
1431     result.w = cosres;
1432
1433     result = QuaternionNormalize(result);
1434
1435     return result;
1436 }
1437
1438 // Returns the rotation angle and axis for a given quaternion
1439 RMDEF void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle)
1440 {
1441     if (fabs(q.w) > 1.0f) q = QuaternionNormalize(q);
1442
1443     Vector3 resAxis = { 0.0f, 0.0f, 0.0f };
1444     float resAngle = 2.0f*acosf(q.w);
1445     float den = sqrtf(1.0f - q.w*q.w);
1446
1447     if (den > 0.0001f)
1448     {
1449         resAxis.x = q.x/den;
1450         resAxis.y = q.y/den;
1451         resAxis.z = q.z/den;
1452     }
1453     else
1454     {
1455         // This occurs when the angle is zero.
1456         // Not a problem: just set an arbitrary normalized axis.
1457         resAxis.x = 1.0f;
1458     }
1459
1460     *outAxis = resAxis;
1461     *outAngle = resAngle;
1462 }
1463
1464 // Returns the quaternion equivalent to Euler angles
1465 // NOTE: Rotation order is ZYX
1466 RMDEF Quaternion QuaternionFromEuler(float pitch, float yaw, float roll)
1467 {
1468     Quaternion q = { 0 };
1469
1470     float x0 = cosf(pitch*0.5f);
1471     float x1 = sinf(pitch*0.5f);
1472     float y0 = cosf(yaw*0.5f);
1473     float y1 = sinf(yaw*0.5f);
1474     float z0 = cosf(roll*0.5f);
1475     float z1 = sinf(roll*0.5f);
1476
1477     q.x = x1*y0*z0 - x0*y1*z1;
1478     q.y = x0*y1*z0 + x1*y0*z1;
1479     q.z = x0*y0*z1 - x1*y1*z0;
1480     q.w = x0*y0*z0 + x1*y1*z1;
1481
1482     return q;
1483 }
1484
1485 // Return the Euler angles equivalent to quaternion (roll, pitch, yaw)
1486 // NOTE: Angles are returned in a Vector3 struct in degrees
1487 RMDEF Vector3 QuaternionToEuler(Quaternion q)
1488 {
1489     Vector3 result = { 0 };
1490
1491     // roll (x-axis rotation)
1492     float x0 = 2.0f*(q.w*q.x + q.y*q.z);
1493     float x1 = 1.0f - 2.0f*(q.x*q.x + q.y*q.y);
1494     result.x = atan2f(x0, x1)*RAD2DEG;
1495
1496     // pitch (y-axis rotation)
1497     float y0 = 2.0f*(q.w*q.y - q.z*q.x);
1498     y0 = y0 > 1.0f ? 1.0f : y0;
1499     y0 = y0 < -1.0f ? -1.0f : y0;
1500     result.y = asinf(y0)*RAD2DEG;
1501
1502     // yaw (z-axis rotation)
1503     float z0 = 2.0f*(q.w*q.z + q.x*q.y);
1504     float z1 = 1.0f - 2.0f*(q.y*q.y + q.z*q.z);
1505     result.z = atan2f(z0, z1)*RAD2DEG;
1506
1507     return result;
1508 }
1509
1510 // Transform a quaternion given a transformation matrix
1511 RMDEF Quaternion QuaternionTransform(Quaternion q, Matrix mat)
1512 {
1513     Quaternion result = { 0 };
1514
1515     result.x = mat.m0*q.x + mat.m4*q.y + mat.m8*q.z + mat.m12*q.w;
1516     result.y = mat.m1*q.x + mat.m5*q.y + mat.m9*q.z + mat.m13*q.w;
1517     result.z = mat.m2*q.x + mat.m6*q.y + mat.m10*q.z + mat.m14*q.w;
1518     result.w = mat.m3*q.x + mat.m7*q.y + mat.m11*q.z + mat.m15*q.w;
1519
1520     return result;
1521 }
1522
1523 // Projects a Vector3 from screen space into object space
1524 RMDEF Vector3 Vector3Unproject(Vector3 source, Matrix projection, Matrix view)
1525 {
1526     Vector3 result = { 0.0f, 0.0f, 0.0f };
1527
1528     // Calculate unproject matrix (multiply view patrix by projection matrix) and invert it
1529     Matrix matViewProj = MatrixMultiply(view, projection);
1530     matViewProj = MatrixInvert(matViewProj);
1531
1532     // Create quaternion from source point
1533     Quaternion quat = { source.x, source.y, source.z, 1.0f };
1534
1535     // Multiply quat point by unproject matrix
1536     quat = QuaternionTransform(quat, matViewProj);
1537
1538     // Normalized world points in vectors
1539     result.x = quat.x/quat.w;
1540     result.y = quat.y/quat.w;
1541     result.z = quat.z/quat.w;
1542
1543     return result;
1544 }
1545
1546 #endif  // RAYMATH_H