Structs continued

Basic functions finished and tested for matrix and vector.

8 files changed, 624 insertions, 296 deletions

diff --git a/src/math/const.h b/src/math/const.h
index c96da44..79ad564 100644
--- a/src/math/const.h
+++ b/src/math/const.h
@@ -14,15 +14,18 @@
 // * 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/const.h
-
-/* Math constants */
+/** @defgroup MathConstModule math/const.h
+   Contains the math constants used in math functions.
+ */
 
 #pragma once
 
+
 // Math module namespace
 namespace Math
 {
+  /* @{ */ // start of group
+
   //! Tolerance level -- minimum accepted float value
   const float TOLERANCE = 1e-6f;
 
@@ -41,4 +44,6 @@ namespace Math
   const float DEG_TO_RAD =  0.01745329251994329547f;
   //! Radians to degrees multiplier
   const float RAD_TO_DEG = 57.29577951308232286465f;
-};
+
+  /* @} */ // end of group
+}; // namespace Math
diff --git a/src/math/func.h b/src/math/func.h
index 8043329..8dcae99 100644
--- a/src/math/func.h
+++ b/src/math/func.h
@@ -14,9 +14,9 @@
 // * 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/func.h
-
-/* Common math functions */
+/** @defgroup MathFuncModule math/func.h
+   Contains common math functions.
+ */
 
 #pragma once
 
@@ -26,9 +26,13 @@
 #include <cmath>
 #include <cstdlib>
 
+
+// Math module namespace
 namespace Math
 {
 
+/* @{ */ // start of group
+
 //! Compares \a a and \a b within \a tolerance
 inline bool IsEqual(float a, float b, float tolerance = Math::TOLERANCE)
 {
@@ -254,5 +258,6 @@ inline float Bounce(float progress, float middle, float bounce)
   }
 }
 
-}; // namespace Math
+/* @} */ // end of group
 
+}; // namespace Math
diff --git a/src/math/matrix.h b/src/math/matrix.h
index 7aed5e5..f02c1cf 100644
--- a/src/math/matrix.h
+++ b/src/math/matrix.h
@@ -14,9 +14,9 @@
 // * 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/matrix.h
-
-/* Matrix struct and functions */
+/** @defgroup MathMatrixModule math/matrix.h
+   Contains the Matrix struct and related functions.
+ */
 
 #pragma once
 
@@ -27,35 +27,39 @@
 #include <cmath>
 #include <cassert>
 
