suwako / cpputils

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