diff --git a/include/Nazara/Math/Algorithm.hpp b/include/Nazara/Math/Algorithm.hpp
index a842c1df8..f1820e844 100644
--- a/include/Nazara/Math/Algorithm.hpp
+++ b/include/Nazara/Math/Algorithm.hpp
@@ -16,6 +16,10 @@
 #define M_PI 3.141592653589793238462643
 #endif
 
+#ifndef M_PI_2
+#define M_PI_2  1.5707963267948966192313217
+#endif
+
 #ifndef M_SQRT2
 #define M_SQRT2 1.4142135623730950488016887
 #endif
diff --git a/include/Nazara/Math/Algorithm.inl b/include/Nazara/Math/Algorithm.inl
index 86ed6d9f6..6a7e5b4ed 100644
--- a/include/Nazara/Math/Algorithm.inl
+++ b/include/Nazara/Math/Algorithm.inl
@@ -223,10 +223,10 @@ T NzNormalizeAngle(T angle)
 template<typename T>
 bool NzNumberEquals(T a, T b, T maxDifference)
 {
-	T diff = a - b;
-	if (diff < F(0.0))
-		diff = -diff;
+	if (b > a)
+		std::swap(a, b);
 
+	T diff = a - b;
 	return diff <= maxDifference;
 }
 
