nuclear@27: /* nuclear@27: libvmath - a vector math library nuclear@27: Copyright (C) 2004-2011 John Tsiombikas nuclear@27: nuclear@27: This program is free software: you can redistribute it and/or modify nuclear@27: it under the terms of the GNU Lesser General Public License as published nuclear@27: by the Free Software Foundation, either version 3 of the License, or nuclear@27: (at your option) any later version. nuclear@27: nuclear@27: This program is distributed in the hope that it will be useful, nuclear@27: but WITHOUT ANY WARRANTY; without even the implied warranty of nuclear@27: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the nuclear@27: GNU Lesser General Public License for more details. nuclear@27: nuclear@27: You should have received a copy of the GNU Lesser General Public License nuclear@27: along with this program. If not, see . nuclear@27: */ nuclear@27: nuclear@27: nuclear@27: #include nuclear@27: #include "matrix.h" nuclear@27: #include "vector.h" nuclear@27: #include "quat.h" nuclear@27: nuclear@27: void m3_to_m4(mat4_t dest, mat3_t src) nuclear@27: { nuclear@27: int i, j; nuclear@27: nuclear@27: memset(dest, 0, sizeof(mat4_t)); nuclear@27: for(i=0; i<3; i++) { nuclear@27: for(j=0; j<3; j++) { nuclear@27: dest[i][j] = src[i][j]; nuclear@27: } nuclear@27: } nuclear@27: dest[3][3] = 1.0; nuclear@27: } nuclear@27: nuclear@27: void m3_print(FILE *fp, mat3_t m) nuclear@27: { nuclear@27: int i; nuclear@27: for(i=0; i<3; i++) { nuclear@27: fprintf(fp, "[ %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2]); nuclear@27: } nuclear@27: } nuclear@27: nuclear@27: /* C matrix 4x4 functions */ nuclear@27: void m4_to_m3(mat3_t dest, mat4_t src) nuclear@27: { nuclear@27: int i, j; nuclear@27: for(i=0; i<3; i++) { nuclear@27: for(j=0; j<3; j++) { nuclear@27: dest[i][j] = src[i][j]; nuclear@27: } nuclear@27: } nuclear@27: } nuclear@27: nuclear@27: void m4_set_translation(mat4_t m, scalar_t x, scalar_t y, scalar_t z) nuclear@27: { nuclear@27: m4_identity(m); nuclear@27: m[0][3] = x; nuclear@27: m[1][3] = y; nuclear@27: m[2][3] = z; nuclear@27: } nuclear@27: nuclear@27: void m4_translate(mat4_t m, scalar_t x, scalar_t y, scalar_t z) nuclear@27: { nuclear@27: mat4_t tm; nuclear@27: m4_set_translation(tm, x, y, z); nuclear@27: m4_mult(m, m, tm); nuclear@27: } nuclear@27: nuclear@27: void m4_rotate(mat4_t m, scalar_t x, scalar_t y, scalar_t z) nuclear@27: { nuclear@27: m4_rotate_x(m, x); nuclear@27: m4_rotate_y(m, y); nuclear@27: m4_rotate_z(m, z); nuclear@27: } nuclear@27: nuclear@27: void m4_set_rotation_x(mat4_t m, scalar_t angle) nuclear@27: { nuclear@27: m4_identity(m); nuclear@27: m[1][1] = cos(angle); m[1][2] = -sin(angle); nuclear@27: m[2][1] = sin(angle); m[2][2] = cos(angle); nuclear@27: } nuclear@27: nuclear@27: void m4_rotate_x(mat4_t m, scalar_t angle) nuclear@27: { nuclear@27: mat4_t rm; nuclear@27: m4_set_rotation_x(m, angle); nuclear@27: m4_mult(m, m, rm); nuclear@27: } nuclear@27: nuclear@27: void m4_set_rotation_y(mat4_t m, scalar_t angle) nuclear@27: { nuclear@27: m4_identity(m); nuclear@27: m[0][0] = cos(angle); m[0][2] = sin(angle); nuclear@27: m[2][0] = -sin(angle); m[2][2] = cos(angle); nuclear@27: } nuclear@27: nuclear@27: void m4_rotate_y(mat4_t m, scalar_t angle) nuclear@27: { nuclear@27: mat4_t rm; nuclear@27: m4_set_rotation_y(rm, angle); nuclear@27: m4_mult(m, m, rm); nuclear@27: } nuclear@27: nuclear@27: void m4_set_rotation_z(mat4_t m, scalar_t angle) nuclear@27: { nuclear@27: m4_identity(m); nuclear@27: m[0][0] = cos(angle); m[0][1] = -sin(angle); nuclear@27: m[1][0] = sin(angle); m[1][1] = cos(angle); nuclear@27: } nuclear@27: nuclear@27: void m4_rotate_z(mat4_t m, scalar_t angle) nuclear@27: { nuclear@27: mat4_t rm; nuclear@27: m4_set_rotation_z(rm, angle); nuclear@27: m4_mult(m, m, rm); nuclear@27: } nuclear@27: nuclear@27: void m4_set_rotation_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z) nuclear@27: { nuclear@27: scalar_t sina = sin(angle); nuclear@27: scalar_t cosa = cos(angle); nuclear@27: scalar_t one_minus_cosa = 1.0 - cosa; nuclear@27: scalar_t nxsq = x * x; nuclear@27: scalar_t nysq = y * y; nuclear@27: scalar_t nzsq = z * z; nuclear@27: nuclear@27: m[0][0] = nxsq + (1.0 - nxsq) * cosa; nuclear@27: m[0][1] = x * y * one_minus_cosa - z * sina; nuclear@27: m[0][2] = x * z * one_minus_cosa + y * sina; nuclear@27: m[1][0] = x * y * one_minus_cosa + z * sina; nuclear@27: m[1][1] = nysq + (1.0 - nysq) * cosa; nuclear@27: m[1][2] = y * z * one_minus_cosa - x * sina; nuclear@27: m[2][0] = x * z * one_minus_cosa - y * sina; nuclear@27: m[2][1] = y * z * one_minus_cosa + x * sina; nuclear@27: m[2][2] = nzsq + (1.0 - nzsq) * cosa; nuclear@27: nuclear@27: /* the rest are identity */ nuclear@27: m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0; nuclear@27: m[3][3] = 1.0; nuclear@27: } nuclear@27: nuclear@27: void m4_rotate_axis(mat4_t m, scalar_t angle, scalar_t x, scalar_t y, scalar_t z) nuclear@27: { nuclear@27: mat4_t xform; nuclear@27: m4_set_rotation_axis(xform, angle, x, y, z); nuclear@27: m4_mult(m, m, xform); nuclear@27: } nuclear@27: nuclear@27: void m4_rotate_quat(mat4_t m, quat_t q) nuclear@27: { nuclear@27: mat4_t rm; nuclear@27: quat_to_mat4(rm, q); nuclear@27: m4_mult(m, m, rm); nuclear@27: } nuclear@27: nuclear@27: void m4_scale(mat4_t m, scalar_t x, scalar_t y, scalar_t z) nuclear@27: { nuclear@27: mat4_t sm; nuclear@27: m4_identity(sm); nuclear@27: sm[0][0] = x; nuclear@27: sm[1][1] = y; nuclear@27: sm[2][2] = z; nuclear@27: m4_mult(m, m, sm); nuclear@27: } nuclear@27: nuclear@27: void m4_transpose(mat4_t res, mat4_t m) nuclear@27: { nuclear@27: int i, j; nuclear@27: mat4_t tmp; nuclear@27: m4_copy(tmp, m); nuclear@27: nuclear@27: for(i=0; i<4; i++) { nuclear@27: for(j=0; j<4; j++) { nuclear@27: res[i][j] = tmp[j][i]; nuclear@27: } nuclear@27: } nuclear@27: } nuclear@27: nuclear@27: scalar_t m4_determinant(mat4_t m) nuclear@27: { nuclear@27: scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@27: (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + nuclear@27: (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); nuclear@27: nuclear@27: scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@27: (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@27: (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); nuclear@27: nuclear@27: scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - nuclear@27: (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@27: (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@27: nuclear@27: scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - nuclear@27: (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + nuclear@27: (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@27: nuclear@27: return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14; nuclear@27: } nuclear@27: nuclear@27: void m4_adjoint(mat4_t res, mat4_t m) nuclear@27: { nuclear@27: int i, j; nuclear@27: mat4_t coef; nuclear@27: nuclear@27: coef[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@27: (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + nuclear@27: (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); nuclear@27: coef[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@27: (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@27: (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); nuclear@27: coef[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - nuclear@27: (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@27: (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@27: coef[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - nuclear@27: (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + nuclear@27: (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@27: nuclear@27: coef[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@27: (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + nuclear@27: (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); nuclear@27: coef[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@27: (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@27: (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); nuclear@27: coef[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - nuclear@27: (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@27: (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@27: coef[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - nuclear@27: (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + nuclear@27: (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@27: nuclear@27: coef[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) - nuclear@27: (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) + nuclear@27: (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])); nuclear@27: coef[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) - nuclear@27: (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) + nuclear@27: (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])); nuclear@27: coef[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) - nuclear@27: (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) + nuclear@27: (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1])); nuclear@27: coef[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) - nuclear@27: (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) + nuclear@27: (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1])); nuclear@27: nuclear@27: coef[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) - nuclear@27: (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) + nuclear@27: (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])); nuclear@27: coef[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) - nuclear@27: (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) + nuclear@27: (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])); nuclear@27: coef[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) - nuclear@27: (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) + nuclear@27: (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1])); nuclear@27: coef[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) - nuclear@27: (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) + nuclear@27: (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1])); nuclear@27: nuclear@27: m4_transpose(res, coef); nuclear@27: nuclear@27: for(i=0; i<4; i++) { nuclear@27: for(j=0; j<4; j++) { nuclear@27: res[i][j] = j % 2 ? -res[i][j] : res[i][j]; nuclear@27: if(i % 2) res[i][j] = -res[i][j]; nuclear@27: } nuclear@27: } nuclear@27: } nuclear@27: nuclear@27: void m4_inverse(mat4_t res, mat4_t m) nuclear@27: { nuclear@27: int i, j; nuclear@27: mat4_t adj; nuclear@27: scalar_t det; nuclear@27: nuclear@27: m4_adjoint(adj, m); nuclear@27: det = m4_determinant(m); nuclear@27: nuclear@27: for(i=0; i<4; i++) { nuclear@27: for(j=0; j<4; j++) { nuclear@27: res[i][j] = adj[i][j] / det; nuclear@27: } nuclear@27: } nuclear@27: } nuclear@27: nuclear@27: void m4_print(FILE *fp, mat4_t m) nuclear@27: { nuclear@27: int i; nuclear@27: for(i=0; i<4; i++) { nuclear@27: fprintf(fp, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)m[i][0], (float)m[i][1], (float)m[i][2], (float)m[i][3]); nuclear@27: } nuclear@27: }