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/const.h | 2 +- src/math/func.h | 24 +-- src/math/matrix.h | 340 +++++++++++++++++++++++++++++++++--------- src/math/test/CMakeLists.txt | 14 ++ src/math/test/gendata.m | 86 +++++++++++ src/math/test/matrix_test.cpp | 287 +++++++++++++++++++++++++++++++++++ src/math/vector.h | 100 ++++++++++--- 7 files changed, 750 insertions(+), 103 deletions(-) create mode 100644 src/math/test/CMakeLists.txt create mode 100644 src/math/test/gendata.m create mode 100644 src/math/test/matrix_test.cpp (limited to 'src') diff --git a/src/math/const.h b/src/math/const.h index c33f8f4..c96da44 100644 --- a/src/math/const.h +++ b/src/math/const.h @@ -40,5 +40,5 @@ namespace Math //! Degrees to radians multiplier const float DEG_TO_RAD = 0.01745329251994329547f; //! Radians to degrees multiplier - const FLOAT RAD_TO_DEG = 57.29577951308232286465f; + const float RAD_TO_DEG = 57.29577951308232286465f; }; diff --git a/src/math/func.h b/src/math/func.h index d9a3c59..8043329 100644 --- a/src/math/func.h +++ b/src/math/func.h @@ -21,6 +21,7 @@ #pragma once #include "const.h" +#include "point.h" #include #include @@ -28,10 +29,16 @@ namespace Math { -//! Compares a and b within TOLERANCE -bool IsEqual(float a, float b) +//! Compares \a a and \a b within \a tolerance +inline bool IsEqual(float a, float b, float tolerance = Math::TOLERANCE) { - return Abs(a - b) < TOLERANCE; + return fabs(a - b) < tolerance; +} + +//! Compares \a a to zero within \a tolerance +inline bool IsZero(float a, float tolerance = Math::TOLERANCE) +{ + return IsEqual(a, 0.0f, tolerance); } //! Minimum @@ -86,12 +93,6 @@ inline float Norm(float a) return a; } -//! Returns the absolute value -inline float Abs(float a) -{ - return (float)std::fabs(a); -} - //! Swaps two integers inline void Swap(int &a, int &b) { @@ -188,7 +189,7 @@ inline float Rand() out: -1 0 0 1 */ float Neutral(float value, float dead) { - if ( Abs(value) <= dead ) + if ( fabs(value) <= dead ) { return 0.0f; } @@ -252,3 +253,6 @@ inline float Bounce(float progress, float middle, float bounce) return (1.0f-bounce/2.0f)+sinf((0.5f+progress*2.0f)*PI)*(bounce/2.0f); } } + +}; // namespace Math + 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 diff --git a/src/math/test/CMakeLists.txt b/src/math/test/CMakeLists.txt new file mode 100644 index 0000000..d82a9b9 --- /dev/null +++ b/src/math/test/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8) + +set(CMAKE_BUILD_TYPE debug) +set(CMAKE_CXX_FLAGS_DEBUG "-Wall -g -O0") + +add_executable(matrix_test matrix_test.cpp) + +enable_testing() + +add_test(matrix_test ./matrix_test) + +add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS matrix_test) + +add_custom_target(distclean COMMAND rm -rf ./matrix_test CMakeFiles Testing cmake_install.cmake CMakeCache.txt CTestTestfile.cmake Makefile) \ No newline at end of file diff --git a/src/math/test/gendata.m b/src/math/test/gendata.m new file mode 100644 index 0000000..5c13491 --- /dev/null +++ b/src/math/test/gendata.m @@ -0,0 +1,86 @@ +% Script in Octave for generating test data + +1; + +% Returns the minor matrix +function m = minor(A, r, c) + + m = A; + m(r,:) = []; + m(:,c) = []; + +end; + +% Returns the cofactor matrix +function m = cofactors(A) + + m = zeros(rows(A), columns(A)); + + for r = [1 : rows(A)] + for c = [1 : columns(A)] + m(r, c) = det(minor(A, r, c)); + if (mod(r + c, 2) == 1) + m(r, c) = -m(r, c); + end; + end; + end; + +end; + +% Prints the matrix as C++ code +function printout(A, name) + + printf('const float %s[16] = \n', name); + printf('{\n'); + + for c = [1 : columns(A)] + for r = [1 : rows(A)] + printf(' %f', A(r,c)); + if (! ( (r == 4) && (c == 4) ) ) + printf(','); + end; + printf('\n'); + end; + end; + + printf('};\n'); + +end; + +printf('// Cofactors\n'); +A = randn(4,4); +printout(A, 'COF_MAT'); +printf('\n'); +printout(cofactors(A), 'COF_RESULT'); +printf('\n'); + +printf('\n'); + +printf('// Det\n'); +A = randn(4,4); +printout(A, 'DET_MAT'); +printf('\n'); +printf('const float DET_RESULT = %f;', det(A)); +printf('\n'); + +printf('\n'); + +printf('// Invert\n'); +A = randn(4,4); +printout(A, 'INV_MAT'); +printf('\n'); +printout(inv(A), 'COF_RESULT'); +printf('\n'); + +printf('\n'); + +printf('// Multiplication\n'); +A = randn(4,4); +printout(A, 'MUL_A'); +printf('\n'); +B = randn(4,4); +printout(B, 'MUL_B'); +printf('\n'); +C = A * B; +printout(C, 'MUL_RESULT'); +printf('\n'); diff --git a/src/math/test/matrix_test.cpp b/src/math/test/matrix_test.cpp new file mode 100644 index 0000000..c7e9c01 --- /dev/null +++ b/src/math/test/matrix_test.cpp @@ -0,0 +1,287 @@ +// * This file is part of the COLOBOT source code +// * Copyright (C) 2012, Polish Portal of Colobot (PPC) +// * +// * This program is free software: you can redistribute it and/or modify +// * it under the terms of the GNU General Public License as published by +// * the Free Software Foundation, either version 3 of the License, or +// * (at your option) any later version. +// * +// * This program is distributed in the hope that it will be useful, +// * but WITHOUT ANY WARRANTY; without even the implied warranty of +// * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// * GNU General Public License for more details. +// * +// * You should have received a copy of the GNU General Public License +// * along with this program. If not, see http://www.gnu.org/licenses/. + +// math/test/matrix_test.cpp + +/* Unit tests for Matrix struct + + Test data was randomly generated and the expected + results calculated using GNU Octave. + + */ + +#include "../func.h" +#include "../matrix.h" + +#include + +using namespace std; + +const float TEST_TOLERANCE = 1e-5; + +int TestCofactor() +{ + const float TEST_MATRIX[16] = + { + -0.306479, + -0.520207, + 0.127906, + 0.632922, + + -0.782876, + 0.015264, + 0.337479, + 1.466013, + + 0.072725, + -0.315123, + 1.613198, + -0.577377, + + 0.962397, + -1.320724, + 1.467588, + 0.579020 + }; + + const float EXPECTED_RESULTS[4][4] = + { + { 2.791599, -0.249952, 1.065075, -1.356570 }, + { 3.715943, -1.537511, 0.094812, -0.074520 }, + { 1.034500, -0.731752, -0.920756, -0.196235 }, + { 1.213928, -1.236857, 0.779741, -0.678482 } + }; + + Math::Matrix mat(TEST_MATRIX); + + for (int r = 0; r < 4; ++r) + { + for (int c = 0; c < 4; ++c) + { + float ret = mat.Cofactor(r, c); + float exp = EXPECTED_RESULTS[r][c]; + if (! Math::IsEqual(ret, exp, TEST_TOLERANCE)) + { + fprintf(stderr, "Cofactor r=%d, c=%d, %f (returned) != %f (expected)\n", r, c, ret, exp); + return 4*c+r; + } + } + } + + return 0; +} + +int TestDet() +{ + const float TEST_MATRIX[16] = + { + 0.85554, + 0.11624, + 1.30411, + 0.81467, + + 0.49692, + -1.92483, + -1.33543, + 0.85042, + + -0.16775, + 0.35344, + 1.40673, + 0.13961, + + 1.40709, + 0.11731, + 0.69042, + 0.91216 + }; + + const float EXPECTED_RESULT = 0.084360; + + float ret = Math::Matrix(TEST_MATRIX).Det(); + if (! Math::IsEqual(ret, EXPECTED_RESULT, TEST_TOLERANCE)) + { + fprintf(stderr, "Det %f (returned) != %f (expected)\n", ret, EXPECTED_RESULT); + return 1; + } + + return 0; +} + +int TestInvert() +{ + const float TEST_MATRIX[16] = + { + 1.4675123, + -0.2857923, + -0.0496217, + -1.2825408, + + -0.2804135, + -0.0826255, + -0.6825495, + 1.1661259, + + 0.0032798, + 0.5999200, + -1.8359883, + -1.1894424, + + -1.1501538, + -2.8792485, + 0.0299345, + 0.3730919 + }; + + const float EXPECTED_RESULT[16] = + { + 0.685863, + 0.562274, + -0.229722, + -0.132079, + + -0.266333, + -0.139862, + 0.054211, + -0.305568, + + -0.130817, + -0.494076, + -0.358226, + -0.047477, + + 0.069486, + 0.693649, + -0.261074, + -0.081200 + }; + + Math::Matrix mat(TEST_MATRIX); + mat.Invert(); + + if (! Math::MatricesEqual(mat, EXPECTED_RESULT, TEST_TOLERANCE)) + { + fprintf(stderr, "Invert mismatch\n"); + return 1; + } + + return 0; +} + +int TestMultiply() +{ + const float TEST_MATRIX_A[16] = + { + -1.931420, + 0.843410, + 0.476929, + -0.493435, + 1.425659, + -0.176331, + 0.129096, + 0.551081, + -0.543530, + -0.190783, + -0.084744, + 1.379547, + -0.473377, + 1.643398, + 0.400539, + 0.702937 + }; + + const float TEST_MATRIX_B[16] = + { + 0.3517561, + 1.3903778, + -0.8048254, + -0.4090024, + + -1.5542159, + -0.6798636, + 1.6003393, + -0.1467117, + + 0.5043620, + -0.0068779, + 2.0697285, + -0.0463650, + + 0.9605451, + -0.4620149, + 1.2525952, + -1.3409909 + }; + + const float EXPECTED_RESULT[16] = + { + 1.933875, + -0.467099, + 0.251638, + -0.805156, + + 1.232207, + -1.737383, + -1.023401, + 2.496859, + + -2.086953, + -0.044468, + 0.045688, + 2.570036, + + -2.559921, + -1.551155, + -0.244802, + 0.056808 + }; + + Math::Matrix matA(TEST_MATRIX_A); + Math::Matrix matB(TEST_MATRIX_B); + + Math::Matrix mat; + Math::MultiplyMatrices(matA, matB, mat); + if (! Math::MatricesEqual(mat, Math::Matrix(EXPECTED_RESULT), TEST_TOLERANCE ) ) + { + fprintf(stderr, "Multiply mismath!\n"); + return 1; + } + + return 0; +} + +int main() +{ + int result = 0; + + result = TestCofactor(); + if (result != 0) + return result; + + result = TestDet(); + if (result != 0) + return result; + + result = TestInvert(); + if (result != 0) + return result; + + result = TestMultiply(); + if (result != 0) + return result; + + return result; +} diff --git a/src/math/vector.h b/src/math/vector.h index d8c5511..89deb25 100644 --- a/src/math/vector.h +++ b/src/math/vector.h @@ -21,6 +21,7 @@ #pragma once #include "const.h" +#include "func.h" #include @@ -85,16 +86,16 @@ struct Vector this->z = z; } - //! Loads the zero vector (0, 0, 0, 0) + //! Loads the zero vector (0, 0, 0) inline void LoadZero() { - x = y = z = w = 0.0f; + x = y = z = 0.0f; } //! Returns the vector length inline float Length() const { - return sqrt(x*x + y*y + z*z + w*w); + return sqrt(x*x + y*y + z*z); } //! Normalizes the vector @@ -104,12 +105,11 @@ struct Vector x /= l; y /= l; z /= l; - w /= l; } //! Calculates the cross product with another vector /** \a right right-hand side vector */ - inline void Cross(const Vector &right) + inline void CrossMultiply(const Vector &right) { Vector left = *this; x = left.y * right.z - left.z * right.y; @@ -118,7 +118,7 @@ struct Vector } //! Calculates the dot product with another vector - inline float Dot(const Vector &right) const + inline float DotMultiply(const Vector &right) const { return x * right.x + y * right.y + z * right.z; } @@ -126,7 +126,7 @@ struct Vector //! Returns the cosine of angle between this and another vector inline float CosAngle(const Vector &right) const { - return Dot(right) / (Length() * right.Length()); + return DotMultiply(right) / (Length() * right.Length()); } //! Returns angle (in radians) between this and another vector @@ -135,34 +135,90 @@ struct Vector return acos(CosAngle(right)); } - //! Calculates the result of multiplying m * this vector - inline MatrixMultiply(const Matrix &m) + //! Returns the inverted vector + inline Vector operator-() const { - float tx = x * m.m[0 ] + y * m.m[4 ] + z * m.m[8 ] + m.m[12]; - float ty = x * m.m[1 ] + y * m.m[5 ] + z * m.m[9 ] + m.m[13]; - float tz = x * m.m[2 ] + y * m.m[6 ] + z * m.m[10] + m.m[14]; - float tw = x * m.m[4 ] + y * m.m[7 ] + z * m.m[11] + m.m[15]; + return Vector(-x, -y, -z); + } - if (IsZero(tw)) - return; + //! Adds the given vector + inline const Vector& operator+=(const Vector &right) + { + x += right.x; + y += right.y; + z += right.z; + return *this; + } - x = tx / tw; - y = ty / tw; - z = tz / tw; + //! Adds two vectors + inline friend const Vector operator+(const Vector &left, const Vector &right) + { + return Vector(left.x + right.x, left.y + right.y, left.z + right.z); + } + + //! Subtracts the given vector + inline const Vector& operator-=(const Vector &right) + { + x -= right.x; + y -= right.y; + z -= right.z; + return *this; + } + + //! Subtracts two vectors + inline friend const Vector operator-(const Vector &left, const Vector &right) + { + return Vector(left.x - right.x, left.y - right.y, left.z - right.z); + } + + //! Multiplies by given scalar + inline const Vector& operator*=(const float &right) + { + x *= right; + y *= right; + z *= right; + return *this; + } + + //! Multiplies vector by scalar + inline friend const Vector operator*(const float &left, const Vector &right) + { + return Vector(left * right.x, left * right.y, left * right.z); + } + + //! Multiplies vector by scalar + inline friend const Vector operator*(const Vector &left, const float &right) + { + return Vector(left.x * right, left.y * right, left.z * right); + } + + //! Divides by given scalar + inline const Vector& operator/=(const float &right) + { + x /= right; + y /= right; + z /= right; + return *this; + } + + //! Divides vector by scalar + inline friend const Vector operator/(const Vector &left, const float &right) + { + return Vector(left.x / right, left.y / right, left.z / right); } }; //! Convenience function for calculating dot product -float DotProduct(const Vector &left, const Vector &right) +inline float DotProduct(const Vector &left, const Vector &right) { - return left.DotProduct(right); + return left.DotMultiply(right); } //! Convenience function for calculating cross product -Vector CrossProduct(const Vector &left, const Vector &right) +inline Vector CrossProduct(const Vector &left, const Vector &right) { Vector result = left; - result.CrossProduct(right); + result.CrossMultiply(right); return result; } -- cgit v1.2.3-1-g7c22