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