1 /******************************************************************************
2 Copyright (C) 2013 by Hugh Bailey <obs.jim@gmail.com>
3
4 This program is free software: you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation, either version 2 of the License, or
7 (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with this program. If not, see <http://www.gnu.org/licenses/>.
16 ******************************************************************************/
17
18 #include "math-defs.h"
19 #include "matrix4.h"
20 #include "matrix3.h"
21 #include "quat.h"
22
matrix4_from_matrix3(struct matrix4 * dst,const struct matrix3 * m)23 void matrix4_from_matrix3(struct matrix4 *dst, const struct matrix3 *m)
24 {
25 dst->x.m = m->x.m;
26 dst->y.m = m->y.m;
27 dst->z.m = m->z.m;
28 dst->t.m = m->t.m;
29 dst->t.w = 1.0f;
30 }
31
matrix4_from_quat(struct matrix4 * dst,const struct quat * q)32 void matrix4_from_quat(struct matrix4 *dst, const struct quat *q)
33 {
34 float norm = quat_dot(q, q);
35 float s = (norm > 0.0f) ? (2.0f / norm) : 0.0f;
36
37 float xx = q->x * q->x * s;
38 float yy = q->y * q->y * s;
39 float zz = q->z * q->z * s;
40 float xy = q->x * q->y * s;
41 float xz = q->x * q->z * s;
42 float yz = q->y * q->z * s;
43 float wx = q->w * q->x * s;
44 float wy = q->w * q->y * s;
45 float wz = q->w * q->z * s;
46
47 vec4_set(&dst->x, 1.0f - (yy + zz), xy + wz, xz - wy, 0.0f);
48 vec4_set(&dst->y, xy - wz, 1.0f - (xx + zz), yz + wx, 0.0f);
49 vec4_set(&dst->z, xz + wy, yz - wx, 1.0f - (xx + yy), 0.0f);
50 vec4_set(&dst->t, 0.0f, 0.0f, 0.0f, 1.0f);
51 }
52
matrix4_from_axisang(struct matrix4 * dst,const struct axisang * aa)53 void matrix4_from_axisang(struct matrix4 *dst, const struct axisang *aa)
54 {
55 struct quat q;
56 quat_from_axisang(&q, aa);
57 matrix4_from_quat(dst, &q);
58 }
59
matrix4_mul(struct matrix4 * dst,const struct matrix4 * m1,const struct matrix4 * m2)60 void matrix4_mul(struct matrix4 *dst, const struct matrix4 *m1,
61 const struct matrix4 *m2)
62 {
63 const struct vec4 *m1v = (const struct vec4 *)m1;
64 const float *m2f = (const float *)m2;
65 struct vec4 out[4];
66 int i, j;
67
68 for (i = 0; i < 4; i++) {
69 for (j = 0; j < 4; j++) {
70 struct vec4 temp;
71 vec4_set(&temp, m2f[j], m2f[j + 4], m2f[j + 8],
72 m2f[j + 12]);
73 out[i].ptr[j] = vec4_dot(&m1v[i], &temp);
74 }
75 }
76
77 matrix4_copy(dst, (struct matrix4 *)out);
78 }
79
get_3x3_submatrix(float * dst,const struct matrix4 * m,int i,int j)80 static inline void get_3x3_submatrix(float *dst, const struct matrix4 *m, int i,
81 int j)
82 {
83 const float *mf = (const float *)m;
84 int ti, tj, idst, jdst;
85
86 for (ti = 0; ti < 4; ti++) {
87 if (ti < i)
88 idst = ti;
89 else if (ti > i)
90 idst = ti - 1;
91 else
92 continue;
93
94 for (tj = 0; tj < 4; tj++) {
95 if (tj < j)
96 jdst = tj;
97 else if (tj > j)
98 jdst = tj - 1;
99 else
100 continue;
101
102 dst[(idst * 3) + jdst] = mf[(ti * 4) + tj];
103 }
104 }
105 }
106
get_3x3_determinant(const float * m)107 static inline float get_3x3_determinant(const float *m)
108 {
109 return (m[0] * ((m[4] * m[8]) - (m[7] * m[5]))) -
110 (m[1] * ((m[3] * m[8]) - (m[6] * m[5]))) +
111 (m[2] * ((m[3] * m[7]) - (m[6] * m[4])));
112 }
113
matrix4_determinant(const struct matrix4 * m)114 float matrix4_determinant(const struct matrix4 *m)
115 {
116 const float *mf = (const float *)m;
117 float det, result = 0.0f, i = 1.0f;
118 float m3x3[9];
119 int n;
120
121 for (n = 0; n < 4; n++, i = -i) { // NOLINT(clang-tidy-cert-flp30-c)
122 get_3x3_submatrix(m3x3, m, 0, n);
123
124 det = get_3x3_determinant(m3x3);
125 result += mf[n] * det * i;
126 }
127
128 return result;
129 }
130
matrix4_translate3v(struct matrix4 * dst,const struct matrix4 * m,const struct vec3 * v)131 void matrix4_translate3v(struct matrix4 *dst, const struct matrix4 *m,
132 const struct vec3 *v)
133 {
134 struct matrix4 temp;
135 vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
136 vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
137 vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
138 vec4_from_vec3(&temp.t, v);
139
140 matrix4_mul(dst, m, &temp);
141 }
142
matrix4_translate4v(struct matrix4 * dst,const struct matrix4 * m,const struct vec4 * v)143 void matrix4_translate4v(struct matrix4 *dst, const struct matrix4 *m,
144 const struct vec4 *v)
145 {
146 struct matrix4 temp;
147 vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
148 vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
149 vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
150 vec4_copy(&temp.t, v);
151
152 matrix4_mul(dst, m, &temp);
153 }
154
matrix4_rotate(struct matrix4 * dst,const struct matrix4 * m,const struct quat * q)155 void matrix4_rotate(struct matrix4 *dst, const struct matrix4 *m,
156 const struct quat *q)
157 {
158 struct matrix4 temp;
159 matrix4_from_quat(&temp, q);
160 matrix4_mul(dst, m, &temp);
161 }
162
matrix4_rotate_aa(struct matrix4 * dst,const struct matrix4 * m,const struct axisang * aa)163 void matrix4_rotate_aa(struct matrix4 *dst, const struct matrix4 *m,
164 const struct axisang *aa)
165 {
166 struct matrix4 temp;
167 matrix4_from_axisang(&temp, aa);
168 matrix4_mul(dst, m, &temp);
169 }
170
matrix4_scale(struct matrix4 * dst,const struct matrix4 * m,const struct vec3 * v)171 void matrix4_scale(struct matrix4 *dst, const struct matrix4 *m,
172 const struct vec3 *v)
173 {
174 struct matrix4 temp;
175 vec4_set(&temp.x, v->x, 0.0f, 0.0f, 0.0f);
176 vec4_set(&temp.y, 0.0f, v->y, 0.0f, 0.0f);
177 vec4_set(&temp.z, 0.0f, 0.0f, v->z, 0.0f);
178 vec4_set(&temp.t, 0.0f, 0.0f, 0.0f, 1.0f);
179 matrix4_mul(dst, m, &temp);
180 }
181
matrix4_translate3v_i(struct matrix4 * dst,const struct vec3 * v,const struct matrix4 * m)182 void matrix4_translate3v_i(struct matrix4 *dst, const struct vec3 *v,
183 const struct matrix4 *m)
184 {
185 struct matrix4 temp;
186 vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
187 vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
188 vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
189 vec4_from_vec3(&temp.t, v);
190
191 matrix4_mul(dst, &temp, m);
192 }
193
matrix4_translate4v_i(struct matrix4 * dst,const struct vec4 * v,const struct matrix4 * m)194 void matrix4_translate4v_i(struct matrix4 *dst, const struct vec4 *v,
195 const struct matrix4 *m)
196 {
197 struct matrix4 temp;
198 vec4_set(&temp.x, 1.0f, 0.0f, 0.0f, 0.0f);
199 vec4_set(&temp.y, 0.0f, 1.0f, 0.0f, 0.0f);
200 vec4_set(&temp.z, 0.0f, 0.0f, 1.0f, 0.0f);
201 vec4_copy(&temp.t, v);
202
203 matrix4_mul(dst, &temp, m);
204 }
205
matrix4_rotate_i(struct matrix4 * dst,const struct quat * q,const struct matrix4 * m)206 void matrix4_rotate_i(struct matrix4 *dst, const struct quat *q,
207 const struct matrix4 *m)
208 {
209 struct matrix4 temp;
210 matrix4_from_quat(&temp, q);
211 matrix4_mul(dst, &temp, m);
212 }
213
matrix4_rotate_aa_i(struct matrix4 * dst,const struct axisang * aa,const struct matrix4 * m)214 void matrix4_rotate_aa_i(struct matrix4 *dst, const struct axisang *aa,
215 const struct matrix4 *m)
216 {
217 struct matrix4 temp;
218 matrix4_from_axisang(&temp, aa);
219 matrix4_mul(dst, &temp, m);
220 }
221
matrix4_scale_i(struct matrix4 * dst,const struct vec3 * v,const struct matrix4 * m)222 void matrix4_scale_i(struct matrix4 *dst, const struct vec3 *v,
223 const struct matrix4 *m)
224 {
225 struct matrix4 temp;
226 vec4_set(&temp.x, v->x, 0.0f, 0.0f, 0.0f);
227 vec4_set(&temp.y, 0.0f, v->y, 0.0f, 0.0f);
228 vec4_set(&temp.z, 0.0f, 0.0f, v->z, 0.0f);
229 vec4_set(&temp.t, 0.0f, 0.0f, 0.0f, 1.0f);
230 matrix4_mul(dst, &temp, m);
231 }
232
matrix4_inv(struct matrix4 * dst,const struct matrix4 * m)233 bool matrix4_inv(struct matrix4 *dst, const struct matrix4 *m)
234 {
235 struct vec4 *dstv;
236 float det;
237 float m3x3[9];
238 int i, j, sign;
239
240 if (dst == m) {
241 struct matrix4 temp = *m;
242 return matrix4_inv(dst, &temp);
243 }
244
245 dstv = (struct vec4 *)dst;
246 det = matrix4_determinant(m);
247
248 if (fabs(det) < 0.0005f)
249 return false;
250
251 for (i = 0; i < 4; i++) {
252 for (j = 0; j < 4; j++) {
253 sign = 1 - ((i + j) % 2) * 2;
254 get_3x3_submatrix(m3x3, m, i, j);
255 dstv[j].ptr[i] =
256 get_3x3_determinant(m3x3) * (float)sign / det;
257 }
258 }
259
260 return true;
261 }
262
matrix4_transpose(struct matrix4 * dst,const struct matrix4 * m)263 void matrix4_transpose(struct matrix4 *dst, const struct matrix4 *m)
264 {
265 if (dst == m) {
266 struct matrix4 temp = *m;
267 matrix4_transpose(dst, &temp);
268 return;
269 }
270
271 #ifdef NO_INTRINSICS
272 dst->x.x = m->x.x;
273 dst->x.y = m->y.x;
274 dst->x.z = m->z.x;
275 dst->x.w = m->t.x;
276 dst->y.x = m->x.y;
277 dst->y.y = m->y.y;
278 dst->y.z = m->z.y;
279 dst->y.w = m->t.y;
280 dst->z.x = m->x.z;
281 dst->z.y = m->y.z;
282 dst->z.z = m->z.z;
283 dst->z.w = m->t.z;
284 dst->t.x = m->x.w;
285 dst->t.y = m->y.w;
286 dst->t.z = m->z.w;
287 dst->t.w = m->t.w;
288 #else
289 __m128 a0 = _mm_unpacklo_ps(m->x.m, m->z.m);
290 __m128 a1 = _mm_unpacklo_ps(m->y.m, m->t.m);
291 __m128 a2 = _mm_unpackhi_ps(m->x.m, m->z.m);
292 __m128 a3 = _mm_unpackhi_ps(m->y.m, m->t.m);
293
294 dst->x.m = _mm_unpacklo_ps(a0, a1);
295 dst->y.m = _mm_unpackhi_ps(a0, a1);
296 dst->z.m = _mm_unpacklo_ps(a2, a3);
297 dst->t.m = _mm_unpackhi_ps(a2, a3);
298 #endif
299 }
300