From 813216d03f41018b9c46b8a0e6eb648e0a704dc1 Mon Sep 17 00:00:00 2001 From: Tomasz Kramkowski Date: Tue, 27 Mar 2018 18:26:28 +0100 Subject: linmath: more cleanup --- linmath.h | 107 ++++++++++++++++++++++++++++++++++++++++---------------------- 1 file changed, 69 insertions(+), 38 deletions(-) diff --git a/linmath.h b/linmath.h index 8d9ff8c..ffbe7cc 100644 --- a/linmath.h +++ b/linmath.h @@ -93,43 +93,51 @@ static inline void mat4x4_identity(mat4x4 M) for (int j = 0; j < 4; ++j) M[i][j] = i == j ? 1.0f : 0.0f; } + static inline void mat4x4_dup(mat4x4 M, mat4x4 N) { for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) M[i][j] = N[i][j]; } + static inline void mat4x4_row(vec4 r, mat4x4 M, int i) { for (int k = 0; k < 4; ++k) r[k] = M[k][i]; } + static inline void mat4x4_col(vec4 r, mat4x4 M, int i) { for (int k = 0; k < 4; ++k) r[k] = M[i][k]; } + static inline void mat4x4_transpose(mat4x4 M, mat4x4 N) { for (int j = 0; j < 4; ++j) for (int i = 0; i < 4; ++i) M[i][j] = N[j][i]; } + static inline void mat4x4_add(mat4x4 M, mat4x4 a, mat4x4 b) { for (int i = 0; i < 4; ++i) vec4_add(M[i], a[i], b[i]); } + static inline void mat4x4_sub(mat4x4 M, mat4x4 a, mat4x4 b) { for (int i = 0; i < 4; ++i) vec4_sub(M[i], a[i], b[i]); } + static inline void mat4x4_scale(mat4x4 M, mat4x4 a, lm_elem k) { for (int i = 0; i < 4; ++i) vec4_scale(M[i], a[i], k); } + static inline void mat4x4_scale_aniso(mat4x4 M, mat4x4 a, lm_elem x, lm_elem y, lm_elem z) { vec4_scale(M[0], a[0], x); @@ -138,6 +146,7 @@ static inline void mat4x4_scale_aniso(mat4x4 M, mat4x4 a, lm_elem x, lm_elem y, for (int i = 0; i < 4; ++i) M[3][i] = a[3][i]; } + static inline void mat4x4_mul(mat4x4 M, mat4x4 a, mat4x4 b) { mat4x4 temp; @@ -150,6 +159,7 @@ static inline void mat4x4_mul(mat4x4 M, mat4x4 a, mat4x4 b) } mat4x4_dup(M, temp); } + static inline void mat4x4_mul_vec4(vec4 r, mat4x4 M, vec4 v) { for (int j = 0; j < 4; ++j) { @@ -158,6 +168,7 @@ static inline void mat4x4_mul_vec4(vec4 r, mat4x4 M, vec4 v) r[j] += M[i][j] * v[i]; } } + static inline void mat4x4_translate(mat4x4 T, lm_elem x, lm_elem y, lm_elem z) { mat4x4_identity(T); @@ -165,6 +176,7 @@ static inline void mat4x4_translate(mat4x4 T, lm_elem x, lm_elem y, lm_elem z) T[3][1] = y; T[3][2] = z; } + static inline void mat4x4_translate_in_place(mat4x4 M, lm_elem x, lm_elem y, lm_elem z) { vec4 t = {x, y, z, 0}; @@ -174,82 +186,89 @@ static inline void mat4x4_translate_in_place(mat4x4 M, lm_elem x, lm_elem y, lm_ M[3][i] += vec4_mul_inner(r, t); } } + static inline void mat4x4_from_vec3_mul_outer(mat4x4 M, vec3 a, vec3 b) { for (int i = 0; i < 4; ++i) for (int j = 0; j < 4; ++j) M[i][j] = i < 3 && j < 3 ? a[i] * b[j] : 0.0f; } + static inline void mat4x4_rotate(mat4x4 R, mat4x4 M, lm_elem x, lm_elem y, lm_elem z, lm_elem angle) { lm_elem s = sinf(angle); lm_elem c = cosf(angle); vec3 u = {x, y, z}; - if (vec3_len(u) > 1e-4) { - vec3_norm(u, u); - mat4x4 T; - mat4x4_from_vec3_mul_outer(T, u, u); + if (vec3_len(u) <= 1e-4) { + mat4x4_dup(R, M); + return; + } - mat4x4 S = { - { 0, u[2], -u[1], 0}, - {-u[2], 0, u[0], 0}, - { u[1], -u[0], 0, 0}, - { 0, 0, 0, 0} - }; - mat4x4_scale(S, S, s); + vec3_norm(u, u); + mat4x4 T; + mat4x4_from_vec3_mul_outer(T, u, u); - mat4x4 C; - mat4x4_identity(C); - mat4x4_sub(C, C, T); + mat4x4 S = { + { 0, u[2], -u[1], 0 }, + { -u[2], 0, u[0], 0 }, + { u[1], -u[0], 0, 0 }, + { 0, 0, 0, 0 } + }; + mat4x4_scale(S, S, s); - mat4x4_scale(C, C, c); + mat4x4 C; + mat4x4_identity(C); + mat4x4_sub(C, C, T); - mat4x4_add(T, T, C); - mat4x4_add(T, T, S); + mat4x4_scale(C, C, c); - T[3][3] = 1.; - mat4x4_mul(R, M, T); - } else { - mat4x4_dup(R, M); - } + mat4x4_add(T, T, C); + mat4x4_add(T, T, S); + + T[3][3] = 1.; + mat4x4_mul(R, M, T); } + static inline void mat4x4_rotate_X(mat4x4 Q, mat4x4 M, lm_elem angle) { lm_elem s = sinf(angle); lm_elem c = cosf(angle); mat4x4 R = { - {1.0f, 0.0f, 0.0f, 0.0f}, - {0.0f, c, s, 0.0f}, - {0.0f, -s, c, 0.0f}, - {0.0f, 0.0f, 0.0f, 1.0f} + { 1.0f, 0.0f, 0.0f, 0.0f }, + { 0.0f, c, s, 0.0f }, + { 0.0f, -s, c, 0.0f }, + { 0.0f, 0.0f, 0.0f, 1.0f } }; mat4x4_mul(Q, M, R); } + static inline void mat4x4_rotate_Y(mat4x4 Q, mat4x4 M, lm_elem angle) { lm_elem s = sinf(angle); lm_elem c = cosf(angle); mat4x4 R = { - { c, 0.0f, s, 0.0f}, - { 0.0f, 1.0f, 0.0f, 0.0f}, - { -s, 0.0f, c, 0.0f}, - { 0.0f, 0.0f, 0.0f, 1.0f} + { c, 0.0f, s, 0.0f }, + { 0.0f, 1.0f, 0.0f, 0.0f }, + { -s, 0.0f, c, 0.0f }, + { 0.0f, 0.0f, 0.0f, 1.0f } }; mat4x4_mul(Q, M, R); } + static inline void mat4x4_rotate_Z(mat4x4 Q, mat4x4 M, lm_elem angle) { lm_elem s = sinf(angle); lm_elem c = cosf(angle); mat4x4 R = { - { c, s, 0.0f, 0.0f}, - { -s, c, 0.0f, 0.0f}, - { 0.0f, 0.0f, 1.0f, 0.0f}, - { 0.0f, 0.0f, 0.0f, 1.0f} + { c, s, 0.0f, 0.0f }, + { -s, c, 0.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 0.0f, 1.0f } }; mat4x4_mul(Q, M, R); } + static inline void mat4x4_invert(mat4x4 T, mat4x4 M) { lm_elem s[6]; @@ -293,10 +312,11 @@ static inline void mat4x4_invert(mat4x4 T, mat4x4 M) T[3][2] = (-M[3][0] * s[3] + M[3][1] * s[1] - M[3][2] * s[0]) * idet; T[3][3] = ( M[2][0] * s[3] - M[2][1] * s[1] + M[2][2] * s[0]) * idet; } + static inline void mat4x4_orthonormalize(mat4x4 R, mat4x4 M) { mat4x4_dup(R, M); - lm_elem s = 1.; + lm_elem s; vec3 h; vec3_norm(R[2], R[2]); @@ -333,6 +353,7 @@ static inline void mat4x4_frustum(mat4x4 M, lm_elem l, lm_elem r, lm_elem b, lm_ M[3][2] = -2.0f * (f * n)/(f-n); M[3][0] = M[3][1] = M[3][3] = 0.0f; } + static inline void mat4x4_ortho(mat4x4 M, lm_elem l, lm_elem r, lm_elem b, lm_elem t, lm_elem n, lm_elem f) { M[0][0] = 2.0f/(r-l); @@ -349,6 +370,7 @@ static inline void mat4x4_ortho(mat4x4 M, lm_elem l, lm_elem r, lm_elem b, lm_el M[3][2] = -(f+n)/(f-n); M[3][3] = 1.0f; } + static inline void mat4x4_perspective(mat4x4 m, lm_elem y_fov, lm_elem aspect, lm_elem n, lm_elem f) { lm_elem const a = 1.0f / tan(y_fov / 2.0f); @@ -373,6 +395,7 @@ static inline void mat4x4_perspective(mat4x4 m, lm_elem y_fov, lm_elem aspect, l m[3][2] = -((2.0f * f * n) / (f - n)); m[3][3] = 0.0f; } + static inline void mat4x4_look_at(mat4x4 m, vec3 eye, vec3 center, vec3 up) { /* Adapted from Android's OpenGL Matrix.java. */ @@ -421,16 +444,19 @@ static inline void quat_identity(quat q) q[0] = q[1] = q[2] = 0.0f; q[3] = 1.0f; } + static inline void quat_add(quat r, quat a, quat b) { for (int i = 0; i < 4; ++i) r[i] = a[i] + b[i]; } + static inline void quat_sub(quat r, quat a, quat b) { for (int i = 0; i < 4; ++i) r[i] = a[i] - b[i]; } + static inline void quat_mul(quat r, quat p, quat q) { vec3 w; @@ -441,11 +467,13 @@ static inline void quat_mul(quat r, quat p, quat q) vec3_add(r, r, w); r[3] = p[3]*q[3] - vec3_mul_inner(p, q); } + static inline void quat_scale(quat r, quat v, lm_elem s) { for (int i = 0; i < 4; ++i) r[i] = v[i] * s; } + static inline lm_elem quat_inner_product(quat a, quat b) { lm_elem p = 0.0f; @@ -453,12 +481,14 @@ static inline lm_elem quat_inner_product(quat a, quat b) p += b[i]*a[i]; return p; } + static inline void quat_conj(quat r, quat q) { for (int i = 0; i < 3; ++i) r[i] = -q[i]; r[3] = q[3]; } + static inline void quat_rotate(quat r, lm_elem angle, vec3 axis) { vec3 v; vec3_scale(v, axis, sinf(angle / 2)); @@ -466,13 +496,14 @@ static inline void quat_rotate(quat r, lm_elem angle, vec3 axis) { r[i] = v[i]; r[3] = cosf(angle / 2); } + #define quat_norm vec4_norm static inline void quat_mul_vec3(vec3 r, quat q, vec3 v) { /* * Method by Fabian 'ryg' Giessen (of Farbrausch) -t = 2 * cross(q.xyz, v) -v' = v + q.w * t + cross(q.xyz, t) + * t = 2 * cross(q.xyz, v) + * v' = v + q.w * t + cross(q.xyz, t) */ vec3 t; vec3 q_xyz = { q[0], q[1], q[2] }; @@ -487,6 +518,7 @@ v' = v + q.w * t + cross(q.xyz, t) vec3_add(r, v, t); vec3_add(r, r, u); } + static inline void mat4x4_from_quat(mat4x4 M, quat q) { lm_elem a = q[3]; @@ -532,7 +564,6 @@ static inline void mat4x4o_mul_quat(mat4x4 R, mat4x4 M, quat q) static inline void quat_from_mat4x4(quat q, mat4x4 M) { lm_elem r = 0.0f; - int i; int perm[] = { 0, 1, 2, 0, 1 }; int *p = perm; -- cgit v1.2.3-54-g00ecf