github.com/Konstantin8105/c4go@v0.0.0-20240505174241-768bb1c65a51/tests/raylib/raymath.h (about) 1 /********************************************************************************************** 2 * 3 * raymath v1.5 - Math functions to work with Vector2, 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_STATIC_INLINE 13 * Define static inline functions code, so #include header suffices for use. 14 * This may use up lots of memory. 15 * 16 * CONVENTIONS: 17 * 18 * - Functions are always self-contained, no function use another raymath function inside, 19 * required code is directly re-implemented inside 20 * - Functions input parameters are always received by value (2 unavoidable exceptions) 21 * - Functions use always a "result" variable for return 22 * - Functions are always defined inline 23 * - Angles are always in radians (DEG2RAD/RAD2DEG macros provided for convenience) 24 * 25 * 26 * LICENSE: zlib/libpng 27 * 28 * Copyright (c) 2015-2022 Ramon Santamaria (@raysan5) 29 * 30 * This software is provided "as-is", without any express or implied warranty. In no event 31 * will the authors be held liable for any damages arising from the use of this software. 32 * 33 * Permission is granted to anyone to use this software for any purpose, including commercial 34 * applications, and to alter it and redistribute it freely, subject to the following restrictions: 35 * 36 * 1. The origin of this software must not be misrepresented; you must not claim that you 37 * wrote the original software. If you use this software in a product, an acknowledgment 38 * in the product documentation would be appreciated but is not required. 39 * 40 * 2. Altered source versions must be plainly marked as such, and must not be misrepresented 41 * as being the original software. 42 * 43 * 3. This notice may not be removed or altered from any source distribution. 44 * 45 **********************************************************************************************/ 46 47 #ifndef RAYMATH_H 48 #define RAYMATH_H 49 50 #if defined(RAYMATH_IMPLEMENTATION) && defined(RAYMATH_STATIC_INLINE) 51 #error "Specifying both RAYMATH_IMPLEMENTATION and RAYMATH_STATIC_INLINE is contradictory" 52 #endif 53 54 // Function specifiers definition 55 #if defined(RAYMATH_IMPLEMENTATION) 56 #if defined(_WIN32) && defined(BUILD_LIBTYPE_SHARED) 57 #define RMAPI __declspec(dllexport) extern inline // We are building raylib as a Win32 shared library (.dll). 58 #elif defined(_WIN32) && defined(USE_LIBTYPE_SHARED) 59 #define RMAPI __declspec(dllimport) // We are using raylib as a Win32 shared library (.dll) 60 #else 61 #define RMAPI extern inline // Provide external definition 62 #endif 63 #elif defined(RAYMATH_STATIC_INLINE) 64 #define RMAPI static inline // Functions may be inlined, no external out-of-line definition 65 #else 66 #if defined(__TINYC__) 67 #define RMAPI static inline // plain inline not supported by tinycc (See issue #435) 68 #else 69 #define RMAPI inline // Functions may be inlined or external definition used 70 #endif 71 #endif 72 73 //---------------------------------------------------------------------------------- 74 // Defines and Macros 75 //---------------------------------------------------------------------------------- 76 #ifndef PI 77 #define PI 3.14159265358979323846f 78 #endif 79 80 #ifndef EPSILON 81 #define EPSILON 0.000001f 82 #endif 83 84 #ifndef DEG2RAD 85 #define DEG2RAD (PI/180.0f) 86 #endif 87 88 #ifndef RAD2DEG 89 #define RAD2DEG (180.0f/PI) 90 #endif 91 92 // Get float vector for Matrix 93 #ifndef MatrixToFloat 94 #define MatrixToFloat(mat) (MatrixToFloatV(mat).v) 95 #endif 96 97 // Get float vector for Vector3 98 #ifndef Vector3ToFloat 99 #define Vector3ToFloat(vec) (Vector3ToFloatV(vec).v) 100 #endif 101 102 //---------------------------------------------------------------------------------- 103 // Types and Structures Definition 104 //---------------------------------------------------------------------------------- 105 #if !defined(RL_VECTOR2_TYPE) 106 // Vector2 type 107 typedef struct Vector2 { 108 float x; 109 float y; 110 } Vector2; 111 #define RL_VECTOR2_TYPE 112 #endif 113 114 #if !defined(RL_VECTOR3_TYPE) 115 // Vector3 type 116 typedef struct Vector3 { 117 float x; 118 float y; 119 float z; 120 } Vector3; 121 #define RL_VECTOR3_TYPE 122 #endif 123 124 #if !defined(RL_VECTOR4_TYPE) 125 // Vector4 type 126 typedef struct Vector4 { 127 float x; 128 float y; 129 float z; 130 float w; 131 } Vector4; 132 #define RL_VECTOR4_TYPE 133 #endif 134 135 #if !defined(RL_QUATERNION_TYPE) 136 // Quaternion type 137 typedef Vector4 Quaternion; 138 #define RL_QUATERNION_TYPE 139 #endif 140 141 #if !defined(RL_MATRIX_TYPE) 142 // Matrix type (OpenGL style 4x4 - right handed, column major) 143 typedef struct Matrix { 144 float m0, m4, m8, m12; // Matrix first row (4 components) 145 float m1, m5, m9, m13; // Matrix second row (4 components) 146 float m2, m6, m10, m14; // Matrix third row (4 components) 147 float m3, m7, m11, m15; // Matrix fourth row (4 components) 148 } Matrix; 149 #define RL_MATRIX_TYPE 150 #endif 151 152 // NOTE: Helper types to be used instead of array return types for *ToFloat functions 153 typedef struct float3 { 154 float v[3]; 155 } float3; 156 157 typedef struct float16 { 158 float v[16]; 159 } float16; 160 161 #include <math.h> // Required for: sinf(), cosf(), tan(), atan2f(), sqrtf(), floor(), fminf(), fmaxf(), fabs() 162 163 //---------------------------------------------------------------------------------- 164 // Module Functions Definition - Utils math 165 //---------------------------------------------------------------------------------- 166 167 // Clamp float value 168 RMAPI float Clamp(float value, float min, float max) 169 { 170 float result = (value < min)? min : value; 171 172 if (result > max) result = max; 173 174 return result; 175 } 176 177 // Calculate linear interpolation between two floats 178 RMAPI float Lerp(float start, float end, float amount) 179 { 180 float result = start + amount*(end - start); 181 182 return result; 183 } 184 185 // Normalize input value within input range 186 RMAPI float Normalize(float value, float start, float end) 187 { 188 float result = (value - start)/(end - start); 189 190 return result; 191 } 192 193 // Remap input value within input range to output range 194 RMAPI float Remap(float value, float inputStart, float inputEnd, float outputStart, float outputEnd) 195 { 196 float result = (value - inputStart)/(inputEnd - inputStart)*(outputEnd - outputStart) + outputStart; 197 198 return result; 199 } 200 201 // Wrap input value from min to max 202 RMAPI float Wrap(float value, float min, float max) 203 { 204 float result = value - (max - min)*floorf((value - min)/(max - min)); 205 206 return result; 207 } 208 209 // Check whether two given floats are almost equal 210 RMAPI int FloatEquals(float x, float y) 211 { 212 int result = (fabsf(x - y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(x), fabsf(y)))); 213 214 return result; 215 } 216 217 //---------------------------------------------------------------------------------- 218 // Module Functions Definition - Vector2 math 219 //---------------------------------------------------------------------------------- 220 221 // Vector with components value 0.0f 222 RMAPI Vector2 Vector2Zero(void) 223 { 224 Vector2 result = { 0.0f, 0.0f }; 225 226 return result; 227 } 228 229 // Vector with components value 1.0f 230 RMAPI Vector2 Vector2One(void) 231 { 232 Vector2 result = { 1.0f, 1.0f }; 233 234 return result; 235 } 236 237 // Add two vectors (v1 + v2) 238 RMAPI Vector2 Vector2Add(Vector2 v1, Vector2 v2) 239 { 240 Vector2 result = { v1.x + v2.x, v1.y + v2.y }; 241 242 return result; 243 } 244 245 // Add vector and float value 246 RMAPI Vector2 Vector2AddValue(Vector2 v, float add) 247 { 248 Vector2 result = { v.x + add, v.y + add }; 249 250 return result; 251 } 252 253 // Subtract two vectors (v1 - v2) 254 RMAPI Vector2 Vector2Subtract(Vector2 v1, Vector2 v2) 255 { 256 Vector2 result = { v1.x - v2.x, v1.y - v2.y }; 257 258 return result; 259 } 260 261 // Subtract vector by float value 262 RMAPI Vector2 Vector2SubtractValue(Vector2 v, float sub) 263 { 264 Vector2 result = { v.x - sub, v.y - sub }; 265 266 return result; 267 } 268 269 // Calculate vector length 270 RMAPI float Vector2Length(Vector2 v) 271 { 272 float result = sqrtf((v.x*v.x) + (v.y*v.y)); 273 274 return result; 275 } 276 277 // Calculate vector square length 278 RMAPI float Vector2LengthSqr(Vector2 v) 279 { 280 float result = (v.x*v.x) + (v.y*v.y); 281 282 return result; 283 } 284 285 // Calculate two vectors dot product 286 RMAPI float Vector2DotProduct(Vector2 v1, Vector2 v2) 287 { 288 float result = (v1.x*v2.x + v1.y*v2.y); 289 290 return result; 291 } 292 293 // Calculate distance between two vectors 294 RMAPI float Vector2Distance(Vector2 v1, Vector2 v2) 295 { 296 float result = sqrtf((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y)); 297 298 return result; 299 } 300 301 // Calculate square distance between two vectors 302 RMAPI float Vector2DistanceSqr(Vector2 v1, Vector2 v2) 303 { 304 float result = ((v1.x - v2.x)*(v1.x - v2.x) + (v1.y - v2.y)*(v1.y - v2.y)); 305 306 return result; 307 } 308 309 // Calculate angle from two vectors 310 RMAPI float Vector2Angle(Vector2 v1, Vector2 v2) 311 { 312 float result = atan2f(v2.y, v2.x) - atan2f(v1.y, v1.x); 313 314 return result; 315 } 316 317 // Scale vector (multiply by value) 318 RMAPI Vector2 Vector2Scale(Vector2 v, float scale) 319 { 320 Vector2 result = { v.x*scale, v.y*scale }; 321 322 return result; 323 } 324 325 // Multiply vector by vector 326 RMAPI Vector2 Vector2Multiply(Vector2 v1, Vector2 v2) 327 { 328 Vector2 result = { v1.x*v2.x, v1.y*v2.y }; 329 330 return result; 331 } 332 333 // Negate vector 334 RMAPI Vector2 Vector2Negate(Vector2 v) 335 { 336 Vector2 result = { -v.x, -v.y }; 337 338 return result; 339 } 340 341 // Divide vector by vector 342 RMAPI Vector2 Vector2Divide(Vector2 v1, Vector2 v2) 343 { 344 Vector2 result = { v1.x/v2.x, v1.y/v2.y }; 345 346 return result; 347 } 348 349 // Normalize provided vector 350 RMAPI Vector2 Vector2Normalize(Vector2 v) 351 { 352 Vector2 result = { 0 }; 353 float length = sqrtf((v.x*v.x) + (v.y*v.y)); 354 355 if (length > 0) 356 { 357 float ilength = 1.0f/length; 358 result.x = v.x*ilength; 359 result.y = v.y*ilength; 360 } 361 362 return result; 363 } 364 365 // Transforms a Vector2 by a given Matrix 366 RMAPI Vector2 Vector2Transform(Vector2 v, Matrix mat) 367 { 368 Vector2 result = { 0 }; 369 370 float x = v.x; 371 float y = v.y; 372 float z = 0; 373 374 result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12; 375 result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13; 376 377 return result; 378 } 379 380 // Calculate linear interpolation between two vectors 381 RMAPI Vector2 Vector2Lerp(Vector2 v1, Vector2 v2, float amount) 382 { 383 Vector2 result = { 0 }; 384 385 result.x = v1.x + amount*(v2.x - v1.x); 386 result.y = v1.y + amount*(v2.y - v1.y); 387 388 return result; 389 } 390 391 // Calculate reflected vector to normal 392 RMAPI Vector2 Vector2Reflect(Vector2 v, Vector2 normal) 393 { 394 Vector2 result = { 0 }; 395 396 float dotProduct = (v.x*normal.x + v.y*normal.y); // Dot product 397 398 result.x = v.x - (2.0f*normal.x)*dotProduct; 399 result.y = v.y - (2.0f*normal.y)*dotProduct; 400 401 return result; 402 } 403 404 // Rotate vector by angle 405 RMAPI Vector2 Vector2Rotate(Vector2 v, float angle) 406 { 407 Vector2 result = { 0 }; 408 409 float cosres = cosf(angle); 410 float sinres = sinf(angle); 411 412 result.x = v.x*cosres - v.y*sinres; 413 result.y = v.x*sinres + v.y*cosres; 414 415 return result; 416 } 417 418 // Move Vector towards target 419 RMAPI Vector2 Vector2MoveTowards(Vector2 v, Vector2 target, float maxDistance) 420 { 421 Vector2 result = { 0 }; 422 423 float dx = target.x - v.x; 424 float dy = target.y - v.y; 425 float value = (dx*dx) + (dy*dy); 426 427 if ((value == 0) || ((maxDistance >= 0) && (value <= maxDistance*maxDistance))) return target; 428 429 float dist = sqrtf(value); 430 431 result.x = v.x + dx/dist*maxDistance; 432 result.y = v.y + dy/dist*maxDistance; 433 434 return result; 435 } 436 437 // Invert the given vector 438 RMAPI Vector2 Vector2Invert(Vector2 v) 439 { 440 Vector2 result = { 1.0f/v.x, 1.0f/v.y }; 441 442 return result; 443 } 444 445 // Clamp the components of the vector between 446 // min and max values specified by the given vectors 447 RMAPI Vector2 Vector2Clamp(Vector2 v, Vector2 min, Vector2 max) 448 { 449 Vector2 result = { 0 }; 450 451 result.x = fminf(max.x, fmaxf(min.x, v.x)); 452 result.y = fminf(max.y, fmaxf(min.y, v.y)); 453 454 return result; 455 } 456 457 // Clamp the magnitude of the vector between two min and max values 458 RMAPI Vector2 Vector2ClampValue(Vector2 v, float min, float max) 459 { 460 Vector2 result = v; 461 462 float length = (v.x*v.x) + (v.y*v.y); 463 if (length > 0.0f) 464 { 465 length = sqrtf(length); 466 467 if (length < min) 468 { 469 float scale = min/length; 470 result.x = v.x*scale; 471 result.y = v.y*scale; 472 } 473 else if (length > max) 474 { 475 float scale = max/length; 476 result.x = v.x*scale; 477 result.y = v.y*scale; 478 } 479 } 480 481 return result; 482 } 483 484 // Check whether two given vectors are almost equal 485 RMAPI int Vector2Equals(Vector2 p, Vector2 q) 486 { 487 int result = ((fabsf(p.x - q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && 488 ((fabsf(p.y - q.y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))); 489 490 return result; 491 } 492 493 //---------------------------------------------------------------------------------- 494 // Module Functions Definition - Vector3 math 495 //---------------------------------------------------------------------------------- 496 497 // Vector with components value 0.0f 498 RMAPI Vector3 Vector3Zero(void) 499 { 500 Vector3 result = { 0.0f, 0.0f, 0.0f }; 501 502 return result; 503 } 504 505 // Vector with components value 1.0f 506 RMAPI Vector3 Vector3One(void) 507 { 508 Vector3 result = { 1.0f, 1.0f, 1.0f }; 509 510 return result; 511 } 512 513 // Add two vectors 514 RMAPI Vector3 Vector3Add(Vector3 v1, Vector3 v2) 515 { 516 Vector3 result = { v1.x + v2.x, v1.y + v2.y, v1.z + v2.z }; 517 518 return result; 519 } 520 521 // Add vector and float value 522 RMAPI Vector3 Vector3AddValue(Vector3 v, float add) 523 { 524 Vector3 result = { v.x + add, v.y + add, v.z + add }; 525 526 return result; 527 } 528 529 // Subtract two vectors 530 RMAPI Vector3 Vector3Subtract(Vector3 v1, Vector3 v2) 531 { 532 Vector3 result = { v1.x - v2.x, v1.y - v2.y, v1.z - v2.z }; 533 534 return result; 535 } 536 537 // Subtract vector by float value 538 RMAPI Vector3 Vector3SubtractValue(Vector3 v, float sub) 539 { 540 Vector3 result = { v.x - sub, v.y - sub, v.z - sub }; 541 542 return result; 543 } 544 545 // Multiply vector by scalar 546 RMAPI Vector3 Vector3Scale(Vector3 v, float scalar) 547 { 548 Vector3 result = { v.x*scalar, v.y*scalar, v.z*scalar }; 549 550 return result; 551 } 552 553 // Multiply vector by vector 554 RMAPI Vector3 Vector3Multiply(Vector3 v1, Vector3 v2) 555 { 556 Vector3 result = { v1.x*v2.x, v1.y*v2.y, v1.z*v2.z }; 557 558 return result; 559 } 560 561 // Calculate two vectors cross product 562 RMAPI Vector3 Vector3CrossProduct(Vector3 v1, Vector3 v2) 563 { 564 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 }; 565 566 return result; 567 } 568 569 // Calculate one vector perpendicular vector 570 RMAPI Vector3 Vector3Perpendicular(Vector3 v) 571 { 572 Vector3 result = { 0 }; 573 574 float min = (float) fabs(v.x); 575 Vector3 cardinalAxis = {1.0f, 0.0f, 0.0f}; 576 577 if (fabsf(v.y) < min) 578 { 579 min = (float) fabs(v.y); 580 Vector3 tmp = {0.0f, 1.0f, 0.0f}; 581 cardinalAxis = tmp; 582 } 583 584 if (fabsf(v.z) < min) 585 { 586 Vector3 tmp = {0.0f, 0.0f, 1.0f}; 587 cardinalAxis = tmp; 588 } 589 590 // Cross product between vectors 591 result.x = v.y*cardinalAxis.z - v.z*cardinalAxis.y; 592 result.y = v.z*cardinalAxis.x - v.x*cardinalAxis.z; 593 result.z = v.x*cardinalAxis.y - v.y*cardinalAxis.x; 594 595 return result; 596 } 597 598 // Calculate vector length 599 RMAPI float Vector3Length(const Vector3 v) 600 { 601 float result = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 602 603 return result; 604 } 605 606 // Calculate vector square length 607 RMAPI float Vector3LengthSqr(const Vector3 v) 608 { 609 float result = v.x*v.x + v.y*v.y + v.z*v.z; 610 611 return result; 612 } 613 614 // Calculate two vectors dot product 615 RMAPI float Vector3DotProduct(Vector3 v1, Vector3 v2) 616 { 617 float result = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z); 618 619 return result; 620 } 621 622 // Calculate distance between two vectors 623 RMAPI float Vector3Distance(Vector3 v1, Vector3 v2) 624 { 625 float result = 0.0f; 626 627 float dx = v2.x - v1.x; 628 float dy = v2.y - v1.y; 629 float dz = v2.z - v1.z; 630 result = sqrtf(dx*dx + dy*dy + dz*dz); 631 632 return result; 633 } 634 635 // Calculate square distance between two vectors 636 RMAPI float Vector3DistanceSqr(Vector3 v1, Vector3 v2) 637 { 638 float result = 0.0f; 639 640 float dx = v2.x - v1.x; 641 float dy = v2.y - v1.y; 642 float dz = v2.z - v1.z; 643 result = dx*dx + dy*dy + dz*dz; 644 645 return result; 646 } 647 648 // Calculate angle between two vectors 649 RMAPI float Vector3Angle(Vector3 v1, Vector3 v2) 650 { 651 float result = 0.0f; 652 653 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 }; 654 float len = sqrtf(cross.x*cross.x + cross.y*cross.y + cross.z*cross.z); 655 float dot = (v1.x*v2.x + v1.y*v2.y + v1.z*v2.z); 656 result = atan2f(len, dot); 657 658 return result; 659 } 660 661 // Negate provided vector (invert direction) 662 RMAPI Vector3 Vector3Negate(Vector3 v) 663 { 664 Vector3 result = { -v.x, -v.y, -v.z }; 665 666 return result; 667 } 668 669 // Divide vector by vector 670 RMAPI Vector3 Vector3Divide(Vector3 v1, Vector3 v2) 671 { 672 Vector3 result = { v1.x/v2.x, v1.y/v2.y, v1.z/v2.z }; 673 674 return result; 675 } 676 677 // Normalize provided vector 678 RMAPI Vector3 Vector3Normalize(Vector3 v) 679 { 680 Vector3 result = v; 681 682 float length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 683 if (length == 0.0f) length = 1.0f; 684 float ilength = 1.0f/length; 685 686 result.x *= ilength; 687 result.y *= ilength; 688 result.z *= ilength; 689 690 return result; 691 } 692 693 // Orthonormalize provided vectors 694 // Makes vectors normalized and orthogonal to each other 695 // Gram-Schmidt function implementation 696 RMAPI void Vector3OrthoNormalize(Vector3 *v1, Vector3 *v2) 697 { 698 float length = 0.0f; 699 float ilength = 0.0f; 700 701 // Vector3Normalize(*v1); 702 Vector3 v = *v1; 703 length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 704 if (length == 0.0f) length = 1.0f; 705 ilength = 1.0f/length; 706 v1->x *= ilength; 707 v1->y *= ilength; 708 v1->z *= ilength; 709 710 // Vector3CrossProduct(*v1, *v2) 711 Vector3 vn1 = { v1->y*v2->z - v1->z*v2->y, v1->z*v2->x - v1->x*v2->z, v1->x*v2->y - v1->y*v2->x }; 712 713 // Vector3Normalize(vn1); 714 v = vn1; 715 length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 716 if (length == 0.0f) length = 1.0f; 717 ilength = 1.0f/length; 718 vn1.x *= ilength; 719 vn1.y *= ilength; 720 vn1.z *= ilength; 721 722 // Vector3CrossProduct(vn1, *v1) 723 Vector3 vn2 = { vn1.y*v1->z - vn1.z*v1->y, vn1.z*v1->x - vn1.x*v1->z, vn1.x*v1->y - vn1.y*v1->x }; 724 725 *v2 = vn2; 726 } 727 728 // Transforms a Vector3 by a given Matrix 729 RMAPI Vector3 Vector3Transform(Vector3 v, Matrix mat) 730 { 731 Vector3 result = { 0 }; 732 733 float x = v.x; 734 float y = v.y; 735 float z = v.z; 736 737 result.x = mat.m0*x + mat.m4*y + mat.m8*z + mat.m12; 738 result.y = mat.m1*x + mat.m5*y + mat.m9*z + mat.m13; 739 result.z = mat.m2*x + mat.m6*y + mat.m10*z + mat.m14; 740 741 return result; 742 } 743 744 // Transform a vector by quaternion rotation 745 RMAPI Vector3 Vector3RotateByQuaternion(Vector3 v, Quaternion q) 746 { 747 Vector3 result = { 0 }; 748 749 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); 750 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); 751 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); 752 753 return result; 754 } 755 756 // Rotates a vector around an axis 757 RMAPI Vector3 Vector3RotateByAxisAngle(Vector3 v, Vector3 axis, float angle) 758 { 759 // Using Euler-Rodrigues Formula 760 // Ref.: https://en.wikipedia.org/w/index.php?title=Euler%E2%80%93Rodrigues_formula 761 762 Vector3 result = v; 763 764 // Vector3Normalize(axis); 765 float length = sqrtf(axis.x * axis.x + axis.y * axis.y + axis.z * axis.z); 766 if (length == 0.0f) length = 1.0f; 767 float ilength = 1.0f / length; 768 axis.x *= ilength; 769 axis.y *= ilength; 770 axis.z *= ilength; 771 772 angle /= 2.0f; 773 float a = sinf(angle); 774 float b = axis.x * a; 775 float c = axis.y * a; 776 float d = axis.z * a; 777 a = cosf(angle); 778 Vector3 w = { b, c, d }; 779 780 // Vector3CrossProduct(w, v) 781 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 }; 782 783 // Vector3CrossProduct(w, wv) 784 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 }; 785 786 // Vector3Scale(wv, 2 * a) 787 a *= 2; 788 wv.x *= a; 789 wv.y *= a; 790 wv.z *= a; 791 792 // Vector3Scale(wwv, 2) 793 wwv.x *= 2; 794 wwv.y *= 2; 795 wwv.z *= 2; 796 797 result.x += wv.x; 798 result.y += wv.y; 799 result.z += wv.z; 800 801 result.x += wwv.x; 802 result.y += wwv.y; 803 result.z += wwv.z; 804 805 return result; 806 } 807 808 // Calculate linear interpolation between two vectors 809 RMAPI Vector3 Vector3Lerp(Vector3 v1, Vector3 v2, float amount) 810 { 811 Vector3 result = { 0 }; 812 813 result.x = v1.x + amount*(v2.x - v1.x); 814 result.y = v1.y + amount*(v2.y - v1.y); 815 result.z = v1.z + amount*(v2.z - v1.z); 816 817 return result; 818 } 819 820 // Calculate reflected vector to normal 821 RMAPI Vector3 Vector3Reflect(Vector3 v, Vector3 normal) 822 { 823 Vector3 result = { 0 }; 824 825 // I is the original vector 826 // N is the normal of the incident plane 827 // R = I - (2*N*(DotProduct[I, N])) 828 829 float dotProduct = (v.x*normal.x + v.y*normal.y + v.z*normal.z); 830 831 result.x = v.x - (2.0f*normal.x)*dotProduct; 832 result.y = v.y - (2.0f*normal.y)*dotProduct; 833 result.z = v.z - (2.0f*normal.z)*dotProduct; 834 835 return result; 836 } 837 838 // Get min value for each pair of components 839 RMAPI Vector3 Vector3Min(Vector3 v1, Vector3 v2) 840 { 841 Vector3 result = { 0 }; 842 843 result.x = fminf(v1.x, v2.x); 844 result.y = fminf(v1.y, v2.y); 845 result.z = fminf(v1.z, v2.z); 846 847 return result; 848 } 849 850 // Get max value for each pair of components 851 RMAPI Vector3 Vector3Max(Vector3 v1, Vector3 v2) 852 { 853 Vector3 result = { 0 }; 854 855 result.x = fmaxf(v1.x, v2.x); 856 result.y = fmaxf(v1.y, v2.y); 857 result.z = fmaxf(v1.z, v2.z); 858 859 return result; 860 } 861 862 // Compute barycenter coordinates (u, v, w) for point p with respect to triangle (a, b, c) 863 // NOTE: Assumes P is on the plane of the triangle 864 RMAPI Vector3 Vector3Barycenter(Vector3 p, Vector3 a, Vector3 b, Vector3 c) 865 { 866 Vector3 result = { 0 }; 867 868 Vector3 v0 = { b.x - a.x, b.y - a.y, b.z - a.z }; // Vector3Subtract(b, a) 869 Vector3 v1 = { c.x - a.x, c.y - a.y, c.z - a.z }; // Vector3Subtract(c, a) 870 Vector3 v2 = { p.x - a.x, p.y - a.y, p.z - a.z }; // Vector3Subtract(p, a) 871 float d00 = (v0.x*v0.x + v0.y*v0.y + v0.z*v0.z); // Vector3DotProduct(v0, v0) 872 float d01 = (v0.x*v1.x + v0.y*v1.y + v0.z*v1.z); // Vector3DotProduct(v0, v1) 873 float d11 = (v1.x*v1.x + v1.y*v1.y + v1.z*v1.z); // Vector3DotProduct(v1, v1) 874 float d20 = (v2.x*v0.x + v2.y*v0.y + v2.z*v0.z); // Vector3DotProduct(v2, v0) 875 float d21 = (v2.x*v1.x + v2.y*v1.y + v2.z*v1.z); // Vector3DotProduct(v2, v1) 876 877 float denom = d00*d11 - d01*d01; 878 879 result.y = (d11*d20 - d01*d21)/denom; 880 result.z = (d00*d21 - d01*d20)/denom; 881 result.x = 1.0f - (result.z + result.y); 882 883 return result; 884 } 885 886 // Projects a Vector3 from screen space into object space 887 // NOTE: We are avoiding calling other raymath functions despite available 888 RMAPI Vector3 Vector3Unproject(Vector3 source, Matrix projection, Matrix view) 889 { 890 Vector3 result = { 0 }; 891 892 // Calculate unproject matrix (multiply view patrix by projection matrix) and invert it 893 Matrix matViewProj = { // MatrixMultiply(view, projection); 894 view.m0*projection.m0 + view.m1*projection.m4 + view.m2*projection.m8 + view.m3*projection.m12, 895 view.m0*projection.m1 + view.m1*projection.m5 + view.m2*projection.m9 + view.m3*projection.m13, 896 view.m0*projection.m2 + view.m1*projection.m6 + view.m2*projection.m10 + view.m3*projection.m14, 897 view.m0*projection.m3 + view.m1*projection.m7 + view.m2*projection.m11 + view.m3*projection.m15, 898 view.m4*projection.m0 + view.m5*projection.m4 + view.m6*projection.m8 + view.m7*projection.m12, 899 view.m4*projection.m1 + view.m5*projection.m5 + view.m6*projection.m9 + view.m7*projection.m13, 900 view.m4*projection.m2 + view.m5*projection.m6 + view.m6*projection.m10 + view.m7*projection.m14, 901 view.m4*projection.m3 + view.m5*projection.m7 + view.m6*projection.m11 + view.m7*projection.m15, 902 view.m8*projection.m0 + view.m9*projection.m4 + view.m10*projection.m8 + view.m11*projection.m12, 903 view.m8*projection.m1 + view.m9*projection.m5 + view.m10*projection.m9 + view.m11*projection.m13, 904 view.m8*projection.m2 + view.m9*projection.m6 + view.m10*projection.m10 + view.m11*projection.m14, 905 view.m8*projection.m3 + view.m9*projection.m7 + view.m10*projection.m11 + view.m11*projection.m15, 906 view.m12*projection.m0 + view.m13*projection.m4 + view.m14*projection.m8 + view.m15*projection.m12, 907 view.m12*projection.m1 + view.m13*projection.m5 + view.m14*projection.m9 + view.m15*projection.m13, 908 view.m12*projection.m2 + view.m13*projection.m6 + view.m14*projection.m10 + view.m15*projection.m14, 909 view.m12*projection.m3 + view.m13*projection.m7 + view.m14*projection.m11 + view.m15*projection.m15 }; 910 911 // Calculate inverted matrix -> MatrixInvert(matViewProj); 912 // Cache the matrix values (speed optimization) 913 float a00 = matViewProj.m0, a01 = matViewProj.m1, a02 = matViewProj.m2, a03 = matViewProj.m3; 914 float a10 = matViewProj.m4, a11 = matViewProj.m5, a12 = matViewProj.m6, a13 = matViewProj.m7; 915 float a20 = matViewProj.m8, a21 = matViewProj.m9, a22 = matViewProj.m10, a23 = matViewProj.m11; 916 float a30 = matViewProj.m12, a31 = matViewProj.m13, a32 = matViewProj.m14, a33 = matViewProj.m15; 917 918 float b00 = a00*a11 - a01*a10; 919 float b01 = a00*a12 - a02*a10; 920 float b02 = a00*a13 - a03*a10; 921 float b03 = a01*a12 - a02*a11; 922 float b04 = a01*a13 - a03*a11; 923 float b05 = a02*a13 - a03*a12; 924 float b06 = a20*a31 - a21*a30; 925 float b07 = a20*a32 - a22*a30; 926 float b08 = a20*a33 - a23*a30; 927 float b09 = a21*a32 - a22*a31; 928 float b10 = a21*a33 - a23*a31; 929 float b11 = a22*a33 - a23*a32; 930 931 // Calculate the invert determinant (inlined to avoid double-caching) 932 float invDet = 1.0f/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06); 933 934 Matrix matViewProjInv = { 935 (a11*b11 - a12*b10 + a13*b09)*invDet, 936 (-a01*b11 + a02*b10 - a03*b09)*invDet, 937 (a31*b05 - a32*b04 + a33*b03)*invDet, 938 (-a21*b05 + a22*b04 - a23*b03)*invDet, 939 (-a10*b11 + a12*b08 - a13*b07)*invDet, 940 (a00*b11 - a02*b08 + a03*b07)*invDet, 941 (-a30*b05 + a32*b02 - a33*b01)*invDet, 942 (a20*b05 - a22*b02 + a23*b01)*invDet, 943 (a10*b10 - a11*b08 + a13*b06)*invDet, 944 (-a00*b10 + a01*b08 - a03*b06)*invDet, 945 (a30*b04 - a31*b02 + a33*b00)*invDet, 946 (-a20*b04 + a21*b02 - a23*b00)*invDet, 947 (-a10*b09 + a11*b07 - a12*b06)*invDet, 948 (a00*b09 - a01*b07 + a02*b06)*invDet, 949 (-a30*b03 + a31*b01 - a32*b00)*invDet, 950 (a20*b03 - a21*b01 + a22*b00)*invDet }; 951 952 // Create quaternion from source point 953 Quaternion quat = { source.x, source.y, source.z, 1.0f }; 954 955 // Multiply quat point by unproject matrix 956 Quaternion qtransformed = { // QuaternionTransform(quat, matViewProjInv) 957 matViewProjInv.m0*quat.x + matViewProjInv.m4*quat.y + matViewProjInv.m8*quat.z + matViewProjInv.m12*quat.w, 958 matViewProjInv.m1*quat.x + matViewProjInv.m5*quat.y + matViewProjInv.m9*quat.z + matViewProjInv.m13*quat.w, 959 matViewProjInv.m2*quat.x + matViewProjInv.m6*quat.y + matViewProjInv.m10*quat.z + matViewProjInv.m14*quat.w, 960 matViewProjInv.m3*quat.x + matViewProjInv.m7*quat.y + matViewProjInv.m11*quat.z + matViewProjInv.m15*quat.w }; 961 962 // Normalized world points in vectors 963 result.x = qtransformed.x/qtransformed.w; 964 result.y = qtransformed.y/qtransformed.w; 965 result.z = qtransformed.z/qtransformed.w; 966 967 return result; 968 } 969 970 // Get Vector3 as float array 971 RMAPI float3 Vector3ToFloatV(Vector3 v) 972 { 973 float3 buffer = { 0 }; 974 975 buffer.v[0] = v.x; 976 buffer.v[1] = v.y; 977 buffer.v[2] = v.z; 978 979 return buffer; 980 } 981 982 // Invert the given vector 983 RMAPI Vector3 Vector3Invert(Vector3 v) 984 { 985 Vector3 result = { 1.0f/v.x, 1.0f/v.y, 1.0f/v.z }; 986 987 return result; 988 } 989 990 // Clamp the components of the vector between 991 // min and max values specified by the given vectors 992 RMAPI Vector3 Vector3Clamp(Vector3 v, Vector3 min, Vector3 max) 993 { 994 Vector3 result = { 0 }; 995 996 result.x = fminf(max.x, fmaxf(min.x, v.x)); 997 result.y = fminf(max.y, fmaxf(min.y, v.y)); 998 result.z = fminf(max.z, fmaxf(min.z, v.z)); 999 1000 return result; 1001 } 1002 1003 // Clamp the magnitude of the vector between two values 1004 RMAPI Vector3 Vector3ClampValue(Vector3 v, float min, float max) 1005 { 1006 Vector3 result = v; 1007 1008 float length = (v.x*v.x) + (v.y*v.y) + (v.z*v.z); 1009 if (length > 0.0f) 1010 { 1011 length = sqrtf(length); 1012 1013 if (length < min) 1014 { 1015 float scale = min/length; 1016 result.x = v.x*scale; 1017 result.y = v.y*scale; 1018 result.z = v.z*scale; 1019 } 1020 else if (length > max) 1021 { 1022 float scale = max/length; 1023 result.x = v.x*scale; 1024 result.y = v.y*scale; 1025 result.z = v.z*scale; 1026 } 1027 } 1028 1029 return result; 1030 } 1031 1032 // Check whether two given vectors are almost equal 1033 RMAPI int Vector3Equals(Vector3 p, Vector3 q) 1034 { 1035 int result = ((fabsf(p.x - q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && 1036 ((fabsf(p.y - q.y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) && 1037 ((fabsf(p.z - q.z)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z))))); 1038 1039 return result; 1040 } 1041 1042 // Compute the direction of a refracted ray where v specifies the 1043 // normalized direction of the incoming ray, n specifies the 1044 // normalized normal vector of the interface of two optical media, 1045 // and r specifies the ratio of the refractive index of the medium 1046 // from where the ray comes to the refractive index of the medium 1047 // on the other side of the surface 1048 RMAPI Vector3 Vector3Refract(Vector3 v, Vector3 n, float r) 1049 { 1050 Vector3 result = { 0 }; 1051 1052 float dot = v.x*n.x + v.y*n.y + v.z*n.z; 1053 float d = 1.0f - r*r*(1.0f - dot*dot); 1054 1055 if (d >= 0.0f) 1056 { 1057 d = sqrtf(d); 1058 v.x = r*v.x - (r*dot + d)*n.x; 1059 v.y = r*v.y - (r*dot + d)*n.y; 1060 v.z = r*v.z - (r*dot + d)*n.z; 1061 1062 result = v; 1063 } 1064 1065 return result; 1066 } 1067 1068 //---------------------------------------------------------------------------------- 1069 // Module Functions Definition - Matrix math 1070 //---------------------------------------------------------------------------------- 1071 1072 // Compute matrix determinant 1073 RMAPI float MatrixDeterminant(Matrix mat) 1074 { 1075 float result = 0.0f; 1076 1077 // Cache the matrix values (speed optimization) 1078 float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3; 1079 float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7; 1080 float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11; 1081 float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15; 1082 1083 result = a30*a21*a12*a03 - a20*a31*a12*a03 - a30*a11*a22*a03 + a10*a31*a22*a03 + 1084 a20*a11*a32*a03 - a10*a21*a32*a03 - a30*a21*a02*a13 + a20*a31*a02*a13 + 1085 a30*a01*a22*a13 - a00*a31*a22*a13 - a20*a01*a32*a13 + a00*a21*a32*a13 + 1086 a30*a11*a02*a23 - a10*a31*a02*a23 - a30*a01*a12*a23 + a00*a31*a12*a23 + 1087 a10*a01*a32*a23 - a00*a11*a32*a23 - a20*a11*a02*a33 + a10*a21*a02*a33 + 1088 a20*a01*a12*a33 - a00*a21*a12*a33 - a10*a01*a22*a33 + a00*a11*a22*a33; 1089 1090 return result; 1091 } 1092 1093 // Get the trace of the matrix (sum of the values along the diagonal) 1094 RMAPI float MatrixTrace(Matrix mat) 1095 { 1096 float result = (mat.m0 + mat.m5 + mat.m10 + mat.m15); 1097 1098 return result; 1099 } 1100 1101 // Transposes provided matrix 1102 RMAPI Matrix MatrixTranspose(Matrix mat) 1103 { 1104 Matrix result = { 0 }; 1105 1106 result.m0 = mat.m0; 1107 result.m1 = mat.m4; 1108 result.m2 = mat.m8; 1109 result.m3 = mat.m12; 1110 result.m4 = mat.m1; 1111 result.m5 = mat.m5; 1112 result.m6 = mat.m9; 1113 result.m7 = mat.m13; 1114 result.m8 = mat.m2; 1115 result.m9 = mat.m6; 1116 result.m10 = mat.m10; 1117 result.m11 = mat.m14; 1118 result.m12 = mat.m3; 1119 result.m13 = mat.m7; 1120 result.m14 = mat.m11; 1121 result.m15 = mat.m15; 1122 1123 return result; 1124 } 1125 1126 // Invert provided matrix 1127 RMAPI Matrix MatrixInvert(Matrix mat) 1128 { 1129 Matrix result = { 0 }; 1130 1131 // Cache the matrix values (speed optimization) 1132 float a00 = mat.m0, a01 = mat.m1, a02 = mat.m2, a03 = mat.m3; 1133 float a10 = mat.m4, a11 = mat.m5, a12 = mat.m6, a13 = mat.m7; 1134 float a20 = mat.m8, a21 = mat.m9, a22 = mat.m10, a23 = mat.m11; 1135 float a30 = mat.m12, a31 = mat.m13, a32 = mat.m14, a33 = mat.m15; 1136 1137 float b00 = a00*a11 - a01*a10; 1138 float b01 = a00*a12 - a02*a10; 1139 float b02 = a00*a13 - a03*a10; 1140 float b03 = a01*a12 - a02*a11; 1141 float b04 = a01*a13 - a03*a11; 1142 float b05 = a02*a13 - a03*a12; 1143 float b06 = a20*a31 - a21*a30; 1144 float b07 = a20*a32 - a22*a30; 1145 float b08 = a20*a33 - a23*a30; 1146 float b09 = a21*a32 - a22*a31; 1147 float b10 = a21*a33 - a23*a31; 1148 float b11 = a22*a33 - a23*a32; 1149 1150 // Calculate the invert determinant (inlined to avoid double-caching) 1151 float invDet = 1.0f/(b00*b11 - b01*b10 + b02*b09 + b03*b08 - b04*b07 + b05*b06); 1152 1153 result.m0 = (a11*b11 - a12*b10 + a13*b09)*invDet; 1154 result.m1 = (-a01*b11 + a02*b10 - a03*b09)*invDet; 1155 result.m2 = (a31*b05 - a32*b04 + a33*b03)*invDet; 1156 result.m3 = (-a21*b05 + a22*b04 - a23*b03)*invDet; 1157 result.m4 = (-a10*b11 + a12*b08 - a13*b07)*invDet; 1158 result.m5 = (a00*b11 - a02*b08 + a03*b07)*invDet; 1159 result.m6 = (-a30*b05 + a32*b02 - a33*b01)*invDet; 1160 result.m7 = (a20*b05 - a22*b02 + a23*b01)*invDet; 1161 result.m8 = (a10*b10 - a11*b08 + a13*b06)*invDet; 1162 result.m9 = (-a00*b10 + a01*b08 - a03*b06)*invDet; 1163 result.m10 = (a30*b04 - a31*b02 + a33*b00)*invDet; 1164 result.m11 = (-a20*b04 + a21*b02 - a23*b00)*invDet; 1165 result.m12 = (-a10*b09 + a11*b07 - a12*b06)*invDet; 1166 result.m13 = (a00*b09 - a01*b07 + a02*b06)*invDet; 1167 result.m14 = (-a30*b03 + a31*b01 - a32*b00)*invDet; 1168 result.m15 = (a20*b03 - a21*b01 + a22*b00)*invDet; 1169 1170 return result; 1171 } 1172 1173 // Get identity matrix 1174 RMAPI Matrix MatrixIdentity(void) 1175 { 1176 Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 1177 0.0f, 1.0f, 0.0f, 0.0f, 1178 0.0f, 0.0f, 1.0f, 0.0f, 1179 0.0f, 0.0f, 0.0f, 1.0f }; 1180 1181 return result; 1182 } 1183 1184 // Add two matrices 1185 RMAPI Matrix MatrixAdd(Matrix left, Matrix right) 1186 { 1187 Matrix result = { 0 }; 1188 1189 result.m0 = left.m0 + right.m0; 1190 result.m1 = left.m1 + right.m1; 1191 result.m2 = left.m2 + right.m2; 1192 result.m3 = left.m3 + right.m3; 1193 result.m4 = left.m4 + right.m4; 1194 result.m5 = left.m5 + right.m5; 1195 result.m6 = left.m6 + right.m6; 1196 result.m7 = left.m7 + right.m7; 1197 result.m8 = left.m8 + right.m8; 1198 result.m9 = left.m9 + right.m9; 1199 result.m10 = left.m10 + right.m10; 1200 result.m11 = left.m11 + right.m11; 1201 result.m12 = left.m12 + right.m12; 1202 result.m13 = left.m13 + right.m13; 1203 result.m14 = left.m14 + right.m14; 1204 result.m15 = left.m15 + right.m15; 1205 1206 return result; 1207 } 1208 1209 // Subtract two matrices (left - right) 1210 RMAPI Matrix MatrixSubtract(Matrix left, Matrix right) 1211 { 1212 Matrix result = { 0 }; 1213 1214 result.m0 = left.m0 - right.m0; 1215 result.m1 = left.m1 - right.m1; 1216 result.m2 = left.m2 - right.m2; 1217 result.m3 = left.m3 - right.m3; 1218 result.m4 = left.m4 - right.m4; 1219 result.m5 = left.m5 - right.m5; 1220 result.m6 = left.m6 - right.m6; 1221 result.m7 = left.m7 - right.m7; 1222 result.m8 = left.m8 - right.m8; 1223 result.m9 = left.m9 - right.m9; 1224 result.m10 = left.m10 - right.m10; 1225 result.m11 = left.m11 - right.m11; 1226 result.m12 = left.m12 - right.m12; 1227 result.m13 = left.m13 - right.m13; 1228 result.m14 = left.m14 - right.m14; 1229 result.m15 = left.m15 - right.m15; 1230 1231 return result; 1232 } 1233 1234 // Get two matrix multiplication 1235 // NOTE: When multiplying matrices... the order matters! 1236 RMAPI Matrix MatrixMultiply(Matrix left, Matrix right) 1237 { 1238 Matrix result = { 0 }; 1239 1240 result.m0 = left.m0*right.m0 + left.m1*right.m4 + left.m2*right.m8 + left.m3*right.m12; 1241 result.m1 = left.m0*right.m1 + left.m1*right.m5 + left.m2*right.m9 + left.m3*right.m13; 1242 result.m2 = left.m0*right.m2 + left.m1*right.m6 + left.m2*right.m10 + left.m3*right.m14; 1243 result.m3 = left.m0*right.m3 + left.m1*right.m7 + left.m2*right.m11 + left.m3*right.m15; 1244 result.m4 = left.m4*right.m0 + left.m5*right.m4 + left.m6*right.m8 + left.m7*right.m12; 1245 result.m5 = left.m4*right.m1 + left.m5*right.m5 + left.m6*right.m9 + left.m7*right.m13; 1246 result.m6 = left.m4*right.m2 + left.m5*right.m6 + left.m6*right.m10 + left.m7*right.m14; 1247 result.m7 = left.m4*right.m3 + left.m5*right.m7 + left.m6*right.m11 + left.m7*right.m15; 1248 result.m8 = left.m8*right.m0 + left.m9*right.m4 + left.m10*right.m8 + left.m11*right.m12; 1249 result.m9 = left.m8*right.m1 + left.m9*right.m5 + left.m10*right.m9 + left.m11*right.m13; 1250 result.m10 = left.m8*right.m2 + left.m9*right.m6 + left.m10*right.m10 + left.m11*right.m14; 1251 result.m11 = left.m8*right.m3 + left.m9*right.m7 + left.m10*right.m11 + left.m11*right.m15; 1252 result.m12 = left.m12*right.m0 + left.m13*right.m4 + left.m14*right.m8 + left.m15*right.m12; 1253 result.m13 = left.m12*right.m1 + left.m13*right.m5 + left.m14*right.m9 + left.m15*right.m13; 1254 result.m14 = left.m12*right.m2 + left.m13*right.m6 + left.m14*right.m10 + left.m15*right.m14; 1255 result.m15 = left.m12*right.m3 + left.m13*right.m7 + left.m14*right.m11 + left.m15*right.m15; 1256 1257 return result; 1258 } 1259 1260 // Get translation matrix 1261 RMAPI Matrix MatrixTranslate(float x, float y, float z) 1262 { 1263 Matrix result = { 1.0f, 0.0f, 0.0f, x, 1264 0.0f, 1.0f, 0.0f, y, 1265 0.0f, 0.0f, 1.0f, z, 1266 0.0f, 0.0f, 0.0f, 1.0f }; 1267 1268 return result; 1269 } 1270 1271 // Create rotation matrix from axis and angle 1272 // NOTE: Angle should be provided in radians 1273 RMAPI Matrix MatrixRotate(Vector3 axis, float angle) 1274 { 1275 Matrix result = { 0 }; 1276 1277 float x = axis.x, y = axis.y, z = axis.z; 1278 1279 float lengthSquared = x*x + y*y + z*z; 1280 1281 if ((lengthSquared != 1.0f) && (lengthSquared != 0.0f)) 1282 { 1283 float ilength = 1.0f/sqrtf(lengthSquared); 1284 x *= ilength; 1285 y *= ilength; 1286 z *= ilength; 1287 } 1288 1289 float sinres = sinf(angle); 1290 float cosres = cosf(angle); 1291 float t = 1.0f - cosres; 1292 1293 result.m0 = x*x*t + cosres; 1294 result.m1 = y*x*t + z*sinres; 1295 result.m2 = z*x*t - y*sinres; 1296 result.m3 = 0.0f; 1297 1298 result.m4 = x*y*t - z*sinres; 1299 result.m5 = y*y*t + cosres; 1300 result.m6 = z*y*t + x*sinres; 1301 result.m7 = 0.0f; 1302 1303 result.m8 = x*z*t + y*sinres; 1304 result.m9 = y*z*t - x*sinres; 1305 result.m10 = z*z*t + cosres; 1306 result.m11 = 0.0f; 1307 1308 result.m12 = 0.0f; 1309 result.m13 = 0.0f; 1310 result.m14 = 0.0f; 1311 result.m15 = 1.0f; 1312 1313 return result; 1314 } 1315 1316 // Get x-rotation matrix 1317 // NOTE: Angle must be provided in radians 1318 RMAPI Matrix MatrixRotateX(float angle) 1319 { 1320 Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 1321 0.0f, 1.0f, 0.0f, 0.0f, 1322 0.0f, 0.0f, 1.0f, 0.0f, 1323 0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity() 1324 1325 float cosres = cosf(angle); 1326 float sinres = sinf(angle); 1327 1328 result.m5 = cosres; 1329 result.m6 = sinres; 1330 result.m9 = -sinres; 1331 result.m10 = cosres; 1332 1333 return result; 1334 } 1335 1336 // Get y-rotation matrix 1337 // NOTE: Angle must be provided in radians 1338 RMAPI Matrix MatrixRotateY(float angle) 1339 { 1340 Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 1341 0.0f, 1.0f, 0.0f, 0.0f, 1342 0.0f, 0.0f, 1.0f, 0.0f, 1343 0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity() 1344 1345 float cosres = cosf(angle); 1346 float sinres = sinf(angle); 1347 1348 result.m0 = cosres; 1349 result.m2 = -sinres; 1350 result.m8 = sinres; 1351 result.m10 = cosres; 1352 1353 return result; 1354 } 1355 1356 // Get z-rotation matrix 1357 // NOTE: Angle must be provided in radians 1358 RMAPI Matrix MatrixRotateZ(float angle) 1359 { 1360 Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 1361 0.0f, 1.0f, 0.0f, 0.0f, 1362 0.0f, 0.0f, 1.0f, 0.0f, 1363 0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity() 1364 1365 float cosres = cosf(angle); 1366 float sinres = sinf(angle); 1367 1368 result.m0 = cosres; 1369 result.m1 = sinres; 1370 result.m4 = -sinres; 1371 result.m5 = cosres; 1372 1373 return result; 1374 } 1375 1376 1377 // Get xyz-rotation matrix 1378 // NOTE: Angle must be provided in radians 1379 RMAPI Matrix MatrixRotateXYZ(Vector3 angle) 1380 { 1381 Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 1382 0.0f, 1.0f, 0.0f, 0.0f, 1383 0.0f, 0.0f, 1.0f, 0.0f, 1384 0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity() 1385 1386 float cosz = cosf(-angle.z); 1387 float sinz = sinf(-angle.z); 1388 float cosy = cosf(-angle.y); 1389 float siny = sinf(-angle.y); 1390 float cosx = cosf(-angle.x); 1391 float sinx = sinf(-angle.x); 1392 1393 result.m0 = cosz*cosy; 1394 result.m1 = (cosz*siny*sinx) - (sinz*cosx); 1395 result.m2 = (cosz*siny*cosx) + (sinz*sinx); 1396 1397 result.m4 = sinz*cosy; 1398 result.m5 = (sinz*siny*sinx) + (cosz*cosx); 1399 result.m6 = (sinz*siny*cosx) - (cosz*sinx); 1400 1401 result.m8 = -siny; 1402 result.m9 = cosy*sinx; 1403 result.m10= cosy*cosx; 1404 1405 return result; 1406 } 1407 1408 // Get zyx-rotation matrix 1409 // NOTE: Angle must be provided in radians 1410 RMAPI Matrix MatrixRotateZYX(Vector3 angle) 1411 { 1412 Matrix result = { 0 }; 1413 1414 float cz = cosf(angle.z); 1415 float sz = sinf(angle.z); 1416 float cy = cosf(angle.y); 1417 float sy = sinf(angle.y); 1418 float cx = cosf(angle.x); 1419 float sx = sinf(angle.x); 1420 1421 result.m0 = cz*cy; 1422 result.m4 = cz*sy*sx - cx*sz; 1423 result.m8 = sz*sx + cz*cx*sy; 1424 result.m12 = 0; 1425 1426 result.m1 = cy*sz; 1427 result.m5 = cz*cx + sz*sy*sx; 1428 result.m9 = cx*sz*sy - cz*sx; 1429 result.m13 = 0; 1430 1431 result.m2 = -sy; 1432 result.m6 = cy*sx; 1433 result.m10 = cy*cx; 1434 result.m14 = 0; 1435 1436 result.m3 = 0; 1437 result.m7 = 0; 1438 result.m11 = 0; 1439 result.m15 = 1; 1440 1441 return result; 1442 } 1443 1444 // Get scaling matrix 1445 RMAPI Matrix MatrixScale(float x, float y, float z) 1446 { 1447 Matrix result = { x, 0.0f, 0.0f, 0.0f, 1448 0.0f, y, 0.0f, 0.0f, 1449 0.0f, 0.0f, z, 0.0f, 1450 0.0f, 0.0f, 0.0f, 1.0f }; 1451 1452 return result; 1453 } 1454 1455 // Get perspective projection matrix 1456 RMAPI Matrix MatrixFrustum(double left, double right, double bottom, double top, double near, double far) 1457 { 1458 Matrix result = { 0 }; 1459 1460 float rl = (float)(right - left); 1461 float tb = (float)(top - bottom); 1462 float fn = (float)(far - near); 1463 1464 result.m0 = ((float)near*2.0f)/rl; 1465 result.m1 = 0.0f; 1466 result.m2 = 0.0f; 1467 result.m3 = 0.0f; 1468 1469 result.m4 = 0.0f; 1470 result.m5 = ((float)near*2.0f)/tb; 1471 result.m6 = 0.0f; 1472 result.m7 = 0.0f; 1473 1474 result.m8 = ((float)right + (float)left)/rl; 1475 result.m9 = ((float)top + (float)bottom)/tb; 1476 result.m10 = -((float)far + (float)near)/fn; 1477 result.m11 = -1.0f; 1478 1479 result.m12 = 0.0f; 1480 result.m13 = 0.0f; 1481 result.m14 = -((float)far*(float)near*2.0f)/fn; 1482 result.m15 = 0.0f; 1483 1484 return result; 1485 } 1486 1487 // Get perspective projection matrix 1488 // NOTE: Fovy angle must be provided in radians 1489 RMAPI Matrix MatrixPerspective(double fovy, double aspect, double near, double far) 1490 { 1491 Matrix result = { 0 }; 1492 1493 double top = near*tan(fovy*0.5); 1494 double bottom = -top; 1495 double right = top*aspect; 1496 double left = -right; 1497 1498 // MatrixFrustum(-right, right, -top, top, near, far); 1499 float rl = (float)(right - left); 1500 float tb = (float)(top - bottom); 1501 float fn = (float)(far - near); 1502 1503 result.m0 = ((float)near*2.0f)/rl; 1504 result.m5 = ((float)near*2.0f)/tb; 1505 result.m8 = ((float)right + (float)left)/rl; 1506 result.m9 = ((float)top + (float)bottom)/tb; 1507 result.m10 = -((float)far + (float)near)/fn; 1508 result.m11 = -1.0f; 1509 result.m14 = -((float)far*(float)near*2.0f)/fn; 1510 1511 return result; 1512 } 1513 1514 // Get orthographic projection matrix 1515 RMAPI Matrix MatrixOrtho(double left, double right, double bottom, double top, double near, double far) 1516 { 1517 Matrix result = { 0 }; 1518 1519 float rl = (float)(right - left); 1520 float tb = (float)(top - bottom); 1521 float fn = (float)(far - near); 1522 1523 result.m0 = 2.0f/rl; 1524 result.m1 = 0.0f; 1525 result.m2 = 0.0f; 1526 result.m3 = 0.0f; 1527 result.m4 = 0.0f; 1528 result.m5 = 2.0f/tb; 1529 result.m6 = 0.0f; 1530 result.m7 = 0.0f; 1531 result.m8 = 0.0f; 1532 result.m9 = 0.0f; 1533 result.m10 = -2.0f/fn; 1534 result.m11 = 0.0f; 1535 result.m12 = -((float)left + (float)right)/rl; 1536 result.m13 = -((float)top + (float)bottom)/tb; 1537 result.m14 = -((float)far + (float)near)/fn; 1538 result.m15 = 1.0f; 1539 1540 return result; 1541 } 1542 1543 // Get camera look-at matrix (view matrix) 1544 RMAPI Matrix MatrixLookAt(Vector3 eye, Vector3 target, Vector3 up) 1545 { 1546 Matrix result = { 0 }; 1547 1548 float length = 0.0f; 1549 float ilength = 0.0f; 1550 1551 // Vector3Subtract(eye, target) 1552 Vector3 vz = { eye.x - target.x, eye.y - target.y, eye.z - target.z }; 1553 1554 // Vector3Normalize(vz) 1555 Vector3 v = vz; 1556 length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 1557 if (length == 0.0f) length = 1.0f; 1558 ilength = 1.0f/length; 1559 vz.x *= ilength; 1560 vz.y *= ilength; 1561 vz.z *= ilength; 1562 1563 // Vector3CrossProduct(up, vz) 1564 Vector3 vx = { up.y*vz.z - up.z*vz.y, up.z*vz.x - up.x*vz.z, up.x*vz.y - up.y*vz.x }; 1565 1566 // Vector3Normalize(x) 1567 v = vx; 1568 length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 1569 if (length == 0.0f) length = 1.0f; 1570 ilength = 1.0f/length; 1571 vx.x *= ilength; 1572 vx.y *= ilength; 1573 vx.z *= ilength; 1574 1575 // Vector3CrossProduct(vz, vx) 1576 Vector3 vy = { vz.y*vx.z - vz.z*vx.y, vz.z*vx.x - vz.x*vx.z, vz.x*vx.y - vz.y*vx.x }; 1577 1578 result.m0 = vx.x; 1579 result.m1 = vy.x; 1580 result.m2 = vz.x; 1581 result.m3 = 0.0f; 1582 result.m4 = vx.y; 1583 result.m5 = vy.y; 1584 result.m6 = vz.y; 1585 result.m7 = 0.0f; 1586 result.m8 = vx.z; 1587 result.m9 = vy.z; 1588 result.m10 = vz.z; 1589 result.m11 = 0.0f; 1590 result.m12 = -(vx.x*eye.x + vx.y*eye.y + vx.z*eye.z); // Vector3DotProduct(vx, eye) 1591 result.m13 = -(vy.x*eye.x + vy.y*eye.y + vy.z*eye.z); // Vector3DotProduct(vy, eye) 1592 result.m14 = -(vz.x*eye.x + vz.y*eye.y + vz.z*eye.z); // Vector3DotProduct(vz, eye) 1593 result.m15 = 1.0f; 1594 1595 return result; 1596 } 1597 1598 // Get float array of matrix data 1599 RMAPI float16 MatrixToFloatV(Matrix mat) 1600 { 1601 float16 result = { 0 }; 1602 1603 result.v[0] = mat.m0; 1604 result.v[1] = mat.m1; 1605 result.v[2] = mat.m2; 1606 result.v[3] = mat.m3; 1607 result.v[4] = mat.m4; 1608 result.v[5] = mat.m5; 1609 result.v[6] = mat.m6; 1610 result.v[7] = mat.m7; 1611 result.v[8] = mat.m8; 1612 result.v[9] = mat.m9; 1613 result.v[10] = mat.m10; 1614 result.v[11] = mat.m11; 1615 result.v[12] = mat.m12; 1616 result.v[13] = mat.m13; 1617 result.v[14] = mat.m14; 1618 result.v[15] = mat.m15; 1619 1620 return result; 1621 } 1622 1623 //---------------------------------------------------------------------------------- 1624 // Module Functions Definition - Quaternion math 1625 //---------------------------------------------------------------------------------- 1626 1627 // Add two quaternions 1628 RMAPI Quaternion QuaternionAdd(Quaternion q1, Quaternion q2) 1629 { 1630 Quaternion result = {q1.x + q2.x, q1.y + q2.y, q1.z + q2.z, q1.w + q2.w}; 1631 1632 return result; 1633 } 1634 1635 // Add quaternion and float value 1636 RMAPI Quaternion QuaternionAddValue(Quaternion q, float add) 1637 { 1638 Quaternion result = {q.x + add, q.y + add, q.z + add, q.w + add}; 1639 1640 return result; 1641 } 1642 1643 // Subtract two quaternions 1644 RMAPI Quaternion QuaternionSubtract(Quaternion q1, Quaternion q2) 1645 { 1646 Quaternion result = {q1.x - q2.x, q1.y - q2.y, q1.z - q2.z, q1.w - q2.w}; 1647 1648 return result; 1649 } 1650 1651 // Subtract quaternion and float value 1652 RMAPI Quaternion QuaternionSubtractValue(Quaternion q, float sub) 1653 { 1654 Quaternion result = {q.x - sub, q.y - sub, q.z - sub, q.w - sub}; 1655 1656 return result; 1657 } 1658 1659 // Get identity quaternion 1660 RMAPI Quaternion QuaternionIdentity(void) 1661 { 1662 Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f }; 1663 1664 return result; 1665 } 1666 1667 // Computes the length of a quaternion 1668 RMAPI float QuaternionLength(Quaternion q) 1669 { 1670 float result = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); 1671 1672 return result; 1673 } 1674 1675 // Normalize provided quaternion 1676 RMAPI Quaternion QuaternionNormalize(Quaternion q) 1677 { 1678 Quaternion result = { 0 }; 1679 1680 float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); 1681 if (length == 0.0f) length = 1.0f; 1682 float ilength = 1.0f/length; 1683 1684 result.x = q.x*ilength; 1685 result.y = q.y*ilength; 1686 result.z = q.z*ilength; 1687 result.w = q.w*ilength; 1688 1689 return result; 1690 } 1691 1692 // Invert provided quaternion 1693 RMAPI Quaternion QuaternionInvert(Quaternion q) 1694 { 1695 Quaternion result = q; 1696 1697 float lengthSq = q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w; 1698 1699 if (lengthSq != 0.0f) 1700 { 1701 float invLength = 1.0f/lengthSq; 1702 1703 result.x *= -invLength; 1704 result.y *= -invLength; 1705 result.z *= -invLength; 1706 result.w *= invLength; 1707 } 1708 1709 return result; 1710 } 1711 1712 // Calculate two quaternion multiplication 1713 RMAPI Quaternion QuaternionMultiply(Quaternion q1, Quaternion q2) 1714 { 1715 Quaternion result = { 0 }; 1716 1717 float qax = q1.x, qay = q1.y, qaz = q1.z, qaw = q1.w; 1718 float qbx = q2.x, qby = q2.y, qbz = q2.z, qbw = q2.w; 1719 1720 result.x = qax*qbw + qaw*qbx + qay*qbz - qaz*qby; 1721 result.y = qay*qbw + qaw*qby + qaz*qbx - qax*qbz; 1722 result.z = qaz*qbw + qaw*qbz + qax*qby - qay*qbx; 1723 result.w = qaw*qbw - qax*qbx - qay*qby - qaz*qbz; 1724 1725 return result; 1726 } 1727 1728 // Scale quaternion by float value 1729 RMAPI Quaternion QuaternionScale(Quaternion q, float mul) 1730 { 1731 Quaternion result = { 0 }; 1732 1733 result.x = q.x*mul; 1734 result.y = q.y*mul; 1735 result.z = q.z*mul; 1736 result.w = q.w*mul; 1737 1738 return result; 1739 } 1740 1741 // Divide two quaternions 1742 RMAPI Quaternion QuaternionDivide(Quaternion q1, Quaternion q2) 1743 { 1744 Quaternion result = { q1.x/q2.x, q1.y/q2.y, q1.z/q2.z, q1.w/q2.w }; 1745 1746 return result; 1747 } 1748 1749 // Calculate linear interpolation between two quaternions 1750 RMAPI Quaternion QuaternionLerp(Quaternion q1, Quaternion q2, float amount) 1751 { 1752 Quaternion result = { 0 }; 1753 1754 result.x = q1.x + amount*(q2.x - q1.x); 1755 result.y = q1.y + amount*(q2.y - q1.y); 1756 result.z = q1.z + amount*(q2.z - q1.z); 1757 result.w = q1.w + amount*(q2.w - q1.w); 1758 1759 return result; 1760 } 1761 1762 // Calculate slerp-optimized interpolation between two quaternions 1763 RMAPI Quaternion QuaternionNlerp(Quaternion q1, Quaternion q2, float amount) 1764 { 1765 Quaternion result = { 0 }; 1766 1767 // QuaternionLerp(q1, q2, amount) 1768 result.x = q1.x + amount*(q2.x - q1.x); 1769 result.y = q1.y + amount*(q2.y - q1.y); 1770 result.z = q1.z + amount*(q2.z - q1.z); 1771 result.w = q1.w + amount*(q2.w - q1.w); 1772 1773 // QuaternionNormalize(q); 1774 Quaternion q = result; 1775 float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); 1776 if (length == 0.0f) length = 1.0f; 1777 float ilength = 1.0f/length; 1778 1779 result.x = q.x*ilength; 1780 result.y = q.y*ilength; 1781 result.z = q.z*ilength; 1782 result.w = q.w*ilength; 1783 1784 return result; 1785 } 1786 1787 // Calculates spherical linear interpolation between two quaternions 1788 RMAPI Quaternion QuaternionSlerp(Quaternion q1, Quaternion q2, float amount) 1789 { 1790 Quaternion result = { 0 }; 1791 1792 float cosHalfTheta = q1.x*q2.x + q1.y*q2.y + q1.z*q2.z + q1.w*q2.w; 1793 1794 if (cosHalfTheta < 0) 1795 { 1796 q2.x = -q2.x; q2.y = -q2.y; q2.z = -q2.z; q2.w = -q2.w; 1797 cosHalfTheta = -cosHalfTheta; 1798 } 1799 1800 if (fabsf(cosHalfTheta) >= 1.0f) result = q1; 1801 else if (cosHalfTheta > 0.95f) result = QuaternionNlerp(q1, q2, amount); 1802 else 1803 { 1804 float halfTheta = acosf(cosHalfTheta); 1805 float sinHalfTheta = sqrtf(1.0f - cosHalfTheta*cosHalfTheta); 1806 1807 if (fabsf(sinHalfTheta) < 0.001f) 1808 { 1809 result.x = (q1.x*0.5f + q2.x*0.5f); 1810 result.y = (q1.y*0.5f + q2.y*0.5f); 1811 result.z = (q1.z*0.5f + q2.z*0.5f); 1812 result.w = (q1.w*0.5f + q2.w*0.5f); 1813 } 1814 else 1815 { 1816 float ratioA = sinf((1 - amount)*halfTheta)/sinHalfTheta; 1817 float ratioB = sinf(amount*halfTheta)/sinHalfTheta; 1818 1819 result.x = (q1.x*ratioA + q2.x*ratioB); 1820 result.y = (q1.y*ratioA + q2.y*ratioB); 1821 result.z = (q1.z*ratioA + q2.z*ratioB); 1822 result.w = (q1.w*ratioA + q2.w*ratioB); 1823 } 1824 } 1825 1826 return result; 1827 } 1828 1829 // Calculate quaternion based on the rotation from one vector to another 1830 RMAPI Quaternion QuaternionFromVector3ToVector3(Vector3 from, Vector3 to) 1831 { 1832 Quaternion result = { 0 }; 1833 1834 float cos2Theta = (from.x*to.x + from.y*to.y + from.z*to.z); // Vector3DotProduct(from, to) 1835 Vector3 cross = { from.y*to.z - from.z*to.y, from.z*to.x - from.x*to.z, from.x*to.y - from.y*to.x }; // Vector3CrossProduct(from, to) 1836 1837 result.x = cross.x; 1838 result.y = cross.y; 1839 result.z = cross.z; 1840 result.w = 1.0f + cos2Theta; 1841 1842 // QuaternionNormalize(q); 1843 // NOTE: Normalize to essentially nlerp the original and identity to 0.5 1844 Quaternion q = result; 1845 float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); 1846 if (length == 0.0f) length = 1.0f; 1847 float ilength = 1.0f/length; 1848 1849 result.x = q.x*ilength; 1850 result.y = q.y*ilength; 1851 result.z = q.z*ilength; 1852 result.w = q.w*ilength; 1853 1854 return result; 1855 } 1856 1857 // Get a quaternion for a given rotation matrix 1858 RMAPI Quaternion QuaternionFromMatrix(Matrix mat) 1859 { 1860 Quaternion result = { 0 }; 1861 1862 float fourWSquaredMinus1 = mat.m0 + mat.m5 + mat.m10; 1863 float fourXSquaredMinus1 = mat.m0 - mat.m5 - mat.m10; 1864 float fourYSquaredMinus1 = mat.m5 - mat.m0 - mat.m10; 1865 float fourZSquaredMinus1 = mat.m10 - mat.m0 - mat.m5; 1866 1867 int biggestIndex = 0; 1868 float fourBiggestSquaredMinus1 = fourWSquaredMinus1; 1869 if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) 1870 { 1871 fourBiggestSquaredMinus1 = fourXSquaredMinus1; 1872 biggestIndex = 1; 1873 } 1874 1875 if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) 1876 { 1877 fourBiggestSquaredMinus1 = fourYSquaredMinus1; 1878 biggestIndex = 2; 1879 } 1880 1881 if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) 1882 { 1883 fourBiggestSquaredMinus1 = fourZSquaredMinus1; 1884 biggestIndex = 3; 1885 } 1886 1887 float biggestVal = sqrtf(fourBiggestSquaredMinus1 + 1.0f) * 0.5f; 1888 float mult = 0.25f / biggestVal; 1889 1890 switch (biggestIndex) 1891 { 1892 case 0: 1893 result.w = biggestVal; 1894 result.x = (mat.m6 - mat.m9) * mult; 1895 result.y = (mat.m8 - mat.m2) * mult; 1896 result.z = (mat.m1 - mat.m4) * mult; 1897 break; 1898 case 1: 1899 result.x = biggestVal; 1900 result.w = (mat.m6 - mat.m9) * mult; 1901 result.y = (mat.m1 + mat.m4) * mult; 1902 result.z = (mat.m8 + mat.m2) * mult; 1903 break; 1904 case 2: 1905 result.y = biggestVal; 1906 result.w = (mat.m8 - mat.m2) * mult; 1907 result.x = (mat.m1 + mat.m4) * mult; 1908 result.z = (mat.m6 + mat.m9) * mult; 1909 break; 1910 case 3: 1911 result.z = biggestVal; 1912 result.w = (mat.m1 - mat.m4) * mult; 1913 result.x = (mat.m8 + mat.m2) * mult; 1914 result.y = (mat.m6 + mat.m9) * mult; 1915 break; 1916 } 1917 1918 return result; 1919 } 1920 1921 // Get a matrix for a given quaternion 1922 RMAPI Matrix QuaternionToMatrix(Quaternion q) 1923 { 1924 Matrix result = { 1.0f, 0.0f, 0.0f, 0.0f, 1925 0.0f, 1.0f, 0.0f, 0.0f, 1926 0.0f, 0.0f, 1.0f, 0.0f, 1927 0.0f, 0.0f, 0.0f, 1.0f }; // MatrixIdentity() 1928 1929 float a2 = q.x*q.x; 1930 float b2 = q.y*q.y; 1931 float c2 = q.z*q.z; 1932 float ac = q.x*q.z; 1933 float ab = q.x*q.y; 1934 float bc = q.y*q.z; 1935 float ad = q.w*q.x; 1936 float bd = q.w*q.y; 1937 float cd = q.w*q.z; 1938 1939 result.m0 = 1 - 2*(b2 + c2); 1940 result.m1 = 2*(ab + cd); 1941 result.m2 = 2*(ac - bd); 1942 1943 result.m4 = 2*(ab - cd); 1944 result.m5 = 1 - 2*(a2 + c2); 1945 result.m6 = 2*(bc + ad); 1946 1947 result.m8 = 2*(ac + bd); 1948 result.m9 = 2*(bc - ad); 1949 result.m10 = 1 - 2*(a2 + b2); 1950 1951 return result; 1952 } 1953 1954 // Get rotation quaternion for an angle and axis 1955 // NOTE: Angle must be provided in radians 1956 RMAPI Quaternion QuaternionFromAxisAngle(Vector3 axis, float angle) 1957 { 1958 Quaternion result = { 0.0f, 0.0f, 0.0f, 1.0f }; 1959 1960 float axisLength = sqrtf(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); 1961 1962 if (axisLength != 0.0f) 1963 { 1964 angle *= 0.5f; 1965 1966 float length = 0.0f; 1967 float ilength = 0.0f; 1968 1969 // Vector3Normalize(axis) 1970 Vector3 v = axis; 1971 length = sqrtf(v.x*v.x + v.y*v.y + v.z*v.z); 1972 if (length == 0.0f) length = 1.0f; 1973 ilength = 1.0f/length; 1974 axis.x *= ilength; 1975 axis.y *= ilength; 1976 axis.z *= ilength; 1977 1978 float sinres = sinf(angle); 1979 float cosres = cosf(angle); 1980 1981 result.x = axis.x*sinres; 1982 result.y = axis.y*sinres; 1983 result.z = axis.z*sinres; 1984 result.w = cosres; 1985 1986 // QuaternionNormalize(q); 1987 Quaternion q = result; 1988 length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); 1989 if (length == 0.0f) length = 1.0f; 1990 ilength = 1.0f/length; 1991 result.x = q.x*ilength; 1992 result.y = q.y*ilength; 1993 result.z = q.z*ilength; 1994 result.w = q.w*ilength; 1995 } 1996 1997 return result; 1998 } 1999 2000 // Get the rotation angle and axis for a given quaternion 2001 RMAPI void QuaternionToAxisAngle(Quaternion q, Vector3 *outAxis, float *outAngle) 2002 { 2003 if (fabsf(q.w) > 1.0f) 2004 { 2005 // QuaternionNormalize(q); 2006 float length = sqrtf(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w); 2007 if (length == 0.0f) length = 1.0f; 2008 float ilength = 1.0f/length; 2009 2010 q.x = q.x*ilength; 2011 q.y = q.y*ilength; 2012 q.z = q.z*ilength; 2013 q.w = q.w*ilength; 2014 } 2015 2016 Vector3 resAxis = { 0.0f, 0.0f, 0.0f }; 2017 float resAngle = 2.0f*acosf(q.w); 2018 float den = sqrtf(1.0f - q.w*q.w); 2019 2020 if (den > 0.0001f) 2021 { 2022 resAxis.x = q.x/den; 2023 resAxis.y = q.y/den; 2024 resAxis.z = q.z/den; 2025 } 2026 else 2027 { 2028 // This occurs when the angle is zero. 2029 // Not a problem: just set an arbitrary normalized axis. 2030 resAxis.x = 1.0f; 2031 } 2032 2033 *outAxis = resAxis; 2034 *outAngle = resAngle; 2035 } 2036 2037 // Get the quaternion equivalent to Euler angles 2038 // NOTE: Rotation order is ZYX 2039 RMAPI Quaternion QuaternionFromEuler(float pitch, float yaw, float roll) 2040 { 2041 Quaternion result = { 0 }; 2042 2043 float x0 = cosf(pitch*0.5f); 2044 float x1 = sinf(pitch*0.5f); 2045 float y0 = cosf(yaw*0.5f); 2046 float y1 = sinf(yaw*0.5f); 2047 float z0 = cosf(roll*0.5f); 2048 float z1 = sinf(roll*0.5f); 2049 2050 result.x = x1*y0*z0 - x0*y1*z1; 2051 result.y = x0*y1*z0 + x1*y0*z1; 2052 result.z = x0*y0*z1 - x1*y1*z0; 2053 result.w = x0*y0*z0 + x1*y1*z1; 2054 2055 return result; 2056 } 2057 2058 // Get the Euler angles equivalent to quaternion (roll, pitch, yaw) 2059 // NOTE: Angles are returned in a Vector3 struct in radians 2060 RMAPI Vector3 QuaternionToEuler(Quaternion q) 2061 { 2062 Vector3 result = { 0 }; 2063 2064 // Roll (x-axis rotation) 2065 float x0 = 2.0f*(q.w*q.x + q.y*q.z); 2066 float x1 = 1.0f - 2.0f*(q.x*q.x + q.y*q.y); 2067 result.x = atan2f(x0, x1); 2068 2069 // Pitch (y-axis rotation) 2070 float y0 = 2.0f*(q.w*q.y - q.z*q.x); 2071 y0 = y0 > 1.0f ? 1.0f : y0; 2072 y0 = y0 < -1.0f ? -1.0f : y0; 2073 result.y = asinf(y0); 2074 2075 // Yaw (z-axis rotation) 2076 float z0 = 2.0f*(q.w*q.z + q.x*q.y); 2077 float z1 = 1.0f - 2.0f*(q.y*q.y + q.z*q.z); 2078 result.z = atan2f(z0, z1); 2079 2080 return result; 2081 } 2082 2083 // Transform a quaternion given a transformation matrix 2084 RMAPI Quaternion QuaternionTransform(Quaternion q, Matrix mat) 2085 { 2086 Quaternion result = { 0 }; 2087 2088 result.x = mat.m0*q.x + mat.m4*q.y + mat.m8*q.z + mat.m12*q.w; 2089 result.y = mat.m1*q.x + mat.m5*q.y + mat.m9*q.z + mat.m13*q.w; 2090 result.z = mat.m2*q.x + mat.m6*q.y + mat.m10*q.z + mat.m14*q.w; 2091 result.w = mat.m3*q.x + mat.m7*q.y + mat.m11*q.z + mat.m15*q.w; 2092 2093 return result; 2094 } 2095 2096 // Check whether two given quaternions are almost equal 2097 RMAPI int QuaternionEquals(Quaternion p, Quaternion q) 2098 { 2099 int result = (((fabsf(p.x - q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && 2100 ((fabsf(p.y - q.y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) && 2101 ((fabsf(p.z - q.z)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z))))) && 2102 ((fabsf(p.w - q.w)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.w), fabsf(q.w)))))) || 2103 (((fabsf(p.x + q.x)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.x), fabsf(q.x))))) && 2104 ((fabsf(p.y + q.y)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.y), fabsf(q.y))))) && 2105 ((fabsf(p.z + q.z)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.z), fabsf(q.z))))) && 2106 ((fabsf(p.w + q.w)) <= (EPSILON*fmaxf(1.0f, fmaxf(fabsf(p.w), fabsf(q.w)))))); 2107 2108 return result; 2109 } 2110 2111 #endif // RAYMATH_H