+
 // Math module namespace
 namespace Math
 {
 
-/** 4x4 matrix
+/* @{ */ // start of group
+
+/** \struct Matrix math/matrix.h
+    \brief 4x4 matrix
 
   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]
+  \verbatim 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] \endverbatim
 
   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).
+  (see the function MatrixVectorMultiply).
 
   All methods are made inline to maximize optimization.
 
-  TODO test
+  Unit tests for the structure and related functions are in module: math/test/matrix_test.cpp.
 
  **/
 struct Matrix
 {
-  //! Matrix values in column-major format
+  //! Matrix values in column-major order
   float m[16];
 
   //! Creates the indentity matrix
@@ -64,14 +68,28 @@ struct Matrix
     LoadIdentity();
   }
 
-  //! Creates the matrix from given values
-  /** \a m  values in column-major format */
+  //! Creates the matrix from 1D array
+  /** \a m matrix values in column-major order */
   inline Matrix(const float (&m)[16])
   {
     for (int i = 0; i < 16; ++i)
       this->m[i] = m[i];
   }
 
+  //! Creates the matrix from 2D array
+  /** The array's first index is row, second is column.
+      \a m array with values */
+  inline Matrix(const float (&m)[4][4])
+  {
+    for (int c = 0; c < 4; ++c)
+    {
+      for (int r = 0; r < 4; ++r)
+      {
+        this->m[4*c+r] = m[r][c];
+      }
+    }
+  }
+
   //! Loads the zero matrix
   inline void LoadZero()
   {
@@ -83,7 +101,10 @@ struct Matrix
   inline void LoadIdentity()
   {
     LoadZero();
-    m[0] = m[5] = m[10] = m[15] = 1.0f;
+    /* (1,1) */ m[0 ] = 1.0f;
+    /* (2,2) */ m[5 ] = 1.0f;
+    /* (3,3) */ m[10] = 1.0f;
+    /* (4,4) */ m[15] = 1.0f;
   }
 
   //! Transposes the matrix
@@ -114,7 +135,7 @@ struct Matrix
   //! 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*/
+      \returns the cofactor */
   inline float Cofactor(int r, int c) const
   {
     assert(r >= 0 && r <= 3);
@@ -293,38 +314,48 @@ struct Matrix
     return result;
   }
 
-  //! Inverts the matrix
-  inline void Invert()
+  //! Calculates the inverse matrix
+  /** The determinant of the matrix must not be zero.
+      \returns the inverted matrix */
+  inline Matrix Inverse() const
   {
     float d = Det();
     assert(! IsZero(d));
 
-    Matrix temp = *this;
+    float result[16] = { 0.0f };
+
     for (int r = 0; r < 4; ++r)
     {
       for (int c = 0; c < 4; ++c)
       {
-        m[4*r+c] = (1.0f / d) * temp.Cofactor(r, c);
+        // Already transposed!
+        result[4*r+c] = (1.0f / d) * Cofactor(r, c);
       }
     }
+
+    return Matrix(result);
   }
 
-  //! Multiplies the matrix with the given matrix
-  /** \a right right-hand matrix */
-  inline void Multiply(const Matrix &right)
+  //! Calculates the multiplication of this matrix * given matrix
+  /** \a right right-hand matrix
+      \returns multiplication result */
+  inline Matrix Multiply(const Matrix &right) const
   {
-    Matrix left = *this;
+    float result[16] = { 0.0f };
+
     for (int c = 0; c < 4; ++c)
     {
       for (int r = 0; r < 4; ++r)
       {
-        m[4*c+r] = 0.0f;
+        result[4*c+r] = 0.0f;
         for (int i = 0; i < 4; ++i)
         {
-          m[4*c+r] += left.m[4*i+r] * right.m[4*c+i];
+          result[4*c+r] += m[4*i+r] * right.m[4*c+i];
         }
       }
     }
+
+    return Matrix(result);
   }
 
   //! Loads view matrix from the given vectors
@@ -374,56 +405,122 @@ struct Matrix
     // Start building the matrix. The first three rows contains the basis
     // vectors used to rotate the view to point at the lookat point
     LoadIdentity();
-    m[0 ] = right.x;   m[1 ] = up.x;   m[2 ] = view.x;
-    m[4 ] = right.y;   m[5 ] = up.y;   m[6 ] = view.y;
-    m[8 ] = right.z;   m[9 ] = up.z;   m[10] = view.z;
+
+    /* (1,1) */ m[0 ] = right.x;
+    /* (2,1) */ m[1 ] = up.x;
+    /* (3,1) */ m[2 ] = view.x;
+    /* (1,2) */ m[4 ] = right.y;
+    /* (2,2) */ m[5 ] = up.y;
+    /* (3,2) */ m[6 ] = view.y;
+    /* (1,3) */ m[8 ] = right.z;
+    /* (2,3) */ m[9 ] = up.z;
+    /* (3,3) */ m[10] = view.z;
 
     // Do the translation values (rotations are still about the eyepoint)
-    m[12] = - DotProduct(from, right);
-    m[13] = - DotProduct(from, up);
-    m[14] = - DotProduct(from, view);
+    /* (1,4) */ m[12] = -DotProduct(from, right);
+    /* (2,4) */ m[13] = -DotProduct(from, up);
+    /* (3,4) */ m[14] = -DotProduct(from, view);
   }
 
+  //! Loads a perspective projection matrix
+  /** \a fov field of view in radians
+      \a aspect aspect ratio (width / height)
+      \a nearPlane distance to near cut plane
+      \a farPlane distance to far cut plane */
   inline void LoadProjection(float fov = 1.570795f, float aspect = 1.0f,
                              float nearPlane = 1.0f, float farPlane = 1000.0f)
   {
-    // TODO
+    assert(fabs(farPlane - nearPlane) >= 0.01f);
+    assert(fabs(sin(fov / 2)) >= 0.01f);
+
+    float w = aspect * (cosf(fov / 2) / sinf(fov / 2));
+    float h = 1.0f   * (cosf(fov / 2) / sinf(fov / 2));
+    float q = farPlane / (farPlane - nearPlane);
+
+    LoadZero();
+
+    /* (1,1) */ m[0 ] = w;
+    /* (2,2) */ m[5 ] = h;
+    /* (3,3) */ m[10] = q;
+    /* (3,4) */ m[14] = 1.0f;
+    /* (4,3) */ m[11] = -q * nearPlane;
   }
 
+  //! Loads a translation matrix from given vector
+  /** \a trans vector of translation*/
   inline void LoadTranslation(const Vector &trans)
   {
     LoadIdentity();
-    m[12] = trans.x;
-    m[13] = trans.y;
-    m[14] = trans.z;
+    /* (1,4) */ m[12] = trans.x;
+    /* (2,4) */ m[13] = trans.y;
+    /* (3,4) */ m[14] = trans.z;
   }
 
+  //! Loads a scaling matrix fom given vector
+  /** \a scale vector with scaling factors for X, Y, Z */
   inline void LoadScale(const Vector &scale)
   {
     LoadIdentity();
-    m[0] = scale.x;
-    m[5] = scale.y;
-    m[10] = scale.z;
+    /* (1,1) */ m[0 ] = scale.x;
+    /* (2,2) */ m[5 ] = scale.y;
+    /* (3,3) */ m[10] = scale.z;
   }
 
+  //! Loads a rotation matrix along the X axis
+  /** \a angle angle in radians */
   inline void LoadRotationX(float angle)
   {
-    // TODO
+    LoadIdentity();
+    /* (2,2) */ m[5 ] =  cosf(angle);
+    /* (3,2) */ m[6 ] =  sinf(angle);
+    /* (2,3) */ m[9 ] = -sinf(angle);
+    /* (3,3) */ m[10] =  cosf(angle);
   }
 
+  //! Loads a rotation matrix along the Y axis
+  /** \a angle angle in radians */
   inline void LoadRotationY(float angle)
   {
-    // TODO
+    LoadIdentity();
+    /* (1,1) */ m[0 ] =  cosf(angle);
+    /* (3,1) */ m[2 ] = -sinf(angle);
+    /* (1,3) */ m[8 ] =  sinf(angle);
+    /* (3,3) */ m[10] =  cosf(angle);
   }
 
+  //! Loads a rotation matrix along the Z axis
+  /** \a angle angle in radians */
   inline void LoadRotationZ(float angle)
   {
-    // TODO
+    LoadIdentity();
+    /* (1,1) */ m[0 ] =  cosf(angle);
+    /* (2,1) */ m[1 ] =  sinf(angle);
+    /* (1,2) */ m[4 ] = -sinf(angle);
+    /* (2,2) */ m[5 ] =  cosf(angle);
   }
 
+  //! Loads a rotation matrix along the given axis
+  /** \a dir axis of rotation
+      \a angle angle in radians */
   inline void LoadRotation(const Vector &dir, float angle)
   {
-    // TODO
+    float cos = cosf(angle);
+    float sin = sinf(angle);
+    Vector v = Math::Normalize(dir);
+
+    LoadIdentity();
+
+    /* (1,1) */ m[0 ] = (v.x * v.x) * (1.0f - cos) + cos;
+    /* (2,1) */ m[1 ] = (v.x * v.y) * (1.0f - cos) - (v.z * sin);
+    /* (3,1) */ m[2 ] = (v.x * v.z) * (1.0f - cos) + (v.y * sin);
+
+    /* (1,2) */ m[4 ] = (v.y * v.x) * (1.0f - cos) + (v.z * sin);
+    /* (2,2) */ m[5 ] = (v.y * v.y) * (1.0f - cos) + cos ;
+    /* (3,2) */ m[6 ] = (v.y * v.z) * (1.0f - cos) - (v.x * sin);
+
+    /* (1,3) */ m[8 ] = (v.z * v.x) * (1.0f - cos) - (v.y * sin);
+    /* (2,3) */ m[9 ] = (v.z * v.y) * (1.0f - cos) + (v.x * sin);
+    /* (3,3) */ m[10] = (v.z * v.z) * (1.0f - cos) + cos;
   }
 
   //! Calculates the matrix to make three rotations in the order X, Z and Y
@@ -453,32 +550,38 @@ struct Matrix
   }
 };
 
