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