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