-//! Convenience function for inverting a matrix
-/** \a m input matrix
-    \a result result -- inverted matrix */
-inline void InvertMatrix(const Matrix &m, Matrix &result)
+//! Convenience function for getting transposed matrix
+inline Matrix Transpose(const Matrix &m)
 {
-  result = m;
-  result.Invert();
+  Matrix result = m;
+  result.Transpose();
+  return result;
 }
 
 //! Convenience function for multiplying a matrix
 /** \a left left-hand matrix
     \a right right-hand matrix
-    \a result result -- multiplied matrices */
-inline void MultiplyMatrices(const Matrix &left, const Matrix &right, Matrix &result)
+    \returns multiplied matrices */
+inline Matrix MultiplyMatrices(const Matrix &left, const Matrix &right)
 {
-  result = left;
-  result.Multiply(right);
+  return left.Multiply(right);
 }
 
 //! Calculates the result of multiplying m * v
+/** The multiplication is performed thus:
+\verbatim [  m.m[0 ] m.m[4 ] m.m[8 ] m.m[12]  ]   [ v.x ]
+[  m.m[1 ] m.m[5 ] m.m[9 ] m.m[13]  ]   [ v.y ]
+[  m.m[2 ] m.m[6 ] m.m[10] m.m[14]  ] * [ v.z ]
+[  m.m[3 ] m.m[7 ] m.m[11] m.m[15]  ]   [  1  ] \endverbatim
+
+    The result, a 4x1 vector is then converted to 3x1 by dividing
+    x,y,z coords by the fourth coord (w). */
 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];
+  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);
@@ -503,4 +606,6 @@ inline bool MatricesEqual(const Math::Matrix &m1, const Math::Matrix &m2,
   return true;
 }
 
+/* @} */ // end of group
+
 }; // namespace Math
diff --git a/src/math/point.h b/src/math/point.h
index f28411e..fa411d0 100644
--- a/src/math/point.h
+++ b/src/math/point.h
@@ -14,26 +14,49 @@
 // * 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/point.h
-
-/* Point struct and functions */
+/** @defgroup MathPointModule math/point.h
+   Contains the Point struct and related functions.
+ */
 
 #pragma once
 
 #include <cmath>
 
