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