123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284 |
- /******************************************************************************
- Copyright (C) 2023 by Lain Bailey <lain@obsproject.com>
- This program is free software: you can redistribute it and/or modify
- it under the terms of the GNU General Public License as published by
- the Free Software Foundation, either version 2 of the License, or
- (at your option) any later version.
- This program is distributed in the hope that it will be useful,
- but WITHOUT ANY WARRANTY; without even the implied warranty of
- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- GNU General Public License for more details.
- You should have received a copy of the GNU General Public License
- along with this program. If not, see <http://www.gnu.org/licenses/>.
- ******************************************************************************/
- #include "math-defs.h"
- #include "matrix4.h"
- #include "matrix3.h"
- #include "quat.h"
- void matrix4_from_matrix3(struct matrix4 *dst, const struct matrix3 *m)
- {
- dst->x.m = m->x.m;
- dst->y.m = m->y.m;
- dst->z.m = m->z.m;
- dst->t.m = m->t.m;
- dst->t.w = 1.0f;
- }
- void matrix4_from_quat(struct matrix4 *dst, const struct quat *q)
- {
- float norm = quat_dot(q, q);
- float s = (norm > 0.0f) ? (2.0f / norm) : 0.0f;
- float xx = q->x * q->x * s;
- float yy = q->y * q->y * s;
- float zz = q->z * q->z * s;
- float xy = q->x * q->y * s;
- float xz = q->x * q->z * s;
- float yz = q->y * q->z * s;
- float wx = q->w * q->x * s;
- float wy = q->w * q->y * s;
- float wz = q->w * q->z * s;
- vec4_set(&dst->x, 1.0f - (yy + zz), xy + wz, xz - wy, 0.0f);
- vec4_set(&dst->y, xy - wz, 1.0f - (xx + zz), yz + wx, 0.0f);
- vec4_set(&dst->z, xz + wy, yz - wx, 1.0f - (xx + yy), 0.0f);
- vec4_set(&dst->t, 0.0f, 0.0f, 0.0f, 1.0f);
- }
- void matrix4_from_axisang(struct matrix4 *dst, const struct axisang *aa)
- {
- struct quat q;
- quat_from_axisang(&q, aa);
- matrix4_from_quat(dst, &q);
- }
- void matrix4_mul(struct matrix4 *dst, const struct matrix4 *m1, const struct matrix4 *m2)
- {
- const struct vec4 *m1v = (const struct vec4 *)m1;
- const float *m2f = (const float *)m2;
- struct vec4 out[4];
- int i, j;
- for (i = 0; i < 4; i++) {
- for (j = 0; j < 4; j++) {
- struct vec4 temp;
- vec4_set(&temp, m2f[j], m2f[j + 4], m2f[j + 8], m2f[j + 12]);
- out[i].ptr[j] = vec4_dot(&m1v[i], &temp);
- }
- }
- matrix4_copy(dst, (struct matrix4 *)out);
- }
- static inline void get_3x3_submatrix(float *dst, const struct matrix4 *m, int i, int j)
- {
- const float *mf = (const float *)m;
- int ti, tj, idst, jdst;
- for (ti = 0; ti < 4; ti++) {
- if (ti < i)
- idst = ti;
- else if (ti > i)
- idst = ti - 1;
- else
- continue;
- for (tj = 0; tj < 4; tj++) {
- if (tj < j)
- jdst = tj;
- else if (tj > j)
- jdst = tj - 1;
- else
- continue;
- dst[(idst * 3) + jdst] = mf[(ti * 4) + tj];
- }
- }
- }
- static inline float get_3x3_determinant(const float *m)
- {
- return (m[0] * ((m[4] * m[8]) - (m[7] * m[5]))) - (m[1] * ((m[3] * m[8]) - (m[6] * m[5]))) +
- (m[2] * ((m[3] * m[7]) - (m[6] * m[4])));
- }
- float matrix4_determinant(const struct matrix4 *m)
- {
- const float *mf = (const float *)m;
- float det, result = 0.0f, i = 1.0f;
- float m3x3[9];
- int n;
- for (n = 0; n < 4; n++, i = -i) { // NOLINT(clang-tidy-cert-flp30-c)
- get_3x3_submatrix(m3x3, m, 0, n);
- det = get_3x3_determinant(m3x3);
- result += mf[n] * det * i;
- }
- return result;
- }
- void matrix4_translate3v(struct matrix4 *dst, const struct matrix4 *m, const struct vec3 *v)
- {
- struct matrix4 temp;
- vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
- vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
- vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
- vec4_from_vec3(&temp.t, v);
- matrix4_mul(dst, m, &temp);
- }
- void matrix4_translate4v(struct matrix4 *dst, const struct matrix4 *m, const struct vec4 *v)
- {
- struct matrix4 temp;
- vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
- vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
- vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
- vec4_copy(&temp.t, v);
- matrix4_mul(dst, m, &temp);
- }
- void matrix4_rotate(struct matrix4 *dst, const struct matrix4 *m, const struct quat *q)
- {
- struct matrix4 temp;
- matrix4_from_quat(&temp, q);
- matrix4_mul(dst, m, &temp);
- }
- void matrix4_rotate_aa(struct matrix4 *dst, const struct matrix4 *m, const struct axisang *aa)
- {
- struct matrix4 temp;
- matrix4_from_axisang(&temp, aa);
- matrix4_mul(dst, m, &temp);
- }
- void matrix4_scale(struct matrix4 *dst, const struct matrix4 *m, const struct vec3 *v)
- {
- struct matrix4 temp;
- vec4_set(&temp.x, v->x, 0.0f, 0.0f, 0.0f);
- vec4_set(&temp.y, 0.0f, v->y, 0.0f, 0.0f);
- vec4_set(&temp.z, 0.0f, 0.0f, v->z, 0.0f);
- vec4_set(&temp.t, 0.0f, 0.0f, 0.0f, 1.0f);
- matrix4_mul(dst, m, &temp);
- }
- void matrix4_translate3v_i(struct matrix4 *dst, const struct vec3 *v, const struct matrix4 *m)
- {
- struct matrix4 temp;
- vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
- vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
- vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
- vec4_from_vec3(&temp.t, v);
- matrix4_mul(dst, &temp, m);
- }
- void matrix4_translate4v_i(struct matrix4 *dst, const struct vec4 *v, const struct matrix4 *m)
- {
- struct matrix4 temp;
- vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
- vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
- vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
- vec4_copy(&temp.t, v);
- matrix4_mul(dst, &temp, m);
- }
- void matrix4_rotate_i(struct matrix4 *dst, const struct quat *q, const struct matrix4 *m)
- {
- struct matrix4 temp;
- matrix4_from_quat(&temp, q);
- matrix4_mul(dst, &temp, m);
- }
- void matrix4_rotate_aa_i(struct matrix4 *dst, const struct axisang *aa, const struct matrix4 *m)
- {
- struct matrix4 temp;
- matrix4_from_axisang(&temp, aa);
- matrix4_mul(dst, &temp, m);
- }
- void matrix4_scale_i(struct matrix4 *dst, const struct vec3 *v, const struct matrix4 *m)
- {
- struct matrix4 temp;
- vec4_set(&temp.x, v->x, 0.0f, 0.0f, 0.0f);
- vec4_set(&temp.y, 0.0f, v->y, 0.0f, 0.0f);
- vec4_set(&temp.z, 0.0f, 0.0f, v->z, 0.0f);
- vec4_set(&temp.t, 0.0f, 0.0f, 0.0f, 1.0f);
- matrix4_mul(dst, &temp, m);
- }
- bool matrix4_inv(struct matrix4 *dst, const struct matrix4 *m)
- {
- struct vec4 *dstv;
- float det;
- float m3x3[9];
- int i, j, sign;
- if (dst == m) {
- struct matrix4 temp = *m;
- return matrix4_inv(dst, &temp);
- }
- dstv = (struct vec4 *)dst;
- det = matrix4_determinant(m);
- if (fabs(det) < 0.0005f)
- return false;
- for (i = 0; i < 4; i++) {
- for (j = 0; j < 4; j++) {
- sign = 1 - ((i + j) % 2) * 2;
- get_3x3_submatrix(m3x3, m, i, j);
- dstv[j].ptr[i] = get_3x3_determinant(m3x3) * (float)sign / det;
- }
- }
- return true;
- }
- void matrix4_transpose(struct matrix4 *dst, const struct matrix4 *m)
- {
- if (dst == m) {
- struct matrix4 temp = *m;
- matrix4_transpose(dst, &temp);
- return;
- }
- #ifdef NO_INTRINSICS
- dst->x.x = m->x.x;
- dst->x.y = m->y.x;
- dst->x.z = m->z.x;
- dst->x.w = m->t.x;
- dst->y.x = m->x.y;
- dst->y.y = m->y.y;
- dst->y.z = m->z.y;
- dst->y.w = m->t.y;
- dst->z.x = m->x.z;
- dst->z.y = m->y.z;
- dst->z.z = m->z.z;
- dst->z.w = m->t.z;
- dst->t.x = m->x.w;
- dst->t.y = m->y.w;
- dst->t.z = m->z.w;
- dst->t.w = m->t.w;
- #else
- __m128 a0 = _mm_unpacklo_ps(m->x.m, m->z.m);
- __m128 a1 = _mm_unpacklo_ps(m->y.m, m->t.m);
- __m128 a2 = _mm_unpackhi_ps(m->x.m, m->z.m);
- __m128 a3 = _mm_unpackhi_ps(m->y.m, m->t.m);
- dst->x.m = _mm_unpacklo_ps(a0, a1);
- dst->y.m = _mm_unpackhi_ps(a0, a1);
- dst->z.m = _mm_unpacklo_ps(a2, a3);
- dst->t.m = _mm_unpackhi_ps(a2, a3);
- #endif
- }
|