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