LibGame  v0.4.0
The LG Game Engine - Copyright (C) 2024 ETMSoftware
math_3d.h
Go to the documentation of this file.
1 
18 /*
19 Math 3D v1.0
20 By Stephan Soller <[email protected]> and Tobias Malmsheimer
21 Licensed under the MIT license
22 
23 Math 3D is a compact C99 library meant to be used with OpenGL. It provides basic
24 3D vector and 4x4 matrix operations as well as functions to create transformation
25 and projection matrices. The OpenGL binary layout is used so you can just upload
26 vectors and matrices into shaders and work with them without any conversions.
27 
28 It's an stb style single header file library. Define MATH_3D_IMPLEMENTATION
29 before you include this file in *one* C file to create the implementation.
30 
31 
32 QUICK NOTES
33 
34 - If not explicitly stated by a parameter name all angles are in radians.
35 - The matrices use column-major indices. This is the same as in OpenGL and GLSL.
36  The matrix documentation below for details.
37 - Matrices are passed by value. This is probably a bit inefficient but
38  simplifies code quite a bit. Most operations will be inlined by the compiler
39  anyway so the difference shouldn't matter that much. A matrix fits into 4 of
40  the 16 SSE2 registers anyway. If profiling shows significant slowdowns the
41  matrix type might change but ease of use is more important than every last
42  percent of performance.
43 - When combining matrices with multiplication the effects apply right to left.
44  This is the convention used in mathematics and OpenGL. Source:
45  https://en.wikipedia.org/wiki/Transformation_matrix#Composing_and_inverting_transformations
46  Direct3D does it differently.
47 - The `m4_mul_pos()` and `m4_mul_dir()` functions do a correct perspective
48  divide (division by w) when necessary. This is a bit slower but ensures that
49  the functions will properly work with projection matrices. If profiling shows
50  this is a bottleneck special functions without perspective division can be
51  added. But the normal multiplications should avoid any surprises.
52 - The library consistently uses a right-handed coordinate system. The old
53  `glOrtho()` broke that rule and `m4_ortho()` has be slightly modified so you
54  can always think of right-handed cubes that are projected into OpenGLs
55  normalized device coordinates.
56 - Special care has been taken to document all complex operations and important
57  sources. Most code is covered by test cases that have been manually calculated
58  and checked on the whiteboard. Since indices and math code is prone to be
59  confusing we used pair programming to avoid mistakes.
60 
61 
62 FURTHER IDEAS
63 
64 These are ideas for future work on the library. They're implemented as soon as
65 there is a proper use case and we can find good names for them.
66 
67 - bool v3_is_null(vec3_t v, float epsilon)
68  To check if the length of a vector is smaller than `epsilon`.
69 - vec3_t v3_length_default(vec3_t v, float default_length, float epsilon)
70  Returns `default_length` if the length of `v` is smaller than `epsilon`.
71  Otherwise same as `v3_length()`.
72 - vec3_t v3_norm_default(vec3_t v, vec3_t default_vector, float epsilon)
73  Returns `default_vector` if the length of `v` is smaller than `epsilon`.
74  Otherwise the same as `v3_norm()`.
75 - mat4_t m4_invert(mat4_t matrix)
76  Matrix inversion that works with arbitrary matrices. `m4_invert_affine()` can
77  already invert translation, rotation, scaling, mirroring, reflection and
78  shearing matrices. So a general inversion might only be useful to invert
79  projection matrices for picking. But with orthographic and perspective
80  projection it's probably simpler to calculate the ray into the scene directly
81  based on the screen coordinates.
82 
83 
84 VERSION HISTORY
85 
86 v1.0 2016-02-15 Initial release
87 
88 */
89 
90 #ifndef MATH_3D_HEADER
91 #define MATH_3D_HEADER
92 
93 // Define PI directly because we would need to define the _BSD_SOURCE or
94 // _XOPEN_SOURCE feature test macros to get it from math.h. That would be a
95 // rather harsh dependency. So we define it directly if necessary.
96 #ifndef M_PI
97 #define M_PI 3.14159265358979323846
98 #endif
99 
100 
101 //
102 // 3D vectors
103 //
104 // Use the `vec3()` function to create vectors. All other vector functions start
105 // with the `v3_` prefix.
106 //
107 // The binary layout is the same as in GLSL and everything else (just 3 floats).
108 // So you can just upload the vectors into shaders as they are.
109 //
110 
111 typedef struct {float x, y, z; } vec3_t;
112 static inline vec3_t vec3(float x, float y, float z) {return (vec3_t){x, y, z };}
113 
114 static inline vec3_t v3_add (vec3_t a, vec3_t b) {return (vec3_t){a.x + b.x, a.y + b.y, a.z + b.z };}
115 static inline vec3_t v3_adds (vec3_t a, float s) {return (vec3_t){a.x + s, a.y + s, a.z + s };}
116 static inline vec3_t v3_sub (vec3_t a, vec3_t b) {return (vec3_t){a.x - b.x, a.y - b.y, a.z - b.z };}
117 static inline vec3_t v3_subs (vec3_t a, float s) {return (vec3_t){a.x - s, a.y - s, a.z - s };}
118 static inline vec3_t v3_mul (vec3_t a, vec3_t b) {return (vec3_t){a.x * b.x, a.y * b.y, a.z * b.z };}
119 static inline vec3_t v3_muls (vec3_t a, float s) {return (vec3_t){a.x * s, a.y * s, a.z * s };}
120 static inline vec3_t v3_div (vec3_t a, vec3_t b) {return (vec3_t){a.x / b.x, a.y / b.y, a.z / b.z };}
121 static inline vec3_t v3_divs (vec3_t a, float s) {return (vec3_t){a.x / s, a.y / s, a.z / s };}
122 static inline float v3_length(vec3_t v) {return sqrtf(v.x * v.x + v.y * v.y + v.z * v.z);}
123 static inline vec3_t v3_norm (vec3_t v);
124 static inline float v3_dot (vec3_t a, vec3_t b) {return a.x * b.x + a.y * b.y + a.z * b.z;}
125 static inline vec3_t v3_proj (vec3_t v, vec3_t onto);
126 static inline vec3_t v3_cross (vec3_t a, vec3_t b);
127 static inline float v3_angle_between(vec3_t a, vec3_t b);
128 
129 
130 //
131 // 4x4 matrices
132 //
133 // Use the `mat4()` function to create a matrix. You can write the matrix
134 // members in the same way as you would write them on paper or on a whiteboard:
135 //
136 // mat4_t m = mat4(
137 // 1, 0, 0, 7,
138 // 0, 1, 0, 5,
139 // 0, 0, 1, 3,
140 // 0, 0, 0, 1
141 // )
142 //
143 // This creates a matrix that translates points by vec3(7, 5, 3). All other
144 // matrix functions start with the `m4_` prefix. Among them functions to create
145 // identity, translation, rotation, scaling and projection matrices.
146 //
147 // The matrix is stored in column-major order, just as OpenGL expects. Members
148 // can be accessed by indices or member names. When you write a matrix on paper
149 // or on the whiteboard the indices and named members correspond to these
150 // positions:
151 //
152 // | m[0][0] m[1][0] m[2][0] m[3][0] |
153 // | m[0][1] m[1][1] m[2][1] m[3][1] |
154 // | m[0][2] m[1][2] m[2][2] m[3][2] |
155 // | m[0][3] m[1][3] m[2][3] m[3][3] |
156 //
157 // | m00 m10 m20 m30 |
158 // | m01 m11 m21 m31 |
159 // | m02 m12 m22 m32 |
160 // | m03 m13 m23 m33 |
161 //
162 // The first index or number in a name denotes the column, the second the row.
163 // So m[i][j] denotes the member in the ith column and the jth row. This is the
164 // same as in GLSL (source: GLSL v1.3 specification, 5.6 Matrix Components).
165 //
166 
167 typedef union {
168  // The first index is the column index, the second the row index. The memory
169  // layout of nested arrays in C matches the memory layout expected by OpenGL.
170  float m[4][4];
171  // OpenGL expects the first 4 floats to be the first column of the matrix.
172  // So we need to define the named members column by column for the names to
173  // match the memory locations of the array elements.
174  struct {
175  float m00, m01, m02, m03;
176  float m10, m11, m12, m13;
177  float m20, m21, m22, m23;
178  float m30, m31, m32, m33;
179  };
180 } mat4_t;
181 
182 static inline mat4_t mat4(
183  float m00, float m10, float m20, float m30,
184  float m01, float m11, float m21, float m31,
185  float m02, float m12, float m22, float m32,
186  float m03, float m13, float m23, float m33
187 );
188 
189 static inline mat4_t m4_identity ();
190 static inline mat4_t m4_translation (vec3_t offset);
191 static inline mat4_t m4_scaling (vec3_t scale);
192 static inline mat4_t m4_rotation_x (float angle_in_rad);
193 static inline mat4_t m4_rotation_y (float angle_in_rad);
194 static inline mat4_t m4_rotation_z (float angle_in_rad);
195  mat4_t m4_rotation (float angle_in_rad, vec3_t axis);
196 
197  mat4_t m4_ortho (float left, float right, float bottom, float top, float back, float front);
198  mat4_t m4_perspective (float vertical_field_of_view_in_deg, float aspect_ratio, float near_view_distance, float far_view_distance);
199  mat4_t m4_frustum (float left, float right, float bottom, float top, float near, float far);
200  mat4_t m4_look_at (vec3_t from, vec3_t to, vec3_t up);
201 
202 static inline mat4_t m4_transpose (mat4_t matrix);
203 static inline mat4_t m4_mul (mat4_t a, mat4_t b);
204  mat4_t m4_invert_affine(mat4_t matrix);
205  vec3_t m4_mul_pos (mat4_t matrix, vec3_t position);
206  vec3_t m4_mul_dir (mat4_t matrix, vec3_t direction);
207 
208  void m4_print (mat4_t matrix);
209  void m4_printp (mat4_t matrix, int width, int precision);
210  void m4_fprint (FILE* stream, mat4_t matrix);
211  void m4_fprintp (FILE* stream, mat4_t matrix, int width, int precision);
212 
213  /* New stuff here */
214  void m4_print2 (mat4_t matrix, const char *line_start);
215  void m4_printp2 (mat4_t matrix, int width, int precision, const char *line_start);
216  void m4_fprint2 (FILE* stream, mat4_t matrix, const char *line_start);
217  void m4_fprintp2 (FILE* stream, mat4_t matrix, int width, int precision, const char *line_start);
218 
219 
220 //
221 // 3D vector functions header implementation
222 //
223 
224 static inline vec3_t v3_norm(vec3_t v)
225 {
226  float len = v3_length(v);
227  if (len > 0) {
228  return (vec3_t){v.x / len, v.y / len, v.z / len};
229  } else {
230  return (vec3_t){0, 0, 0};
231  }
232 }
233 
234 static inline vec3_t v3_proj(vec3_t v, vec3_t onto)
235 {
236  return v3_muls(onto, v3_dot(v, onto) / v3_dot(onto, onto));
237 }
238 
239 static inline vec3_t v3_cross(vec3_t a, vec3_t b)
240 {
241  return (vec3_t){
242  a.y * b.z - a.z * b.y,
243  a.z * b.x - a.x * b.z,
244  a.x * b.y - a.y * b.x
245  };
246 }
247 
248 static inline float v3_angle_between(vec3_t a, vec3_t b)
249 {
250  return acosf(v3_dot(a, b) / (v3_length(a) * v3_length(b)));
251 }
252 
253 //
254 // Matrix functions header implementation
255 //
256 
257 static inline mat4_t mat4(
258  float m00, float m10, float m20, float m30,
259  float m01, float m11, float m21, float m31,
260  float m02, float m12, float m22, float m32,
261  float m03, float m13, float m23, float m33
262 )
263 {
264  return (mat4_t){
265  .m[0][0] = m00, .m[1][0] = m10, .m[2][0] = m20, .m[3][0] = m30,
266  .m[0][1] = m01, .m[1][1] = m11, .m[2][1] = m21, .m[3][1] = m31,
267  .m[0][2] = m02, .m[1][2] = m12, .m[2][2] = m22, .m[3][2] = m32,
268  .m[0][3] = m03, .m[1][3] = m13, .m[2][3] = m23, .m[3][3] = m33
269  };
270 }
271 
272 static inline mat4_t m4_identity()
273 {
274  return mat4(
275  1, 0, 0, 0,
276  0, 1, 0, 0,
277  0, 0, 1, 0,
278  0, 0, 0, 1
279  );
280 }
281 
282 static inline mat4_t m4_translation(vec3_t offset)
283 {
284  return mat4(
285  1, 0, 0, offset.x,
286  0, 1, 0, offset.y,
287  0, 0, 1, offset.z,
288  0, 0, 0, 1
289  );
290 }
291 
292 static inline mat4_t m4_scaling(vec3_t scale)
293 {
294  float x = scale.x, y = scale.y, z = scale.z;
295  return mat4(
296  x, 0, 0, 0,
297  0, y, 0, 0,
298  0, 0, z, 0,
299  0, 0, 0, 1
300  );
301 }
302 
303 static inline mat4_t m4_rotation_x(float angle_in_rad)
304 {
305  float s = sinf(angle_in_rad), c = cosf(angle_in_rad);
306  return mat4(
307  1, 0, 0, 0,
308  0, c, -s, 0,
309  0, s, c, 0,
310  0, 0, 0, 1
311  );
312 }
313 
314 static inline mat4_t m4_rotation_y(float angle_in_rad)
315 {
316  float s = sinf(angle_in_rad), c = cosf(angle_in_rad);
317  return mat4(
318  c, 0, s, 0,
319  0, 1, 0, 0,
320  -s, 0, c, 0,
321  0, 0, 0, 1
322  );
323 }
324 
325 static inline mat4_t m4_rotation_z(float angle_in_rad)
326 {
327  float s = sinf(angle_in_rad), c = cosf(angle_in_rad);
328  return mat4(
329  c, -s, 0, 0,
330  s, c, 0, 0,
331  0, 0, 1, 0,
332  0, 0, 0, 1
333  );
334 }
335 
336 static inline mat4_t m4_transpose(mat4_t matrix)
337 {
338  return mat4(
339  matrix.m00, matrix.m01, matrix.m02, matrix.m03,
340  matrix.m10, matrix.m11, matrix.m12, matrix.m13,
341  matrix.m20, matrix.m21, matrix.m22, matrix.m23,
342  matrix.m30, matrix.m31, matrix.m32, matrix.m33
343  );
344 }
345 
346 /*
347  * Multiplication of two 4x4 matrices.
348  *
349  * Implemented by following the row times column rule and illustrating it on a
350  * whiteboard with the proper indices in mind.
351  *
352  * Further reading: https://en.wikipedia.org/wiki/Matrix_multiplication
353  * But note that the article use the first index for rows and the second for
354  * columns.
355  */
356 static inline mat4_t m4_mul(mat4_t a, mat4_t b)
357 {
358  mat4_t result;
359  int i, j, k;
360 
361  for (i = 0; i < 4; i++) {
362  for (j = 0; j < 4; j++) {
363  float sum = 0;
364  for (k = 0; k < 4; k++) {
365  sum += a.m[k][j] * b.m[i][k];
366  }
367  result.m[i][j] = sum;
368  }
369  }
370 
371  return result;
372 }
373 
374 #endif // MATH_3D_HEADER
375 
376 
377 #ifdef MATH_3D_IMPLEMENTATION
378 
379 /*
380  * Creates a matrix to rotate around an axis by a given angle. The axis doesn't
381  * need to be normalized.
382  *
383  * Sources:
384  *
385  * https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
386  */
387 mat4_t m4_rotation(float angle_in_rad, vec3_t axis)
388 {
389  vec3_t normalized_axis = v3_norm(axis);
390  float x = normalized_axis.x, y = normalized_axis.y, z = normalized_axis.z;
391  float c = cosf(angle_in_rad), s = sinf(angle_in_rad);
392 
393  return mat4(
394  c + x * x * (1 - c), x * y * (1 - c) - z * s, x * z * (1 - c) + y * s, 0,
395  y * x * (1 - c) + z * s, c + y * y * (1 - c), y * z * (1 - c) - x * s, 0,
396  z * x * (1 - c) - y * s, z * y * (1 - c) + x * s, c + z * z * (1 - c), 0,
397  0, 0, 0, 1
398  );
399 }
400 
401 /*
402  * Creates an orthographic projection matrix. It maps the right handed cube
403  * defined by left, right, bottom, top, back and front onto the screen and
404  * z-buffer. You can think of it as a cube you move through world or camera
405  * space and everything inside is visible.
406  *
407  * This is slightly different from the traditional glOrtho() and from the linked
408  * sources. These functions require the user to negate the last two arguments
409  * (creating a left-handed coordinate system). We avoid that here so you can
410  * think of this function as moving a right-handed cube through world space.
411  *
412  * The arguments are ordered in a way that for each axis you specify the minimum
413  * followed by the maximum. Thats why it's bottom to top and back to front.
414  *
415  * Implementation details:
416  *
417  * To be more exact the right-handed cube is mapped into normalized device
418  * coordinates, a left-handed cube where (-1 -1) is the lower left corner,
419  * (1, 1) the upper right corner and a z-value of -1 is the nearest point and
420  * 1 the furthest point. OpenGL takes it from there and puts it on the screen
421  * and into the z-buffer.
422  *
423  * Sources:
424  *
425  * https://msdn.microsoft.com/en-us/library/windows/desktop/dd373965(v=vs.85).aspx
426  * https://unspecified.wordpress.com/2012/06/21/calculating-the-gluperspective-matrix-and-other-opengl-matrix-maths/
427  */
428 mat4_t m4_ortho(float left, float right, float bottom, float top, float back, float front)
429 {
430  float l = left, r = right, b = bottom, t = top, n = front, f = back;
431  float tx = -(r + l) / (r - l);
432  float ty = -(t + b) / (t - b);
433  float tz = -(f + n) / (f - n);
434 
435  return mat4(
436  2 / (r - l), 0, 0, tx,
437  0, 2 / (t - b), 0, ty,
438  0, 0, 2 / (f - n), tz,
439  0, 0, 0, 1
440  );
441 }
442 
443 /*
444  * Creates a perspective projection matrix for a camera.
445  *
446  * The camera is at the origin and looks in the direction of the negative Z axis.
447  * `near_view_distance` and `far_view_distance` have to be positive and > 0.
448  * They are distances from the camera eye, not values on an axis.
449  *
450  * `near_view_distance` can be small but not 0. 0 breaks the projection and
451  * everything ends up at the max value (far end) of the z-buffer. Making the
452  * z-buffer useless.
453  *
454  * The matrix is the same as `gluPerspective()` builds. The view distance is
455  * mapped to the z-buffer with a reciprocal function (1/x). Therefore the z-buffer
456  * resolution for near objects is very good while resolution for far objects is
457  * limited.
458  *
459  * Sources:
460  *
461  * https://unspecified.wordpress.com/2012/06/21/calculating-the-gluperspective-matrix-and-other-opengl-matrix-maths/
462  */
463 mat4_t m4_perspective(float vertical_field_of_view_in_deg, float aspect_ratio, float near_view_distance, float far_view_distance)
464 {
465  float fovy_in_rad = vertical_field_of_view_in_deg / 180 * M_PI;
466  float f = 1.0f / tanf(fovy_in_rad / 2.0f);
467  float ar = aspect_ratio;
468  float nd = near_view_distance, fd = far_view_distance;
469 
470  return mat4(
471  f / ar, 0, 0, 0,
472  0, f, 0, 0,
473  0, 0, (fd + nd) / (nd - fd), (2 * fd * nd) / (nd - fd),
474  0, 0, -1, 0
475  );
476 }
477 
478 /*
479  * Some extra stuff by Emmanuel Thomas-Maurin
480  *
481  * Frustum matrix
482  * Similar to Android OpenGL ES 2.0 frustum matrix:
483  * https://developer.android.com/reference/android/opengl/Matrix#frustumM(float[],%20int,%20float,%20float,%20float,%20float,%20float,%20float)
484  *
485  * NOT USED SO FAR
486  */
487 mat4_t m4_frustum(float left, float right, float bottom, float top, float near, float far)
488 {
489  /* Too much checking here ? */
490  if (fabs(left - right) < LG_FLOAT_EPSILON) {
491  INFO_ERR("left == right\n")
492  } else if (fabs(bottom - top) < LG_FLOAT_EPSILON) {
493  INFO_ERR("bottom == top\n")
494  } else if (far <= near) {
495  INFO_ERR("far <= near\n")
496  } else if (near <= 0.0f) {
497  INFO_ERR("near <= 0.0f\n")
498  } else if (far <= 0.0f) {
499  INFO_ERR("far <= 0.0f\n")
500  }
501 
502  float width = right - left;
503  float height = top - bottom;
504  float depth = far - near;
505  float x = (2.0 * near) / width;
506  float y = (2.0 * near) / height;
507  float A = (right + left) / width;
508  float B = (top + bottom) / height;
509  float C = - (far + near) / depth;
510  float D = - (2.0 * far * near) / depth;
511 
512  return mat4(
513  x, 0.0, A, 0.0,
514  0.0, y, B, 0.0,
515  0.0, 0.0, C, D,
516  0.0, 0.0, -1.0, 0.0
517  );
518 }
519 /* (until here) */
520 
521 /*
522  * Builds a transformation matrix for a camera that looks from `from` towards
523  * `to`. `up` defines the direction that's upwards for the camera. All three
524  * vectors are given in world space and `up` doesn't need to be normalized.
525  *
526  * Sources: Derived on whiteboard.
527  *
528  * Implementation details:
529  *
530  * x, y and z are the right-handed base vectors of the cameras subspace.
531  * x has to be normalized because the cross product only produces a normalized
532  * output vector if both input vectors are orthogonal to each other. And up
533  * probably isn't orthogonal to z.
534  *
535  * These vectors are then used to build a 3x3 rotation matrix. This matrix
536  * rotates a vector by the same amount the camera is rotated. But instead we
537  * need to rotate all incoming vertices backwards by that amount. That's what a
538  * camera matrix is for: To move the world so that the camera is in the origin.
539  * So we take the inverse of that rotation matrix and in case of a rotation
540  * matrix this is just the transposed matrix. That's why the 3x3 part of the
541  * matrix are the x, y and z vectors but written horizontally instead of
542  * vertically.
543  *
544  * The translation is derived by creating a translation matrix to move the world
545  * into the origin (thats translate by minus `from`). The complete lookat matrix
546  * is then this translation followed by the rotation. Written as matrix
547  * multiplication:
548  *
549  * lookat = rotation * translation
550  *
551  * Since we're right-handed this equals to first doing the translation and after
552  * that doing the rotation. During that multiplication the rotation 3x3 part
553  * doesn't change but the translation vector is multiplied with each rotation
554  * axis. The dot product is just a more compact way to write the actual
555  * multiplications.
556  */
557 mat4_t m4_look_at(vec3_t from, vec3_t to, vec3_t up)
558 {
559  vec3_t z = v3_muls(v3_norm(v3_sub(to, from)), -1);
560  vec3_t x = v3_norm(v3_cross(up, z));
561  vec3_t y = v3_cross(z, x);
562 
563  return mat4(
564  x.x, x.y, x.z, -v3_dot(from, x),
565  y.x, y.y, y.z, -v3_dot(from, y),
566  z.x, z.y, z.z, -v3_dot(from, z),
567  0, 0, 0, 1
568  );
569 }
570 
571 /*
572  * Inverts an affine transformation matrix. That are translation, scaling,
573  * mirroring, reflection, rotation and shearing matrices or any combination of
574  * them.
575  *
576  * Implementation details:
577  *
578  * - Invert the 3x3 part of the 4x4 matrix to handle rotation, scaling, etc.
579  * correctly (see source).
580  * - Invert the translation part of the 4x4 matrix by multiplying it with the
581  * inverted rotation matrix and negating it.
582  *
583  * When a 3D point is multiplied with a transformation matrix it is first
584  * rotated and then translated. The inverted transformation matrix is the
585  * inverse translation followed by the inverse rotation. Written as a matrix
586  * multiplication (remember, the effect applies right to left):
587  *
588  * inv(matrix) = inv(rotation) * inv(translation)
589  *
590  * The inverse translation is a translation into the opposite direction, just
591  * the negative translation. The rotation part isn't changed by that
592  * multiplication but the translation part is multiplied by the inverse rotation
593  * matrix. It's the same situation as with `m4_look_at()`. But since we don't
594  * store the rotation matrix as 3D vectors we can't use the dot product and have
595  * to write the matrix multiplication operations by hand.
596  *
597  * Sources for 3x3 matrix inversion:
598  *
599  * https://www.khanacademy.org/math/precalculus/precalc-matrices/determinants-and-inverses-of-large-matrices/v/inverting-3x3-part-2-determinant-and-adjugate-of-a-matrix
600  */
601 mat4_t m4_invert_affine(mat4_t matrix)
602 {
603  // Create shorthands to access matrix members
604  float m00 = matrix.m00, m10 = matrix.m10, m20 = matrix.m20, m30 = matrix.m30;
605  float m01 = matrix.m01, m11 = matrix.m11, m21 = matrix.m21, m31 = matrix.m31;
606  float m02 = matrix.m02, m12 = matrix.m12, m22 = matrix.m22, m32 = matrix.m32;
607 
608  // Invert 3x3 part of the 4x4 matrix that contains the rotation, etc.
609  // That part is called R from here on.
610 
611  // Calculate cofactor matrix of R
612  float c00 = m11 * m22 - m12 * m21, c10 = -(m01 * m22 - m02 * m21), c20 = m01 * m12 - m02 * m11;
613  float c01 = -(m10 * m22 - m12 * m20), c11 = m00 * m22 - m02 * m20, c21 = -(m00 * m12 - m02 * m10);
614  float c02 = m10 * m21 - m11 * m20, c12 = -(m00 * m21 - m01 * m20), c22 = m00 * m11 - m01 * m10;
615 
616  // Caclculate the determinant by using the already calculated determinants
617  // in the cofactor matrix.
618  // Second sign is already minus from the cofactor matrix.
619  float det = m00 * c00 + m10 * c10 + m20 * c20;
620  if (fabsf(det) < 0.00001) {
621  return m4_identity();
622  }
623 
624  // Calcuate inverse of R by dividing the transposed cofactor matrix by the
625  // determinant.
626  float i00 = c00 / det, i10 = c01 / det, i20 = c02 / det;
627  float i01 = c10 / det, i11 = c11 / det, i21 = c12 / det;
628  float i02 = c20 / det, i12 = c21 / det, i22 = c22 / det;
629 
630  // Combine the inverted R with the inverted translation
631  return mat4(
632  i00, i10, i20, -(i00 * m30 + i10 * m31 + i20 * m32),
633  i01, i11, i21, -(i01 * m30 + i11 * m31 + i21 * m32),
634  i02, i12, i22, -(i02 * m30 + i12 * m31 + i22 * m32),
635  0, 0, 0, 1
636  );
637 }
638 
639 /*
640  * Multiplies a 4x4 matrix with a 3D vector representing a point in 3D space.
641  *
642  * Before the matrix multiplication the vector is first expanded to a 4D vector
643  * (x, y, z, 1). After the multiplication the vector is reduced to 3D again by
644  * dividing through the 4th component (if it's not 0 or 1).
645  */
646 vec3_t m4_mul_pos(mat4_t matrix, vec3_t position)
647 {
648  vec3_t result = vec3(
649  matrix.m00 * position.x + matrix.m10 * position.y + matrix.m20 * position.z + matrix.m30,
650  matrix.m01 * position.x + matrix.m11 * position.y + matrix.m21 * position.z + matrix.m31,
651  matrix.m02 * position.x + matrix.m12 * position.y + matrix.m22 * position.z + matrix.m32
652  );
653 
654  float w = matrix.m03 * position.x + matrix.m13 * position.y + matrix.m23 * position.z + matrix.m33;
655  if (w != 0 && w != 1) {
656  return vec3(result.x / w, result.y / w, result.z / w);
657  }
658 
659  return result;
660 }
661 
662 /*
663  * Multiplies a 4x4 matrix with a 3D vector representing a direction in 3D space.
664  *
665  * Before the matrix multiplication the vector is first expanded to a 4D vector
666  * (x, y, z, 0). For directions the 4th component is set to 0 because directions
667  * are only rotated, not translated. After the multiplication the vector is
668  * reduced to 3D again by dividing through the 4th component (if it's not 0 or
669  * 1). This is necessary because the matrix might contains something other than
670  * (0, 0, 0, 1) in the bottom row which might set w to something other than 0
671  * or 1.
672  */
673 vec3_t m4_mul_dir(mat4_t matrix, vec3_t direction)
674 {
675  vec3_t result = vec3(
676  matrix.m00 * direction.x + matrix.m10 * direction.y + matrix.m20 * direction.z,
677  matrix.m01 * direction.x + matrix.m11 * direction.y + matrix.m21 * direction.z,
678  matrix.m02 * direction.x + matrix.m12 * direction.y + matrix.m22 * direction.z
679  );
680 
681  float w = matrix.m03 * direction.x + matrix.m13 * direction.y + matrix.m23 * direction.z;
682  if (w != 0 && w != 1) {
683  return vec3(result.x / w, result.y / w, result.z / w);
684  }
685 
686  return result;
687 }
688 
689 void m4_print(mat4_t matrix)
690 {
691  m4_fprintp(STD_OUT, matrix, 6, 2);
692 }
693 
694 void m4_printp(mat4_t matrix, int width, int precision)
695 {
696  m4_fprintp(STD_OUT, matrix, width, precision);
697 }
698 
699 void m4_fprint(FILE* stream, mat4_t matrix)
700 {
701  m4_fprintp(stream, matrix, 6, 2);
702 }
703 
704 void m4_fprintp(FILE* stream, mat4_t matrix, int width, int precision)
705 {
706  mat4_t m = matrix;
707  int w = width, p = precision, r;
708 
709  for (r = 0; r < 4; r++) {
710  fprintf(stream, "| %*.*f %*.*f %*.*f %*.*f |\n",
711  w, p, m.m[0][r], w, p, m.m[1][r], w, p, m.m[2][r], w, p, m.m[3][r]
712  );
713  }
714 }
715 
716 /* New stuff here */
717 void m4_print2(mat4_t matrix, const char *line_start)
718 {
719  m4_fprintp2(STD_OUT, matrix, 6, 2, line_start);
720 }
721 
722 void m4_printp2(mat4_t matrix, int width, int precision, const char *line_start)
723 {
724  m4_fprintp2(STD_OUT, matrix, width, precision, line_start);
725 }
726 
727 void m4_fprint2(FILE* stream, mat4_t matrix, const char *line_start)
728 {
729  m4_fprintp2(stream, matrix, 6, 2, line_start);
730 }
731 void m4_fprintp2(FILE* stream, mat4_t matrix, int width, int precision, const char *line_start)
732 {
733  mat4_t m = matrix;
734  int w = width, p = precision, r;
735 
736  for (r = 0; r < 4; r++) {
737  fprintf(stream, "%s| %*.*f %*.*f %*.*f %*.*f |\n",
738  line_start, w, p, m.m[0][r], w, p, m.m[1][r], w, p, m.m[2][r], w, p, m.m[3][r]
739  );
740  }
741 }
742 
743 #endif // MATH_3D_IMPLEMENTATION
vec3_t
Definition: math_3d.h:111
mat4_t
Definition: math_3d.h:167