1 static inline void cgm_mcopy(float *dest, const float *src)
3 memcpy(dest, src, 16 * sizeof(float));
6 static inline void cgm_mzero(float *m)
12 static inline void cgm_midentity(float *m)
14 static float id[16] = {1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1};
18 static inline void cgm_mmul(float *a, const float *b)
27 *resptr++ = arow[0] * b[j] + arow[1] * b[4 + j] +
28 arow[2] * b[8 + j] + arow[3] * b[12 + j];
35 static inline void cgm_msubmatrix(float *m, int row, int col)
44 if(i == col) continue;
48 if(j == row) continue;
50 m[subi * 4 + subj++] = orig[i * 4 + j];
58 static inline void cgm_mupper3(float *m)
60 m[3] = m[7] = m[11] = m[12] = m[13] = m[14] = 0.0f;
64 static inline float cgm_msubdet(float *m, int row, int col)
66 cgm_msubmatrix(m, row, col);
70 static inline float cgm_mcofactor(float *m, int row, int col)
72 float min = cgm_msubdet(m, row, col);
73 return (row + col) & 1 ? -min : min;
76 static inline float cgm_mdet(float *m)
78 return m[0] * cgm_msubdet(m, 0, 0) - m[1] * cgm_msubdet(m, 0, 1) +
79 m[2] * cgm_msubdet(m, 0, 2) - m[3] * cgm_msubdet(m, 0, 3);
82 static inline void cgm_mtranspose(float *m)
96 static inline void cgm_mcofmatrix(float *m)
105 m[i * 4 + j] = cgm_mcofactor(tmp, i, j);
110 static inline int cgm_minverse(float *m)
115 float det = cgm_mdet(m);
116 if(det == 0.0f) return -1;
117 inv_det = 1.0f / det;
123 m[i * 4 + j] = cgm_mcofactor(tmp, j, i) * inv_det; /* transposed */
128 static inline void cgm_mtranslation(float *m, float x, float y, float z)
136 static inline void cgm_mscaling(float *m, float sx, float sy, float sz)
145 static inline void cgm_mrotation_x(float *m, float angle)
147 float sa = sin(angle);
148 float ca = cos(angle);
157 static inline void cgm_mrotation_y(float *m, float angle)
159 float sa = sin(angle);
160 float ca = cos(angle);
169 static inline void cgm_mrotation_z(float *m, float angle)
171 float sa = sin(angle);
172 float ca = cos(angle);
181 static inline void cgm_mrotation_axis(float *m, int idx, float angle)
185 cgm_mrotation_x(m, angle);
188 cgm_mrotation_y(m, angle);
191 cgm_mrotation_z(m, angle);
196 static inline void cgm_mrotation(float *m, float angle, float x, float y, float z)
198 float sa = sin(angle);
199 float ca = cos(angle);
200 float invca = 1.0f - ca;
208 m[0] = xsq + (1.0f - xsq) * ca;
209 m[4] = x * y * invca - z * sa;
210 m[8] = x * z * invca + y * sa;
212 m[1] = x * y * invca + z * sa;
213 m[5] = ysq + (1.0f - ysq) * ca;
214 m[9] = y * z * invca - x * sa;
216 m[2] = x * z * invca - y * sa;
217 m[6] = y * z * invca + x * sa;
218 m[10] = zsq + (1.0f - zsq) * ca;
221 static inline void cgm_mrotation_euler(float *m, float a, float b, float c, int mode)
223 /* this array must match the EulerMode enum */
224 static const int axis[][3] = {
225 {0, 1, 2}, {0, 2, 1},
226 {1, 0, 2}, {1, 2, 0},
227 {2, 0, 1}, {2, 1, 0},
228 {2, 0, 2}, {2, 1, 2},
229 {1, 0, 1}, {1, 2, 1},
233 float ma[16], mb[16];
234 cgm_mrotation_axis(ma, axis[mode][0], a);
235 cgm_mrotation_axis(mb, axis[mode][1], b);
236 cgm_mrotation_axis(m, axis[mode][2], c);
241 static inline void cgm_mtranslate(float *m, float x, float y, float z)
244 cgm_mtranslation(tm, x, y, z);
248 static inline void cgm_mscale(float *m, float sx, float sy, float sz)
251 cgm_mscaling(sm, sx, sy, sz);
255 static inline void cgm_mrotate_x(float *m, float angle)
258 cgm_mrotation_x(rm, angle);
262 static inline void cgm_mrotate_y(float *m, float angle)
265 cgm_mrotation_y(rm, angle);
269 static inline void cgm_mrotate_z(float *m, float angle)
272 cgm_mrotation_z(rm, angle);
276 static inline void cgm_mrotate_axis(float *m, int idx, float angle)
279 cgm_mrotation_axis(rm, idx, angle);
283 static inline void cgm_mrotate(float *m, float angle, float x, float y, float z)
286 cgm_mrotation(rm, angle, x, y, z);
290 static inline void cgm_mrotate_euler(float *m, float a, float b, float c, int mode)
293 cgm_mrotation_euler(rm, a, b, c, mode);
298 static inline void cgm_mpretranslate(float *m, float x, float y, float z)
302 cgm_mtranslation(m, x, y, z);
306 static inline void cgm_mprescale(float *m, float sx, float sy, float sz)
310 cgm_mscaling(m, sx, sy, sz);
314 static inline void cgm_mprerotate_x(float *m, float angle)
318 cgm_mrotation_x(m, angle);
322 static inline void cgm_mprerotate_y(float *m, float angle)
326 cgm_mrotation_y(m, angle);
330 static inline void cgm_mprerotate_z(float *m, float angle)
334 cgm_mrotation_z(m, angle);
338 static inline void cgm_mprerotate_axis(float *m, int idx, float angle)
342 cgm_mrotation_axis(m, idx, angle);
346 static inline void cgm_mprerotate(float *m, float angle, float x, float y, float z)
350 cgm_mrotation(m, angle, x, y, z);
354 static inline void cgm_mprerotate_euler(float *m, float a, float b, float c, int mode)
358 cgm_mrotation_euler(m, a, b, c, mode);
363 static inline void cgm_mget_translation(const float *m, cgm_vec3 *res)
370 /* Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
371 * article "Quaternion Calculus and Fast Animation".
372 * adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl
374 static inline void cgm_mget_rotation(const float *m, cgm_quat *res)
376 static const int next[3] = {1, 2, 0};
380 float trace = m[0] + m[5] + m[10];
385 root = sqrt(trace + 1.0f); // 2w
386 res->w = 0.5f * root;
387 root = 0.5f / root; // 1 / 4w
388 res->x = (m[6] - m[9]) * root;
389 res->y = (m[8] - m[2]) * root;
390 res->z = (m[1] - m[4]) * root;
397 if(m[10] > m[i * 4 + i]) {
403 root = sqrt(m[i * 4 + i] - m[j * 4 + j] - m[k * 4 + k] + 1.0f);
404 quat[i + 1] = 0.5f * root;
406 quat[0] = (m[j + 4 + k] - m[k * 4 + j]) * root;
407 quat[j + 1] = (m[i * 4 + j] - m[j * 4 + i]) * root;
408 quat[k + 1] = (m[i * 4 + k] - m[k * 4 + i]) * root;
416 static inline void cgm_mget_scaling(const float *m, cgm_vec3 *res)
418 res->x = sqrt(m[0] * m[0] + m[4] * m[4] + m[8] * m[8]);
419 res->y = sqrt(m[1] * m[1] + m[5] * m[5] + m[9] * m[9]);
420 res->z = sqrt(m[2] * m[2] + m[6] * m[6] + m[10] * m[10]);
423 static inline void cgm_mget_frustum_plane(const float *m, int p, cgm_vec4 *res)
426 const float *rowptr = m + row * 4;
429 res->x = m[12] + rowptr[0];
430 res->y = m[13] + rowptr[1];
431 res->z = m[14] + rowptr[2];
432 res->w = m[15] + rowptr[3];
434 res->x = m[12] - rowptr[0];
435 res->y = m[13] - rowptr[1];
436 res->z = m[14] - rowptr[2];
437 res->w = m[15] - rowptr[3];
441 static inline void cgm_mlookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
444 float rot[16], trans[16];
445 cgm_vec3 dir = targ, right, vup;
448 cgm_vnormalize(&dir);
449 cgm_vcross(&right, &dir, up);
450 cgm_vnormalize(&right);
451 cgm_vcross(&vup, &right, &dir);
452 cgm_vnormalize(&vup);
459 static inline void cgm_minv_lookat(float *m, const cgm_vec3 *pos, const cgm_vec3 *targ,
462 static inline void cgm_mortho(float *m, float left, float right, float bot, float top,
463 float znear, float zfar);
464 static inline void cgm_mfrustum(float *m, float left, float right, float bot, float top,
465 float znear, float zfar);
466 static inline void cgm_mperspective(float *m, float vfov, float aspect, float znear, float zfar);
468 static inline void cgm_mmirror(float *m, float a, float b, float c, float d);