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 |
135 | typedef struct float3 { float v[3]; } float3; |
136 | typedef 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 |
145 | RMDEF 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 |
152 | RMDEF 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 |
162 | RMDEF Vector2 Vector2Zero(void) |
163 | { |
164 | Vector2 result = { 0.0f, 0.0f }; |
165 | return result; |
166 | } |
167 | |
168 | // Vector with components value 1.0f |
169 | RMDEF Vector2 Vector2One(void) |
170 | { |
171 | Vector2 result = { 1.0f, 1.0f }; |
172 | return result; |
173 | } |
174 | |
175 | // Add two vectors (v1 + v2) |
176 | RMDEF 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) |
183 | RMDEF 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 |
190 | RMDEF 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 |
197 | RMDEF 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 |
204 | RMDEF 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 |
211 | RMDEF 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) |
219 | RMDEF 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 |
226 | RMDEF 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 |
233 | RMDEF Vector2 Vector2Negate(Vector2 v) |
234 | { |
235 | Vector2 result = { -v.x, -v.y }; |
236 | return result; |
237 | } |
238 | |
239 | // Divide vector by a float value |
240 | RMDEF 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 |
247 | RMDEF 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 |
254 | RMDEF Vector2 Vector2Normalize(Vector2 v) |
255 | { |
256 | Vector2 result = Vector2Divide(v, Vector2Length(v)); |
257 | return result; |
258 | } |
259 | |
260 | // Calculate linear interpolation between two vectors |
261 | RMDEF 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. |
272 | RMDEF 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 |
284 | RMDEF 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 |
291 | RMDEF Vector3 Vector3One(void) |
292 | { |
293 | Vector3 result = { 1.0f, 1.0f, 1.0f }; |
294 | return result; |
295 | } |
296 | |
297 | // Add two vectors |
298 | RMDEF 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 |
305 | RMDEF 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 |
312 | RMDEF 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 |
319 | RMDEF 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 |
326 | RMDEF 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 |
333 | RMDEF 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 |
359 | RMDEF 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 |
366 | RMDEF 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 |
373 | RMDEF 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) |
383 | RMDEF 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 |
390 | RMDEF 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 |
397 | RMDEF 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 |
404 | RMDEF 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 |
423 | RMDEF 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 |
432 | RMDEF 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 |
447 | RMDEF 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 |
459 | RMDEF 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 |
471 | RMDEF 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 |
489 | RMDEF 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 |
501 | RMDEF 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 |
514 | RMDEF 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 |
539 | RMDEF 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 |
555 | RMDEF 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) |
574 | RMDEF 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 |
581 | RMDEF 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 |
606 | RMDEF 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 |
653 | RMDEF 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 |
680 | RMDEF 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 |
691 | RMDEF 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) |
716 | RMDEF 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 |
741 | RMDEF 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 |
753 | RMDEF 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) |
797 | RMDEF 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) |
824 | RMDEF 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) |
840 | RMDEF 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) |
856 | RMDEF 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 |
872 | RMDEF 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! |
884 | RMDEF 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 |
909 | RMDEF 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 |
942 | RMDEF 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 |
952 | RMDEF 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) |
981 | RMDEF 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 |
1015 | RMDEF 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 |
1044 | RMDEF 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 |
1051 | RMDEF 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 |
1058 | RMDEF 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 |
1076 | RMDEF 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 |
1096 | RMDEF 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 |
1112 | RMDEF 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 |
1125 | RMDEF 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 |
1134 | RMDEF 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 |
1170 | RMDEF 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 |
1192 | RMDEF 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 |
1248 | RMDEF 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 |
1295 | RMDEF 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 |
1319 | RMDEF 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 |
1345 | RMDEF 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 |
1366 | RMDEF 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 |
1390 | RMDEF 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 | |