From 7369b10a87aed982de328fbfa242666928e021d6 Mon Sep 17 00:00:00 2001 From: Piotr Dziwinski Date: Sun, 29 Apr 2012 23:21:35 +0200 Subject: Structs continued --- src/math/matrix.h | 340 +++++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 270 insertions(+), 70 deletions(-) (limited to 'src/math/matrix.h') diff --git a/src/math/matrix.h b/src/math/matrix.h index 864789e..7aed5e5 100644 --- a/src/math/matrix.h +++ b/src/math/matrix.h @@ -21,8 +21,11 @@ #pragma once #include "const.h" +#include "func.h" +#include "vector.h" #include +#include // Math module namespace namespace Math @@ -33,6 +36,18 @@ namespace Math Represents an universal 4x4 matrix that can be used in OpenGL and DirectX engines. Contains the required methods for operating on matrices (inverting, multiplying, etc.). + The internal representation is a 16-value table in column-major order, thus: + + m[0 ] m[4 ] m[8 ] m[12] + m[1 ] m[5 ] m[9 ] m[13] + m[2 ] m[6 ] m[10] m[14] + m[3 ] m[7 ] m[11] m[15] + + This representation is native to OpenGL; DirectX requires transposing the matrix. + + The order of multiplication of matrix and vector is also OpenGL-native + (see the method Vector::MultiplyMatrix). + All methods are made inline to maximize optimization. TODO test @@ -40,7 +55,7 @@ namespace Math **/ struct Matrix { - //! Matrix values in row-major format + //! Matrix values in column-major format float m[16]; //! Creates the indentity matrix @@ -50,10 +65,11 @@ struct Matrix } //! Creates the matrix from given values - /** \a m values in row-major format */ - inline Matrix(float m[16]) + /** \a m values in column-major format */ + inline Matrix(const float (&m)[16]) { - this->m = m; + for (int i = 0; i < 16; ++i) + this->m[i] = m[i]; } //! Loads the zero matrix @@ -70,20 +86,8 @@ struct Matrix m[0] = m[5] = m[10] = m[15] = 1.0f; } - //! Calculates the determinant of the matrix - /** \returns the determinant */ - float Det() const - { - float result = 0.0f; - for (int i = 0; i < 4; ++i) - { - result += m[0][i] * Cofactor(0, i); - } - return result; - } - //! Transposes the matrix - void Transpose() + inline void Transpose() { Matrix temp = *this; for (int r = 0; r < 4; ++r) @@ -95,36 +99,196 @@ struct Matrix } } + //! Calculates the determinant of the matrix + /** \returns the determinant */ + inline float Det() const + { + float result = 0.0f; + for (int i = 0; i < 4; ++i) + { + result += m[i] * Cofactor(i, 0); + } + return result; + } + //! Calculates the cofactor of the matrix /** \a r row (0 to 3) \a c column (0 to 3) \returns the cofactor or 0.0f if invalid r, c given*/ - float Cofactor(int r, int c) const + inline float Cofactor(int r, int c) const { - if ((r < 0) || (r > 3) || (c < 0) || (c > 3)) - return 0.0f; + assert(r >= 0 && r <= 3); + assert(c >= 0 && c <= 3); - float tab[3][3]; - int tabR = 0; - for (int r = 0; r < 4; ++r) - { - if (r == i) continue; - int tabC = 0; - for (int c = 0; c < 4; ++c) - { - if (c == j) continue; - tab[tabR][tabC] = m[4*r + c]; - ++tabC; - } - ++tabR; - } + float result = 0.0f; - float result = tab[0][0] * (tab[1][1] * tab[2][2] - tab[1][2] * tab[2][1]) - - tab[0][1] * (tab[1][0] * tab[2][2] - tab[1][2] * tab[2][0]) - + tab[0][2] * (tab[1][0] * tab[2][1] - tab[1][1] * tab[2][0]); + /* That looks horrible, I know. But it's fast :) */ - if ((i + j) % 2 == 0) - result = -result; + switch (4*r + c) + { + // r=0, c=0 + /* 05 09 13 + 06 10 14 + 07 11 15 */ + case 0: + result = + m[5 ] * (m[10] * m[15] - m[14] * m[11]) + - m[9 ] * (m[6 ] * m[15] - m[14] * m[7 ]) + + m[13] * (m[6 ] * m[11] - m[10] * m[7 ]); + break; + + // r=0, c=1 + /* 01 09 13 + 02 10 14 + 03 11 15 */ + case 1: + result = - m[1 ] * (m[10] * m[15] - m[14] * m[11]) + + m[9 ] * (m[2 ] * m[15] - m[14] * m[3 ]) + - m[13] * (m[2 ] * m[11] - m[10] * m[3 ]); + break; + + // r=0, c=2 + /* 01 05 13 + 02 06 14 + 03 07 15 */ + case 2: + result = + m[1 ] * (m[6 ] * m[15] - m[14] * m[7 ]) + - m[5 ] * (m[2 ] * m[15] - m[14] * m[3 ]) + + m[13] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]); + break; + + // r=0, c=3 + /* 01 05 09 + 02 06 10 + 03 07 11 */ + case 3: + result = - m[1 ] * (m[6 ] * m[11] - m[10] * m[7 ]) + + m[5 ] * (m[2 ] * m[11] - m[10] * m[3 ]) + - m[9 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]); + break; + + // r=1, c=0 + /* 04 08 12 + 06 10 14 + 07 11 15 */ + case 4: + result = - m[4 ] * (m[10] * m[15] - m[14] * m[11]) + + m[8 ] * (m[6 ] * m[15] - m[14] * m[7 ]) + - m[12] * (m[6 ] * m[11] - m[10] * m[7 ]); + break; + + // r=1, c=1 + /* 00 08 12 + 02 10 14 + 03 11 15 */ + case 5: + result = + m[0 ] * (m[10] * m[15] - m[14] * m[11]) + - m[8 ] * (m[2 ] * m[15] - m[14] * m[3 ]) + + m[12] * (m[2 ] * m[11] - m[10] * m[3 ]); + break; + + // r=1, c=2 + /* 00 04 12 + 02 06 14 + 03 07 15 */ + case 6: + result = - m[0 ] * (m[6 ] * m[15] - m[14] * m[7 ]) + + m[4 ] * (m[2 ] * m[15] - m[14] * m[3 ]) + - m[12] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]); + break; + + // r=1, c=3 + /* 00 04 08 + 02 06 10 + 03 07 11 */ + case 7: + result = + m[0 ] * (m[6 ] * m[11] - m[10] * m[7 ]) + - m[4 ] * (m[2 ] * m[11] - m[10] * m[3 ]) + + m[8 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]); + break; + + // r=2, c=0 + /* 04 08 12 + 05 09 13 + 07 11 15 */ + case 8: + result = + m[4 ] * (m[9 ] * m[15] - m[13] * m[11]) + - m[8 ] * (m[5 ] * m[15] - m[13] * m[7 ]) + + m[12] * (m[5 ] * m[11] - m[9 ] * m[7 ]); + break; + + // r=2, c=1 + /* 00 08 12 + 01 09 13 + 03 11 15 */ + case 9: + result = - m[0 ] * (m[9 ] * m[15] - m[13] * m[11]) + + m[8 ] * (m[1 ] * m[15] - m[13] * m[3 ]) + - m[12] * (m[1 ] * m[11] - m[9 ] * m[3 ]); + break; + + // r=2, c=2 + /* 00 04 12 + 01 05 13 + 03 07 15 */ + case 10: + result = + m[0 ] * (m[5 ] * m[15] - m[13] * m[7 ]) + - m[4 ] * (m[1 ] * m[15] - m[13] * m[3 ]) + + m[12] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]); + break; + + // r=2, c=3 + /* 00 04 08 + 01 05 09 + 03 07 11 */ + case 11: + result = - m[0 ] * (m[5 ] * m[11] - m[9 ] * m[7 ]) + + m[4 ] * (m[1 ] * m[11] - m[9 ] * m[3 ]) + - m[8 ] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]); + break; + + // r=3, c=0 + /* 04 08 12 + 05 09 13 + 06 10 14 */ + case 12: + result = - m[4 ] * (m[9 ] * m[14] - m[13] * m[10]) + + m[8 ] * (m[5 ] * m[14] - m[13] * m[6 ]) + - m[12] * (m[5 ] * m[10] - m[9 ] * m[6 ]); + break; + + // r=3, c=1 + /* 00 08 12 + 01 09 13 + 02 10 14 */ + case 13: + result = + m[0 ] * (m[9 ] * m[14] - m[13] * m[10]) + - m[8 ] * (m[1 ] * m[14] - m[13] * m[2 ]) + + m[12] * (m[1 ] * m[10] - m[9 ] * m[2 ]); + break; + + // r=3, c=2 + /* 00 04 12 + 01 05 13 + 02 06 14 */ + case 14: + result = - m[0 ] * (m[5 ] * m[14] - m[13] * m[6 ]) + + m[4 ] * (m[1 ] * m[14] - m[13] * m[2 ]) + - m[12] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]); + break; + + // r=3, c=3 + /* 00 04 08 + 01 05 09 + 02 06 10 */ + case 15: + result = + m[0 ] * (m[5 ] * m[10] - m[9 ] * m[6 ]) + - m[4 ] * (m[1 ] * m[10] - m[9 ] * m[2 ]) + + m[8 ] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]); + break; + + default: + break; + } return result; } @@ -133,19 +297,16 @@ struct Matrix inline void Invert() { float d = Det(); - if (fabs(d) <= Math::TOLERANCE) - return; + assert(! IsZero(d)); Matrix temp = *this; for (int r = 0; r < 4; ++r) { for (int c = 0; c < 4; ++c) { - m[r][c] = (1.0f / d) * temp.Cofactor(r, c); + m[4*r+c] = (1.0f / d) * temp.Cofactor(r, c); } } - - Tranpose(); } //! Multiplies the matrix with the given matrix @@ -153,14 +314,14 @@ struct Matrix inline void Multiply(const Matrix &right) { Matrix left = *this; - for (int r = 0; r < 4; ++r) + for (int c = 0; c < 4; ++c) { - for (int c = 0; c < 4; ++c) + for (int r = 0; r < 4; ++r) { - m[r][c] = 0.0; + m[4*c+r] = 0.0f; for (int i = 0; i < 4; ++i) { - m[4*r+c] += left.m[4*r+i] * right.m[4*i+c]; + m[4*c+r] += left.m[4*i+r] * right.m[4*c+i]; } } } @@ -168,17 +329,16 @@ struct Matrix //! Loads view matrix from the given vectors /** \a from origin - \a at direction - \a up up vector */ - inline void LoadView(const Vector &from, const Vector &at, const Vector &up) + \a at view direction + \a worldUp up vector */ + inline void LoadView(const Vector &from, const Vector &at, const Vector &worldUp) { // Get the z basis vector, which points straight ahead. This is the // difference from the eyepoint to the lookat point. Vector view = at - from; - FLOAT length = view.Length(); - if( IsZero(length) ) - return; + float length = view.Length(); + assert(! Math::IsZero(length) ); // Normalize the z basis vector view /= length; @@ -200,8 +360,7 @@ struct Matrix { up = Vector(0.0f, 0.0f, 1.0f) - view.z * view; - if ( IsZero(up.Length()) ) - return; + assert(! IsZero(up.Length()) ); } } @@ -220,9 +379,9 @@ struct Matrix m[8 ] = right.z; m[9 ] = up.z; m[10] = view.z; // Do the translation values (rotations are still about the eyepoint) - m[3 ] = - DotProduct(from, right); - m[7 ] = - DotProduct(from, up); - m[11] = - DotProduct(from, view); + m[12] = - DotProduct(from, right); + m[13] = - DotProduct(from, up); + m[14] = - DotProduct(from, view); } inline void LoadProjection(float fov = 1.570795f, float aspect = 1.0f, @@ -233,12 +392,18 @@ struct Matrix inline void LoadTranslation(const Vector &trans) { - // TODO + LoadIdentity(); + m[12] = trans.x; + m[13] = trans.y; + m[14] = trans.z; } inline void LoadScale(const Vector &scale) { - // TODO + LoadIdentity(); + m[0] = scale.x; + m[5] = scale.y; + m[10] = scale.z; } inline void LoadRotationX(float angle) @@ -264,22 +429,26 @@ struct Matrix //! Calculates the matrix to make three rotations in the order X, Z and Y inline void RotateXZY(const Vector &angle) { + this->LoadRotationX(angle.x); + Matrix temp; - temp.SetRotateXMatrix(angle.x); - this->SetRotateZMatrix(angle.z); + temp.LoadRotationZ(angle.z); this->Multiply(temp); - temp.SetRotateYMatrix(angle.y); + + temp.LoadRotationY(angle.y); this->Multiply(temp); } //! Calculates the matrix to make three rotations in the order Z, X and Y inline void RotateZXY(const Vector &angle) { + this->LoadRotationZ(angle.z); + Matrix temp; - temp.SetRotateZMatrix(angle.z); - this->SetRotateXMatrix(angle.x); + temp.LoadRotationX(angle.x); this->Multiply(temp); - temp.SetRotateYMatrix(angle.y); + + temp.LoadRotationY(angle.y); this->Multiply(temp); } }; @@ -287,7 +456,7 @@ struct Matrix //! Convenience function for inverting a matrix /** \a m input matrix \a result result -- inverted matrix */ -void InvertMatrix(const Matrix &m, Matrix &result) +inline void InvertMatrix(const Matrix &m, Matrix &result) { result = m; result.Invert(); @@ -296,11 +465,42 @@ void InvertMatrix(const Matrix &m, Matrix &result) //! Convenience function for multiplying a matrix /** \a left left-hand matrix \a right right-hand matrix - \a result result -- multiplied matrices */* -void MultiplyMatrices(const Matrix &left, const Matrix &right, Matrix &result) + \a result result -- multiplied matrices */ +inline void MultiplyMatrices(const Matrix &left, const Matrix &right, Matrix &result) { result = left; result.Multiply(right); } +//! Calculates the result of multiplying m * v +inline Vector MatrixVectorMultiply(const Matrix &m, const Vector &v) +{ + float x = v.x * m.m[0 ] + v.y * m.m[4 ] + v.z * m.m[8 ] + m.m[12]; + float y = v.x * m.m[1 ] + v.y * m.m[5 ] + v.z * m.m[9 ] + m.m[13]; + float z = v.x * m.m[2 ] + v.y * m.m[6 ] + v.z * m.m[10] + m.m[14]; + float w = v.x * m.m[4 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15]; + + if (IsZero(w)) + return Vector(x, y, z); + + x /= w; + y /= w; + z /= w; + + return Vector(x, y, z); +} + +//! Checks if two matrices are equal within given \a tolerance +inline bool MatricesEqual(const Math::Matrix &m1, const Math::Matrix &m2, + float tolerance = Math::TOLERANCE) +{ + for (int i = 0; i < 16; ++i) + { + if (! IsEqual(m1.m[i], m2.m[i], tolerance)) + return false; + } + + return true; +} + }; // namespace Math -- cgit v1.2.3-1-g7c22