diff --git a/sources/reactphysics3d/mathematics/Vector3D.h b/sources/reactphysics3d/mathematics/Vector3D.h
index 5ea1b49e..9a7acc7c 100644
--- a/sources/reactphysics3d/mathematics/Vector3D.h
+++ b/sources/reactphysics3d/mathematics/Vector3D.h
@@ -23,6 +23,7 @@
 // Libraries
 #include <cmath>
 #include "exceptions.h"
+#include "mathematics_functions.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -129,7 +130,7 @@ inline Vector3D Vector3D::crossProduct(const Vector3D& vector) const {
 
 // Return true if two vectors are parallel
 inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
-    return (approxEqual(this->scalarProduct(vector), length * vector.getLength()));
+    return (approxEqual(this->scalarProduct(vector), length() * vector.length()));
 }
 
 // Overloaded operator for multiplication between a number and a Vector3D (inline)
diff --git a/sources/reactphysics3d/mathematics/mathematics.h b/sources/reactphysics3d/mathematics/mathematics.h
index f8bb7caa..60274f4a 100644
--- a/sources/reactphysics3d/mathematics/mathematics.h
+++ b/sources/reactphysics3d/mathematics/mathematics.h
@@ -32,19 +32,13 @@
 #include "Vector3D.h"
 #include "constants.h"
 #include "exceptions.h"
+#include "mathematics_functions.h"
 #include <cstdio>
 #include <cassert>
 #include <cmath>
 
 // ---------- Mathematics functions ---------- //
 
-// function to test if two numbers are (almost) equal
-// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
-inline bool approxEqual(double a, double b) {
-    double difference = a - b;
-    return (difference < EPSILON && difference > -EPSILON);
-}
-
 // TODO : Test this method
 // Rotate a vector according to a rotation quaternion.
 // The function returns the vector rotated according to the quaternion in argument
@@ -59,6 +53,7 @@ inline reactphysics3d::Vector3D rotateVectorWithQuaternion(const reactphysics3d:
     return quaternionResult.vectorV();
 }
 
+/*
 // TODO : Test this method
 // Move a set of points by a given vector.
 // The method returns a set of points moved by the given vector.
@@ -121,19 +116,6 @@ inline reactphysics3d::Segment3D computeParallelSegmentsIntersection(const react
     // We should never go here
     assert(false);
 }
-
-// TODO : Delete this method if not needed
-// Return the intersection between a Segment3D and a Polygon3D that are on the same plane. The result of the intersection
-// will be another Segment3D.
-//inline reactphysics3d::Segment3D computeSegmentPolygonIntersection(const reactphysics3d::Segment3D& segment, const reactphysics3d::Polygon3D& polygon) {
-    // TODO : Implement this method
-//}
-
-// TODO : Delete this method if not needed
-// Return the intersection between two Polygon3D that are on the same plane. The result of the intersection
-// will be another Polygon3D.
-//inline reactphysics3d::Polygon3D computePolygonPolygonIntersection(const reactphysics3d::Polygon3D& polygon1, const reactphysics3d::Polygon3D& polygon2) {
-    // TODO : Implement this method
-//}
+*/
 
 #endif
diff --git a/sources/reactphysics3d/mathematics/mathematics_functions.h b/sources/reactphysics3d/mathematics/mathematics_functions.h
new file mode 100644
index 00000000..91eefa2b
--- /dev/null
+++ b/sources/reactphysics3d/mathematics/mathematics_functions.h
@@ -0,0 +1,37 @@
+/****************************************************************************
+* Copyright (C) 2009      Daniel Chappuis                                  *
+****************************************************************************
+* This file is part of ReactPhysics3D.                                     *
+*                                                                          *
+* ReactPhysics3D is free software: you can redistribute it and/or modify   *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or     *
+* (at your option) any later version.                                      *
+*                                                                          *
+* ReactPhysics3D 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 Lesser General Public License for more details.                      *
+*                                                                          *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see <http://www.gnu.org/licenses/>.   *
+***************************************************************************/
+
+#ifndef MATHEMATICS_FUNCTIONS_H
+#define MATHEMATICS_FUNCTIONS_H
+
+// Libraries
+#include "constants.h"
+
+// ---------- Mathematics functions ---------- //
+
+// function to test if two numbers are (almost) equal
+// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
+inline bool approxEqual(double a, double b) {
+    double difference = a - b;
+    return (difference < EPSILON && difference > -EPSILON);
+}
+
+
+
+#endif