diff --git a/src/math/all.h b/src/math/all.h
index fcbcb195..13a9290a 100644
--- a/src/math/all.h
+++ b/src/math/all.h
@@ -29,4 +29,6 @@
 #include "matrix.h"
 #include "geometry.h"
 
+#include "conv.h"
+
 /* @} */ // end of group
diff --git a/src/math/const.h b/src/math/const.h
index 89802870..67212844 100644
--- a/src/math/const.h
+++ b/src/math/const.h
@@ -39,14 +39,6 @@ namespace Math
 
   //! PI
   const float PI       = 3.14159265358979323846f;
-  //! 2 * PI
-  const float PI_MUL_2 = 6.28318530717958623200f;
-  //! PI / 2
-  const float PI_DIV_2 = 1.57079632679489655800f;
-  //! PI / 4
-  const float PI_DIV_4 = 0.78539816339744827900f;
-  //! 1 / PI
-  const float INV_PI   = 0.31830988618379069122f;
 
   //! Degrees to radians multiplier
   const float DEG_TO_RAD =  0.01745329251994329547f;
diff --git a/src/math/conv.h b/src/math/conv.h
new file mode 100644
index 00000000..8b77db90
--- /dev/null
+++ b/src/math/conv.h
@@ -0,0 +1,33 @@
+/* math/conv.h
+
+ Temporary conversion functions for D3DVECTOR and FPOINT */
+
+#pragma once
+
+#define STRICT
+#define D3D_OVERLOADS
+#include <d3d.h>
+
+#include "common/struct.h"
+#include "vector.h"
+#include "point.h"
+
+inline D3DVECTOR V_TO_D3D(Math::Vector vec)
+{
+  return D3DVECTOR(vec.x, vec.y, vec.z);
+}
+
+inline Math::Vector D3D_TO_V(D3DVECTOR vec)
+{
+  return Math::Vector(vec.x, vec.y, vec.z);
+}
+
+inline FPOINT P_TO_FP(Math::Point pt)
+{
+  return FPOINT(pt.x, pt.y);
+}
+
+inline Math::Point FP_TO_P(FPOINT pt)
+{
+  return Math::Point(pt.x, pt.y);
+}
diff --git a/src/math/func.h b/src/math/func.h
index 79f43c16..e5e1321f 100644
--- a/src/math/func.h
+++ b/src/math/func.h
@@ -100,9 +100,7 @@ inline float Norm(float a)
 //! Swaps two integers
 inline void Swap(int &a, int &b)
 {
-  int   c;
-
-  c = a;
+  int c = a;
   a = b;
   b = c;
 }
@@ -110,9 +108,7 @@ inline void Swap(int &a, int &b)
 //! Swaps two real numbers
 inline void Swap(float &a, float &b)
 {
-  float c;
-
-  c = a;
+  float c = a;
   a = b;
   b = c;
 }
@@ -125,12 +121,18 @@ inline float Mod(float a, float m)
   return a - ((int)(a/m))*m;
 }
 
+//! Returns a random value between 0 and 1.
+inline float Rand()
+{
+  return (float)rand()/RAND_MAX;
+}
+
 //! Returns a normalized angle, that is in other words between 0 and 2 * PI
 inline float NormAngle(float angle)
 {
-  angle = Mod(angle, PI_MUL_2);
+  angle = Mod(angle, PI*2.0f);
   if ( angle < 0.0f )
-    return PI_MUL_2 + angle;
+    return PI*2.0f + angle;
 
   return angle;
 }
@@ -148,6 +150,15 @@ inline bool TestAngle(float angle, float min, float max)
   return ( angle >= min && angle <= max );
 }
 
+//! Calculates a value (radians) proportional between a and b (degrees)
+float PropAngle(int a, int b, float p)
+{
+  float aa = (float)a * DEG_TO_RAD;
+  float bb = (float)b * DEG_TO_RAD;
+
+  return aa+p*(bb-aa);
+}
+
 //! Calculates the angle to rotate the angle \a a to the angle \a g
 /** A positive angle is counterclockwise (CCW). */
 inline float Direction(float a, float g)
@@ -167,20 +178,13 @@ inline float Direction(float a, float g)
   return g-a;
 }
 