+
+/* TODO
+
+ FPOINT    RotatePoint(FPOINT center, float angle, FPOINT p);
+ FPOINT    RotatePoint(float angle, FPOINT p);
+ FPOINT    RotatePoint(float angle, float dist);
+ void      RotatePoint(float cx, float cy, float angle, float &px, float &py);
+ void      RotatePoint(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p);
+ void      RotatePoint2(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p);
+
+ float   RotateAngle(float x, float y);
+ float   RotateAngle(FPOINT center, FPOINT p1, FPOINT p2);
+ float   MidPoint(FPOINT a, FPOINT b, float px);
+ BOOL      IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c, FPOINT p);
+
+ BOOL      LineFunction(FPOINT p1, FPOINT p2, float &a, float &b);
+
+ float IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c);
+
+ */
+
+// Math module namespace
 namespace Math
 {
 
-/** 2D Point
+/* @{ */ // start of group
+
+/** \struct Point math/point.h
+    \brief 2D point
 
   Represents a 2D point (x, y).
   Contains the required methods for operating on points.
 
   All methods are made inline to maximize optimization.
 
-  TODO test
-
  */
 struct Point
 {
@@ -42,43 +65,38 @@ struct Point
   //! Y coord
   float y;
 
+  //! Constructs a zero point: (0,0)
   inline Point()
   {
     LoadZero();
   }
 
+  //! Constructs a point from given coords: (x,y)
   inline Point(float x, float y)
   {
     this->x = x;
     this->y = y;
   }
 
+  //! Sets the zero point: (0,0)
   inline void LoadZero()
   {
     x = y = 0.0f;
   }
 
+  //! Returns the distance from (0,0) to the point (x,y)
   inline float Length()
   {
     return std::sqrt(x*x + y*y);
   }
 };
-/* TODO
-FPOINT    RotatePoint(FPOINT center, float angle, FPOINT p);
-FPOINT    RotatePoint(float angle, FPOINT p);
-FPOINT    RotatePoint(float angle, float dist);
-void      RotatePoint(float cx, float cy, float angle, float &px, float &py);
-void      RotatePoint(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p);
-void      RotatePoint2(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p);
-float   Length(FPOINT a, FPOINT b);
-
-float   RotateAngle(float x, float y);
-float   RotateAngle(FPOINT center, FPOINT p1, FPOINT p2);
-float   MidPoint(FPOINT a, FPOINT b, float px);
-BOOL      IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c, FPOINT p);
 
-BOOL      LineFunction(FPOINT p1, FPOINT p2, float &a, float &b);
+//! Returns the distance between two points
+inline float Distance(const Point &a, const Point &b)
+{
+  return sqrt((a.x-b.x)*(a.x-b.x) + (a.y-b.y)*(a.y-b.y));
+}
 
-*/
+/* @} */ // end of group
 
-};
+}; // namespace Math
diff --git a/src/math/test/CMakeLists.txt b/src/math/test/CMakeLists.txt
index d82a9b9..af9bcca 100644
--- a/src/math/test/CMakeLists.txt
+++ b/src/math/test/CMakeLists.txt
@@ -4,11 +4,22 @@ set(CMAKE_BUILD_TYPE debug)
 set(CMAKE_CXX_FLAGS_DEBUG "-Wall -g -O0")
 
 add_executable(matrix_test matrix_test.cpp)
+add_executable(vector_test vector_test.cpp)
 
 enable_testing()
 
 add_test(matrix_test ./matrix_test)
+add_test(vector_test ./vector_test)
 
-add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS matrix_test)
+# 'make check' will compile the required test programs
+# Note that 'make test' will still fail without compiled programs
+add_custom_target(check COMMAND ${CMAKE_CTEST_COMMAND} DEPENDS matrix_test vector_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
+# Files to be removed in distclean
+set(REMOVE_FILES
+ CMakeFiles Testing cmake_install.cmake CMakeCache.txt CTestTestfile.cmake Makefile
+ ./matrix_test
+ ./vector_test
+)
+
+add_custom_target(distclean COMMAND rm -rf ${REMOVE_FILES})
\ No newline at end of file
diff --git a/src/math/test/matrix_test.cpp b/src/math/test/matrix_test.cpp
index c7e9c01..6ca92be 100644
--- a/src/math/test/matrix_test.cpp
+++ b/src/math/test/matrix_test.cpp
@@ -18,8 +18,8 @@
 
 /* Unit tests for Matrix struct
 
-   Test data was randomly generated and the expected
-   results calculated using GNU Octave.
+   Test data was randomly generated and the expected results
+   calculated using GNU Octave.
 
  */
 
@@ -30,53 +30,77 @@
 
 using namespace std;
 
-const float TEST_TOLERANCE = 1e-5;
+const float TEST_TOLERANCE = 1e-6;
 
 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 Math::Matrix mat1(
+    (float[4][4])
+    {
+      {  0.610630320796245,  1.059932357918312, -1.581674311378210,  1.782214448453331 },
+      {  0.191028848211526, -0.813898708757524,  1.516114203870644,  0.395202639476002 },
+      {  0.335142750345279, -0.346586619596529,  0.545382042472336, -0.879268918923072 },
+      {  1.417588151657198,  1.450841789070141,  0.219080104196171,  0.378724047481655 }
+    }
+  );
+
+  const Math::Matrix expectedCofactors1(
+    (float[4][4])
+    {
+      { -2.402679369186782,  2.282452509293019,  1.722732204057644, -0.746939701104385 },
+      { -0.687677756877654,  1.168949180331164, -0.985354966837796, -1.334071111592705 },
+      { -5.115621958424845,  4.229724770159009,  2.529000630782808,  1.481632618355891 },
+      {  0.147480897398694, -2.140677680337111, -1.207189492265546,  0.151236920408051 }
+    }
+  );
 
-  const float EXPECTED_RESULTS[4][4] =
+  for (int r = 0; r < 4; ++r)
   {
-    {  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 }
-  };
+    for (int c = 0; c < 4; ++c)
+    {
+      float ret = mat1.Cofactor(r, c);
+      float exp = expectedCofactors1.m[4*c+r];
+      if (! Math::IsEqual(ret, exp, TEST_TOLERANCE))
+      {
+        fprintf(stderr, "Cofactors 1 mismatch!\n");
+        fprintf(stderr, "r=%d, c=%d, %f (returned) != %f (expected)\n", r, c, ret, exp);
+        return __LINE__;
+      }
+    }
+  }
+
+  const Math::Matrix mat2(
+    (float[4][4])
+    {
+      {  0.9845099464982393, -0.9091233416532389, -0.6272243714245945,  0.4645001858944354 },
+      { -0.1333308471483736,  0.9128181433725897, -1.0937461393836190,  0.3180936795928376 },
+      { -0.0654324396846289,  0.1014641705415945,  1.5107709042683430, -0.0240560430414690 },
+      {  0.0179638644093347, -1.0695585982782767, -0.1741250853101032,  1.0803106709464336 }
+    }
+  );
+
+  const Math::Matrix expectedCofactors2(
+    (float[4][4])
+    {
+      {  2.0861102207614466,  0.2989010779528912,  0.0746276150537432,  0.2732659822656097 },
+      {  0.6850002886584565,  1.5513169659641379, -0.0503743176545917,  1.5163672441575642 },
+      {  1.2385556680997216,  1.1827709562505695,  1.2282813085138962,  1.3483789679871401 },
+      { -1.0710790241539783, -0.5589604503588883,  0.0100959837872308,  1.1897872684455839 }
+    }
+  );
 
