Structs continued

7 files changed, 750 insertions, 103 deletions

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 <cmath>
 #include <cstdlib>
@@ -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 <cmath>
+#include <cassert>
 
 // 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 <cstdio>
+
+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 <cmath>
 
@@ -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;
 }