1 /* gph-cmath - C graphics math library
2 * Copyright (C) 2018 John Tsiombikas <nuclear@member.fsf.org>
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.
9 static inline void cgm_mcopy(float *dest, const float *src)
11 memcpy(dest, src, 16 * sizeof(float));
14 static inline void cgm_mzero(float *m)
20 static inline void cgm_midentity(float *m)
22 static float id[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
26 static inline void cgm_mmul(float *a, const float *b)
35 *resptr++ = arow[0] * b[j] + arow[1] * b[4 + j] +
36 arow[2] * b[8 + j] + arow[3] * b[12 + j];
43 static inline void cgm_mpremul(float *a, const float *b)
48 const float *brow = b;
52 *resptr++ = brow[0] * a[j] + brow[1] * a[4 + j] +
53 brow[2] * a[8 + j] + brow[3] * a[12 + j];
60 static inline void cgm_msubmatrix(float *m, int row, int col)
69 if(i == row) continue;
73 if(j == col) continue;
75 m[subi * 4 + subj++] = orig[i * 4 + j];
83 static inline void cgm_mupper3(float *m)
85 m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
89 static inline float cgm_msubdet(const float *m, int row, int col)
92 float subdet00, subdet01, subdet02;
95 cgm_msubmatrix(tmp, row, col);
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];
101 return tmp[0] * subdet00 - tmp[1] * subdet01 + tmp[2] * subdet02;
104 static inline float cgm_mcofactor(const float *m, int row, int col)
106 float min = cgm_msubdet(m, row, col);
107 return (row + col) & 1 ? -min : min;
110 static inline float cgm_mdet(const float *m)
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);
116 static inline void cgm_mtranspose(float *m)
130 static inline void cgm_mcofmatrix(float *m)
139 m[i * 4 + j] = cgm_mcofactor(tmp, i, j);
144 static inline int cgm_minverse(float *m)
149 float det = cgm_mdet(m);
150 if(det == 0.0f) return -1;
151 inv_det = 1.0f / det;
157 m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det; /* transposed */
163 static inline void cgm_mtranslation(float *m, float x, float y, float z)
171 static inline void cgm_mscaling(float *m, float sx, float sy, float sz)
180 static inline void cgm_mrotation_x(float *m, float angle)
182 float sa = sin(angle);
183 float ca = cos(angle);
192 static inline void cgm_mrotation_y(float *m, float angle)
194 float sa = sin(angle);
195 float ca = cos(angle);
204 static inline void cgm_mrotation_z(float *m, float angle)
206 float sa = sin(angle);
207 float ca = cos(angle);
216 static inline void cgm_mrotation_axis(float *m, int idx, float angle)
220 cgm_mrotation_x(m, angle);
223 cgm_mrotation_y(m, angle);
226 cgm_mrotation_z(m, angle);
231 static inline void cgm_mrotation(float *m, float angle, float x, float y, float z)
233 float sa = sin(angle);
234 float ca = cos(angle);
235 float invca = 1.0f - ca;
243 m[0] = xsq + (1.0f - xsq) * ca;
244 m[4] = x * y * invca - z * sa;
245 m[8] = x * z * invca + y * sa;
247 m[1] = x * y * invca + z * sa;
248 m[5] = ysq + (1.0f - ysq) * ca;
249 m[9] = y * z * invca - x * sa;
251 m[2] = x * z * invca - y * sa;
252 m[6] = y * z * invca + x * sa;
253 m[10] = zsq + (1.0f - zsq) * ca;
256 static inline void cgm_mrotation_euler(float *m, float a, float b, float c, int mode)
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},
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);
276 static inline void cgm_mrotation_quat(float *m, const cgm_quat *q)
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;
285 m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
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;
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;
299 static inline void cgm_mtranslate(float *m, float x, float y, float z)
302 cgm_mtranslation(tm, x, y, z);
306 static inline void cgm_mscale(float *m, float sx, float sy, float sz)
309 cgm_mscaling(sm, sx, sy, sz);
313 static inline void cgm_mrotate_x(float *m, float angle)
316 cgm_mrotation_x(rm, angle);
320 static inline void cgm_mrotate_y(float *m, float angle)
323 cgm_mrotation_y(rm, angle);
327 static inline void cgm_mrotate_z(float *m, float angle)
330 cgm_mrotation_z(rm, angle);
334 static inline void cgm_mrotate_axis(float *m, int idx, float angle)
337 cgm_mrotation_axis(rm, idx, angle);
341 static inline void cgm_mrotate(float *m, float angle, float x, float y, float z)
344 cgm_mrotation(rm, angle, x, y, z);
348 static inline void cgm_mrotate_euler(float *m, float a, float b, float c, int mode)
351 cgm_mrotation_euler(rm, a, b, c, mode);
355 static inline void cgm_mrotate_quat(float *m, const cgm_quat *q)
358 cgm_mrotation_quat(rm, q);
363 static inline void cgm_mpretranslate(float *m, float x, float y, float z)
366 cgm_mtranslation(tm, x, y, z);
370 static inline void cgm_mprescale(float *m, float sx, float sy, float sz)
373 cgm_mscaling(sm, sx, sy, sz);
377 static inline void cgm_mprerotate_x(float *m, float angle)
380 cgm_mrotation_x(rm, angle);
384 static inline void cgm_mprerotate_y(float *m, float angle)
387 cgm_mrotation_y(rm, angle);
391 static inline void cgm_mprerotate_z(float *m, float angle)
394 cgm_mrotation_z(rm, angle);
398 static inline void cgm_mprerotate_axis(float *m, int idx, float angle)
401 cgm_mrotation_axis(rm, idx, angle);
405 static inline void cgm_mprerotate(float *m, float angle, float x, float y, float z)
408 cgm_mrotation(rm, angle, x, y, z);
412 static inline void cgm_mprerotate_euler(float *m, float a, float b, float c, int mode)
415 cgm_mrotation_euler(rm, a, b, c, mode);
419 static inline void cgm_mprerotate_quat(float *m, const cgm_quat *q)
422 cgm_mrotation_quat(rm, q);
427 static inline void cgm_mget_translation(const float *m, cgm_vec3 *res)
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
438 static inline void cgm_mget_rotation(const float *m, cgm_quat *res)
440 static const int next[3] = {1, 2, 0};
444 float trace = m[0] + m[5] + m[10];
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;
461 if(m[10] > m[i * 4 + i]) {
467 root = sqrt(m[i * 4 + i] - m[j * 4 + j] - m[k * 4 + k] + 1.0f);
468 quat[i + 1] = 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;
480 static inline void cgm_mget_scaling(const float *m, cgm_vec3 *res)
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]);
487 static inline void cgm_mget_frustum_plane(const float *m, int p, cgm_vec4 *res)
490 const float *rowptr = m + row * 4;
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];
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];
505 static inline void cgm_mlookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
509 cgm_vec3 dir = *targ, right, vup;
512 cgm_vnormalize(&dir);
513 cgm_vcross(&right, &dir, up);
514 cgm_vnormalize(&right);
515 cgm_vcross(&vup, &right, &dir);
516 cgm_vnormalize(&vup);
529 cgm_mtranslation(trans, pos->x, pos->y, pos->z);
533 static inline void cgm_minv_lookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
537 cgm_vec3 dir = *targ, right, vup;
540 cgm_vnormalize(&dir);
541 cgm_vcross(&right, &dir, up);
542 cgm_vnormalize(&right);
543 cgm_vcross(&vup, &right, &dir);
544 cgm_vnormalize(&vup);
557 cgm_mtranslation(m, -pos->x, -pos->y, -pos->z);
561 static inline void cgm_mortho(float *m, float left, float right, float bot, float top,
562 float znear, float zfar)
564 float dx = right - left;
565 float dy = top - bot;
566 float dz = zfar - znear;
572 m[12] = -(right + left) / dx;
573 m[13] = -(top + bot) / dy;
574 m[14] = -(zfar + znear) / dz;
577 static inline void cgm_mfrustum(float *m, float left, float right, float bot, float top,
578 float znear, float zfar)
580 float dx = right - left;
581 float dy = top - bot;
582 float dz = zfar - znear;
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;
594 static inline void cgm_mperspective(float *m, float vfov, float aspect, float znear, float zfar)
596 float s = 1.0f / (float)tan(vfov / 2.0f);
597 float range = znear - zfar;
602 m[10] = (znear + zfar) / range;
603 m[14] = 2.0f * znear * zfar / range;
607 static inline void cgm_mmirror(float *m, float a, float b, float c, float d)
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;
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;
618 m[12] = -2.0f * a * d;
619 m[13] = -2.0f * b * d;
620 m[14] = -2.0f * c * d;
622 m[3] = m[7] = m[11] = 0.0f;