-  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];
+      float ret = mat2.Cofactor(r, c);
+      float exp = expectedCofactors2.m[4*c+r];
       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;
+        fprintf(stderr, "Cofactors 2 mismatch!\n");
+        fprintf(stderr, "r=%d, c=%d, %f (returned) != %f (expected)\n", r, c, ret, exp);
+        return __LINE__;
       }
     }
   }
@@ -86,96 +110,105 @@ int TestCofactor()
 
 int TestDet()
 {
-  const float TEST_MATRIX[16] =
+  const Math::Matrix mat1(
+    (float[4][4])
+    {
+      { -0.95880162984708284,  0.24004047608997131, -0.78172309932665407, -0.11604124457222834 },
+      { -0.36230592086261376, -0.75778166876017261,  0.33041059404631740, -1.06001391941094836 },
+      {  0.00260215210936187,  1.27485610196385113, -0.26149859846418033, -0.59669701186364876 },
+      {  0.36899429848485432,  3.01720896813933104,  2.10311476609438719, -1.68627076626448269 }
+    }
+  );
+
+  const float expectedDet1 = 4.07415413729671;
+
+  float ret1 = mat1.Det();
+  if (! Math::IsEqual(ret1, expectedDet1, TEST_TOLERANCE))
   {
-     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
-  };
+    fprintf(stderr, "Det mismatch!\n");
+    fprintf(stderr, "%f (returned) != %f (expected)\n", ret1, expectedDet1);
+    return __LINE__;
+  }
 
-  const float EXPECTED_RESULT = 0.084360;
+  const Math::Matrix mat2(
+    (float[4][4])
+    {
+      { -1.0860073221346871,  0.9150354098189495, -0.2723201933559999,  0.2922832160271507 },
+      { -1.0248331304801788, -2.5081237461125205, -1.0277123574586633, -0.2254690663329798 },
+      { -1.4227635282899367, -0.0403846809122684,  0.9216148477171653,  1.2517067488015878 },
+      { -0.1160254467152022,  0.8270675274393656,  1.0327218739781614, -0.3674886870220400 }
+    }
+  );
 
-  float ret = Math::Matrix(TEST_MATRIX).Det();
-  if (! Math::IsEqual(ret, EXPECTED_RESULT, TEST_TOLERANCE))
+  const float expectedDet2 = -6.35122307880942;
+
+  float ret2 = mat2.Det();
+  if (! Math::IsEqual(ret2, expectedDet2, TEST_TOLERANCE))
   {
-    fprintf(stderr, "Det %f (returned) != %f (expected)\n", ret, EXPECTED_RESULT);
-    return 1;
+    fprintf(stderr, "Det mismatch!\n");
+    fprintf(stderr, "%f (returned) != %f (expected)\n", ret2, expectedDet2);
+    return __LINE__;
   }
 
   return 0;
 }
 
