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