diff options
Diffstat (limited to 'include/cglm/mat3.h')
| -rw-r--r-- | include/cglm/mat3.h | 480 |
1 files changed, 480 insertions, 0 deletions
diff --git a/include/cglm/mat3.h b/include/cglm/mat3.h new file mode 100644 index 0000000..10b373e --- /dev/null +++ b/include/cglm/mat3.h @@ -0,0 +1,480 @@ +/* + * Copyright (c), Recep Aslantas. + * + * MIT License (MIT), http://opensource.org/licenses/MIT + * Full license can be found in the LICENSE file + */ + +/* + Macros: + GLM_MAT3_IDENTITY_INIT + GLM_MAT3_ZERO_INIT + GLM_MAT3_IDENTITY + GLM_MAT3_ZERO + glm_mat3_dup(mat, dest) + + Functions: + CGLM_INLINE void glm_mat3_copy(mat3 mat, mat3 dest); + CGLM_INLINE void glm_mat3_identity(mat3 mat); + CGLM_INLINE void glm_mat3_identity_array(mat3 * restrict mat, size_t count); + CGLM_INLINE void glm_mat3_zero(mat3 mat); + CGLM_INLINE void glm_mat3_mul(mat3 m1, mat3 m2, mat3 dest); + CGLM_INLINE void glm_mat3_transpose_to(mat3 m, mat3 dest); + CGLM_INLINE void glm_mat3_transpose(mat3 m); + CGLM_INLINE void glm_mat3_mulv(mat3 m, vec3 v, vec3 dest); + CGLM_INLINE float glm_mat3_trace(mat3 m); + CGLM_INLINE void glm_mat3_quat(mat3 m, versor dest); + CGLM_INLINE void glm_mat3_scale(mat3 m, float s); + CGLM_INLINE float glm_mat3_det(mat3 mat); + CGLM_INLINE void glm_mat3_inv(mat3 mat, mat3 dest); + CGLM_INLINE void glm_mat3_swap_col(mat3 mat, int col1, int col2); + CGLM_INLINE void glm_mat3_swap_row(mat3 mat, int row1, int row2); + CGLM_INLINE float glm_mat3_rmc(vec3 r, mat3 m, vec3 c); + CGLM_INLINE void glm_mat3_make(float * restrict src, mat3 dest); + CGLM_INLINE void glm_mat3_textrans(float sx, float sy, float rot, float tx, float ty, mat3 dest); + */ + +#ifndef cglm_mat3_h +#define cglm_mat3_h + +#include "common.h" +#include "vec3.h" + +#ifdef CGLM_SSE_FP +# include "simd/sse2/mat3.h" +#endif + +#ifdef CGLM_SIMD_WASM +# include "simd/wasm/mat3.h" +#endif + +#define GLM_MAT3_IDENTITY_INIT {{1.0f, 0.0f, 0.0f}, \ + {0.0f, 1.0f, 0.0f}, \ + {0.0f, 0.0f, 1.0f}} +#define GLM_MAT3_ZERO_INIT {{0.0f, 0.0f, 0.0f}, \ + {0.0f, 0.0f, 0.0f}, \ + {0.0f, 0.0f, 0.0f}} + + +/* for C only */ +#define GLM_MAT3_IDENTITY ((mat3)GLM_MAT3_IDENTITY_INIT) +#define GLM_MAT3_ZERO ((mat3)GLM_MAT3_ZERO_INIT) + +/* DEPRECATED! use _copy, _ucopy versions */ +#define glm_mat3_dup(mat, dest) glm_mat3_copy(mat, dest) + +/*! + * @brief copy all members of [mat] to [dest] + * + * @param[in] mat source + * @param[out] dest destination + */ +CGLM_INLINE +void +glm_mat3_copy(mat3 mat, mat3 dest) { + dest[0][0] = mat[0][0]; + dest[0][1] = mat[0][1]; + dest[0][2] = mat[0][2]; + + dest[1][0] = mat[1][0]; + dest[1][1] = mat[1][1]; + dest[1][2] = mat[1][2]; + + dest[2][0] = mat[2][0]; + dest[2][1] = mat[2][1]; + dest[2][2] = mat[2][2]; +} + +/*! + * @brief make given matrix identity. It is identical with below, + * but it is more easy to do that with this func especially for members + * e.g. glm_mat3_identity(aStruct->aMatrix); + * + * @code + * glm_mat3_copy(GLM_MAT3_IDENTITY, mat); // C only + * + * // or + * mat3 mat = GLM_MAT3_IDENTITY_INIT; + * @endcode + * + * @param[in, out] mat destination + */ +CGLM_INLINE +void +glm_mat3_identity(mat3 mat) { + CGLM_ALIGN_MAT mat3 t = GLM_MAT3_IDENTITY_INIT; + glm_mat3_copy(t, mat); +} + +/*! + * @brief make given matrix array's each element identity matrix + * + * @param[in, out] mat matrix array (must be aligned (16/32) + * if alignment is not disabled) + * + * @param[in] count count of matrices + */ +CGLM_INLINE +void +glm_mat3_identity_array(mat3 * __restrict mat, size_t count) { + CGLM_ALIGN_MAT mat3 t = GLM_MAT3_IDENTITY_INIT; + size_t i; + + for (i = 0; i < count; i++) { + glm_mat3_copy(t, mat[i]); + } +} + +/*! + * @brief make given matrix zero. + * + * @param[in, out] mat matrix + */ +CGLM_INLINE +void +glm_mat3_zero(mat3 mat) { + CGLM_ALIGN_MAT mat3 t = GLM_MAT3_ZERO_INIT; + glm_mat3_copy(t, mat); +} + +/*! + * @brief multiply m1 and m2 to dest + * + * m1, m2 and dest matrices can be same matrix, it is possible to write this: + * + * @code + * mat3 m = GLM_MAT3_IDENTITY_INIT; + * glm_mat3_mul(m, m, m); + * @endcode + * + * @param[in] m1 left matrix + * @param[in] m2 right matrix + * @param[out] dest destination matrix + */ +CGLM_INLINE +void +glm_mat3_mul(mat3 m1, mat3 m2, mat3 dest) { +#if defined(__wasm__) && defined(__wasm_simd128__) + glm_mat3_mul_wasm(m1, m2, dest); +#elif defined( __SSE__ ) || defined( __SSE2__ ) + glm_mat3_mul_sse2(m1, m2, dest); +#else + float a00 = m1[0][0], a01 = m1[0][1], a02 = m1[0][2], + a10 = m1[1][0], a11 = m1[1][1], a12 = m1[1][2], + a20 = m1[2][0], a21 = m1[2][1], a22 = m1[2][2], + + b00 = m2[0][0], b01 = m2[0][1], b02 = m2[0][2], + b10 = m2[1][0], b11 = m2[1][1], b12 = m2[1][2], + b20 = m2[2][0], b21 = m2[2][1], b22 = m2[2][2]; + + dest[0][0] = a00 * b00 + a10 * b01 + a20 * b02; + dest[0][1] = a01 * b00 + a11 * b01 + a21 * b02; + dest[0][2] = a02 * b00 + a12 * b01 + a22 * b02; + dest[1][0] = a00 * b10 + a10 * b11 + a20 * b12; + dest[1][1] = a01 * b10 + a11 * b11 + a21 * b12; + dest[1][2] = a02 * b10 + a12 * b11 + a22 * b12; + dest[2][0] = a00 * b20 + a10 * b21 + a20 * b22; + dest[2][1] = a01 * b20 + a11 * b21 + a21 * b22; + dest[2][2] = a02 * b20 + a12 * b21 + a22 * b22; +#endif +} + +/*! + * @brief transpose mat3 and store in dest + * + * source matrix will not be transposed unless dest is m + * + * @param[in] m matrix + * @param[out] dest result + */ +CGLM_INLINE +void +glm_mat3_transpose_to(mat3 m, mat3 dest) { + dest[0][0] = m[0][0]; + dest[0][1] = m[1][0]; + dest[0][2] = m[2][0]; + dest[1][0] = m[0][1]; + dest[1][1] = m[1][1]; + dest[1][2] = m[2][1]; + dest[2][0] = m[0][2]; + dest[2][1] = m[1][2]; + dest[2][2] = m[2][2]; +} + +/*! + * @brief transpose mat3 and store result in same matrix + * + * @param[in, out] m source and dest + */ +CGLM_INLINE +void +glm_mat3_transpose(mat3 m) { + CGLM_ALIGN_MAT mat3 tmp; + + tmp[0][1] = m[1][0]; + tmp[0][2] = m[2][0]; + tmp[1][0] = m[0][1]; + tmp[1][2] = m[2][1]; + tmp[2][0] = m[0][2]; + tmp[2][1] = m[1][2]; + + m[0][1] = tmp[0][1]; + m[0][2] = tmp[0][2]; + m[1][0] = tmp[1][0]; + m[1][2] = tmp[1][2]; + m[2][0] = tmp[2][0]; + m[2][1] = tmp[2][1]; +} + +/*! + * @brief multiply mat3 with vec3 (column vector) and store in dest vector + * + * @param[in] m mat3 (left) + * @param[in] v vec3 (right, column vector) + * @param[out] dest vec3 (result, column vector) + */ +CGLM_INLINE +void +glm_mat3_mulv(mat3 m, vec3 v, vec3 dest) { + vec3 res; + res[0] = m[0][0] * v[0] + m[1][0] * v[1] + m[2][0] * v[2]; + res[1] = m[0][1] * v[0] + m[1][1] * v[1] + m[2][1] * v[2]; + res[2] = m[0][2] * v[0] + m[1][2] * v[1] + m[2][2] * v[2]; + glm_vec3_copy(res, dest); +} + +/*! + * @brief trace of matrix + * + * sum of the elements on the main diagonal from upper left to the lower right + * + * @param[in] m matrix + */ +CGLM_INLINE +float +glm_mat3_trace(mat3 m) { + return m[0][0] + m[1][1] + m[2][2]; +} + +/*! + * @brief convert mat3 to quaternion + * + * @param[in] m rotation matrix + * @param[out] dest destination quaternion + */ +CGLM_INLINE +void +glm_mat3_quat(mat3 m, versor dest) { + float trace, r, rinv; + + /* it seems using like m12 instead of m[1][2] causes extra instructions */ + + trace = m[0][0] + m[1][1] + m[2][2]; + if (trace >= 0.0f) { + r = sqrtf(1.0f + trace); + rinv = 0.5f / r; + + dest[0] = rinv * (m[1][2] - m[2][1]); + dest[1] = rinv * (m[2][0] - m[0][2]); + dest[2] = rinv * (m[0][1] - m[1][0]); + dest[3] = r * 0.5f; + } else if (m[0][0] >= m[1][1] && m[0][0] >= m[2][2]) { + r = sqrtf(1.0f - m[1][1] - m[2][2] + m[0][0]); + rinv = 0.5f / r; + + dest[0] = r * 0.5f; + dest[1] = rinv * (m[0][1] + m[1][0]); + dest[2] = rinv * (m[0][2] + m[2][0]); + dest[3] = rinv * (m[1][2] - m[2][1]); + } else if (m[1][1] >= m[2][2]) { + r = sqrtf(1.0f - m[0][0] - m[2][2] + m[1][1]); + rinv = 0.5f / r; + + dest[0] = rinv * (m[0][1] + m[1][0]); + dest[1] = r * 0.5f; + dest[2] = rinv * (m[1][2] + m[2][1]); + dest[3] = rinv * (m[2][0] - m[0][2]); + } else { + r = sqrtf(1.0f - m[0][0] - m[1][1] + m[2][2]); + rinv = 0.5f / r; + + dest[0] = rinv * (m[0][2] + m[2][0]); + dest[1] = rinv * (m[1][2] + m[2][1]); + dest[2] = r * 0.5f; + dest[3] = rinv * (m[0][1] - m[1][0]); + } +} + +/*! + * @brief scale (multiply with scalar) matrix + * + * multiply matrix with scalar + * + * @param[in, out] m matrix + * @param[in] s scalar + */ +CGLM_INLINE +void +glm_mat3_scale(mat3 m, float s) { + m[0][0] *= s; m[0][1] *= s; m[0][2] *= s; + m[1][0] *= s; m[1][1] *= s; m[1][2] *= s; + m[2][0] *= s; m[2][1] *= s; m[2][2] *= s; +} + +/*! + * @brief mat3 determinant + * + * @param[in] mat matrix + * + * @return determinant + */ +CGLM_INLINE +float +glm_mat3_det(mat3 mat) { + float a = mat[0][0], b = mat[0][1], c = mat[0][2], + d = mat[1][0], e = mat[1][1], f = mat[1][2], + g = mat[2][0], h = mat[2][1], i = mat[2][2]; + + return a * (e * i - h * f) - d * (b * i - h * c) + g * (b * f - e * c); +} + +/*! + * @brief inverse mat3 and store in dest + * + * @param[in] mat matrix + * @param[out] dest inverse matrix + */ +CGLM_INLINE +void +glm_mat3_inv(mat3 mat, mat3 dest) { + float a = mat[0][0], b = mat[0][1], c = mat[0][2], + d = mat[1][0], e = mat[1][1], f = mat[1][2], + g = mat[2][0], h = mat[2][1], i = mat[2][2], + + c1 = e * i - f * h, c2 = d * i - g * f, c3 = d * h - g * e, + idt = 1.0f / (a * c1 - b * c2 + c * c3), ndt = -idt; + + dest[0][0] = idt * c1; + dest[0][1] = ndt * (b * i - h * c); + dest[0][2] = idt * (b * f - e * c); + dest[1][0] = ndt * c2; + dest[1][1] = idt * (a * i - g * c); + dest[1][2] = ndt * (a * f - d * c); + dest[2][0] = idt * c3; + dest[2][1] = ndt * (a * h - g * b); + dest[2][2] = idt * (a * e - d * b); +} + +/*! + * @brief swap two matrix columns + * + * @param[in,out] mat matrix + * @param[in] col1 col1 + * @param[in] col2 col2 + */ +CGLM_INLINE +void +glm_mat3_swap_col(mat3 mat, int col1, int col2) { + vec3 tmp; + glm_vec3_copy(mat[col1], tmp); + glm_vec3_copy(mat[col2], mat[col1]); + glm_vec3_copy(tmp, mat[col2]); +} + +/*! + * @brief swap two matrix rows + * + * @param[in,out] mat matrix + * @param[in] row1 row1 + * @param[in] row2 row2 + */ +CGLM_INLINE +void +glm_mat3_swap_row(mat3 mat, int row1, int row2) { + vec3 tmp; + tmp[0] = mat[0][row1]; + tmp[1] = mat[1][row1]; + tmp[2] = mat[2][row1]; + + mat[0][row1] = mat[0][row2]; + mat[1][row1] = mat[1][row2]; + mat[2][row1] = mat[2][row2]; + + mat[0][row2] = tmp[0]; + mat[1][row2] = tmp[1]; + mat[2][row2] = tmp[2]; +} + +/*! + * @brief helper for R (row vector) * M (matrix) * C (column vector) + * + * rmc stands for Row * Matrix * Column + * + * the result is scalar because R * M = Matrix1x3 (row vector), + * then Matrix1x3 * Vec3 (column vector) = Matrix1x1 (Scalar) + * + * @param[in] r row vector or matrix1x3 + * @param[in] m matrix3x3 + * @param[in] c column vector or matrix3x1 + * + * @return scalar value e.g. Matrix1x1 + */ +CGLM_INLINE +float +glm_mat3_rmc(vec3 r, mat3 m, vec3 c) { + vec3 tmp; + glm_mat3_mulv(m, c, tmp); + return glm_vec3_dot(r, tmp); +} + +/*! + * @brief Create mat3 matrix from pointer + * + * @param[in] src pointer to an array of floats + * @param[out] dest matrix + */ +CGLM_INLINE +void +glm_mat3_make(const float * __restrict src, mat3 dest) { + dest[0][0] = src[0]; + dest[0][1] = src[1]; + dest[0][2] = src[2]; + + dest[1][0] = src[3]; + dest[1][1] = src[4]; + dest[1][2] = src[5]; + + dest[2][0] = src[6]; + dest[2][1] = src[7]; + dest[2][2] = src[8]; +} + +/*! + * @brief Create mat3 matrix from texture transform parameters + * + * @param[in] sx scale x + * @param[in] sy scale y + * @param[in] rot rotation in radians CCW/RH + * @param[in] tx translate x + * @param[in] ty translate y + * @param[out] dest texture transform matrix + */ +CGLM_INLINE +void +glm_mat3_textrans(float sx, float sy, float rot, float tx, float ty, mat3 dest) { + float c, s; + + c = cosf(rot); + s = sinf(rot); + + glm_mat3_identity(dest); + + dest[0][0] = c * sx; + dest[0][1] = -s * sy; + dest[1][0] = s * sx; + dest[1][1] = c * sy; + dest[2][0] = tx; + dest[2][1] = ty; +} + +#endif /* cglm_mat3_h */ |