1 #ifndef LINMATH_H
2 #define LINMATH_H
3
4 #include <string.h>
5 #include <math.h>
6 #include <string.h>
7
8 /* 2021-03-21 Camilla Löwy <elmindreda@elmindreda.org>
9 * - Replaced double constants with float equivalents
10 */
11
12 #ifdef LINMATH_NO_INLINE
13 #define LINMATH_H_FUNC static
14 #else
15 #define LINMATH_H_FUNC static inline
16 #endif
17
18 #define LINMATH_H_DEFINE_VEC(n) \
19 typedef float vec##n[n]; \
20 LINMATH_H_FUNC void vec##n##_add(vec##n r, vec##n const a, vec##n const b) \
21 { \
22 int i; \
23 for(i=0; i<n; ++i) \
24 r[i] = a[i] + b[i]; \
25 } \
26 LINMATH_H_FUNC void vec##n##_sub(vec##n r, vec##n const a, vec##n const b) \
27 { \
28 int i; \
29 for(i=0; i<n; ++i) \
30 r[i] = a[i] - b[i]; \
31 } \
32 LINMATH_H_FUNC void vec##n##_scale(vec##n r, vec##n const v, float const s) \
33 { \
34 int i; \
35 for(i=0; i<n; ++i) \
36 r[i] = v[i] * s; \
37 } \
38 LINMATH_H_FUNC float vec##n##_mul_inner(vec##n const a, vec##n const b) \
39 { \
40 float p = 0.f; \
41 int i; \
42 for(i=0; i<n; ++i) \
43 p += b[i]*a[i]; \
44 return p; \
45 } \
46 LINMATH_H_FUNC float vec##n##_len(vec##n const v) \
47 { \
48 return sqrtf(vec##n##_mul_inner(v,v)); \
49 } \
50 LINMATH_H_FUNC void vec##n##_norm(vec##n r, vec##n const v) \
51 { \
52 float k = 1.f / vec##n##_len(v); \
53 vec##n##_scale(r, v, k); \
54 } \
55 LINMATH_H_FUNC void vec##n##_min(vec##n r, vec##n const a, vec##n const b) \
56 { \
57 int i; \
58 for(i=0; i<n; ++i) \
59 r[i] = a[i]<b[i] ? a[i] : b[i]; \
60 } \
61 LINMATH_H_FUNC void vec##n##_max(vec##n r, vec##n const a, vec##n const b) \
62 { \
63 int i; \
64 for(i=0; i<n; ++i) \
65 r[i] = a[i]>b[i] ? a[i] : b[i]; \
66 } \
67 LINMATH_H_FUNC void vec##n##_dup(vec##n r, vec##n const src) \
68 { \
69 int i; \
70 for(i=0; i<n; ++i) \
71 r[i] = src[i]; \
72 }
73
74 LINMATH_H_DEFINE_VEC(2)
75 LINMATH_H_DEFINE_VEC(3)
76 LINMATH_H_DEFINE_VEC(4)
77
vec3_mul_cross(vec3 r, vec3 const a, vec3 const b)78 LINMATH_H_FUNC void vec3_mul_cross(vec3 r, vec3 const a, vec3 const b)
79 {
80 r[0] = a[1]*b[2] - a[2]*b[1];
81 r[1] = a[2]*b[0] - a[0]*b[2];
82 r[2] = a[0]*b[1] - a[1]*b[0];
83 }
84
vec3_reflect(vec3 r, vec3 const v, vec3 const n)85 LINMATH_H_FUNC void vec3_reflect(vec3 r, vec3 const v, vec3 const n)
86 {
87 float p = 2.f * vec3_mul_inner(v, n);
88 int i;
89 for(i=0;i<3;++i)
90 r[i] = v[i] - p*n[i];
91 }
92
vec4_mul_cross(vec4 r, vec4 const a, vec4 const b)93 LINMATH_H_FUNC void vec4_mul_cross(vec4 r, vec4 const a, vec4 const b)
94 {
95 r[0] = a[1]*b[2] - a[2]*b[1];
96 r[1] = a[2]*b[0] - a[0]*b[2];
97 r[2] = a[0]*b[1] - a[1]*b[0];
98 r[3] = 1.f;
99 }
100
vec4_reflect(vec4 r, vec4 const v, vec4 const n)101 LINMATH_H_FUNC void vec4_reflect(vec4 r, vec4 const v, vec4 const n)
102 {
103 float p = 2.f*vec4_mul_inner(v, n);
104 int i;
105 for(i=0;i<4;++i)
106 r[i] = v[i] - p*n[i];
107 }
108
109 typedef vec4 mat4x4[4];
mat4x4_identity(mat4x4 M)110 LINMATH_H_FUNC void mat4x4_identity(mat4x4 M)
111 {
112 int i, j;
113 for(i=0; i<4; ++i)
114 for(j=0; j<4; ++j)
115 M[i][j] = i==j ? 1.f : 0.f;
116 }
mat4x4_dup(mat4x4 M, mat4x4 const N)117 LINMATH_H_FUNC void mat4x4_dup(mat4x4 M, mat4x4 const N)
118 {
119 int i;
120 for(i=0; i<4; ++i)
121 vec4_dup(M[i], N[i]);
122 }
mat4x4_row(vec4 r, mat4x4 const M, int i)123 LINMATH_H_FUNC void mat4x4_row(vec4 r, mat4x4 const M, int i)
124 {
125 int k;
126 for(k=0; k<4; ++k)
127 r[k] = M[k][i];
128 }
mat4x4_col(vec4 r, mat4x4 const M, int i)129 LINMATH_H_FUNC void mat4x4_col(vec4 r, mat4x4 const M, int i)
130 {
131 int k;
132 for(k=0; k<4; ++k)
133 r[k] = M[i][k];
134 }
mat4x4_transpose(mat4x4 M, mat4x4 const N)135 LINMATH_H_FUNC void mat4x4_transpose(mat4x4 M, mat4x4 const N)
136 {
137 // Note: if M and N are the same, the user has to
138 // explicitly make a copy of M and set it to N.
139 int i, j;
140 for(j=0; j<4; ++j)
141 for(i=0; i<4; ++i)
142 M[i][j] = N[j][i];
143 }
mat4x4_add(mat4x4 M, mat4x4 const a, mat4x4 const b)144 LINMATH_H_FUNC void mat4x4_add(mat4x4 M, mat4x4 const a, mat4x4 const b)
145 {
146 int i;
147 for(i=0; i<4; ++i)
148 vec4_add(M[i], a[i], b[i]);
149 }
mat4x4_sub(mat4x4 M, mat4x4 const a, mat4x4 const b)150 LINMATH_H_FUNC void mat4x4_sub(mat4x4 M, mat4x4 const a, mat4x4 const b)
151 {
152 int i;
153 for(i=0; i<4; ++i)
154 vec4_sub(M[i], a[i], b[i]);
155 }
mat4x4_scale(mat4x4 M, mat4x4 const a, float k)156 LINMATH_H_FUNC void mat4x4_scale(mat4x4 M, mat4x4 const a, float k)
157 {
158 int i;
159 for(i=0; i<4; ++i)
160 vec4_scale(M[i], a[i], k);
161 }
mat4x4_scale_aniso(mat4x4 M, mat4x4 const a, float x, float y, float z)162 LINMATH_H_FUNC void mat4x4_scale_aniso(mat4x4 M, mat4x4 const a, float x, float y, float z)
163 {
164 vec4_scale(M[0], a[0], x);
165 vec4_scale(M[1], a[1], y);
166 vec4_scale(M[2], a[2], z);
167 vec4_dup(M[3], a[3]);
168 }
mat4x4_mul(mat4x4 M, mat4x4 const a, mat4x4 const b)169 LINMATH_H_FUNC void mat4x4_mul(mat4x4 M, mat4x4 const a, mat4x4 const b)
170 {
171 mat4x4 temp;
172 int k, r, c;
173 for(c=0; c<4; ++c) for(r=0; r<4; ++r) {
174 temp[c][r] = 0.f;
175 for(k=0; k<4; ++k)
176 temp[c][r] += a[k][r] * b[c][k];
177 }
178 mat4x4_dup(M, temp);
179 }
mat4x4_mul_vec4(vec4 r, mat4x4 const M, vec4 const v)180 LINMATH_H_FUNC void mat4x4_mul_vec4(vec4 r, mat4x4 const M, vec4 const v)
181 {
182 int i, j;
183 for(j=0; j<4; ++j) {
184 r[j] = 0.f;
185 for(i=0; i<4; ++i)
186 r[j] += M[i][j] * v[i];
187 }
188 }
mat4x4_translate(mat4x4 T, float x, float y, float z)189 LINMATH_H_FUNC void mat4x4_translate(mat4x4 T, float x, float y, float z)
190 {
191 mat4x4_identity(T);
192 T[3][0] = x;
193 T[3][1] = y;
194 T[3][2] = z;
195 }
mat4x4_translate_in_place(mat4x4 M, float x, float y, float z)196 LINMATH_H_FUNC void mat4x4_translate_in_place(mat4x4 M, float x, float y, float z)
197 {
198 vec4 t = {x, y, z, 0};
199 vec4 r;
200 int i;
201 for (i = 0; i < 4; ++i) {
202 mat4x4_row(r, M, i);
203 M[3][i] += vec4_mul_inner(r, t);
204 }
205 }
mat4x4_from_vec3_mul_outer(mat4x4 M, vec3 const a, vec3 const b)206 LINMATH_H_FUNC void mat4x4_from_vec3_mul_outer(mat4x4 M, vec3 const a, vec3 const b)
207 {
208 int i, j;
209 for(i=0; i<4; ++i) for(j=0; j<4; ++j)
210 M[i][j] = i<3 && j<3 ? a[i] * b[j] : 0.f;
211 }
212 LINMATH_H_FUNC void mat4x4_rotate(mat4x4 R, mat4x4 const M, float x, float y, float z, float angle)
213 {
214 float s = sinf(angle);
215 float c = cosf(angle);
216 vec3 u = {x, y, z};
217
218 if(vec3_len(u) > 1e-4) {
219 vec3_norm(u, u);
220 mat4x4 T;
221 mat4x4_from_vec3_mul_outer(T, u, u);
222
223 mat4x4 S = {
224 { 0, u[2], -u[1], 0},
225 {-u[2], 0, u[0], 0},
226 { u[1], -u[0], 0, 0},
227 { 0, 0, 0, 0}
228 };
229 mat4x4_scale(S, S, s);
230
231 mat4x4 C;
232 mat4x4_identity(C);
233 mat4x4_sub(C, C, T);
234
235 mat4x4_scale(C, C, c);
236
237 mat4x4_add(T, T, C);
238 mat4x4_add(T, T, S);
239
240 T[3][3] = 1.f;
241 mat4x4_mul(R, M, T);
242 } else {
243 mat4x4_dup(R, M);
244 }
245 }
246 LINMATH_H_FUNC void mat4x4_rotate_X(mat4x4 Q, mat4x4 const M, float angle)
247 {
248 float s = sinf(angle);
249 float c = cosf(angle);
250 mat4x4 R = {
251 {1.f, 0.f, 0.f, 0.f},
252 {0.f, c, s, 0.f},
253 {0.f, -s, c, 0.f},
254 {0.f, 0.f, 0.f, 1.f}
255 };
256 mat4x4_mul(Q, M, R);
257 }
258 LINMATH_H_FUNC void mat4x4_rotate_Y(mat4x4 Q, mat4x4 const M, float angle)
259 {
260 float s = sinf(angle);
261 float c = cosf(angle);
262 mat4x4 R = {
263 { c, 0.f, -s, 0.f},
264 { 0.f, 1.f, 0.f, 0.f},
265 { s, 0.f, c, 0.f},
266 { 0.f, 0.f, 0.f, 1.f}
267 };
268 mat4x4_mul(Q, M, R);
269 }
270 LINMATH_H_FUNC void mat4x4_rotate_Z(mat4x4 Q, mat4x4 const M, float angle)
271 {
272 float s = sinf(angle);
273 float c = cosf(angle);
274 mat4x4 R = {
275 { c, s, 0.f, 0.f},
276 { -s, c, 0.f, 0.f},
277 { 0.f, 0.f, 1.f, 0.f},
278 { 0.f, 0.f, 0.f, 1.f}
279 };
280 mat4x4_mul(Q, M, R);
281 }
282 LINMATH_H_FUNC void mat4x4_invert(mat4x4 T, mat4x4 const M)
283 {
284 float s[6];
285 float c[6];
286 s[0] = M[0][0]*M[1][1] - M[1][0]*M[0][1];
287 s[1] = M[0][0]*M[1][2] - M[1][0]*M[0][2];
288 s[2] = M[0][0]*M[1][3] - M[1][0]*M[0][3];
289 s[3] = M[0][1]*M[1][2] - M[1][1]*M[0][2];
290 s[4] = M[0][1]*M[1][3] - M[1][1]*M[0][3];
291 s[5] = M[0][2]*M[1][3] - M[1][2]*M[0][3];
292
293 c[0] = M[2][0]*M[3][1] - M[3][0]*M[2][1];
294 c[1] = M[2][0]*M[3][2] - M[3][0]*M[2][2];
295 c[2] = M[2][0]*M[3][3] - M[3][0]*M[2][3];
296 c[3] = M[2][1]*M[3][2] - M[3][1]*M[2][2];
297 c[4] = M[2][1]*M[3][3] - M[3][1]*M[2][3];
298 c[5] = M[2][2]*M[3][3] - M[3][2]*M[2][3];
299
300 /* Assumes it is invertible */
301 float idet = 1.0f/( s[0]*c[5]-s[1]*c[4]+s[2]*c[3]+s[3]*c[2]-s[4]*c[1]+s[5]*c[0] );
302
303 T[0][0] = ( M[1][1] * c[5] - M[1][2] * c[4] + M[1][3] * c[3]) * idet;
304 T[0][1] = (-M[0][1] * c[5] + M[0][2] * c[4] - M[0][3] * c[3]) * idet;
305 T[0][2] = ( M[3][1] * s[5] - M[3][2] * s[4] + M[3][3] * s[3]) * idet;
306 T[0][3] = (-M[2][1] * s[5] + M[2][2] * s[4] - M[2][3] * s[3]) * idet;
307
308 T[1][0] = (-M[1][0] * c[5] + M[1][2] * c[2] - M[1][3] * c[1]) * idet;
309 T[1][1] = ( M[0][0] * c[5] - M[0][2] * c[2] + M[0][3] * c[1]) * idet;
310 T[1][2] = (-M[3][0] * s[5] + M[3][2] * s[2] - M[3][3] * s[1]) * idet;
311 T[1][3] = ( M[2][0] * s[5] - M[2][2] * s[2] + M[2][3] * s[1]) * idet;
312
313 T[2][0] = ( M[1][0] * c[4] - M[1][1] * c[2] + M[1][3] * c[0]) * idet;
314 T[2][1] = (-M[0][0] * c[4] + M[0][1] * c[2] - M[0][3] * c[0]) * idet;
315 T[2][2] = ( M[3][0] * s[4] - M[3][1] * s[2] + M[3][3] * s[0]) * idet;
316 T[2][3] = (-M[2][0] * s[4] + M[2][1] * s[2] - M[2][3] * s[0]) * idet;
317
318 T[3][0] = (-M[1][0] * c[3] + M[1][1] * c[1] - M[1][2] * c[0]) * idet;
319 T[3][1] = ( M[0][0] * c[3] - M[0][1] * c[1] + M[0][2] * c[0]) * idet;
320 T[3][2] = (-M[3][0] * s[3] + M[3][1] * s[1] - M[3][2] * s[0]) * idet;
321 T[3][3] = ( M[2][0] * s[3] - M[2][1] * s[1] + M[2][2] * s[0]) * idet;
322 }
323 LINMATH_H_FUNC void mat4x4_orthonormalize(mat4x4 R, mat4x4 const M)
324 {
325 mat4x4_dup(R, M);
326 float s = 1.f;
327 vec3 h;
328
329 vec3_norm(R[2], R[2]);
330
331 s = vec3_mul_inner(R[1], R[2]);
332 vec3_scale(h, R[2], s);
333 vec3_sub(R[1], R[1], h);
334 vec3_norm(R[1], R[1]);
335
336 s = vec3_mul_inner(R[0], R[2]);
337 vec3_scale(h, R[2], s);
338 vec3_sub(R[0], R[0], h);
339
340 s = vec3_mul_inner(R[0], R[1]);
341 vec3_scale(h, R[1], s);
342 vec3_sub(R[0], R[0], h);
343 vec3_norm(R[0], R[0]);
344 }
345
346 LINMATH_H_FUNC void mat4x4_frustum(mat4x4 M, float l, float r, float b, float t, float n, float f)
347 {
348 M[0][0] = 2.f*n/(r-l);
349 M[0][1] = M[0][2] = M[0][3] = 0.f;
350
351 M[1][1] = 2.f*n/(t-b);
352 M[1][0] = M[1][2] = M[1][3] = 0.f;
353
354 M[2][0] = (r+l)/(r-l);
355 M[2][1] = (t+b)/(t-b);
356 M[2][2] = -(f+n)/(f-n);
357 M[2][3] = -1.f;
358
359 M[3][2] = -2.f*(f*n)/(f-n);
360 M[3][0] = M[3][1] = M[3][3] = 0.f;
361 }
362 LINMATH_H_FUNC void mat4x4_ortho(mat4x4 M, float l, float r, float b, float t, float n, float f)
363 {
364 M[0][0] = 2.f/(r-l);
365 M[0][1] = M[0][2] = M[0][3] = 0.f;
366
367 M[1][1] = 2.f/(t-b);
368 M[1][0] = M[1][2] = M[1][3] = 0.f;
369
370 M[2][2] = -2.f/(f-n);
371 M[2][0] = M[2][1] = M[2][3] = 0.f;
372
373 M[3][0] = -(r+l)/(r-l);
374 M[3][1] = -(t+b)/(t-b);
375 M[3][2] = -(f+n)/(f-n);
376 M[3][3] = 1.f;
377 }
378 LINMATH_H_FUNC void mat4x4_perspective(mat4x4 m, float y_fov, float aspect, float n, float f)
379 {
380 /* NOTE: Degrees are an unhandy unit to work with.
381 * linmath.h uses radians for everything! */
382 float const a = 1.f / tanf(y_fov / 2.f);
383
384 m[0][0] = a / aspect;
385 m[0][1] = 0.f;
386 m[0][2] = 0.f;
387 m[0][3] = 0.f;
388
389 m[1][0] = 0.f;
390 m[1][1] = a;
391 m[1][2] = 0.f;
392 m[1][3] = 0.f;
393
394 m[2][0] = 0.f;
395 m[2][1] = 0.f;
396 m[2][2] = -((f + n) / (f - n));
397 m[2][3] = -1.f;
398
399 m[3][0] = 0.f;
400 m[3][1] = 0.f;
401 m[3][2] = -((2.f * f * n) / (f - n));
402 m[3][3] = 0.f;
403 }
404 LINMATH_H_FUNC void mat4x4_look_at(mat4x4 m, vec3 const eye, vec3 const center, vec3 const up)
405 {
406 /* Adapted from Android's OpenGL Matrix.java. */
407 /* See the OpenGL GLUT documentation for gluLookAt for a description */
408 /* of the algorithm. We implement it in a straightforward way: */
409
410 /* TODO: The negation of of can be spared by swapping the order of
411 * operands in the following cross products in the right way. */
412 vec3 f;
413 vec3_sub(f, center, eye);
414 vec3_norm(f, f);
415
416 vec3 s;
417 vec3_mul_cross(s, f, up);
418 vec3_norm(s, s);
419
420 vec3 t;
421 vec3_mul_cross(t, s, f);
422
423 m[0][0] = s[0];
424 m[0][1] = t[0];
425 m[0][2] = -f[0];
426 m[0][3] = 0.f;
427
428 m[1][0] = s[1];
429 m[1][1] = t[1];
430 m[1][2] = -f[1];
431 m[1][3] = 0.f;
432
433 m[2][0] = s[2];
434 m[2][1] = t[2];
435 m[2][2] = -f[2];
436 m[2][3] = 0.f;
437
438 m[3][0] = 0.f;
439 m[3][1] = 0.f;
440 m[3][2] = 0.f;
441 m[3][3] = 1.f;
442
443 mat4x4_translate_in_place(m, -eye[0], -eye[1], -eye[2]);
444 }
445
446 typedef float quat[4];
447 #define quat_add vec4_add
448 #define quat_sub vec4_sub
449 #define quat_norm vec4_norm
450 #define quat_scale vec4_scale
451 #define quat_mul_inner vec4_mul_inner
452
453 LINMATH_H_FUNC void quat_identity(quat q)
454 {
455 q[0] = q[1] = q[2] = 0.f;
456 q[3] = 1.f;
457 }
458 LINMATH_H_FUNC void quat_mul(quat r, quat const p, quat const q)
459 {
460 vec3 w;
461 vec3_mul_cross(r, p, q);
462 vec3_scale(w, p, q[3]);
463 vec3_add(r, r, w);
464 vec3_scale(w, q, p[3]);
465 vec3_add(r, r, w);
466 r[3] = p[3]*q[3] - vec3_mul_inner(p, q);
467 }
468 LINMATH_H_FUNC void quat_conj(quat r, quat const q)
469 {
470 int i;
471 for(i=0; i<3; ++i)
472 r[i] = -q[i];
473 r[3] = q[3];
474 }
475 LINMATH_H_FUNC void quat_rotate(quat r, float angle, vec3 const axis) {
476 vec3 axis_norm;
477 vec3_norm(axis_norm, axis);
478 float s = sinf(angle / 2);
479 float c = cosf(angle / 2);
480 vec3_scale(r, axis_norm, s);
481 r[3] = c;
482 }
483 LINMATH_H_FUNC void quat_mul_vec3(vec3 r, quat const q, vec3 const v)
484 {
485 /*
486 * Method by Fabian 'ryg' Giessen (of Farbrausch)
487 t = 2 * cross(q.xyz, v)
488 v' = v + q.w * t + cross(q.xyz, t)
489 */
490 vec3 t;
491 vec3 q_xyz = {q[0], q[1], q[2]};
492 vec3 u = {q[0], q[1], q[2]};
493
494 vec3_mul_cross(t, q_xyz, v);
495 vec3_scale(t, t, 2);
496
497 vec3_mul_cross(u, q_xyz, t);
498 vec3_scale(t, t, q[3]);
499
500 vec3_add(r, v, t);
501 vec3_add(r, r, u);
502 }
503 LINMATH_H_FUNC void mat4x4_from_quat(mat4x4 M, quat const q)
504 {
505 float a = q[3];
506 float b = q[0];
507 float c = q[1];
508 float d = q[2];
509 float a2 = a*a;
510 float b2 = b*b;
511 float c2 = c*c;
512 float d2 = d*d;
513
514 M[0][0] = a2 + b2 - c2 - d2;
515 M[0][1] = 2.f*(b*c + a*d);
516 M[0][2] = 2.f*(b*d - a*c);
517 M[0][3] = 0.f;
518
519 M[1][0] = 2*(b*c - a*d);
520 M[1][1] = a2 - b2 + c2 - d2;
521 M[1][2] = 2.f*(c*d + a*b);
522 M[1][3] = 0.f;
523
524 M[2][0] = 2.f*(b*d + a*c);
525 M[2][1] = 2.f*(c*d - a*b);
526 M[2][2] = a2 - b2 - c2 + d2;
527 M[2][3] = 0.f;
528
529 M[3][0] = M[3][1] = M[3][2] = 0.f;
530 M[3][3] = 1.f;
531 }
532
533 LINMATH_H_FUNC void mat4x4o_mul_quat(mat4x4 R, mat4x4 const M, quat const q)
534 {
535 /* XXX: The way this is written only works for orthogonal matrices. */
536 /* TODO: Take care of non-orthogonal case. */
537 quat_mul_vec3(R[0], q, M[0]);
538 quat_mul_vec3(R[1], q, M[1]);
539 quat_mul_vec3(R[2], q, M[2]);
540
541 R[3][0] = R[3][1] = R[3][2] = 0.f;
542 R[0][3] = M[0][3];
543 R[1][3] = M[1][3];
544 R[2][3] = M[2][3];
545 R[3][3] = M[3][3]; // typically 1.0, but here we make it general
546 }
547 LINMATH_H_FUNC void quat_from_mat4x4(quat q, mat4x4 const M)
548 {
549 float r=0.f;
550 int i;
551
552 int perm[] = { 0, 1, 2, 0, 1 };
553 int *p = perm;
554
555 for(i = 0; i<3; i++) {
556 float m = M[i][i];
557 if( m < r )
558 continue;
559 m = r;
560 p = &perm[i];
561 }
562
563 r = sqrtf(1.f + M[p[0]][p[0]] - M[p[1]][p[1]] - M[p[2]][p[2]] );
564
565 if(r < 1e-6) {
566 q[0] = 1.f;
567 q[1] = q[2] = q[3] = 0.f;
568 return;
569 }
570
571 q[0] = r/2.f;
572 q[1] = (M[p[0]][p[1]] - M[p[1]][p[0]])/(2.f*r);
573 q[2] = (M[p[2]][p[0]] - M[p[0]][p[2]])/(2.f*r);
574 q[3] = (M[p[2]][p[1]] - M[p[1]][p[2]])/(2.f*r);
575 }
576
577 LINMATH_H_FUNC void mat4x4_arcball(mat4x4 R, mat4x4 const M, vec2 const _a, vec2 const _b, float s)
578 {
579 vec2 a; memcpy(a, _a, sizeof(a));
580 vec2 b; memcpy(b, _b, sizeof(b));
581
582 float z_a = 0.f;
583 float z_b = 0.f;
584
585 if(vec2_len(a) < 1.f) {
586 z_a = sqrtf(1.f - vec2_mul_inner(a, a));
587 } else {
588 vec2_norm(a, a);
589 }
590
591 if(vec2_len(b) < 1.f) {
592 z_b = sqrtf(1.f - vec2_mul_inner(b, b));
593 } else {
594 vec2_norm(b, b);
595 }
596
597 vec3 a_ = {a[0], a[1], z_a};
598 vec3 b_ = {b[0], b[1], z_b};
599
600 vec3 c_;
601 vec3_mul_cross(c_, a_, b_);
602
603 float const angle = acos(vec3_mul_inner(a_, b_)) * s;
604 mat4x4_rotate(R, M, c_[0], c_[1], c_[2], angle);
605 }
606 #endif
607