-//! Returns a random value between 0 and 1.
-inline float Rand()
-{
-  return (float)rand()/RAND_MAX;
-}
-
-
 //! Managing the dead zone of a joystick.
 /**
-    in:   -1            0            1
-         --|-------|----o----|-------|-->
-                      <---->
-                       dead
-    out:  -1       0         0       1 */
+\verbatimin:   -1            0            1
+--|-------|----o----|-------|-->
+             <---->
+              dead
+out:  -1       0         0       1\endverbatim */
 float Neutral(float value, float dead)
 {
   if ( fabs(value) <= dead )
@@ -194,16 +198,6 @@ float Neutral(float value, float dead)
   }
 }
 
-
-//! Calculates a value (radians) proportional between a and b (degrees)
-float Prop(int a, int b, float p)
-{
-  float aa = (float)a * DEG_TO_RAD;
-  float bb = (float)b * DEG_TO_RAD;
-
-  return aa+p*(bb-aa);
-}
-
 //! Gently advances a desired value from its current value
 /** Over time, the progression is more rapid. */
 float Smooth(float actual, float hope, float time)
@@ -222,19 +216,19 @@ float Smooth(float actual, float hope, float time)
   return future;
 }
 
-
 //! Bounces any movement
-/**  out
-     |
-    1+------o-------o---
-     |    o | o   o |  | bounce
-     |   o  |   o---|---
-     |  o   |       |
-     | o    |       |
-    -o------|-------+----> progress
-    0|      |       1
-     |<---->|middle */
-inline float Bounce(float progress, float middle, float bounce)
+/**
+\verbatimout
+ |
+1+------o-------o---
+ |    o | o   o |  | bounce
+ |   o  |   o---|---
+ |  o   |       |
+ | o    |       |
+-o------|-------+----> progress
+0|      |       1
+ |<---->|middle\endverbatim */
+inline float Bounce(float progress, float middle = 0.3f, float bounce = 0.4f)
 {
   if ( progress < middle )
   {
diff --git a/src/math/geometry.h b/src/math/geometry.h
index 2d79d8aa..580b9da5 100644
--- a/src/math/geometry.h
+++ b/src/math/geometry.h
@@ -200,44 +200,16 @@ inline Vector RotatePoint2(const Vector center, float angleH, float angleV, Vect
 //! Returns the angle between point (x,y) and (0,0)
 float RotateAngle(float x, float y)
 {
-  float result = std::atan2(x, y);
-  if (result < 0)
-    result = PI_MUL_2 + result;
+  if ( (x == 0.0f) && (y == 0.0f) )
+    return 0.0f;
 
-  return result;
-}
+  float atan = atan2(x, y);
 
-/*inline float RotateAngle(float x, float y)
-{
-  if ( x == 0.0f && y == 0.0f )  return 0.0f;
-
-  if ( x >= 0.0f )
-  {
-    if ( y >= 0.0f )
-    {
-      if ( x > y )  return atanf(y/x);
-      else          return Math::PI*0.5f - atanf(x/y);
-    }
-    else
-    {
-      if ( x > -y )  return Math::PI*2.0f + atanf(y/x);
-      else           return Math::PI*1.5f - atanf(x/y);
-    }
-  }
+  if ((y < 0.0f) && (x >= 0.0f))
+    return -atan + 2.5f*PI;
   else
-  {
-    if ( y >= 0.0f )
-    {
-      if ( -x > y )  return Math::PI*1.0f + atanf(y/x);
-      else           return Math::PI*0.5f - atanf(x/y);
-    }
-    else
-    {
-      if ( -x > -y )  return Math::PI*1.0f + atanf(y/x);
-      else            return Math::PI*1.5f - atanf(x/y);
-    }
-  }
-}*/
+    return -atan + 0.5f*PI;
+}
 
 //! Calculates the angle between two points and one center
 /** \a center the center point
@@ -259,7 +231,7 @@ inline float RotateAngle(const Point &center, const Point &p1, const Point &p2)
 
   float a = a2 - a1;
   if (a < 0)
-    a += PI_MUL_2;
+    a += 2.0f*PI;
 
   return a;
 }
@@ -275,7 +247,7 @@ inline void LoadViewMatrix(Matrix &mat, const Vector &from, const Vector &at, co
   Vector view = at - from;
 
   float length = view.Length();
-  assert(! Math::IsZero(length) );
+  assert(! IsZero(length) );
 
   // Normalize the z basis vector
   view /= length;
@@ -412,7 +384,7 @@ inline void LoadRotationMatrix(Matrix &mat, const Vector &dir, float angle)
 {
   float cos = cosf(angle);
   float sin = sinf(angle);
-  Vector v = Math::Normalize(dir);
+  Vector v = Normalize(dir);
 
   mat.LoadIdentity();
 
@@ -551,7 +523,6 @@ inline bool IntersectY(const Vector &a, const Vector &b, const Vector &c, Vector
 //! Calculates the end point
 inline Vector LookatPoint(const Vector &eye, float angleH, float angleV, float length)
 {
-
   Vector lookat = eye;
   lookat.z += length;
 
diff --git a/src/math/old/d3dmath.h b/src/math/old/d3dmath.h
index 0cab1923..5d95290f 100644
--- a/src/math/old/d3dmath.h
+++ b/src/math/old/d3dmath.h
@@ -47,6 +47,7 @@ const FLOAT g_EPSILON  =  1.0e-5f;                 // Tolerance for FLOATs
 //-----------------------------------------------------------------------------
 // Fuzzy compares (within tolerance)
 //-----------------------------------------------------------------------------
+//>>> func.h IsZero()
 inline bool D3DMath_IsZero( FLOAT a, FLOAT fTol = g_EPSILON )
 { return ( a <= 0.0f ) ? ( a >= -fTol ) : ( a <= fTol ); }
 
@@ -56,7 +57,9 @@ inline bool D3DMath_IsZero( FLOAT a, FLOAT fTol = g_EPSILON )
 //-----------------------------------------------------------------------------
 // Matrix functions
 //-----------------------------------------------------------------------------
+//>>> matrix.h MultiplyMatrices()
 VOID    D3DMath_MatrixMultiply( D3DMATRIX& q, D3DMATRIX& a, D3DMATRIX& b );
+//>>> matrix.h Matrix::Invert()
 HRESULT D3DMath_MatrixInvert( D3DMATRIX& q, D3DMATRIX& a );
 
 
@@ -65,8 +68,11 @@ HRESULT D3DMath_MatrixInvert( D3DMATRIX& q, D3DMATRIX& a );
 //-----------------------------------------------------------------------------
 // Vector functions
 //-----------------------------------------------------------------------------
+
+//>>> matrix.h MatrixVectorMultiply()
 HRESULT D3DMath_VectorMatrixMultiply( D3DVECTOR& vDest, D3DVECTOR& vSrc,
                                       D3DMATRIX& mat);
+// TODO
 HRESULT D3DMath_VertexMatrixMultiply( D3DVERTEX& vDest, D3DVERTEX& vSrc,
                                       D3DMATRIX& mat );
 
@@ -76,6 +82,10 @@ HRESULT D3DMath_VertexMatrixMultiply( D3DVERTEX& vDest, D3DVERTEX& vSrc,
 //-----------------------------------------------------------------------------
 // Quaternion functions
 //-----------------------------------------------------------------------------
+
+// UNUSED
+
+
 VOID D3DMath_QuaternionFromRotation( FLOAT& x, FLOAT& y, FLOAT& z, FLOAT& w,
                                      D3DVECTOR& v, FLOAT fTheta );
 VOID D3DMath_RotationFromQuaternion( D3DVECTOR& v, FLOAT& fTheta,
diff --git a/src/math/old/math3d.h b/src/math/old/math3d.h
index f08f9a61..cb176697 100644
--- a/src/math/old/math3d.h
+++ b/src/math/old/math3d.h
@@ -30,76 +30,137 @@
 
 
 
+//>>> func.h IsEqual()
 bool			IsEqual(float a, float b);
 
+//>>> func.h Min()
 float		Min(float a, float b);
 float		Min(float a, float b, float c);
 float		Min(float a, float b, float c, float d);
 float		Min(float a, float b, float c, float d, float e);
 
+//>>> func.h Max()
 float		Max(float a, float b);
 float		Max(float a, float b, float c);
 float		Max(float a, float b, float c, float d);
 float		Max(float a, float b, float c, float d, float e);
 
+//>>> func.h Norm()
 float		Norm(float a);
+//>>> fabs()
 float		Abs(float a);
-	
+
+//>>> func.h Swap()
 void			Swap(int &a, int &b);
+//>>> func.h Swap()
 void			Swap(float &a, float &b);
+//>>> point.h Swap() (FPOINT -> Point)
 void			Swap(FPOINT &a, FPOINT &b);
 
+//>>> func.h Mod()
 float		Mod(float a, float m);
+//>>> func.h NormAngle()
 float		NormAngle(float angle);
+//>>> func.h TestAngle()
 bool			TestAngle(float angle, float min, float max);
 
+//>>> func.h Direction()
 float		Direction(float a, float g);
+//>>> geometry.h RotatePoint()
 FPOINT		RotatePoint(FPOINT center, float angle, FPOINT p);
+//>>> geometry.h RotatePoint()
 FPOINT		RotatePoint(float angle, FPOINT p);
+//>>> geometry.h RotatePoint()
 FPOINT		RotatePoint(float angle, float dist);
+//>>> geometry.h RotateAngle()
 float		RotateAngle(float x, float y);
+//>>> geometry.h RotateAngle()
 float		RotateAngle(FPOINT center, FPOINT p1, FPOINT p2);
+//>>> geometry.h MidPoint()
 float		MidPoint(FPOINT a, FPOINT b, float px);
+//>>> geometry.h SegmentPoint()
 D3DVECTOR	SegmentDist(const D3DVECTOR &p1, const D3DVECTOR &p2, float dist);
+//>>> geometry.h IsInsideTriangle()
 bool			IsInsideTriangle(FPOINT a, FPOINT b, FPOINT c, FPOINT p);
+//>>> geometry.h Intersect()
 bool			Intersect(D3DVECTOR a, D3DVECTOR b, D3DVECTOR c, D3DVECTOR d, D3DVECTOR e, D3DVECTOR &i);
+//>>> geometry.h IntersectY()
 bool			IntersectY(D3DVECTOR a, D3DVECTOR b, D3DVECTOR c, D3DVECTOR &p);
+//>>> geometry.h RotatePoint()
 void			RotatePoint(float cx, float cy, float angle, float &px, float &py);
+//>>> geometry.h RotatePoint()
 void			RotatePoint(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p);
+//>>> geometry.h RotatePoint2()
 void			RotatePoint2(D3DVECTOR center, float angleH, float angleV, D3DVECTOR &p);
+//>>> geometry.h RotateView()
+// TODO test & verify
 D3DVECTOR	RotateView(D3DVECTOR center, float angleH, float angleV, float dist);
+//>>> geometry.h LookatPoint()
+// TODO test & verify
 D3DVECTOR	LookatPoint( D3DVECTOR eye, float angleH, float angleV, float length );
+//>>> point.h Distance()
 float		Length(FPOINT a, FPOINT b);
+//>>> point.h Point::Length()
 float		Length(float x, float y);
+//>>> vector.h Vector::Length()
 float		Length(const D3DVECTOR &u);
+//>>> vector.h Distance()
 float		Length(const D3DVECTOR &a, const D3DVECTOR &b);
+//>>> geometry.h DistanceProjected()
 float		Length2d(const D3DVECTOR &a, const D3DVECTOR &b);
+//>>> vector.h Angle()
+// TODO test & verify
 float		Angle( D3DVECTOR u, D3DVECTOR v );
+//>>> vector.h CrossProduct()
 D3DVECTOR	Cross( D3DVECTOR u, D3DVECTOR v );
+//>>> geometry.h NormalToPlane()
 D3DVECTOR	ComputeNormal( D3DVECTOR p1, D3DVECTOR p2, D3DVECTOR p3 );
+//>>> geometry.h Transform()
+// TODO test & verify
 D3DVECTOR	Transform(const D3DMATRIX &m, D3DVECTOR p);
+//>>> geometry.h Projection()
+// TODO test & verify
 D3DVECTOR	Projection(const D3DVECTOR &a, const D3DVECTOR &b, const D3DVECTOR &p);
 
+// TODO
 void			MappingObject( D3DVERTEX2* pVertices, int nb, float scale );
+// TODO
 void			SmoothObject( D3DVERTEX2* pVertices, int nb );
+//>>> geometry.h LinearFunction()
 bool			LineFunction(FPOINT p1, FPOINT p2, float &a, float &b);
+//>>> geometry.h DistanceToPlane()
 float		DistancePlanPoint(const D3DVECTOR &a, const D3DVECTOR &b, const D3DVECTOR &c, const D3DVECTOR &p);
+//>>> geometry.h IsSamePlane()
 bool			IsSamePlane(D3DVECTOR *plan1, D3DVECTOR *plan2);
+//>>> geometry.h LoadRotationXZYMatrix()
+// TODO test & verify
 void			MatRotateXZY(D3DMATRIX &mat, D3DVECTOR angle);
+//>>> geometry.h LoadRotationZXYMatrix()
+// TODO test & verify
 void			MatRotateZXY(D3DMATRIX &mat, D3DVECTOR angle);
 
+//>>> func.h Rand()
 float		Rand();
+//>>> func.h Neutral()
 float		Neutral(float value, float dead);
 
+//>>> func.h PropAngle()
 float		Prop(int a, int b, float p);
+//>>> func.h Smooth()
 float		Smooth(float actual, float hope, float time);
+//>>> func.h Bounce()
 float		Bounce(float progress, float middle=0.3f, float bounce=0.4f);
 
+// TODO
 D3DCOLOR		RetColor(float intensity);
+// TODO
 D3DCOLOR		RetColor(D3DCOLORVALUE intensity);
+// TODO
 D3DCOLORVALUE RetColor(D3DCOLOR intensity);
 
+// TODO
 void			RGB2HSV(D3DCOLORVALUE src, ColorHSV &dest);
+// TODO
 void			HSV2RGB(ColorHSV src, D3DCOLORVALUE &dest);
 
 
diff --git a/src/math/test/CMakeLists.txt b/src/math/test/CMakeLists.txt
index af9bccad..3d52dd97 100644
--- a/src/math/test/CMakeLists.txt
+++ b/src/math/test/CMakeLists.txt
@@ -5,11 +5,13 @@ set(CMAKE_CXX_FLAGS_DEBUG "-Wall -g -O0")
 
 add_executable(matrix_test matrix_test.cpp)
 add_executable(vector_test vector_test.cpp)
+add_executable(geometry_test geometry_test.cpp)
 
 enable_testing()
 
 add_test(matrix_test ./matrix_test)
 add_test(vector_test ./vector_test)
+add_test(geometry_test ./geometry_test)
 
 # 'make check' will compile the required test programs
 # Note that 'make test' will still fail without compiled programs
@@ -20,6 +22,7 @@ set(REMOVE_FILES
  CMakeFiles Testing cmake_install.cmake CMakeCache.txt CTestTestfile.cmake Makefile
  ./matrix_test
  ./vector_test
+ ./geometry_test
 )
 
-add_custom_target(distclean COMMAND rm -rf ${REMOVE_FILES})
\ No newline at end of file
+add_custom_target(distclean COMMAND rm -rf ${REMOVE_FILES})
diff --git a/src/math/test/geometry_test.cpp b/src/math/test/geometry_test.cpp
new file mode 100644
index 00000000..9082e3ef
--- /dev/null
+++ b/src/math/test/geometry_test.cpp
@@ -0,0 +1,83 @@
+// * 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/geometry_test.cpp
+
+/* Unit tests for functions in geometry.h */
+
+#include "../func.h"
+#include "../geometry.h"
+
+#include <cstdio>
+
+using namespace std;
+
+const float TEST_TOLERANCE = 1e-5;
+
+// Test for rewritten function RotateAngle()
+int TestRotateAngle()
+{
+  if (! Math::IsEqual(Math::RotateAngle(0.0f, 0.0f), 0.0f, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(1.0f, 0.0f), 0.0f, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(1.0f, 1.0f), 0.25f * Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(0.0f, 2.0f), 0.5f * Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(-0.5f, 0.5f), 0.75f * Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(-1.0f, 0.0f), Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(-1.0f, -1.0f), 1.25f * Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(0.0f, -2.0f), 1.5f * Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  if (! Math::IsEqual(Math::RotateAngle(1.0f, -1.0f), 1.75f * Math::PI, TEST_TOLERANCE))
+    return __LINE__;
+
+  return 0;
+}
+
+int main()
+{
+  // Functions to test
+  int (*TESTS[])() =
+  {
+    TestRotateAngle
+  };
+  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;
+}