nuclear@10: /* nuclear@10: libvmath - a vector math library nuclear@10: Copyright (C) 2004-2015 John Tsiombikas nuclear@10: nuclear@10: This program is free software: you can redistribute it and/or modify nuclear@10: it under the terms of the GNU Lesser General Public License as published nuclear@10: by the Free Software Foundation, either version 3 of the License, or nuclear@10: (at your option) any later version. nuclear@10: nuclear@10: This program is distributed in the hope that it will be useful, nuclear@10: but WITHOUT ANY WARRANTY; without even the implied warranty of nuclear@10: MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the nuclear@10: GNU Lesser General Public License for more details. nuclear@10: nuclear@10: You should have received a copy of the GNU Lesser General Public License nuclear@10: along with this program. If not, see . nuclear@10: */ nuclear@10: #include nuclear@10: #include nuclear@10: #include "matrix.h" nuclear@10: #include "vector.h" nuclear@10: #include "quat.h" nuclear@10: nuclear@10: using namespace std; nuclear@10: nuclear@10: // ----------- Matrix3x3 -------------- nuclear@10: nuclear@10: Matrix3x3 Matrix3x3::identity = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1); nuclear@10: nuclear@10: Matrix3x3::Matrix3x3() nuclear@10: { nuclear@10: *this = Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1); nuclear@10: } nuclear@10: nuclear@10: Matrix3x3::Matrix3x3( scalar_t m11, scalar_t m12, scalar_t m13, nuclear@10: scalar_t m21, scalar_t m22, scalar_t m23, nuclear@10: scalar_t m31, scalar_t m32, scalar_t m33) nuclear@10: { nuclear@10: m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; nuclear@10: m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; nuclear@10: m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; nuclear@10: } nuclear@10: nuclear@10: Matrix3x3::Matrix3x3(const Vector3 &ivec, const Vector3 &jvec, const Vector3 &kvec) nuclear@10: { nuclear@10: set_row_vector(ivec, 0); nuclear@10: set_row_vector(jvec, 1); nuclear@10: set_row_vector(kvec, 2); nuclear@10: } nuclear@10: nuclear@10: Matrix3x3::Matrix3x3(const mat3_t cmat) nuclear@10: { nuclear@10: memcpy(m, cmat, sizeof(mat3_t)); nuclear@10: } nuclear@10: nuclear@10: Matrix3x3::Matrix3x3(const Matrix4x4 &mat4x4) nuclear@10: { nuclear@10: for(int i=0; i<3; i++) { nuclear@10: for(int j=0; j<3; j++) { nuclear@10: m[i][j] = mat4x4[i][j]; nuclear@10: } nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 operator +(const Matrix3x3 &m1, const Matrix3x3 &m2) nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: const scalar_t *op1 = m1.m[0], *op2 = m2.m[0]; nuclear@10: scalar_t *dest = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *dest++ = *op1++ + *op2++; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 operator -(const Matrix3x3 &m1, const Matrix3x3 &m2) nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: const scalar_t *op1 = m1.m[0], *op2 = m2.m[0]; nuclear@10: scalar_t *dest = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *dest++ = *op1++ - *op2++; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 operator *(const Matrix3x3 &m1, const Matrix3x3 &m2) nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: for(int i=0; i<3; i++) { nuclear@10: for(int j=0; j<3; j++) { nuclear@10: res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j]; nuclear@10: } nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: void operator +=(Matrix3x3 &m1, const Matrix3x3 &m2) nuclear@10: { nuclear@10: scalar_t *op1 = m1.m[0]; nuclear@10: const scalar_t *op2 = m2.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *op1++ += *op2++; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: void operator -=(Matrix3x3 &m1, const Matrix3x3 &m2) nuclear@10: { nuclear@10: scalar_t *op1 = m1.m[0]; nuclear@10: const scalar_t *op2 = m2.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *op1++ -= *op2++; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: void operator *=(Matrix3x3 &m1, const Matrix3x3 &m2) nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: for(int i=0; i<3; i++) { nuclear@10: for(int j=0; j<3; j++) { nuclear@10: res.m[i][j] = m1.m[i][0] * m2.m[0][j] + m1.m[i][1] * m2.m[1][j] + m1.m[i][2] * m2.m[2][j]; nuclear@10: } nuclear@10: } nuclear@10: memcpy(m1.m, res.m, 9 * sizeof(scalar_t)); nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 operator *(const Matrix3x3 &mat, scalar_t scalar) nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: const scalar_t *mptr = mat.m[0]; nuclear@10: scalar_t *dptr = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *dptr++ = *mptr++ * scalar; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 operator *(scalar_t scalar, const Matrix3x3 &mat) nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: const scalar_t *mptr = mat.m[0]; nuclear@10: scalar_t *dptr = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *dptr++ = *mptr++ * scalar; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: void operator *=(Matrix3x3 &mat, scalar_t scalar) nuclear@10: { nuclear@10: scalar_t *mptr = mat.m[0]; nuclear@10: nuclear@10: for(int i=0; i<9; i++) { nuclear@10: *mptr++ *= scalar; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::translate(const Vector2 &trans) nuclear@10: { nuclear@10: Matrix3x3 tmat(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1); nuclear@10: *this *= tmat; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_translation(const Vector2 &trans) nuclear@10: { nuclear@10: *this = Matrix3x3(1, 0, trans.x, 0, 1, trans.y, 0, 0, 1); nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::rotate(scalar_t angle) nuclear@10: { nuclear@10: scalar_t cos_a = cos(angle); nuclear@10: scalar_t sin_a = sin(angle); nuclear@10: Matrix3x3 rmat( cos_a, -sin_a, 0, nuclear@10: sin_a, cos_a, 0, nuclear@10: 0, 0, 1); nuclear@10: *this *= rmat; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_rotation(scalar_t angle) nuclear@10: { nuclear@10: scalar_t cos_a = cos(angle); nuclear@10: scalar_t sin_a = sin(angle); nuclear@10: *this = Matrix3x3(cos_a, -sin_a, 0, sin_a, cos_a, 0, 0, 0, 1); nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::rotate(const Vector3 &euler_angles) nuclear@10: { nuclear@10: Matrix3x3 xrot, yrot, zrot; nuclear@10: nuclear@10: xrot = Matrix3x3( 1, 0, 0, nuclear@10: 0, cos(euler_angles.x), -sin(euler_angles.x), nuclear@10: 0, sin(euler_angles.x), cos(euler_angles.x)); nuclear@10: nuclear@10: yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y), nuclear@10: 0, 1, 0, nuclear@10: -sin(euler_angles.y), 0, cos(euler_angles.y)); nuclear@10: nuclear@10: zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0, nuclear@10: sin(euler_angles.z), cos(euler_angles.z), 0, nuclear@10: 0, 0, 1); nuclear@10: nuclear@10: *this *= xrot * yrot * zrot; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_rotation(const Vector3 &euler_angles) nuclear@10: { nuclear@10: Matrix3x3 xrot, yrot, zrot; nuclear@10: nuclear@10: xrot = Matrix3x3( 1, 0, 0, nuclear@10: 0, cos(euler_angles.x), -sin(euler_angles.x), nuclear@10: 0, sin(euler_angles.x), cos(euler_angles.x)); nuclear@10: nuclear@10: yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y), nuclear@10: 0, 1, 0, nuclear@10: -sin(euler_angles.y), 0, cos(euler_angles.y)); nuclear@10: nuclear@10: zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0, nuclear@10: sin(euler_angles.z), cos(euler_angles.z), 0, nuclear@10: 0, 0, 1); nuclear@10: nuclear@10: *this = xrot * yrot * zrot; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::rotate(const Vector3 &axis, scalar_t angle) nuclear@10: { nuclear@10: scalar_t sina = (scalar_t)sin(angle); nuclear@10: scalar_t cosa = (scalar_t)cos(angle); nuclear@10: scalar_t invcosa = 1-cosa; nuclear@10: scalar_t nxsq = axis.x * axis.x; nuclear@10: scalar_t nysq = axis.y * axis.y; nuclear@10: scalar_t nzsq = axis.z * axis.z; nuclear@10: nuclear@10: Matrix3x3 xform; nuclear@10: xform.m[0][0] = nxsq + (1-nxsq) * cosa; nuclear@10: xform.m[0][1] = axis.x * axis.y * invcosa - axis.z * sina; nuclear@10: xform.m[0][2] = axis.x * axis.z * invcosa + axis.y * sina; nuclear@10: nuclear@10: xform.m[1][0] = axis.x * axis.y * invcosa + axis.z * sina; nuclear@10: xform.m[1][1] = nysq + (1-nysq) * cosa; nuclear@10: xform.m[1][2] = axis.y * axis.z * invcosa - axis.x * sina; nuclear@10: nuclear@10: xform.m[2][0] = axis.x * axis.z * invcosa - axis.y * sina; nuclear@10: xform.m[2][1] = axis.y * axis.z * invcosa + axis.x * sina; nuclear@10: xform.m[2][2] = nzsq + (1-nzsq) * cosa; nuclear@10: nuclear@10: *this *= xform; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_rotation(const Vector3 &axis, scalar_t angle) nuclear@10: { nuclear@10: scalar_t sina = (scalar_t)sin(angle); nuclear@10: scalar_t cosa = (scalar_t)cos(angle); nuclear@10: scalar_t invcosa = 1-cosa; nuclear@10: scalar_t nxsq = axis.x * axis.x; nuclear@10: scalar_t nysq = axis.y * axis.y; nuclear@10: scalar_t nzsq = axis.z * axis.z; nuclear@10: nuclear@10: reset_identity(); nuclear@10: m[0][0] = nxsq + (1-nxsq) * cosa; nuclear@10: m[0][1] = axis.x * axis.y * invcosa - axis.z * sina; nuclear@10: m[0][2] = axis.x * axis.z * invcosa + axis.y * sina; nuclear@10: m[1][0] = axis.x * axis.y * invcosa + axis.z * sina; nuclear@10: m[1][1] = nysq + (1-nysq) * cosa; nuclear@10: m[1][2] = axis.y * axis.z * invcosa - axis.x * sina; nuclear@10: m[2][0] = axis.x * axis.z * invcosa - axis.y * sina; nuclear@10: m[2][1] = axis.y * axis.z * invcosa + axis.x * sina; nuclear@10: m[2][2] = nzsq + (1-nzsq) * cosa; nuclear@10: } nuclear@10: nuclear@10: // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes nuclear@10: // article "Quaternion Calculus and Fast Animation". nuclear@10: // adapted from: http://www.geometrictools.com/LibMathematics/Algebra/Wm5Quaternion.inl nuclear@10: Quaternion Matrix3x3::get_rotation_quat() const nuclear@10: { nuclear@10: static const int next[3] = {1, 2, 0}; nuclear@10: nuclear@10: float quat[4]; nuclear@10: nuclear@10: scalar_t trace = m[0][0] + m[1][1] + m[2][2]; nuclear@10: scalar_t root; nuclear@10: nuclear@10: if(trace > 0.0f) { nuclear@10: // |w| > 1/2 nuclear@10: root = sqrt(trace + 1.0f); // 2w nuclear@10: quat[0] = 0.5f * root; nuclear@10: root = 0.5f / root; // 1 / 4w nuclear@10: quat[1] = (m[2][1] - m[1][2]) * root; nuclear@10: quat[2] = (m[0][2] - m[2][0]) * root; nuclear@10: quat[3] = (m[1][0] - m[0][1]) * root; nuclear@10: } else { nuclear@10: // |w| <= 1/2 nuclear@10: int i = 0; nuclear@10: if(m[1][1] > m[0][0]) { nuclear@10: i = 1; nuclear@10: } nuclear@10: if(m[2][2] > m[i][i]) { nuclear@10: i = 2; nuclear@10: } nuclear@10: int j = next[i]; nuclear@10: int k = next[j]; nuclear@10: nuclear@10: root = sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0f); nuclear@10: quat[i + 1] = 0.5f * root; nuclear@10: root = 0.5f / root; nuclear@10: quat[0] = (m[k][j] - m[j][k]) * root; nuclear@10: quat[j + 1] = (m[j][i] - m[i][j]) * root; nuclear@10: quat[k + 1] = (m[k][i] - m[i][k]) * root; nuclear@10: } nuclear@10: return Quaternion(quat[0], quat[1], quat[2], quat[3]); nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::scale(const Vector3 &scale_vec) nuclear@10: { nuclear@10: Matrix3x3 smat( scale_vec.x, 0, 0, nuclear@10: 0, scale_vec.y, 0, nuclear@10: 0, 0, scale_vec.z); nuclear@10: *this *= smat; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_scaling(const Vector3 &scale_vec) nuclear@10: { nuclear@10: *this = Matrix3x3( scale_vec.x, 0, 0, nuclear@10: 0, scale_vec.y, 0, nuclear@10: 0, 0, scale_vec.z); nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_column_vector(const Vector3 &vec, unsigned int col_index) nuclear@10: { nuclear@10: m[0][col_index] = vec.x; nuclear@10: m[1][col_index] = vec.y; nuclear@10: m[2][col_index] = vec.z; nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::set_row_vector(const Vector3 &vec, unsigned int row_index) nuclear@10: { nuclear@10: m[row_index][0] = vec.x; nuclear@10: m[row_index][1] = vec.y; nuclear@10: m[row_index][2] = vec.z; nuclear@10: } nuclear@10: nuclear@10: Vector3 Matrix3x3::get_column_vector(unsigned int col_index) const nuclear@10: { nuclear@10: return Vector3(m[0][col_index], m[1][col_index], m[2][col_index]); nuclear@10: } nuclear@10: nuclear@10: Vector3 Matrix3x3::get_row_vector(unsigned int row_index) const nuclear@10: { nuclear@10: return Vector3(m[row_index][0], m[row_index][1], m[row_index][2]); nuclear@10: } nuclear@10: nuclear@10: void Matrix3x3::transpose() nuclear@10: { nuclear@10: Matrix3x3 tmp = *this; nuclear@10: for(int i=0; i<3; i++) { nuclear@10: for(int j=0; j<3; j++) { nuclear@10: m[i][j] = tmp[j][i]; nuclear@10: } nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 Matrix3x3::transposed() const nuclear@10: { nuclear@10: Matrix3x3 res; nuclear@10: for(int i=0; i<3; i++) { nuclear@10: for(int j=0; j<3; j++) { nuclear@10: res[i][j] = m[j][i]; nuclear@10: } nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: scalar_t Matrix3x3::determinant() const nuclear@10: { nuclear@10: return m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) - nuclear@10: m[0][1] * (m[1][0]*m[2][2] - m[1][2]*m[2][0]) + nuclear@10: m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]); nuclear@10: } nuclear@10: nuclear@10: Matrix3x3 Matrix3x3::inverse() const nuclear@10: { nuclear@10: // TODO: implement 3x3 inverse nuclear@10: return *this; nuclear@10: } nuclear@10: nuclear@10: /*ostream &operator <<(ostream &out, const Matrix3x3 &mat) nuclear@10: { nuclear@10: for(int i=0; i<3; i++) { nuclear@10: char str[100]; nuclear@10: sprintf(str, "[ %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2]); nuclear@10: out << str; nuclear@10: } nuclear@10: return out; nuclear@10: }*/ nuclear@10: nuclear@10: nuclear@10: nuclear@10: /* ----------------- Matrix4x4 implementation --------------- */ nuclear@10: nuclear@10: Matrix4x4 Matrix4x4::identity = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); nuclear@10: nuclear@10: Matrix4x4::Matrix4x4() nuclear@10: { nuclear@10: *this = Matrix4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); nuclear@10: } nuclear@10: nuclear@10: Matrix4x4::Matrix4x4( scalar_t m11, scalar_t m12, scalar_t m13, scalar_t m14, nuclear@10: scalar_t m21, scalar_t m22, scalar_t m23, scalar_t m24, nuclear@10: scalar_t m31, scalar_t m32, scalar_t m33, scalar_t m34, nuclear@10: scalar_t m41, scalar_t m42, scalar_t m43, scalar_t m44) nuclear@10: { nuclear@10: m[0][0] = m11; m[0][1] = m12; m[0][2] = m13; m[0][3] = m14; nuclear@10: m[1][0] = m21; m[1][1] = m22; m[1][2] = m23; m[1][3] = m24; nuclear@10: m[2][0] = m31; m[2][1] = m32; m[2][2] = m33; m[2][3] = m34; nuclear@10: m[3][0] = m41; m[3][1] = m42; m[3][2] = m43; m[3][3] = m44; nuclear@10: } nuclear@10: nuclear@10: Matrix4x4::Matrix4x4(const mat4_t cmat) nuclear@10: { nuclear@10: memcpy(m, cmat, sizeof(mat4_t)); nuclear@10: } nuclear@10: nuclear@10: Matrix4x4::Matrix4x4(const Matrix3x3 &mat3x3) nuclear@10: { nuclear@10: reset_identity(); nuclear@10: for(int i=0; i<3; i++) { nuclear@10: for(int j=0; j<3; j++) { nuclear@10: m[i][j] = mat3x3[i][j]; nuclear@10: } nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: Matrix4x4 operator +(const Matrix4x4 &m1, const Matrix4x4 &m2) nuclear@10: { nuclear@10: Matrix4x4 res; nuclear@10: const scalar_t *op1 = m1.m[0], *op2 = m2.m[0]; nuclear@10: scalar_t *dest = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *dest++ = *op1++ + *op2++; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: Matrix4x4 operator -(const Matrix4x4 &m1, const Matrix4x4 &m2) nuclear@10: { nuclear@10: Matrix4x4 res; nuclear@10: const scalar_t *op1 = m1.m[0], *op2 = m2.m[0]; nuclear@10: scalar_t *dest = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *dest++ = *op1++ - *op2++; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: void operator +=(Matrix4x4 &m1, const Matrix4x4 &m2) nuclear@10: { nuclear@10: scalar_t *op1 = m1.m[0]; nuclear@10: const scalar_t *op2 = m2.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *op1++ += *op2++; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: void operator -=(Matrix4x4 &m1, const Matrix4x4 &m2) nuclear@10: { nuclear@10: scalar_t *op1 = m1.m[0]; nuclear@10: const scalar_t *op2 = m2.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *op1++ -= *op2++; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: Matrix4x4 operator *(const Matrix4x4 &mat, scalar_t scalar) nuclear@10: { nuclear@10: Matrix4x4 res; nuclear@10: const scalar_t *mptr = mat.m[0]; nuclear@10: scalar_t *dptr = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *dptr++ = *mptr++ * scalar; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: Matrix4x4 operator *(scalar_t scalar, const Matrix4x4 &mat) nuclear@10: { nuclear@10: Matrix4x4 res; nuclear@10: const scalar_t *mptr = mat.m[0]; nuclear@10: scalar_t *dptr = res.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *dptr++ = *mptr++ * scalar; nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: void operator *=(Matrix4x4 &mat, scalar_t scalar) nuclear@10: { nuclear@10: scalar_t *mptr = mat.m[0]; nuclear@10: nuclear@10: for(int i=0; i<16; i++) { nuclear@10: *mptr++ *= scalar; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::translate(const Vector3 &trans) nuclear@10: { nuclear@10: Matrix4x4 tmat(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1); nuclear@10: *this *= tmat; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_translation(const Vector3 &trans) nuclear@10: { nuclear@10: *this = Matrix4x4(1, 0, 0, trans.x, 0, 1, 0, trans.y, 0, 0, 1, trans.z, 0, 0, 0, 1); nuclear@10: } nuclear@10: nuclear@10: Vector3 Matrix4x4::get_translation() const nuclear@10: { nuclear@10: return Vector3(m[0][3], m[1][3], m[2][3]); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::rotate(const Vector3 &euler_angles) nuclear@10: { nuclear@10: Matrix3x3 xrot, yrot, zrot; nuclear@10: nuclear@10: xrot = Matrix3x3( 1, 0, 0, nuclear@10: 0, cos(euler_angles.x), -sin(euler_angles.x), nuclear@10: 0, sin(euler_angles.x), cos(euler_angles.x)); nuclear@10: nuclear@10: yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y), nuclear@10: 0, 1, 0, nuclear@10: -sin(euler_angles.y), 0, cos(euler_angles.y)); nuclear@10: nuclear@10: zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0, nuclear@10: sin(euler_angles.z), cos(euler_angles.z), 0, nuclear@10: 0, 0, 1); nuclear@10: nuclear@10: *this *= Matrix4x4(xrot * yrot * zrot); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_rotation(const Vector3 &euler_angles) nuclear@10: { nuclear@10: Matrix3x3 xrot, yrot, zrot; nuclear@10: nuclear@10: xrot = Matrix3x3( 1, 0, 0, nuclear@10: 0, cos(euler_angles.x), -sin(euler_angles.x), nuclear@10: 0, sin(euler_angles.x), cos(euler_angles.x)); nuclear@10: nuclear@10: yrot = Matrix3x3( cos(euler_angles.y), 0, sin(euler_angles.y), nuclear@10: 0, 1, 0, nuclear@10: -sin(euler_angles.y), 0, cos(euler_angles.y)); nuclear@10: nuclear@10: zrot = Matrix3x3( cos(euler_angles.z), -sin(euler_angles.z), 0, nuclear@10: sin(euler_angles.z), cos(euler_angles.z), 0, nuclear@10: 0, 0, 1); nuclear@10: nuclear@10: *this = Matrix4x4(xrot * yrot * zrot); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::rotate(const Vector3 &axis, scalar_t angle) nuclear@10: { nuclear@10: scalar_t sina = (scalar_t)sin(angle); nuclear@10: scalar_t cosa = (scalar_t)cos(angle); nuclear@10: scalar_t invcosa = 1-cosa; nuclear@10: scalar_t nxsq = axis.x * axis.x; nuclear@10: scalar_t nysq = axis.y * axis.y; nuclear@10: scalar_t nzsq = axis.z * axis.z; nuclear@10: nuclear@10: Matrix4x4 xform; nuclear@10: xform[0][0] = nxsq + (1-nxsq) * cosa; nuclear@10: xform[0][1] = axis.x * axis.y * invcosa - axis.z * sina; nuclear@10: xform[0][2] = axis.x * axis.z * invcosa + axis.y * sina; nuclear@10: xform[1][0] = axis.x * axis.y * invcosa + axis.z * sina; nuclear@10: xform[1][1] = nysq + (1-nysq) * cosa; nuclear@10: xform[1][2] = axis.y * axis.z * invcosa - axis.x * sina; nuclear@10: xform[2][0] = axis.x * axis.z * invcosa - axis.y * sina; nuclear@10: xform[2][1] = axis.y * axis.z * invcosa + axis.x * sina; nuclear@10: xform[2][2] = nzsq + (1-nzsq) * cosa; nuclear@10: nuclear@10: *this *= xform; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_rotation(const Vector3 &axis, scalar_t angle) nuclear@10: { nuclear@10: scalar_t sina = (scalar_t)sin(angle); nuclear@10: scalar_t cosa = (scalar_t)cos(angle); nuclear@10: scalar_t invcosa = 1-cosa; nuclear@10: scalar_t nxsq = axis.x * axis.x; nuclear@10: scalar_t nysq = axis.y * axis.y; nuclear@10: scalar_t nzsq = axis.z * axis.z; nuclear@10: nuclear@10: reset_identity(); nuclear@10: m[0][0] = nxsq + (1-nxsq) * cosa; nuclear@10: m[0][1] = axis.x * axis.y * invcosa - axis.z * sina; nuclear@10: m[0][2] = axis.x * axis.z * invcosa + axis.y * sina; nuclear@10: m[1][0] = axis.x * axis.y * invcosa + axis.z * sina; nuclear@10: m[1][1] = nysq + (1-nysq) * cosa; nuclear@10: m[1][2] = axis.y * axis.z * invcosa - axis.x * sina; nuclear@10: m[2][0] = axis.x * axis.z * invcosa - axis.y * sina; nuclear@10: m[2][1] = axis.y * axis.z * invcosa + axis.x * sina; nuclear@10: m[2][2] = nzsq + (1-nzsq) * cosa; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::rotate(const Quaternion &quat) nuclear@10: { nuclear@10: *this *= quat.get_rotation_matrix(); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_rotation(const Quaternion &quat) nuclear@10: { nuclear@10: *this = quat.get_rotation_matrix(); nuclear@10: } nuclear@10: nuclear@10: Quaternion Matrix4x4::get_rotation_quat() const nuclear@10: { nuclear@10: Matrix3x3 mat3 = *this; nuclear@10: return mat3.get_rotation_quat(); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::scale(const Vector4 &scale_vec) nuclear@10: { nuclear@10: Matrix4x4 smat( scale_vec.x, 0, 0, 0, nuclear@10: 0, scale_vec.y, 0, 0, nuclear@10: 0, 0, scale_vec.z, 0, nuclear@10: 0, 0, 0, scale_vec.w); nuclear@10: *this *= smat; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_scaling(const Vector4 &scale_vec) nuclear@10: { nuclear@10: *this = Matrix4x4( scale_vec.x, 0, 0, 0, nuclear@10: 0, scale_vec.y, 0, 0, nuclear@10: 0, 0, scale_vec.z, 0, nuclear@10: 0, 0, 0, scale_vec.w); nuclear@10: } nuclear@10: nuclear@10: Vector3 Matrix4x4::get_scaling() const nuclear@10: { nuclear@10: Vector3 vi = get_row_vector(0); nuclear@10: Vector3 vj = get_row_vector(1); nuclear@10: Vector3 vk = get_row_vector(2); nuclear@10: nuclear@10: return Vector3(vi.length(), vj.length(), vk.length()); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_frustum(float left, float right, float bottom, float top, float znear, float zfar) nuclear@10: { nuclear@10: float dx = right - left; nuclear@10: float dy = top - bottom; nuclear@10: float dz = zfar - znear; nuclear@10: nuclear@10: float a = (right + left) / dx; nuclear@10: float b = (top + bottom) / dy; nuclear@10: float c = -(zfar + znear) / dz; nuclear@10: float d = -2.0 * zfar * znear / dz; nuclear@10: nuclear@10: *this = Matrix4x4(2.0 * znear / dx, 0, a, 0, nuclear@10: 0, 2.0 * znear / dy, b, 0, nuclear@10: 0, 0, c, d, nuclear@10: 0, 0, -1, 0); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_perspective(float vfov, float aspect, float znear, float zfar) nuclear@10: { nuclear@10: float f = 1.0f / tan(vfov * 0.5f); nuclear@10: float dz = znear - zfar; nuclear@10: nuclear@10: reset_identity(); nuclear@10: nuclear@10: m[0][0] = f / aspect; nuclear@10: m[1][1] = f; nuclear@10: m[2][2] = (zfar + znear) / dz; nuclear@10: m[3][2] = -1.0f; nuclear@10: m[2][3] = 2.0f * zfar * znear / dz; nuclear@10: m[3][3] = 0.0f; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_orthographic(float left, float right, float bottom, float top, float znear, float zfar) nuclear@10: { nuclear@10: float dx = right - left; nuclear@10: float dy = top - bottom; nuclear@10: float dz = zfar - znear; nuclear@10: nuclear@10: reset_identity(); nuclear@10: nuclear@10: m[0][0] = 2.0 / dx; nuclear@10: m[1][1] = 2.0 / dy; nuclear@10: m[2][2] = -2.0 / dz; nuclear@10: m[0][3] = -(right + left) / dx; nuclear@10: m[1][3] = -(top + bottom) / dy; nuclear@10: m[2][3] = -(zfar + znear) / dz; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_lookat(const Vector3 &pos, const Vector3 &targ, const Vector3 &up) nuclear@10: { nuclear@10: Vector3 vk = (targ - pos).normalized(); nuclear@10: Vector3 vj = up.normalized(); nuclear@10: Vector3 vi = cross_product(vk, vj).normalized(); nuclear@10: vj = cross_product(vi, vk); nuclear@10: nuclear@10: *this = Matrix4x4( nuclear@10: vi.x, vi.y, vi.z, 0, nuclear@10: vj.x, vj.y, vj.z, 0, nuclear@10: -vk.x, -vk.y, -vk.z, 0, nuclear@10: 0, 0, 0, 1); nuclear@10: translate(-pos); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_column_vector(const Vector4 &vec, unsigned int col_index) nuclear@10: { nuclear@10: m[0][col_index] = vec.x; nuclear@10: m[1][col_index] = vec.y; nuclear@10: m[2][col_index] = vec.z; nuclear@10: m[3][col_index] = vec.w; nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::set_row_vector(const Vector4 &vec, unsigned int row_index) nuclear@10: { nuclear@10: m[row_index][0] = vec.x; nuclear@10: m[row_index][1] = vec.y; nuclear@10: m[row_index][2] = vec.z; nuclear@10: m[row_index][3] = vec.w; nuclear@10: } nuclear@10: nuclear@10: Vector4 Matrix4x4::get_column_vector(unsigned int col_index) const nuclear@10: { nuclear@10: return Vector4(m[0][col_index], m[1][col_index], m[2][col_index], m[3][col_index]); nuclear@10: } nuclear@10: nuclear@10: Vector4 Matrix4x4::get_row_vector(unsigned int row_index) const nuclear@10: { nuclear@10: return Vector4(m[row_index][0], m[row_index][1], m[row_index][2], m[row_index][3]); nuclear@10: } nuclear@10: nuclear@10: void Matrix4x4::transpose() nuclear@10: { nuclear@10: Matrix4x4 tmp = *this; nuclear@10: for(int i=0; i<4; i++) { nuclear@10: for(int j=0; j<4; j++) { nuclear@10: m[i][j] = tmp[j][i]; nuclear@10: } nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: Matrix4x4 Matrix4x4::transposed() const nuclear@10: { nuclear@10: Matrix4x4 res; nuclear@10: for(int i=0; i<4; i++) { nuclear@10: for(int j=0; j<4; j++) { nuclear@10: res[i][j] = m[j][i]; nuclear@10: } nuclear@10: } nuclear@10: return res; nuclear@10: } nuclear@10: nuclear@10: scalar_t Matrix4x4::determinant() const nuclear@10: { nuclear@10: scalar_t det11 = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@10: (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + nuclear@10: (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); nuclear@10: nuclear@10: scalar_t det12 = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@10: (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@10: (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); nuclear@10: nuclear@10: scalar_t det13 = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - nuclear@10: (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@10: (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@10: nuclear@10: scalar_t det14 = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - nuclear@10: (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + nuclear@10: (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@10: nuclear@10: return m[0][0] * det11 - m[0][1] * det12 + m[0][2] * det13 - m[0][3] * det14; nuclear@10: } nuclear@10: nuclear@10: nuclear@10: Matrix4x4 Matrix4x4::adjoint() const nuclear@10: { nuclear@10: Matrix4x4 coef; nuclear@10: nuclear@10: coef.m[0][0] = (m[1][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@10: (m[1][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + nuclear@10: (m[1][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); nuclear@10: coef.m[0][1] = (m[1][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@10: (m[1][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@10: (m[1][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); nuclear@10: coef.m[0][2] = (m[1][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - nuclear@10: (m[1][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@10: (m[1][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@10: coef.m[0][3] = (m[1][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - nuclear@10: (m[1][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + nuclear@10: (m[1][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@10: nuclear@10: coef.m[1][0] = (m[0][1] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@10: (m[0][2] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) + nuclear@10: (m[0][3] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])); nuclear@10: coef.m[1][1] = (m[0][0] * (m[2][2] * m[3][3] - m[3][2] * m[2][3])) - nuclear@10: (m[0][2] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@10: (m[0][3] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])); nuclear@10: coef.m[1][2] = (m[0][0] * (m[2][1] * m[3][3] - m[3][1] * m[2][3])) - nuclear@10: (m[0][1] * (m[2][0] * m[3][3] - m[3][0] * m[2][3])) + nuclear@10: (m[0][3] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@10: coef.m[1][3] = (m[0][0] * (m[2][1] * m[3][2] - m[3][1] * m[2][2])) - nuclear@10: (m[0][1] * (m[2][0] * m[3][2] - m[3][0] * m[2][2])) + nuclear@10: (m[0][2] * (m[2][0] * m[3][1] - m[3][0] * m[2][1])); nuclear@10: nuclear@10: coef.m[2][0] = (m[0][1] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) - nuclear@10: (m[0][2] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) + nuclear@10: (m[0][3] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])); nuclear@10: coef.m[2][1] = (m[0][0] * (m[1][2] * m[3][3] - m[3][2] * m[1][3])) - nuclear@10: (m[0][2] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) + nuclear@10: (m[0][3] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])); nuclear@10: coef.m[2][2] = (m[0][0] * (m[1][1] * m[3][3] - m[3][1] * m[1][3])) - nuclear@10: (m[0][1] * (m[1][0] * m[3][3] - m[3][0] * m[1][3])) + nuclear@10: (m[0][3] * (m[1][0] * m[3][1] - m[3][0] * m[1][1])); nuclear@10: coef.m[2][3] = (m[0][0] * (m[1][1] * m[3][2] - m[3][1] * m[1][2])) - nuclear@10: (m[0][1] * (m[1][0] * m[3][2] - m[3][0] * m[1][2])) + nuclear@10: (m[0][2] * (m[1][0] * m[3][1] - m[3][0] * m[1][1])); nuclear@10: nuclear@10: coef.m[3][0] = (m[0][1] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) - nuclear@10: (m[0][2] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) + nuclear@10: (m[0][3] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])); nuclear@10: coef.m[3][1] = (m[0][0] * (m[1][2] * m[2][3] - m[2][2] * m[1][3])) - nuclear@10: (m[0][2] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) + nuclear@10: (m[0][3] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])); nuclear@10: coef.m[3][2] = (m[0][0] * (m[1][1] * m[2][3] - m[2][1] * m[1][3])) - nuclear@10: (m[0][1] * (m[1][0] * m[2][3] - m[2][0] * m[1][3])) + nuclear@10: (m[0][3] * (m[1][0] * m[2][1] - m[2][0] * m[1][1])); nuclear@10: coef.m[3][3] = (m[0][0] * (m[1][1] * m[2][2] - m[2][1] * m[1][2])) - nuclear@10: (m[0][1] * (m[1][0] * m[2][2] - m[2][0] * m[1][2])) + nuclear@10: (m[0][2] * (m[1][0] * m[2][1] - m[2][0] * m[1][1])); nuclear@10: nuclear@10: coef.transpose(); nuclear@10: nuclear@10: for(int i=0; i<4; i++) { nuclear@10: for(int j=0; j<4; j++) { nuclear@10: coef.m[i][j] = j%2 ? -coef.m[i][j] : coef.m[i][j]; nuclear@10: if(i%2) coef.m[i][j] = -coef.m[i][j]; nuclear@10: } nuclear@10: } nuclear@10: nuclear@10: return coef; nuclear@10: } nuclear@10: nuclear@10: Matrix4x4 Matrix4x4::inverse() const nuclear@10: { nuclear@10: Matrix4x4 adj = adjoint(); nuclear@10: nuclear@10: return adj * (1.0f / determinant()); nuclear@10: } nuclear@10: nuclear@10: /* nuclear@10: ostream &operator <<(ostream &out, const Matrix4x4 &mat) nuclear@10: { nuclear@10: for(int i=0; i<4; i++) { nuclear@10: char str[100]; nuclear@10: sprintf(str, "[ %12.5f %12.5f %12.5f %12.5f ]\n", (float)mat.m[i][0], (float)mat.m[i][1], (float)mat.m[i][2], (float)mat.m[i][3]); nuclear@10: out << str; nuclear@10: } nuclear@10: return out; nuclear@10: } nuclear@10: */