-int TestInvert()
+int TestInverse()
 {
-  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 Math::Matrix mat1(
+    (float[4][4])
+    {
+      { -2.2829352811514658, -0.9103222363187888,  0.2792976509411680, -0.7984393573193174 },
+      {  2.4823665798689589, -0.0599056759070980,  0.3832364352926366, -1.6404257204372739 },
+      { -0.3841952272526398, -0.8377700696457873, -0.3416328338427138,  1.1746577275723329 },
+      {  0.1746031241954947, -0.4952532117949962,  0.2155084379835037, -1.6586460437329220 }
+    }
+  );
+
+  const Math::Matrix expectedInverse1(
+    (float[4][4])
+    {
+      { -0.119472603171041,  0.331675963276297,  0.187516809009720, -0.137720814290806 },
+      { -0.387591686166085, -0.487284946727583, -0.798527541290274,  0.102991635972060 },
+      {  2.601905603425902,  2.606899016264679, -0.528006148839176, -4.204703326522837 },
+      {  0.441220327151392,  0.519128136207318,  0.189567009205522, -1.194469716136194 }
+    }
+  );
+
+  Math::Matrix inverse1 = mat1.Inverse();
 
-  const float EXPECTED_RESULT[16] =
+  if (! Math::MatricesEqual(inverse1, expectedInverse1, TEST_TOLERANCE))
   {
-     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
-  };
+    fprintf(stderr, "Inverse 1 mismatch!\n");
+    return __LINE__;
+  }
+
+  const Math::Matrix mat2(
+    (float[4][4])
+    {
+      { -0.05464332404298505, -0.64357755258235749, -0.13017671677619302, -0.56742332785888006 },
+      {  0.29048383600458222, -0.91517047043724875,  0.84517524415561684,  0.51628195547960565 },
+      {  0.00946488004480186, -0.89077382212689293,  0.73565573766341397, -0.15932513521840930 },
+      { -1.01244718912499132, -0.27840911963972276, -0.39189681211309862,  1.18315064340192055 }
+    }
+  );
 
-  Math::Matrix mat(TEST_MATRIX);
-  mat.Invert();
+  const Math::Matrix expectedInverse2(
+    (float[4][4])
+    {
+      {  0.771302711132012,  1.587542278361995, -2.003075114445104, -0.592574156227379 },
+      { -1.208929259769431, -0.786598967848473,  0.607335305808052, -0.154759693303324 },
+      { -1.500037668208218, -0.774300278997914,  1.917800427261255, -0.123268572651291 },
+      { -0.121314770937944,  0.916925149209746, -0.935924950785014,  0.260875394250671 }
+    }
+  );
+
+  Math::Matrix inverse2 = mat2.Inverse();
 
-  if (! Math::MatricesEqual(mat, EXPECTED_RESULT, TEST_TOLERANCE))
+  if (! Math::MatricesEqual(inverse2, expectedInverse2, TEST_TOLERANCE))
   {
-    fprintf(stderr, "Invert mismatch\n");
-    return 1;
+    fprintf(stderr, "Inverse 2 mismatch!\n");
+    return __LINE__;
   }
 
   return 0;
@@ -183,81 +216,78 @@ int TestInvert()
 
 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 Math::Matrix mat1A(
+    (float[4][4])
+    {
+      {  0.6561727049162027, -1.4180263627131411, -0.8271026046117423,  2.3919331748512578 },
+      { -0.6035665535146352,  0.0150827348790615, -0.7090794192822540,  0.9057604704594814 },
+      { -0.9871045001223655, -0.4980646811455065,  0.3806177002298990,  0.1520583649240934 },
+      { -0.2721911170792712,  0.7627928194552067, -0.1504091336784158,  0.9747545351840121 }
+    }
+  );
 
-  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 Math::Matrix mat1B(
+    (float[4][4])
+    {
+      { -0.2643735892448818, -0.7542994492819621,  0.6082322350568750,  0.0581733424861419 },
+      {  1.0293246070431237,  0.1979285388251341, -0.2932031385332818,  0.8838407179018929 },
+      {  0.3448687251553114,  0.5031654871245456,  0.7554693012922442, -0.4845315903845708 },
+      { -1.8662838497278593, -0.7843850624747805,  0.1389026096476257, -1.3686415408300689 }
+    }
+  );
+
+  const Math::Matrix expectedMultiply1(
+    (float[4][4])
+    {
+      { -6.382352236417988, -3.067984733682130,  0.522270304251466, -4.088079444498280 },
+      { -1.759853366848825, -0.608994052024491, -0.781406179437379, -0.917870775786188 },
+      { -0.404226802169062,  0.718232546720114, -0.145688356880835, -0.890167707987175 },
+      { -1.013918490922430, -0.483971504099758, -0.367442194643757, -0.602858486133615 }
+    }
+  );
 
-  const float EXPECTED_RESULT[16] =
+  Math::Matrix multiply1 = Math::MultiplyMatrices(mat1A, mat1B);
+  if (! Math::MatricesEqual(multiply1, expectedMultiply1, TEST_TOLERANCE ) )
   {
-     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
-  };
+    fprintf(stderr, "Multiply 1 mismath!\n");
+    return __LINE__;
+  }
+
+  const Math::Matrix mat2A(
+    (float[4][4])
+    {
+      {  0.8697203025776754,  2.1259475710644935,  1.7856691009707812, -2.1563963348328126 },
+      {  1.5888074489288735, -0.0794849733953615,  0.7307782768677457,  0.7943129159612630 },
+      {  0.2859761537233830, -0.6231231890384962, -0.0496743172880377, -0.8137857518646087 },
+      {  1.2670547229512983, -0.5305171374831831, -0.4987412674062375, -1.1257327113869595 }
+    }
+  );
 
