1 /* 2 * Copyright (c) 2015-2016 The Khronos Group Inc. 3 * Copyright (c) 2015-2016 Valve Corporation 4 * Copyright (c) 2015-2016 LunarG, Inc. 5 * 6 * Permission is hereby granted, free of charge, to any person obtaining a copy 7 * of this software and/or associated documentation files (the "Materials"), to 8 * deal in the Materials without restriction, including without limitation the 9 * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 10 * sell copies of the Materials, and to permit persons to whom the Materials are 11 * furnished to do so, subject to the following conditions: 12 * 13 * The above copyright notice(s) and this permission notice shall be included in 14 * all copies or substantial portions of the Materials. 15 * 16 * THE MATERIALS ARE PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. 19 * 20 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, 21 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR 22 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE MATERIALS OR THE 23 * USE OR OTHER DEALINGS IN THE MATERIALS. 24 * 25 * Relicensed from the WTFPL (http://www.wtfpl.net/faq/). 26 */ 27 28 #ifndef LINMATH_H 29 #define LINMATH_H 30 31 #include <math.h> 32 33 // Converts degrees to radians. 34 #define degreesToRadians(angleDegrees) (angleDegrees * M_PI / 180.0) 35 36 // Converts radians to degrees. 37 #define radiansToDegrees(angleRadians) (angleRadians * 180.0 / M_PI) 38 39 typedef float vec3[3]; 40 static inline void vec3_add(vec3 r, vec3 const a, vec3 const b) { 41 int i; 42 for (i = 0; i < 3; ++i) 43 r[i] = a[i] + b[i]; 44 } 45 static inline void vec3_sub(vec3 r, vec3 const a, vec3 const b) { 46 int i; 47 for (i = 0; i < 3; ++i) 48 r[i] = a[i] - b[i]; 49 } 50 static inline void vec3_scale(vec3 r, vec3 const v, float const s) { 51 int i; 52 for (i = 0; i < 3; ++i) 53 r[i] = v[i] * s; 54 } 55 static inline float vec3_mul_inner(vec3 const a, vec3 const b) { 56 float p = 0.f; 57 int i; 58 for (i = 0; i < 3; ++i) 59 p += b[i] * a[i]; 60 return p; 61 } 62 static inline void vec3_mul_cross(vec3 r, vec3 const a, vec3 const b) { 63 r[0] = a[1] * b[2] - a[2] * b[1]; 64 r[1] = a[2] * b[0] - a[0] * b[2]; 65 r[2] = a[0] * b[1] - a[1] * b[0]; 66 } 67 static inline float vec3_len(vec3 const v) { 68 return sqrtf(vec3_mul_inner(v, v)); 69 } 70 static inline void vec3_norm(vec3 r, vec3 const v) { 71 float k = 1.f / vec3_len(v); 72 vec3_scale(r, v, k); 73 } 74 static inline void vec3_reflect(vec3 r, vec3 const v, vec3 const n) { 75 float p = 2.f * vec3_mul_inner(v, n); 76 int i; 77 for (i = 0; i < 3; ++i) 78 r[i] = v[i] - p * n[i]; 79 } 80 81 typedef float vec4[4]; 82 static inline void vec4_add(vec4 r, vec4 const a, vec4 const b) { 83 int i; 84 for (i = 0; i < 4; ++i) 85 r[i] = a[i] + b[i]; 86 } 87 static inline void vec4_sub(vec4 r, vec4 const a, vec4 const b) { 88 int i; 89 for (i = 0; i < 4; ++i) 90 r[i] = a[i] - b[i]; 91 } 92 static inline void vec4_scale(vec4 r, vec4 v, float s) { 93 int i; 94 for (i = 0; i < 4; ++i) 95 r[i] = v[i] * s; 96 } 97 static inline float vec4_mul_inner(vec4 a, vec4 b) { 98 float p = 0.f; 99 int i; 100 for (i = 0; i < 4; ++i) 101 p += b[i] * a[i]; 102 return p; 103 } 104 static inline void vec4_mul_cross(vec4 r, vec4 a, vec4 b) { 105 r[0] = a[1] * b[2] - a[2] * b[1]; 106 r[1] = a[2] * b[0] - a[0] * b[2]; 107 r[2] = a[0] * b[1] - a[1] * b[0]; 108 r[3] = 1.f; 109 } 110 static inline float vec4_len(vec4 v) { return sqrtf(vec4_mul_inner(v, v)); } 111 static inline void vec4_norm(vec4 r, vec4 v) { 112 float k = 1.f / vec4_len(v); 113 vec4_scale(r, v, k); 114 } 115 static inline void vec4_reflect(vec4 r, vec4 v, vec4 n) { 116 float p = 2.f * vec4_mul_inner(v, n); 117 int i; 118 for (i = 0; i < 4; ++i) 119 r[i] = v[i] - p * n[i]; 120 } 121 122 typedef vec4 mat4x4[4]; 123 static inline void mat4x4_identity(mat4x4 M) { 124 int i, j; 125 for (i = 0; i < 4; ++i) 126 for (j = 0; j < 4; ++j) 127 M[i][j] = i == j ? 1.f : 0.f; 128 } 129 static inline void mat4x4_dup(mat4x4 M, mat4x4 N) { 130 int i, j; 131 for (i = 0; i < 4; ++i) 132 for (j = 0; j < 4; ++j) 133 M[i][j] = N[i][j]; 134 } 135 static inline void mat4x4_row(vec4 r, mat4x4 M, int i) { 136 int k; 137 for (k = 0; k < 4; ++k) 138 r[k] = M[k][i]; 139 } 140 static inline void mat4x4_col(vec4 r, mat4x4 M, int i) { 141 int k; 142 for (k = 0; k < 4; ++k) 143 r[k] = M[i][k]; 144 } 145 static inline void mat4x4_transpose(mat4x4 M, mat4x4 N) { 146 int i, j; 147 for (j = 0; j < 4; ++j) 148 for (i = 0; i < 4; ++i) 149 M[i][j] = N[j][i]; 150 } 151 static inline void mat4x4_add(mat4x4 M, mat4x4 a, mat4x4 b) { 152 int i; 153 for (i = 0; i < 4; ++i) 154 vec4_add(M[i], a[i], b[i]); 155 } 156 static inline void mat4x4_sub(mat4x4 M, mat4x4 a, mat4x4 b) { 157 int i; 158 for (i = 0; i < 4; ++i) 159 vec4_sub(M[i], a[i], b[i]); 160 } 161 static inline void mat4x4_scale(mat4x4 M, mat4x4 a, float k) { 162 int i; 163 for (i = 0; i < 4; ++i) 164 vec4_scale(M[i], a[i], k); 165 } 166 static inline void mat4x4_scale_aniso(mat4x4 M, mat4x4 a, float x, float y, 167 float z) { 168 int i; 169 vec4_scale(M[0], a[0], x); 170 vec4_scale(M[1], a[1], y); 171 vec4_scale(M[2], a[2], z); 172 for (i = 0; i < 4; ++i) { 173 M[3][i] = a[3][i]; 174 } 175 } 176 static inline void mat4x4_mul(mat4x4 M, mat4x4 a, mat4x4 b) { 177 int k, r, c; 178 for (c = 0; c < 4; ++c) 179 for (r = 0; r < 4; ++r) { 180 M[c][r] = 0.f; 181 for (k = 0; k < 4; ++k) 182 M[c][r] += a[k][r] * b[c][k]; 183 } 184 } 185 static inline void mat4x4_mul_vec4(vec4 r, mat4x4 M, vec4 v) { 186 int i, j; 187 for (j = 0; j < 4; ++j) { 188 r[j] = 0.f; 189 for (i = 0; i < 4; ++i) 190 r[j] += M[i][j] * v[i]; 191 } 192 } 193 static inline void mat4x4_translate(mat4x4 T, float x, float y, float z) { 194 mat4x4_identity(T); 195 T[3][0] = x; 196 T[3][1] = y; 197 T[3][2] = z; 198 } 199 static inline void mat4x4_translate_in_place(mat4x4 M, float x, float y, 200 float z) { 201 vec4 t = {x, y, z, 0}; 202 vec4 r; 203 int i; 204 for (i = 0; i < 4; ++i) { 205 mat4x4_row(r, M, i); 206 M[3][i] += vec4_mul_inner(r, t); 207 } 208 } 209 static inline void mat4x4_from_vec3_mul_outer(mat4x4 M, vec3 a, vec3 b) { 210 int i, j; 211 for (i = 0; i < 4; ++i) 212 for (j = 0; j < 4; ++j) 213 M[i][j] = i < 3 && j < 3 ? a[i] * b[j] : 0.f; 214 } 215 static inline void mat4x4_rotate(mat4x4 R, mat4x4 M, float x, float y, float z, 216 float angle) { 217 float s = sinf(angle); 218 float c = cosf(angle); 219 vec3 u = {x, y, z}; 220 221 if (vec3_len(u) > 1e-4) { 222 vec3_norm(u, u); 223 mat4x4 T; 224 mat4x4_from_vec3_mul_outer(T, u, u); 225 226 mat4x4 S = {{0, u[2], -u[1], 0}, 227 {-u[2], 0, u[0], 0}, 228 {u[1], -u[0], 0, 0}, 229 {0, 0, 0, 0}}; 230 mat4x4_scale(S, S, s); 231 232 mat4x4 C; 233 mat4x4_identity(C); 234 mat4x4_sub(C, C, T); 235 236 mat4x4_scale(C, C, c); 237 238 mat4x4_add(T, T, C); 239 mat4x4_add(T, T, S); 240 241 T[3][3] = 1.; 242 mat4x4_mul(R, M, T); 243 } else { 244 mat4x4_dup(R, M); 245 } 246 } 247 static inline void mat4x4_rotate_X(mat4x4 Q, mat4x4 M, float angle) { 248 float s = sinf(angle); 249 float c = cosf(angle); 250 mat4x4 R = {{1.f, 0.f, 0.f, 0.f}, 251 {0.f, c, s, 0.f}, 252 {0.f, -s, c, 0.f}, 253 {0.f, 0.f, 0.f, 1.f}}; 254 mat4x4_mul(Q, M, R); 255 } 256 static inline void mat4x4_rotate_Y(mat4x4 Q, mat4x4 M, float angle) { 257 float s = sinf(angle); 258 float c = cosf(angle); 259 mat4x4 R = {{c, 0.f, s, 0.f}, 260 {0.f, 1.f, 0.f, 0.f}, 261 {-s, 0.f, c, 0.f}, 262 {0.f, 0.f, 0.f, 1.f}}; 263 mat4x4_mul(Q, M, R); 264 } 265 static inline void mat4x4_rotate_Z(mat4x4 Q, mat4x4 M, float angle) { 266 float s = sinf(angle); 267 float c = cosf(angle); 268 mat4x4 R = {{c, s, 0.f, 0.f}, 269 {-s, c, 0.f, 0.f}, 270 {0.f, 0.f, 1.f, 0.f}, 271 {0.f, 0.f, 0.f, 1.f}}; 272 mat4x4_mul(Q, M, R); 273 } 274 static inline void mat4x4_invert(mat4x4 T, mat4x4 M) { 275 float s[6]; 276 float c[6]; 277 s[0] = M[0][0] * M[1][1] - M[1][0] * M[0][1]; 278 s[1] = M[0][0] * M[1][2] - M[1][0] * M[0][2]; 279 s[2] = M[0][0] * M[1][3] - M[1][0] * M[0][3]; 280 s[3] = M[0][1] * M[1][2] - M[1][1] * M[0][2]; 281 s[4] = M[0][1] * M[1][3] - M[1][1] * M[0][3]; 282 s[5] = M[0][2] * M[1][3] - M[1][2] * M[0][3]; 283 284 c[0] = M[2][0] * M[3][1] - M[3][0] * M[2][1]; 285 c[1] = M[2][0] * M[3][2] - M[3][0] * M[2][2]; 286 c[2] = M[2][0] * M[3][3] - M[3][0] * M[2][3]; 287 c[3] = M[2][1] * M[3][2] - M[3][1] * M[2][2]; 288 c[4] = M[2][1] * M[3][3] - M[3][1] * M[2][3]; 289 c[5] = M[2][2] * M[3][3] - M[3][2] * M[2][3]; 290 291 /* Assumes it is invertible */ 292 float idet = 1.0f / (s[0] * c[5] - s[1] * c[4] + s[2] * c[3] + s[3] * c[2] - 293 s[4] * c[1] + s[5] * c[0]); 294 295 T[0][0] = (M[1][1] * c[5] - M[1][2] * c[4] + M[1][3] * c[3]) * idet; 296 T[0][1] = (-M[0][1] * c[5] + M[0][2] * c[4] - M[0][3] * c[3]) * idet; 297 T[0][2] = (M[3][1] * s[5] - M[3][2] * s[4] + M[3][3] * s[3]) * idet; 298 T[0][3] = (-M[2][1] * s[5] + M[2][2] * s[4] - M[2][3] * s[3]) * idet; 299 300 T[1][0] = (-M[1][0] * c[5] + M[1][2] * c[2] - M[1][3] * c[1]) * idet; 301 T[1][1] = (M[0][0] * c[5] - M[0][2] * c[2] + M[0][3] * c[1]) * idet; 302 T[1][2] = (-M[3][0] * s[5] + M[3][2] * s[2] - M[3][3] * s[1]) * idet; 303 T[1][3] = (M[2][0] * s[5] - M[2][2] * s[2] + M[2][3] * s[1]) * idet; 304 305 T[2][0] = (M[1][0] * c[4] - M[1][1] * c[2] + M[1][3] * c[0]) * idet; 306 T[2][1] = (-M[0][0] * c[4] + M[0][1] * c[2] - M[0][3] * c[0]) * idet; 307 T[2][2] = (M[3][0] * s[4] - M[3][1] * s[2] + M[3][3] * s[0]) * idet; 308 T[2][3] = (-M[2][0] * s[4] + M[2][1] * s[2] - M[2][3] * s[0]) * idet; 309 310 T[3][0] = (-M[1][0] * c[3] + M[1][1] * c[1] - M[1][2] * c[0]) * idet; 311 T[3][1] = (M[0][0] * c[3] - M[0][1] * c[1] + M[0][2] * c[0]) * idet; 312 T[3][2] = (-M[3][0] * s[3] + M[3][1] * s[1] - M[3][2] * s[0]) * idet; 313 T[3][3] = (M[2][0] * s[3] - M[2][1] * s[1] + M[2][2] * s[0]) * idet; 314 } 315 static inline void mat4x4_orthonormalize(mat4x4 R, mat4x4 M) { 316 mat4x4_dup(R, M); 317 float s = 1.; 318 vec3 h; 319 320 vec3_norm(R[2], R[2]); 321 322 s = vec3_mul_inner(R[1], R[2]); 323 vec3_scale(h, R[2], s); 324 vec3_sub(R[1], R[1], h); 325 vec3_norm(R[2], R[2]); 326 327 s = vec3_mul_inner(R[1], R[2]); 328 vec3_scale(h, R[2], s); 329 vec3_sub(R[1], R[1], h); 330 vec3_norm(R[1], R[1]); 331 332 s = vec3_mul_inner(R[0], R[1]); 333 vec3_scale(h, R[1], s); 334 vec3_sub(R[0], R[0], h); 335 vec3_norm(R[0], R[0]); 336 } 337 338 static inline void mat4x4_frustum(mat4x4 M, float l, float r, float b, float t, 339 float n, float f) { 340 M[0][0] = 2.f * n / (r - l); 341 M[0][1] = M[0][2] = M[0][3] = 0.f; 342 343 M[1][1] = 2.f * n / (t - b); 344 M[1][0] = M[1][2] = M[1][3] = 0.f; 345 346 M[2][0] = (r + l) / (r - l); 347 M[2][1] = (t + b) / (t - b); 348 M[2][2] = -(f + n) / (f - n); 349 M[2][3] = -1.f; 350 351 M[3][2] = -2.f * (f * n) / (f - n); 352 M[3][0] = M[3][1] = M[3][3] = 0.f; 353 } 354 static inline void mat4x4_ortho(mat4x4 M, float l, float r, float b, float t, 355 float n, float f) { 356 M[0][0] = 2.f / (r - l); 357 M[0][1] = M[0][2] = M[0][3] = 0.f; 358 359 M[1][1] = 2.f / (t - b); 360 M[1][0] = M[1][2] = M[1][3] = 0.f; 361 362 M[2][2] = -2.f / (f - n); 363 M[2][0] = M[2][1] = M[2][3] = 0.f; 364 365 M[3][0] = -(r + l) / (r - l); 366 M[3][1] = -(t + b) / (t - b); 367 M[3][2] = -(f + n) / (f - n); 368 M[3][3] = 1.f; 369 } 370 static inline void mat4x4_perspective(mat4x4 m, float y_fov, float aspect, 371 float n, float f) { 372 /* NOTE: Degrees are an unhandy unit to work with. 373 * linmath.h uses radians for everything! */ 374 float const a = (float)(1.f / tan(y_fov / 2.f)); 375 376 m[0][0] = a / aspect; 377 m[0][1] = 0.f; 378 m[0][2] = 0.f; 379 m[0][3] = 0.f; 380 381 m[1][0] = 0.f; 382 m[1][1] = a; 383 m[1][2] = 0.f; 384 m[1][3] = 0.f; 385 386 m[2][0] = 0.f; 387 m[2][1] = 0.f; 388 m[2][2] = -((f + n) / (f - n)); 389 m[2][3] = -1.f; 390 391 m[3][0] = 0.f; 392 m[3][1] = 0.f; 393 m[3][2] = -((2.f * f * n) / (f - n)); 394 m[3][3] = 0.f; 395 } 396 static inline void mat4x4_look_at(mat4x4 m, vec3 eye, vec3 center, vec3 up) { 397 /* Adapted from Android's OpenGL Matrix.java. */ 398 /* See the OpenGL GLUT documentation for gluLookAt for a description */ 399 /* of the algorithm. We implement it in a straightforward way: */ 400 401 /* TODO: The negation of of can be spared by swapping the order of 402 * operands in the following cross products in the right way. */ 403 vec3 f; 404 vec3_sub(f, center, eye); 405 vec3_norm(f, f); 406 407 vec3 s; 408 vec3_mul_cross(s, f, up); 409 vec3_norm(s, s); 410 411 vec3 t; 412 vec3_mul_cross(t, s, f); 413 414 m[0][0] = s[0]; 415 m[0][1] = t[0]; 416 m[0][2] = -f[0]; 417 m[0][3] = 0.f; 418 419 m[1][0] = s[1]; 420 m[1][1] = t[1]; 421 m[1][2] = -f[1]; 422 m[1][3] = 0.f; 423 424 m[2][0] = s[2]; 425 m[2][1] = t[2]; 426 m[2][2] = -f[2]; 427 m[2][3] = 0.f; 428 429 m[3][0] = 0.f; 430 m[3][1] = 0.f; 431 m[3][2] = 0.f; 432 m[3][3] = 1.f; 433 434 mat4x4_translate_in_place(m, -eye[0], -eye[1], -eye[2]); 435 } 436 437 typedef float quat[4]; 438 static inline void quat_identity(quat q) { 439 q[0] = q[1] = q[2] = 0.f; 440 q[3] = 1.f; 441 } 442 static inline void quat_add(quat r, quat a, quat b) { 443 int i; 444 for (i = 0; i < 4; ++i) 445 r[i] = a[i] + b[i]; 446 } 447 static inline void quat_sub(quat r, quat a, quat b) { 448 int i; 449 for (i = 0; i < 4; ++i) 450 r[i] = a[i] - b[i]; 451 } 452 static inline void quat_mul(quat r, quat p, quat q) { 453 vec3 w; 454 vec3_mul_cross(r, p, q); 455 vec3_scale(w, p, q[3]); 456 vec3_add(r, r, w); 457 vec3_scale(w, q, p[3]); 458 vec3_add(r, r, w); 459 r[3] = p[3] * q[3] - vec3_mul_inner(p, q); 460 } 461 static inline void quat_scale(quat r, quat v, float s) { 462 int i; 463 for (i = 0; i < 4; ++i) 464 r[i] = v[i] * s; 465 } 466 static inline float quat_inner_product(quat a, quat b) { 467 float p = 0.f; 468 int i; 469 for (i = 0; i < 4; ++i) 470 p += b[i] * a[i]; 471 return p; 472 } 473 static inline void quat_conj(quat r, quat q) { 474 int i; 475 for (i = 0; i < 3; ++i) 476 r[i] = -q[i]; 477 r[3] = q[3]; 478 } 479 #define quat_norm vec4_norm 480 static inline void quat_mul_vec3(vec3 r, quat q, vec3 v) { 481 quat v_ = {v[0], v[1], v[2], 0.f}; 482 483 quat_conj(r, q); 484 quat_norm(r, r); 485 quat_mul(r, v_, r); 486 quat_mul(r, q, r); 487 } 488 static inline void mat4x4_from_quat(mat4x4 M, quat q) { 489 float a = q[3]; 490 float b = q[0]; 491 float c = q[1]; 492 float d = q[2]; 493 float a2 = a * a; 494 float b2 = b * b; 495 float c2 = c * c; 496 float d2 = d * d; 497 498 M[0][0] = a2 + b2 - c2 - d2; 499 M[0][1] = 2.f * (b * c + a * d); 500 M[0][2] = 2.f * (b * d - a * c); 501 M[0][3] = 0.f; 502 503 M[1][0] = 2 * (b * c - a * d); 504 M[1][1] = a2 - b2 + c2 - d2; 505 M[1][2] = 2.f * (c * d + a * b); 506 M[1][3] = 0.f; 507 508 M[2][0] = 2.f * (b * d + a * c); 509 M[2][1] = 2.f * (c * d - a * b); 510 M[2][2] = a2 - b2 - c2 + d2; 511 M[2][3] = 0.f; 512 513 M[3][0] = M[3][1] = M[3][2] = 0.f; 514 M[3][3] = 1.f; 515 } 516 517 static inline void mat4x4o_mul_quat(mat4x4 R, mat4x4 M, quat q) { 518 /* XXX: The way this is written only works for othogonal matrices. */ 519 /* TODO: Take care of non-orthogonal case. */ 520 quat_mul_vec3(R[0], q, M[0]); 521 quat_mul_vec3(R[1], q, M[1]); 522 quat_mul_vec3(R[2], q, M[2]); 523 524 R[3][0] = R[3][1] = R[3][2] = 0.f; 525 R[3][3] = 1.f; 526 } 527 static inline void quat_from_mat4x4(quat q, mat4x4 M) { 528 float r = 0.f; 529 int i; 530 531 int perm[] = {0, 1, 2, 0, 1}; 532 int *p = perm; 533 534 for (i = 0; i < 3; i++) { 535 float m = M[i][i]; 536 if (m < r) 537 continue; 538 m = r; 539 p = &perm[i]; 540 } 541 542 r = sqrtf(1.f + M[p[0]][p[0]] - M[p[1]][p[1]] - M[p[2]][p[2]]); 543 544 if (r < 1e-6) { 545 q[0] = 1.f; 546 q[1] = q[2] = q[3] = 0.f; 547 return; 548 } 549 550 q[0] = r / 2.f; 551 q[1] = (M[p[0]][p[1]] - M[p[1]][p[0]]) / (2.f * r); 552 q[2] = (M[p[2]][p[0]] - M[p[0]][p[2]]) / (2.f * r); 553 q[3] = (M[p[2]][p[1]] - M[p[1]][p[2]]) / (2.f * r); 554 } 555 556 #endif 557