diff --git a/tests/Nazara/Math/Algorithm.cpp b/tests/Nazara/Math/Algorithm.cpp
new file mode 100644
index 000000000..8683a3058
--- /dev/null
+++ b/tests/Nazara/Math/Algorithm.cpp
@@ -0,0 +1,197 @@
+#include <Nazara/Math/Algorithm.hpp>
+#include <catch.hpp>
+
+TEST_CASE("Approach", "[MATH][ALGORITHM]" )
+{
+	SECTION("Approach 8 with 5 by 2")
+	{
+		REQUIRE(NzApproach(5, 8, 2) == 7);
+	}
+
+	SECTION("Approach 5 with 8 by 2")
+	{
+		REQUIRE(NzApproach(8, 5, 2) == 6);
+	}
+
+	SECTION("Approach 8 with 8 by 2")
+	{
+		REQUIRE(NzApproach(8, 8, 2) == 8);
+	}
+}
+
+TEST_CASE("Clamp", "[ALGORITHM]" )
+{
+	SECTION("Clamp 8 between 5 and 10")
+	{
+		REQUIRE(NzClamp(8, 5, 10) == 8);
+	}
+
+	SECTION("Clamp 4 between 5 and 10")
+	{
+		REQUIRE(NzClamp(4, 5, 10) == 5);
+	}
+
+	SECTION("Clamp 12 between 5 and 10")
+	{
+		REQUIRE(NzClamp(12, 5, 10) == 10);
+	}
+}
+
+TEST_CASE("DegreeToRadian", "[ALGORITHM]" )
+{
+	SECTION("Convert 45.f degree to radian")
+	{
+		REQUIRE(NzDegreeToRadian(45.f) == Approx(M_PI / 4));
+	}
+}
+
+TEST_CASE("GetNearestPowerOfTwo", "[ALGORITHM]" )
+{
+	SECTION("Nearest power of two of 16 = 16")
+	{
+		REQUIRE(NzGetNearestPowerOfTwo(16) == 16);
+	}
+
+	SECTION("Nearest power of two of 17 = 32")
+	{
+		REQUIRE(NzGetNearestPowerOfTwo(17) == 32);
+	}
+}
+
+TEST_CASE("GetNumberLength", "[ALGORITHM]" )
+{
+	SECTION("GetNumberLength of -127 signed char")
+	{
+		signed char minus127 = -127;
+		REQUIRE(NzGetNumberLength(minus127) == 4);
+	}
+
+	SECTION("GetNumberLength of 255 unsigned char")
+	{
+		unsigned char plus255 = 255;
+		REQUIRE(NzGetNumberLength(plus255) == 3);
+	}
+
+	SECTION("GetNumberLength of -1270 signed int")
+	{
+		signed int minus1270 = -1270;
+		REQUIRE(NzGetNumberLength(minus1270) == 5);
+	}
+
+	SECTION("GetNumberLength of 2550 unsigned int")
+	{
+		unsigned int plus2550 = 2550;
+		REQUIRE(NzGetNumberLength(plus2550) == 4);
+	}
+
+	SECTION("GetNumberLength of -1270 signed long long")
+	{
+		signed long long minus12700 = -12700;
+		REQUIRE(NzGetNumberLength(minus12700) == 6);
+	}
+
+	SECTION("GetNumberLength of 2550 unsigned long long")
+	{
+		unsigned long long plus25500 = 25500;
+		REQUIRE(NzGetNumberLength(plus25500) == 5);
+	}
+
+	SECTION("GetNumberLength of -2.456f float")
+	{
+		float minus2P456 = -2.456f;
+		REQUIRE(NzGetNumberLength(minus2P456, 3) == 6);
+	}
+
+	SECTION("GetNumberLength of -2.456 double")
+	{
+		double minus2P456 = -2.456;
+		REQUIRE(NzGetNumberLength(minus2P456, 3) == 6);
+	}
+
+	SECTION("GetNumberLength of -2.456 long double")
+	{
+		long double minus2P456 = -2.456L;
+		REQUIRE(NzGetNumberLength(minus2P456, 3) == 6);
+	}
+}
+
+TEST_CASE("IntegralPow", "[ALGORITHM]" )
+{
+	SECTION("2 to power 4")
+	{
+		REQUIRE(NzIntegralPow(2, 4) == 16);
+	}
+}
+
+TEST_CASE("Lerp", "[ALGORITHM]" )
+{
+	SECTION("Lerp 2 to 6 with 0.5")
+	{
+		REQUIRE(NzLerp(2, 6, 0.5) == 4);
+	}
+}
+
+TEST_CASE("MultiplyAdd", "[ALGORITHM]" )
+{
+	SECTION("2 * 3 + 1")
+	{
+		REQUIRE(NzMultiplyAdd(2, 3, 1) == 7);
+	}
+}
+
+TEST_CASE("NumberEquals", "[ALGORITHM]" )
+{
+	SECTION("2.35 and 2.351 should be the same at 0.01")
+	{
+		CHECK(NzNumberEquals(2.35, 2.35, 0.01));
+	}
+
+	SECTION("3 and 4 unsigned should be the same at 1")
+	{
+		CHECK(NzNumberEquals(3U, 4U, 1U));
+	}
+}
+
+TEST_CASE("NumberToString", "[ALGORITHM]" )
+{
+	SECTION("235 to string")
+	{
+		REQUIRE(NzNumberToString(235) == "235");
+	}
+
+	SECTION("-235 to string")
+	{
+		REQUIRE(NzNumberToString(-235) == "-235");
+	}
+
+	SECTION("16 in base 16 to string")
+	{
+		REQUIRE(NzNumberToString(16, 16) == "10");
+	}
+}
+
+TEST_CASE("RadianToDegree", "[ALGORITHM]" )
+{
+	SECTION("PI / 4 to degree")
+	{
+		REQUIRE(NzRadianToDegree(M_PI / 4) == Approx(45.f));
+	}
+}
+
+TEST_CASE("StringToNumber", "[ALGORITHM]" )
+{
+	SECTION("235 in string")
+	{
+		REQUIRE(NzStringToNumber("235") == 235);
+	}
+
+	SECTION("-235 in string")
+	{
+		REQUIRE(NzStringToNumber("-235") == -235);
+	}
+
+	SECTION("16 in base 16 in string")
+	{
+		REQUIRE(NzStringToNumber("10", 16) == 16);
+	}
+}