-  Math::Matrix matA(TEST_MATRIX_A);
-  Math::Matrix matB(TEST_MATRIX_B);
+  const Math::Matrix mat2B(
+    (float[4][4])
+    {
+      {  1.1321105701165317,  0.1759563504574463, -2.0675778912000418,  1.4840339814245538 },
+      { -1.5117280888829916, -0.0933013188828093, -0.2079262944351640,  0.9575727579539316 },
+      {  0.3615378398970173,  1.2465163589027248,  1.1326150997082589,  0.9921208694352303 },
+      { -0.7357104529373861, -0.4774022005969588, -0.2118739096676499,  1.1427567093270703 }
+    }
+  );
+
+  const Math::Matrix expectedMultiply2(
+    (float[4][4])
+    {
+      {  0.00283516267056338,  3.21001319965989307,  0.23910503934370686,  2.63380716363006107 },
+      {  1.59868505822469742,  0.81869715594617765, -2.60905981088293570,  3.91445839239110294 },
+      {  1.84650099286297942,  0.43504079532852930, -0.34555619012424243, -1.15152951542451487 },
+      {  2.88434318563174585,  0.18818239851585700, -2.83579436909308980, -0.40890672198610400 }
+    }
+  );
 
-  Math::Matrix mat;
-  Math::MultiplyMatrices(matA, matB, mat);
-  if (! Math::MatricesEqual(mat, Math::Matrix(EXPECTED_RESULT), TEST_TOLERANCE ) )
+  Math::Matrix multiply2 = Math::MultiplyMatrices(mat2A, mat2B);
+  if (! Math::MatricesEqual(multiply2, expectedMultiply2, TEST_TOLERANCE ) )
   {
-    fprintf(stderr, "Multiply mismath!\n");
-    return 1;
+    fprintf(stderr, "Multiply 2 mismath!\n");
+    return __LINE__;
   }
 
   return 0;
@@ -265,23 +295,25 @@ int TestMultiply()
 
 int main()
 {
-  int result = 0;
-
-  result = TestCofactor();
-  if (result != 0)
-    return result;
-
-  result = TestDet();
-  if (result != 0)
-    return result;
+  // Functions to test
+  int (*TESTS[])() =
+  {
+    TestCofactor,
+    TestDet,
+    TestInverse,
+    TestMultiply
+  };
+  const int TESTS_SIZE = sizeof(TESTS) / sizeof(*TESTS);
 
-  result = TestInvert();
-  if (result != 0)
-    return result;
+  int result = 0;
+  for (int i = 0; i < TESTS_SIZE; ++i)
+  {
+    result = TESTS[i]();
+    if (result != 0)
+      return result;
+  }
 
-  result = TestMultiply();
-  if (result != 0)
-    return result;
+  fprintf(stderr, "All tests successful\n");
 
-  return result;
+  return 0;
 }
diff --git a/src/math/test/vector_test.cpp b/src/math/test/vector_test.cpp
new file mode 100644
index 0000000..2f4dd09
--- /dev/null
+++ b/src/math/test/vector_test.cpp
@@ -0,0 +1,126 @@
+// * 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/vector_test.cpp
+
+/* Unit tests for Vector struct
+
+   Test data was randomly generated and the expected results
+   calculated using GNU Octave.
+
+ */
+
+#include "../func.h"
+#include "../vector.h"
+
+#include <cstdio>
+
+using namespace std;
+
+const float TEST_TOLERANCE = 1e-6;
+
+int TestLength()
+{
+  Math::Vector vec(-1.288447945923275, 0.681452565308134, -0.633761098985957);
+  const float expectedLength = 1.58938001708428;
+
+  if (! Math::IsEqual(vec.Length(), expectedLength, TEST_TOLERANCE) )
+  {
+    fprintf(stderr, "Length mismatch!\n");
+    return __LINE__;
+  }
+
+  return 0;
+}
+
+int TestNormalize()
+{
+  Math::Vector vec(1.848877241804398, -0.157262961268577, -1.963031403332377);
+  const Math::Vector expectedNormalized(0.6844609421393856, -0.0582193085618106, -0.7267212194481797);
+
+  vec.Normalize();
+
+  if (! Math::VectorsEqual(vec, expectedNormalized, TEST_TOLERANCE))
+  {
+    fprintf(stderr, "Normalize mismatch!\n");
+    return __LINE__;
+  }
+
+  return 0;
+}
+
+int TestDot()
+{
+  Math::Vector vecA(0.8202190530968309, 0.0130926060162780, 0.2411914183883510);
+  Math::Vector vecB(-0.0524083951404069, 1.5564932716738220, -0.8971342631500536);
+
+  float expectedDot = -0.238988896477326;
+
+  if (! Math::IsEqual(Math::DotProduct(vecA, vecB), expectedDot, TEST_TOLERANCE) )
+  {
+    fprintf(stderr, "Dot product mismatch!\n");
+    return __LINE__;
+  }
+
+  return 0;
+}
+
+int TestCross()
+{
+  Math::Vector vecA(1.37380499798567, 1.18054518384682, 1.95166361293121);
+  Math::Vector vecB(0.891657855926886, 0.447591335394532, -0.901604070087823);
+
+  Math::Vector expectedCross(-1.937932065431669, 2.978844370287636, -0.437739173833581);
+  Math::Vector expectedReverseCross = -expectedCross;
+
+  if (! Math::VectorsEqual(vecA.CrossMultiply(vecB), expectedCross, TEST_TOLERANCE) )
+  {
+    fprintf(stderr, "Cross product mismatch!\n");
+    return __LINE__;
+  }
+
+  if (! Math::VectorsEqual(vecB.CrossMultiply(vecA), expectedReverseCross, TEST_TOLERANCE) )
+  {
+    fprintf(stderr, "Reverse cross product mismatch!\n");
+    return __LINE__;
+  }
+
+  return 0;
+}
+
+int main()
+{
+  // Functions to test
+  int (*TESTS[])() =
+  {
+    TestNormalize,
+    TestDot,
+    TestCross
+  };
+  const int TESTS_SIZE = sizeof(TESTS) / sizeof(*TESTS);
+
+  int result = 0;
+  for (int i = 0; i < TESTS_SIZE; ++i)
+  {
+    result = TESTS[i]();
+    if (result != 0)
+      return result;
+  }
+
+  fprintf(stderr, "All tests successful\n");
+
+  return 0;
+}
diff --git a/src/math/vector.h b/src/math/vector.h
index 89deb25..885b163 100644
--- a/src/math/vector.h
+++ b/src/math/vector.h
@@ -14,9 +14,9 @@
 // * 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/vector.h
