linmath.h - lightweight vector and matrix math

SUBMITTED BY: Laka

DATE: May 19, 2016, 7:13 a.m.

FORMAT: C

SIZE: 13.5 kB

Raw Download

HITS: 1104

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