foo
[dosdemo] / libs / cgmath / cgmmat.inl
1 /* gph-cmath - C graphics math library
2  * Copyright (C) 2018 John Tsiombikas <nuclear@member.fsf.org>
3  *
4  * This program is free software. Feel free to use, modify, and/or redistribute
5  * it under the terms of the MIT/X11 license. See LICENSE for details.
6  * If you intend to redistribute parts of the code without the LICENSE file
7  * replace this paragraph with the full contents of the LICENSE file.
8  */
9 static inline void cgm_mcopy(float *dest, const float *src)
10 {
11         memcpy(dest, src, 16 * sizeof(float));
12 }
13
14 static inline void cgm_mzero(float *m)
15 {
16         static float z[16];
17         cgm_mcopy(m, z);
18 }
19
20 static inline void cgm_midentity(float *m)
21 {
22         static float id[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
23         cgm_mcopy(m, id);
24 }
25
26 static inline void cgm_mmul(float *a, const float *b)
27 {
28         int i, j;
29         float res[16];
30         float *resptr = res;
31         float *arow = a;
32
33         for(i=0; i<4; i++) {
34                 for(j=0; j<4; j++) {
35                         *resptr++ = arow[0] * b[j] + arow[1] * b[4 + j] +
36                                 arow[2] * b[8 + j] + arow[3] * b[12 + j];
37                 }
38                 arow += 4;
39         }
40         cgm_mcopy(a, res);
41 }
42
43 static inline void cgm_mpremul(float *a, const float *b)
44 {
45         int i, j;
46         float res[16];
47         float *resptr = res;
48         const float *brow = b;
49
50         for(i=0; i<4; i++) {
51                 for(j=0; j<4; j++) {
52                         *resptr++ = brow[0] * a[j] + brow[1] * a[4 + j] +
53                                 brow[2] * a[8 + j] + brow[3] * a[12 + j];
54                 }
55                 brow += 4;
56         }
57         cgm_mcopy(a, res);
58 }
59
60 static inline void cgm_msubmatrix(float *m, int row, int col)
61 {
62         float orig[16];
63         int i, j, subi, subj;
64
65         cgm_mcopy(orig, m);
66
67         subi = 0;
68         for(i=0; i<4; i++) {
69                 if(i == row) continue;
70
71                 subj = 0;
72                 for(j=0; j<4; j++) {
73                         if(j == col) continue;
74
75                         m[subi * 4 + subj++] = orig[i * 4 + j];
76                 }
77                 subi++;
78         }
79
80         cgm_mupper3(m);
81 }
82
83 static inline void cgm_mupper3(float *m)
84 {
85         m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
86         m[15] = 1.0f;
87 }
88
89 static inline float cgm_msubdet(const float *m, int row, int col)
90 {
91         float tmp[16];
92         float subdet00, subdet01, subdet02;
93
94         cgm_mcopy(tmp, m);
95         cgm_msubmatrix(tmp, row, col);
96
97         subdet00 = tmp[5] * tmp[10] - tmp[6] * tmp[9];
98         subdet01 = tmp[4] * tmp[10] - tmp[6] * tmp[8];
99         subdet02 = tmp[4] * tmp[9] - tmp[5] * tmp[8];
100
101         return tmp[0] * subdet00 - tmp[1] * subdet01 + tmp[2] * subdet02;
102 }
103
104 static inline float cgm_mcofactor(const float *m, int row, int col)
105 {
106         float min = cgm_msubdet(m, row, col);
107         return (row + col) & 1 ? -min : min;
108 }
109
110 static inline float cgm_mdet(const float *m)
111 {
112         return m[0] * cgm_msubdet(m, 0, 0) - m[1] * cgm_msubdet(m, 0, 1) +
113                 m[2] * cgm_msubdet(m, 0, 2) - m[3] * cgm_msubdet(m, 0, 3);
114 }
115
116 static inline void cgm_mtranspose(float *m)
117 {
118         int i, j;
119         for(i=0; i<4; i++) {
120                 for(j=0; j<i; j++) {
121                         int a = i * 4 + j;
122                         int b = j * 4 + i;
123                         float tmp = m[a];
124                         m[a] = m[b];
125                         m[b] = tmp;
126                 }
127         }
128 }
129
130 static inline void cgm_mcofmatrix(float *m)
131 {
132         float tmp[16];
133         int i, j;
134
135         cgm_mcopy(tmp, m);
136
137         for(i=0; i<4; i++) {
138                 for(j=0; j<4; j++) {
139                         m[i * 4 + j] = cgm_mcofactor(tmp, i, j);
140                 }
141         }
142 }
143
144 static inline int cgm_minverse(float *m)
145 {
146         int i, j;
147         float tmp[16];
148         float inv_det;
149         float det = cgm_mdet(m);
150         if(det == 0.0f) return -1;
151         inv_det = 1.0f / det;
152
153         cgm_mcopy(tmp, m);
154
155         for(i=0; i<4; i++) {
156                 for(j=0; j<4; j++) {
157                         m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det;      /* transposed */
158                 }
159         }
160         return 0;
161 }
162
163 static inline void cgm_mtranslation(float *m, float x, float y, float z)
164 {
165         cgm_midentity(m);
166         m[12] = x;
167         m[13] = y;
168         m[14] = z;
169 }
170
171 static inline void cgm_mscaling(float *m, float sx, float sy, float sz)
172 {
173         cgm_mzero(m);
174         m[0] = sx;
175         m[5] = sy;
176         m[10] = sz;
177         m[15] = 1.0f;
178 }
179
180 static inline void cgm_mrotation_x(float *m, float angle)
181 {
182         float sa = sin(angle);
183         float ca = cos(angle);
184
185         cgm_midentity(m);
186         m[5] = ca;
187         m[6] = sa;
188         m[9] = -sa;
189         m[10] = ca;
190 }
191
192 static inline void cgm_mrotation_y(float *m, float angle)
193 {
194         float sa = sin(angle);
195         float ca = cos(angle);
196
197         cgm_midentity(m);
198         m[0] = ca;
199         m[2] = -sa;
200         m[8] = sa;
201         m[10] = ca;
202 }
203
204 static inline void cgm_mrotation_z(float *m, float angle)
205 {
206         float sa = sin(angle);
207         float ca = cos(angle);
208
209         cgm_midentity(m);
210         m[0] = ca;
211         m[1] = sa;
212         m[4] = -sa;
213         m[5] = ca;
214 }
215
216 static inline void cgm_mrotation_axis(float *m, int idx, float angle)
217 {
218         switch(idx) {
219         case 0:
220                 cgm_mrotation_x(m, angle);
221                 break;
222         case 1:
223                 cgm_mrotation_y(m, angle);
224                 break;
225         case 2:
226                 cgm_mrotation_z(m, angle);
227                 break;
228         }
229 }
230
231 static inline void cgm_mrotation(float *m, float angle, float x, float y, float z)
232 {
233         float sa = sin(angle);
234         float ca = cos(angle);
235         float invca = 1.0f - ca;
236         float xsq = x * x;
237         float ysq = y * y;
238         float zsq = z * z;
239
240         cgm_mzero(m);
241         m[15] = 1.0f;
242
243         m[0] = xsq + (1.0f - xsq) * ca;
244         m[4] = x * y * invca - z * sa;
245         m[8] = x * z * invca + y * sa;
246
247         m[1] = x * y * invca + z * sa;
248         m[5] = ysq + (1.0f - ysq) * ca;
249         m[9] = y * z * invca - x * sa;
250
251         m[2] = x * z * invca - y * sa;
252         m[6] = y * z * invca + x * sa;
253         m[10] = zsq + (1.0f - zsq) * ca;
254 }
255
256 static inline void cgm_mrotation_euler(float *m, float a, float b, float c, int mode)
257 {
258         /* this array must match the EulerMode enum */
259         static const int axis[][3] = {
260                 {0, 1, 2}, {0, 2, 1},
261                 {1, 0, 2}, {1, 2, 0},
262                 {2, 0, 1}, {2, 1, 0},
263                 {2, 0, 2}, {2, 1, 2},
264                 {1, 0, 1}, {1, 2, 1},
265                 {0, 1, 0}, {0, 2, 0}
266         };
267
268         float ma[16], mb[16];
269         cgm_mrotation_axis(ma, axis[mode][0], a);
270         cgm_mrotation_axis(mb, axis[mode][1], b);
271         cgm_mrotation_axis(m, axis[mode][2], c);
272         cgm_mmul(m, mb);
273         cgm_mmul(m, ma);
274 }
275
276 static inline void cgm_mrotation_quat(float *m, const cgm_quat *q)
277 {
278         float xsq2 = 2.0f * q->x * q->x;
279         float ysq2 = 2.0f * q->y * q->y;
280         float zsq2 = 2.0f * q->z * q->z;
281         float sx = 1.0f - ysq2 - zsq2;
282         float sy = 1.0f - xsq2 - zsq2;
283         float sz = 1.0f - xsq2 - ysq2;
284
285         m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
286         m[15] = 1.0f;
287
288         m[0] = sx;
289         m[1] = 2.0f * q->x * q->y + 2.0f * q->w * q->z;
290         m[2] = 2.0f * q->z * q->x - 2.0f * q->w * q->y;
291         m[4] = 2.0f * q->x * q->y - 2.0f * q->w * q->z;
292         m[5] = sy;
293         m[6] = 2.0f * q->y * q->z + 2.0f * q->w * q->x;
294         m[8] = 2.0f * q->z * q->x + 2.0f * q->w * q->y;
295         m[9] = 2.0f * q->y * q->z - 2.0f * q->w * q->x;
296         m[10] = sz;
297 }
298
299 static inline void cgm_mtranslate(float *m, float x, float y, float z)
300 {
301         float tm[16];
302         cgm_mtranslation(tm, x, y, z);
303         cgm_mmul(m, tm);
304 }
305
306 static inline void cgm_mscale(float *m, float sx, float sy, float sz)
307 {
308         float sm[16];
309         cgm_mscaling(sm, sx, sy, sz);
310         cgm_mmul(m, sm);
311 }
312
313 static inline void cgm_mrotate_x(float *m, float angle)
314 {
315         float rm[16];
316         cgm_mrotation_x(rm, angle);
317         cgm_mmul(m, rm);
318 }
319
320 static inline void cgm_mrotate_y(float *m, float angle)
321 {
322         float rm[16];
323         cgm_mrotation_y(rm, angle);
324         cgm_mmul(m, rm);
325 }
326
327 static inline void cgm_mrotate_z(float *m, float angle)
328 {
329         float rm[16];
330         cgm_mrotation_z(rm, angle);
331         cgm_mmul(m, rm);
332 }
333
334 static inline void cgm_mrotate_axis(float *m, int idx, float angle)
335 {
336         float rm[16];
337         cgm_mrotation_axis(rm, idx, angle);
338         cgm_mmul(m, rm);
339 }
340
341 static inline void cgm_mrotate(float *m, float angle, float x, float y, float z)
342 {
343         float rm[16];
344         cgm_mrotation(rm, angle, x, y, z);
345         cgm_mmul(m, rm);
346 }
347
348 static inline void cgm_mrotate_euler(float *m, float a, float b, float c, int mode)
349 {
350         float rm[16];
351         cgm_mrotation_euler(rm, a, b, c, mode);
352         cgm_mmul(m, rm);
353 }
354
355 static inline void cgm_mrotate_quat(float *m, const cgm_quat *q)
356 {
357         float rm[16];
358         cgm_mrotation_quat(rm, q);
359         cgm_mmul(m, rm);
360 }
361
362
363 static inline void cgm_mpretranslate(float *m, float x, float y, float z)
364 {
365         float tm[16];
366         cgm_mtranslation(tm, x, y, z);
367         cgm_mpremul(m, tm);
368 }
369
370 static inline void cgm_mprescale(float *m, float sx, float sy, float sz)
371 {
372         float sm[16];
373         cgm_mscaling(sm, sx, sy, sz);
374         cgm_mpremul(m, sm);
375 }
376
377 static inline void cgm_mprerotate_x(float *m, float angle)
378 {
379         float rm[16];
380         cgm_mrotation_x(rm, angle);
381         cgm_mpremul(m, rm);
382 }
383
384 static inline void cgm_mprerotate_y(float *m, float angle)
385 {
386         float rm[16];
387         cgm_mrotation_y(rm, angle);
388         cgm_mpremul(m, rm);
389 }
390
391 static inline void cgm_mprerotate_z(float *m, float angle)
392 {
393         float rm[16];
394         cgm_mrotation_z(rm, angle);
395         cgm_mpremul(m, rm);
396 }
397
398 static inline void cgm_mprerotate_axis(float *m, int idx, float angle)
399 {
400         float rm[16];
401         cgm_mrotation_axis(rm, idx, angle);
402         cgm_mpremul(m, rm);
403 }
404
405 static inline void cgm_mprerotate(float *m, float angle, float x, float y, float z)
406 {
407         float rm[16];
408         cgm_mrotation(rm, angle, x, y, z);
409         cgm_mpremul(m, rm);
410 }
411
412 static inline void cgm_mprerotate_euler(float *m, float a, float b, float c, int mode)
413 {
414         float rm[16];
415         cgm_mrotation_euler(rm, a, b, c, mode);
416         cgm_mpremul(m, rm);
417 }
418
419 static inline void cgm_mprerotate_quat(float *m, const cgm_quat *q)
420 {
421         float rm[16];
422         cgm_mrotation_quat(rm, q);
423         cgm_mpremul(m, rm);
424 }
425
426
427 static inline void cgm_mget_translation(const float *m, cgm_vec3 *res)
428 {
429         res->x = m[12];
430         res->y = m[13];
431         res->z = m[14];
432 }
433
434 /* Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
435  * article "Quaternion Calculus and Fast Animation".
436  * adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
437  */
438 static inline void cgm_mget_rotation(const float *m, cgm_quat *res)
439 {
440         static const int next[3] = {1, 2, 0};
441         float quat[4];
442         int i, j, k;
443
444         float trace = m[0] + m[5] + m[10];
445         float root;
446
447         if(trace > 0.0f) {
448                 /* |w| > 1/2 */
449                 root = sqrt(trace + 1.0f);      /* 2w */
450                 res->w = 0.5f * root;
451                 root = 0.5f / root;     /* 1 / 4w */
452                 res->x = (m[6] - m[9]) * root;
453                 res->y = (m[8] - m[2]) * root;
454                 res->z = (m[1] - m[4]) * root;
455         } else {
456                 /* |w| <= 1/2 */
457                 i = 0;
458                 if(m[5] > m[0]) {
459                         i = 1;
460                 }
461                 if(m[10] > m[i * 4 + i]) {
462                         i = 2;
463                 }
464                 j = next[i];
465                 k = next[j];
466
467                 root = sqrt(m[i * 4 + i] - m[j * 4 + j] - m[k * 4 + k] + 1.0f);
468                 quat[i + 1] = 0.5f * root;
469                 root = 0.5f / root;
470                 quat[0] = (m[j + 4 + k] - m[k * 4 + j]) * root;
471                 quat[j + 1] = (m[i * 4 + j] - m[j * 4 + i]) * root;
472                 quat[k + 1] = (m[i * 4 + k] - m[k * 4 + i]) * root;
473                 res->w = quat[0];
474                 res->x = quat[1];
475                 res->y = quat[2];
476                 res->z = quat[3];
477         }
478 }
479
480 static inline void cgm_mget_scaling(const float *m, cgm_vec3 *res)
481 {
482         res->x = sqrt(m[0] * m[0] + m[4] * m[4] + m[8] * m[8]);
483         res->y = sqrt(m[1] * m[1] + m[5] * m[5] + m[9] * m[9]);
484         res->z = sqrt(m[2] * m[2] + m[6] * m[6] + m[10] * m[10]);
485 }
486
487 static inline void cgm_mget_frustum_plane(const float *m, int p, cgm_vec4 *res)
488 {
489         int row = p >> 1;
490         const float *rowptr = m + row * 4;
491
492         if((p & 1) == 0) {
493                 res->x = m[12] + rowptr[0];
494                 res->y = m[13] + rowptr[1];
495                 res->z = m[14] + rowptr[2];
496                 res->w = m[15] + rowptr[3];
497         } else {
498                 res->x = m[12] - rowptr[0];
499                 res->y = m[13] - rowptr[1];
500                 res->z = m[14] - rowptr[2];
501                 res->w = m[15] - rowptr[3];
502         }
503 }
504
505 static inline void cgm_mlookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
506                 const cgm_vec3 *up)
507 {
508         float trans[16];
509         cgm_vec3 dir = *targ, right, vup;
510
511         cgm_vsub(&dir, pos);
512         cgm_vnormalize(&dir);
513         cgm_vcross(&right, &dir, up);
514         cgm_vnormalize(&right);
515         cgm_vcross(&vup, &right, &dir);
516         cgm_vnormalize(&vup);
517
518         cgm_midentity(m);
519         m[0] = right.x;
520         m[1] = right.y;
521         m[2] = right.z;
522         m[4] = vup.x;
523         m[5] = vup.y;
524         m[6] = vup.z;
525         m[8] = -dir.x;
526         m[9] = -dir.y;
527         m[10] = -dir.z;
528
529         cgm_mtranslation(trans, pos->x, pos->y, pos->z);
530         cgm_mmul(m, trans);
531 }
532
533 static inline void cgm_minv_lookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
534                 const cgm_vec3 *up)
535 {
536         float rot[16];
537         cgm_vec3 dir = *targ, right, vup;
538
539         cgm_vsub(&dir, pos);
540         cgm_vnormalize(&dir);
541         cgm_vcross(&right, &dir, up);
542         cgm_vnormalize(&right);
543         cgm_vcross(&vup, &right, &dir);
544         cgm_vnormalize(&vup);
545
546         cgm_midentity(rot);
547         rot[0] = right.x;
548         rot[4] = right.y;
549         rot[8] = right.z;
550         rot[1] = vup.x;
551         rot[5] = vup.y;
552         rot[9] = vup.z;
553         rot[2] = -dir.x;
554         rot[6] = -dir.y;
555         rot[10] = -dir.z;
556
557         cgm_mtranslation(m, -pos->x, -pos->y, -pos->z);
558         cgm_mmul(m, rot);
559 }
560
561 static inline void cgm_mortho(float *m, float left, float right, float bot, float top,
562                 float znear, float zfar)
563 {
564         float dx = right - left;
565         float dy = top - bot;
566         float dz = zfar - znear;
567
568         cgm_midentity(m);
569         m[0] = 2.0f / dx;
570         m[5] = 2.0f / dy;
571         m[10] = -2.0f / dz;
572         m[12] = -(right + left) / dx;
573         m[13] = -(top + bot) / dy;
574         m[14] = -(zfar + znear) / dz;
575 }
576
577 static inline void cgm_mfrustum(float *m, float left, float right, float bot, float top,
578                 float znear, float zfar)
579 {
580         float dx = right - left;
581         float dy = top - bot;
582         float dz = zfar - znear;
583
584         cgm_mzero(m);
585         m[0] = 2.0f * znear / dx;
586         m[5] = 2.0f * znear / dy;
587         m[8] = (right + left) / dx;
588         m[9] = (top + bot) / dy;
589         m[10] = -(zfar + znear) / dz;
590         m[14] = -2.0f * zfar * znear / dz;
591         m[11] = -1.0f;
592 }
593
594 static inline void cgm_mperspective(float *m, float vfov, float aspect, float znear, float zfar)
595 {
596         float s = 1.0f / (float)tan(vfov / 2.0f);
597         float range = znear - zfar;
598
599         cgm_mzero(m);
600         m[0] = s / aspect;
601         m[5] = s;
602         m[10] = (znear + zfar) / range;
603         m[14] = 2.0f * znear * zfar / range;
604         m[11] = -1.0f;
605 }
606
607 static inline void cgm_mmirror(float *m, float a, float b, float c, float d)
608 {
609         m[0] = 1.0f - 2.0f * a * a;
610         m[5] = 1.0f - 2.0f * b * b;
611         m[10] = 1.0f - 2.0f * c * c;
612         m[15] = 1.0f;
613
614         m[1] = m[4] = -2.0f * a * b;
615         m[2] = m[8] = -2.0f * a * c;
616         m[6] = m[9] = -2.0f * b * c;
617
618         m[12] = -2.0f * a * d;
619         m[13] = -2.0f * b * d;
620         m[14] = -2.0f * c * d;
621
622         m[3] = m[7] = m[11] = 0.0f;
623 }