-
-/* Vector struct and functions */
+/** @defgroup MathVectorModule math/vector.h
+   Contains the Vector struct and related functions.
+ */
 
 #pragma once
 
@@ -25,6 +25,7 @@
 
 #include <cmath>
 
+
 /*
  TODO
 
@@ -53,14 +54,17 @@ BOOL      IsSamePlane(D3DVECTOR *plan1, D3DVECTOR *plan2);
 namespace Math
 {
 
-/** 3x1 Vector
+/* @{ */ // start of group
+
+/** \struct Vector math/vector.h
+    \brief 3D (3x1) vector
 
-  Represents an universal 3x1 vector that can be used in OpenGL and DirectX engines.
+  Represents a universal 3x1 vector that can be used in OpenGL and DirectX engines.
   Contains the required methods for operating on vectors.
 
   All methods are made inline to maximize optimization.
 
-  TODO test
+  Unit tests for the structure and related functions are in module: math/test/vector_test.cpp.
 
  */
 struct Vector
@@ -102,22 +106,28 @@ struct Vector
   inline void Normalize()
   {
     float l = Length();
+    if (Math::IsZero(l))
+      return;
+
     x /= l;
     y /= l;
     z /= l;
   }
 
   //! Calculates the cross product with another vector
-  /** \a right right-hand side vector */
-  inline void CrossMultiply(const Vector &right)
+  /** \a right right-hand side vector
+      \returns the cross product*/
+  inline Vector CrossMultiply(const Vector &right) const
   {
-    Vector left = *this;
-    x = left.y * right.z - left.z * right.y;
-    y = left.z * right.x - left.x * right.z;
-    z = left.x * right.y - left.y * right.x;
+    float px = y * right.z - z * right.y;
+    float py = z * right.x - x * right.z;
+    float pz = x * right.y - y * right.x;
+    return Vector(px, py, pz);
   }
 
   //! Calculates the dot product with another vector
+  /** \a right right-hand side vector
+      \returns the dot product */
   inline float DotMultiply(const Vector &right) const
   {
     return x * right.x + y * right.y + z * right.z;
@@ -208,6 +218,14 @@ struct Vector
   }
 };
 
+//! Convenience function for getting normalized vector
+inline Vector Normalize(const Vector &v)
+{
+  Vector result = v;
+  result.Normalize();
+  return result;
+}
+
 //! Convenience function for calculating dot product
 inline float DotProduct(const Vector &left, const Vector &right)
 {
@@ -217,9 +235,17 @@ inline float DotProduct(const Vector &left, const Vector &right)
 //! Convenience function for calculating cross product
 inline Vector CrossProduct(const Vector &left, const Vector &right)
 {
-  Vector result = left;
-  result.CrossMultiply(right);
-  return result;
+  return left.CrossMultiply(right);
 }
 
+//! Checks if two vectors are equal within given \a tolerance
+inline bool VectorsEqual(const Vector &a, const Vector &b, float tolerance = Math::TOLERANCE)
+{
+  return Math::IsEqual(a.x, b.x, tolerance)
+      && Math::IsEqual(a.y, b.y, tolerance)
+      && Math::IsEqual(a.z, b.z, tolerance);
+}
+
+/* @} */ // end of group
+
 }; // namespace Math